From 4eb32636536856769ecf08121dc753db93e2025d Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 18 Apr 2015 22:30:03 +0900
Subject: [PATCH] Camera: mission cmds send do-digicam msgs to components

This allows digicam-control and digicam-configure mission
command to trigger sending the equivalent mavlink messages
to components
---
 libraries/AP_Camera/AP_Camera.cpp | 29 +++++++++++++++++++++++++++++
 libraries/AP_Camera/AP_Camera.h   |  5 +++++
 2 files changed, 34 insertions(+)

diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp
index 32b14e73a6..626fcd59dd 100644
--- a/libraries/AP_Camera/AP_Camera.cpp
+++ b/libraries/AP_Camera/AP_Camera.cpp
@@ -169,6 +169,35 @@ AP_Camera::control_msg(mavlink_message_t* msg)
     }
 }
 
+// Mission command processing
+void AP_Camera::configure_cmd(AP_Mission::Mission_Command& cmd)
+{
+    mavlink_mission_item_t mav_cmd = {};
+
+    // convert command to mavlink message
+    if (AP_Mission::mission_cmd_to_mavlink(cmd, mav_cmd)) {
+        // convert mission item to mavlink message
+        // convert mission item to mavlink message
+        mavlink_message_t msg;
+        mavlink_msg_mission_item_encode(0, 0, &msg, &mav_cmd);
+        // send to all components
+        GCS_MAVLINK::send_to_components(&msg);
+    }
+}
+
+void AP_Camera::control_cmd(AP_Mission::Mission_Command& cmd)
+{
+    mavlink_mission_item_t mav_cmd = {};
+
+    // convert command to mavlink mission item
+    if (AP_Mission::mission_cmd_to_mavlink(cmd, mav_cmd)) {
+        // convert mission item to mavlink message
+        mavlink_message_t msg;
+        mavlink_msg_mission_item_encode(0, 0, &msg, &mav_cmd);
+        // send to all components
+        GCS_MAVLINK::send_to_components(&msg);
+    }
+}
 
 /*
   Send camera feedback to the GCS
diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h
index 932be5748a..6bcc90156b 100644
--- a/libraries/AP_Camera/AP_Camera.h
+++ b/libraries/AP_Camera/AP_Camera.h
@@ -13,6 +13,7 @@
 #include <AP_Relay.h>
 #include <AP_GPS.h>
 #include <AP_AHRS.h>
+#include <AP_Mission.h>
 
 #define AP_CAMERA_TRIGGER_TYPE_SERVO                0
 #define AP_CAMERA_TRIGGER_TYPE_RELAY                1
@@ -52,6 +53,10 @@ public:
     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);
 
+    // Mission command processing
+    void            configure_cmd(AP_Mission::Mission_Command& cmd);
+    void            control_cmd(AP_Mission::Mission_Command& cmd);
+
     // set camera trigger distance in a mission
     void            set_trigger_distance(uint32_t distance_m) { _trigg_dist.set(distance_m); }