From 8fe3475b41b76ecf07aa6cd1d73196c17b4c8ebe Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 17 Mar 2014 20:12:12 +0100
Subject: [PATCH] mavlink: add onboard function for camera capture commands

---
 src/modules/mavlink/mavlink_main.cpp     | 13 +++++++
 src/modules/mavlink/mavlink_main.h       |  3 +-
 src/modules/mavlink/mavlink_messages.cpp | 44 +++++++++++++++++++++++-
 3 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 9c2e0178a3..2457a7cae8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1653,6 +1653,8 @@ Mavlink::task_main(int argc, char *argv[])
 		case 'm':
 			if (strcmp(optarg, "custom") == 0) {
 				_mode = MAVLINK_MODE_CUSTOM;
+			} else if (strcmp(optarg, "camera") == 0) {
+				_mode = MAVLINK_MODE_CAMERA;
 			}
 
 			break;
@@ -1696,6 +1698,10 @@ Mavlink::task_main(int argc, char *argv[])
 		warnx("mode: CUSTOM");
 		break;
 
+	case MAVLINK_MODE_CAMERA:
+		warnx("mode: CAMERA");
+		break;
+
 	default:
 		warnx("ERROR: Unknown mode");
 		break;
@@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
 		break;
 
+	case MAVLINK_MODE_CAMERA:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("ATTITUDE", 20.0f);
+		configure_stream("GLOBAL_POSITION_INT", 20.0f);
+		configure_stream("CAMERA_CAPTURE", 1.0f);
+		break;
+
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index f743c25044..5a118a0ad9 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -146,7 +146,8 @@ public:
 
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
-		MAVLINK_MODE_CUSTOM
+		MAVLINK_MODE_CUSTOM,
+		MAVLINK_MODE_CAMERA
 	};
 
 	void		set_mode(enum MAVLINK_MODE);
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7d388f88da..014b53829d 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -230,7 +230,6 @@ protected:
 					   mavlink_base_mode,
 					   mavlink_custom_mode,
 					   mavlink_state);
-
 	}
 };
 
@@ -1127,6 +1126,48 @@ protected:
 	}
 };
 
+class MavlinkStreamCameraCapture : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "CAMERA_CAPTURE";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamCameraCapture();
+	}
+
+private:
+	MavlinkOrbSubscription *status_sub;
+	struct vehicle_status_s *status;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
+		status = (struct vehicle_status_s *)status_sub->get_data();
+
+
+	}
+
+	void send(const hrt_abstime t)
+	{
+		(void)status_sub->update(t);
+
+		if (status->arming_state == ARMING_STATE_ARMED
+		 || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+
+			/* send camera capture on */
+			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
+		} else {
+			/* send camera capture off */
+			mavlink_msg_command_long_send(_channel, 42, 30, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 0, 0, 0, 0);
+		}
+	}
+};
+
 
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
@@ -1151,6 +1192,7 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamRCChannelsRaw(),
 	new MavlinkStreamManualControl(),
 	new MavlinkStreamOpticalFlow(),
+	new MavlinkStreamCameraCapture(),
 	nullptr
 };