From 01b264951a78d4552ef80359f6d5d6aea92408c8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 29 Jan 2015 20:01:55 +1100
Subject: [PATCH] AP_Mount: added handling of GIMBAL_REPORT messages

---
 libraries/AP_Mount/AP_Mount.cpp         | 19 +++++++++++++++++
 libraries/AP_Mount/AP_Mount.h           |  6 ++++++
 libraries/AP_Mount/AP_Mount_Backend.h   |  6 ++++++
 libraries/AP_Mount/AP_Mount_MAVLink.cpp | 28 +++++++++++++++++++++++++
 libraries/AP_Mount/AP_Mount_MAVLink.h   |  9 ++++++++
 5 files changed, 68 insertions(+)

diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 0688ffcfc8..54f9350dc6 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/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);
+        }
+    }    
+}
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 9a6f599b85..80b4ea1892 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/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);
 
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index df279a409a..ac74a64f52 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/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
diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.cpp b/libraries/AP_Mount/AP_Mount_MAVLink.cpp
index 391c706f40..8dfa54ebaf 100644
--- a/libraries/AP_Mount/AP_Mount_MAVLink.cpp
+++ b/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);
+}
diff --git a/libraries/AP_Mount/AP_Mount_MAVLink.h b/libraries/AP_Mount/AP_Mount_MAVLink.h
index 7c07f99608..adb67d308e 100644
--- a/libraries/AP_Mount/AP_Mount_MAVLink.h
+++ b/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__