From da27a8696ddcd454ce6ef24f40f4f8b4c4de1452 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 30 Jan 2015 17:39:31 +1100 Subject: [PATCH] AP_Mount: added initial ekf estimation of gimbal bias Pair-Programmed-With: Paul Riseborough --- libraries/AP_Mount/AP_Mount.cpp | 2 +- libraries/AP_Mount/AP_Mount.h | 14 +++++++-- libraries/AP_Mount/AP_Mount_MAVLink.cpp | 39 ++++++++++++++++++++++++- libraries/AP_Mount/AP_Mount_MAVLink.h | 17 ++++++----- 4 files changed, 61 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 54f9350dc6..3d69751783 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/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 ¤t_loc) : +AP_Mount::AP_Mount(const AP_AHRS_MOUNT &ahrs, const struct Location ¤t_loc) : _ahrs(ahrs), _current_loc(current_loc), _num_instances(0), diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index 80b4ea1892..f861c07a9b 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/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 ¤t_loc); + AP_Mount(const AP_AHRS_MOUNT &ahrs, const struct Location ¤t_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 diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp index 8dfa54ebaf..a9f9a69915 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp +++ b/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 +#if AP_AHRS_NAVEKF_AVAILABLE #include +#define MOUNT_DEBUG 0 + +#if MOUNT_DEBUG +#include +#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 diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h index adb67d308e..f363a52d89 100644 --- a/libraries/AP_Mount/AP_Mount_MAVLink.h +++ b/libraries/AP_Mount/AP_Mount_MAVLink.h @@ -7,13 +7,17 @@ #ifndef __AP_MOUNT_MAVLINK_H__ #define __AP_MOUNT_MAVLINK_H__ +#include +#include + +#if AP_AHRS_NAVEKF_AVAILABLE #include #include #include -#include #include #include #include +#include #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_Backend(frontend, state, instance), - _initialised(false), - _chan(MAVLINK_COMM_0), - _last_mode(MAV_MOUNT_MODE_RETRACT) - {} + AP_Mount_MAVLink(AP_Mount &frontend, AP_Mount::mount_state state, uint8_t instance); // 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__