AP_Mount: fixed reporting of MAVLink gimbal position to GCS

libraries/AP_Mount/AP_Gimbal.cpp

@@ -30,19 +30,18 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
 
 void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
 {
-    mavlink_gimbal_report_t report_msg;
-    mavlink_msg_gimbal_report_decode(msg, &report_msg);
-
-    _measurement.delta_time = report_msg.delta_time;
-    _measurement.delta_angles.x = report_msg.delta_angle_x;
-    _measurement.delta_angles.y = report_msg.delta_angle_y;
-    _measurement.delta_angles.z = report_msg.delta_angle_z;
-    _measurement.delta_velocity.x = report_msg.delta_velocity_x,
-    _measurement.delta_velocity.y = report_msg.delta_velocity_y;
-    _measurement.delta_velocity.z = report_msg.delta_velocity_z;
-    _measurement.joint_angles.x = report_msg.joint_roll;
-    _measurement.joint_angles.y = report_msg.joint_el;
-    _measurement.joint_angles.z = report_msg.joint_az;
+    mavlink_msg_gimbal_report_decode(msg, &_report_msg);
+
+    _measurement.delta_time = _report_msg.delta_time;
+    _measurement.delta_angles.x = _report_msg.delta_angle_x;
+    _measurement.delta_angles.y = _report_msg.delta_angle_y;
+    _measurement.delta_angles.z = _report_msg.delta_angle_z;
+    _measurement.delta_velocity.x = _report_msg.delta_velocity_x,
+    _measurement.delta_velocity.y = _report_msg.delta_velocity_y;
+    _measurement.delta_velocity.z = _report_msg.delta_velocity_z;
+    _measurement.joint_angles.x = _report_msg.joint_roll;
+    _measurement.joint_angles.y = _report_msg.joint_el;
+    _measurement.joint_angles.z = _report_msg.joint_az;
 
     //apply joint angle compensation
     _measurement.joint_angles -= _gimbalParams.joint_angles_offsets;
@@ -50,6 +49,25 @@ void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
     _measurement.delta_angles -= _gimbalParams.delta_angles_offsets;
 }
 
+/*
+  send a gimbal report to the GCS for display purposes
+ */
+void AP_Gimbal::send_report(mavlink_channel_t chan) const
+{
+    mavlink_msg_gimbal_report_send(chan, 
+                                   0, 0, // send as broadcast
+                                   _report_msg.delta_time, 
+                                   _report_msg.delta_angle_x, 
+                                   _report_msg.delta_angle_y, 
+                                   _report_msg.delta_angle_z, 
+                                   _report_msg.delta_velocity_x, 
+                                   _report_msg.delta_velocity_y, 
+                                   _report_msg.delta_velocity_z, 
+                                   _report_msg.joint_roll, 
+                                   _report_msg.joint_el, 
+                                   _report_msg.joint_az);
+}
+
 void AP_Gimbal::update_state()
 {
     // Run the gimbal attitude and gyro bias estimator

libraries/AP_Mount/AP_Gimbal.h

@@ -39,6 +39,7 @@ public:
 
     void    update_target(Vector3f newTarget);
     void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
+    void    send_report(mavlink_channel_t chan) const;
 
     Vector3f getGimbalEstimateEF();
 
@@ -72,6 +73,8 @@ private:
 
     uint8_t _compid;
 
+    mavlink_gimbal_report_t _report_msg;
+
     void send_control(mavlink_channel_t chan);
     void update_state();
     void decode_feedback(mavlink_message_t *msg);

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -105,6 +105,7 @@ void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_mess
  */
 void AP_Mount_MAVLink::send_gimbal_report(mavlink_channel_t chan)
 {
+    _gimbal.send_report(chan);
 }
 
 #endif // AP_AHRS_NAVEKF_AVAILABLE