AP_Mount: added handling of GIMBAL_REPORT messages

libraries/AP_Mount/AP_Mount.cpp

@@ -548,3 +548,22 @@ void AP_Mount::set_roi_target(uint8_t instance, const struct Location &target_lo
     }
 }
 
+// pass a GIMBAL_REPORT message to the backend
+void AP_Mount::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != NULL) {
+            _backends[instance]->handle_gimbal_report(chan, msg);
+        }
+    }    
+}
+
+// send a GIMBAL_REPORT message to the GCS
+void AP_Mount::send_gimbal_report(mavlink_channel_t chan)
+{
+    for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != NULL) {
+            _backends[instance]->send_gimbal_report(chan);
+        }
+    }    
+}

libraries/AP_Mount/AP_Mount.h

@@ -100,6 +100,12 @@ public:
     void control_msg(mavlink_message_t* msg) { control_msg(_primary, msg); }
     void control_msg(uint8_t instance, mavlink_message_t* msg);
 
+    // handle a GIMBAL_REPORT message
+    void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
+
+    // send a GIMBAL_REPORT message to GCS
+    void send_gimbal_report(mavlink_channel_t chan);
+
     // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
     void status_msg(mavlink_channel_t chan);
 

libraries/AP_Mount/AP_Mount_Backend.h

@@ -62,6 +62,12 @@ public:
     // status_msg - called to allow mounts to send their status to GCS via MAVLink
     virtual void status_msg(mavlink_channel_t chan) {};
 
+    // handle a GIMBAL_REPORT message
+    virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg) {}
+
+    // send a GIMBAL_REPORT message to the GCS
+    virtual void send_gimbal_report(mavlink_channel_t chan) {}
+
 protected:
 
     // update_targets_from_rc - updates angle targets (i.e. _angle_ef_target_rad) using input from receiver

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -142,3 +142,31 @@ void AP_Mount_MAVLink::send_angle_target(const Vector3f& target, bool target_in_
     _last_angle_target = target_deg;
     _last_mode = MAV_MOUNT_MODE_MAVLINK_TARGETING;
 }
+
+/*
+  handle a GIMBAL_REPORT message
+ */
+void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
+{
+    // just save it for future processing and reporting to GCS for now
+    mavlink_msg_gimbal_report_decode(msg, &_gimbal_report);
+}
+
+/*
+  send a GIMBAL_REPORT message to the GCS
+ */
+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_angle_x, 
+                                   _gimbal_report.delta_angle_y, 
+                                   _gimbal_report.delta_angle_z, 
+                                   _gimbal_report.delta_velocity_x, 
+                                   _gimbal_report.delta_velocity_y, 
+                                   _gimbal_report.delta_velocity_z, 
+                                   _gimbal_report.joint_roll, 
+                                   _gimbal_report.joint_pitch, 
+                                   _gimbal_report.joint_yaw);
+}

libraries/AP_Mount/AP_Mount_MAVLink.h

@@ -44,6 +44,12 @@ public:
     // status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
     virtual void status_msg(mavlink_channel_t chan);
 
+    // handle a GIMBAL_REPORT message
+    virtual void handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg);
+
+    // send a GIMBAL_REPORT message to the GCS
+    virtual void send_gimbal_report(mavlink_channel_t chan);
+
 private:
     // send_angle_target - send earth-frame angle targets to mount
     //  set target_in_degrees to true to send degree targets, false if target in radians
@@ -54,6 +60,9 @@ private:
     mavlink_channel_t _chan;        // telemetry channel used to communicate with mount
     enum MAV_MOUNT_MODE _last_mode; // last mode sent to mount
     Vector3f _last_angle_target;    // last angle target sent to mount
+
+    // keep last gimbal report
+    mavlink_gimbal_report_t _gimbal_report;
 };
 
 #endif // __AP_MOUNT_MAVLINK_H__