Camera: Added feedback message support

Now the autopilot can publish the state of the camera via the mavlink_msg_camera_feedback message.

libraries/AP_Camera/AP_Camera.cpp

@@ -161,6 +161,30 @@ AP_Camera::control_msg(mavlink_message_t* msg)
 }
 
 
+/*
+  Send camera feedback to the GCS
+ */
+void AP_Camera::send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc)
+{
+    float altitude, altitude_rel;
+    if (current_loc.flags.relative_alt) {
+        altitude = current_loc.alt+ahrs.get_home().alt;
+        altitude_rel = current_loc.alt;
+    } else {
+        altitude = current_loc.alt;
+        altitude_rel = current_loc.alt - ahrs.get_home().alt;
+    }
+
+    mavlink_msg_camera_feedback_send(chan, 
+        gps.time_epoch_usec(),
+        0, 0, 0,
+        current_loc.lat, current_loc.lng,
+        altitude/100.0, altitude_rel/100.0,
+        ahrs.roll_sensor/100.0, ahrs.pitch_sensor/100.0, ahrs.yaw_sensor/100.0,
+        0.0,0);
+}
+
+
 /*  update location, for triggering by GPS distance moved
     This function returns true if a picture should be taken
     The caller is responsible for taking the picture based on the return value of this function.

libraries/AP_Camera/AP_Camera.h

@@ -10,6 +10,8 @@
 #include <AP_Common.h>
 #include <GCS_MAVLink.h>
 #include <AP_Relay.h>
+#include <AP_GPS.h>
+#include <AP_AHRS.h>
 
 #define AP_CAMERA_TRIGGER_TYPE_SERVO                0
 #define AP_CAMERA_TRIGGER_TYPE_RELAY                1
@@ -45,6 +47,7 @@ public:
     // MAVLink methods
     void            configure_msg(mavlink_message_t* msg);
     void            control_msg(mavlink_message_t* msg);
+    void            send_feedback(mavlink_channel_t chan, AP_GPS &gps, const AP_AHRS &ahrs, const Location &current_loc);
 
     // set camera trigger distance in a mission
     void            set_trigger_distance(uint32_t distance_m) { _trigg_dist.set(distance_m); }