AP_Mount: added initial ekf estimation of gimbal bias

Pair-Programmed-With: Paul Riseborough <p_riseborough@live.com.au>

libraries/AP_Mount/AP_Mount.cpp

@@ -385,7 +385,7 @@ const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
     AP_GROUPEND
 };
 
-AP_Mount::AP_Mount(const AP_AHRS &ahrs, const struct Location &current_loc) :
+AP_Mount::AP_Mount(const AP_AHRS_MOUNT &ahrs, const struct Location &current_loc) :
     _ahrs(ahrs),
     _current_loc(current_loc),
     _num_instances(0),

libraries/AP_Mount/AP_Mount.h

@@ -39,6 +39,16 @@ class AP_Mount_Servo;
 class AP_Mount_MAVLink;
 class AP_Mount_Alexmos;
 
+/*
+  This is a workaround to allow the MAVLink backend access to the
+  SmallEKF. It would be nice to find a neater solution to this
+ */
+#if AP_AHRS_NAVEKF_AVAILABLE
+#define AP_AHRS_MOUNT AP_AHRS_NavEKF
+#else
+#define AP_AHRS_MOUNT AP_AHRS
+#endif
+
 class AP_Mount
 {
     // declare backends as friends
@@ -58,7 +68,7 @@ public:
     };
 
     // Constructor
-    AP_Mount(const AP_AHRS &ahrs, const struct Location &current_loc);
+    AP_Mount(const AP_AHRS_MOUNT &ahrs, const struct Location &current_loc);
 
     // init - detect and initialise all mounts
     void init(const AP_SerialManager& serial_manager);
@@ -115,7 +125,7 @@ public:
 protected:
 
     // private members
-    const AP_AHRS           &_ahrs;
+    const AP_AHRS_MOUNT     &_ahrs;
     const struct Location   &_current_loc;  // reference to the vehicle's current location
 
     // frontend parameters

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -1,8 +1,23 @@
 // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
 
 #include <AP_Mount_MAVLink.h>
+#if AP_AHRS_NAVEKF_AVAILABLE
 #include <GCS_MAVLink.h>
 
+#define MOUNT_DEBUG 0
+
+#if MOUNT_DEBUG
+#include <stdio.h>
+#endif
+
+AP_Mount_MAVLink::AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) :
+    AP_Mount_Backend(frontend, state, instance),
+    _initialised(false),
+    _chan(MAVLINK_COMM_0),
+    _last_mode(MAV_MOUNT_MODE_RETRACT),
+    _ekf(frontend._ahrs)
+{}
+
 // init - performs any required initialisation for this instance
 void AP_Mount_MAVLink::init(const AP_SerialManager& serial_manager)
 {
@@ -150,6 +165,17 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
 {
     // just save it for future processing and reporting to GCS for now
     mavlink_msg_gimbal_report_decode(msg, &_gimbal_report);
+
+    Vector3f delta_angles(_gimbal_report.delta_angle_x,
+                          _gimbal_report.delta_angle_y,
+                          _gimbal_report.delta_angle_z);
+    Vector3f delta_velocity(_gimbal_report.delta_velocity_x,
+                            _gimbal_report.delta_velocity_y,
+                            _gimbal_report.delta_velocity_z);
+    Vector3f joint_angles(_gimbal_report.joint_roll,
+                          _gimbal_report.joint_pitch,
+                          _gimbal_report.joint_yaw);
+    _ekf.RunEKF(_gimbal_report.delta_time, delta_angles, delta_velocity, joint_angles);
 }
 
 /*
@@ -159,7 +185,7 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
 {
     mavlink_msg_gimbal_report_send(chan, 
                                    0, 0, // send as broadcast
-                                   _gimbal_report.counter, 
+                                   _gimbal_report.delta_time, 
                                    _gimbal_report.delta_angle_x, 
                                    _gimbal_report.delta_angle_y, 
                                    _gimbal_report.delta_angle_z, 
@@ -169,4 +195,15 @@ void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
                                    _gimbal_report.joint_roll, 
                                    _gimbal_report.joint_pitch, 
                                    _gimbal_report.joint_yaw);
+    float tilt;
+    Vector3f velocity, euler, gyroBias;
+    _ekf.getDebug(tilt, velocity, euler, gyroBias);
+#if MOUNT_DEBUG
+    ::printf("tilt=%.2f euler(%.2f, %.2f, %.2f) bias=(%.2f, %.2f %.2f)\n",
+             tilt,
+             degrees(euler.x), degrees(euler.y), degrees(euler.z),
+             degrees(gyroBias.x), degrees(gyroBias.y), degrees(gyroBias.z));
+#endif
 }
+
+#endif // AP_AHRS_NAVEKF_AVAILABLE

libraries/AP_Mount/AP_Mount_MAVLink.h

@@ -7,13 +7,17 @@
 #ifndef __AP_MOUNT_MAVLINK_H__
 #define __AP_MOUNT_MAVLINK_H__
 
+#include <AP_HAL.h>
+#include <AP_AHRS.h>
+
+#if AP_AHRS_NAVEKF_AVAILABLE
 #include <AP_Math.h>
 #include <AP_Common.h>
 #include <AP_GPS.h>
-#include <AP_AHRS.h>
 #include <GCS_MAVLink.h>
 #include <RC_Channel.h>
 #include <AP_Mount_Backend.h>
+#include <AP_SmallEKF.h>
 
 #define AP_MOUNT_MAVLINK_COMPID     10      // Use hard-coded component id to communicate with gimbal, sysid is taken from globally defined mavlink_system.sysid
 
@@ -22,12 +26,7 @@ class AP_Mount_MAVLink : public AP_Mount_Backend
 
 public:
     // Constructor
-    AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance) :
+    AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance);
-        AP_Mount_Backend(frontend, state, instance),
-        _initialised(false),
-        _chan(MAVLINK_COMM_0),
-        _last_mode(MAV_MOUNT_MODE_RETRACT)
-    {}
 
     // init - performs any required initialisation for this instance
     virtual void init(const AP_SerialManager& serial_manager);
@@ -61,8 +60,12 @@ private:
     enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount
     Vector3f _last_angle_target;    // last angle target sent to mount
 
+    // state of small EKF for gimbal
+    SmallEKF _ekf;
+
     // keep last gimbal report
     mavlink_gimbal_report_t _gimbal_report;
 };
+#endif // AP_AHRS_NAVEKF_AVAILABLE
 
 #endif // __AP_MOUNT_MAVLINK_H__