Merge pull request #754 from julianoes/beta_mavlink2_camera

Beta mavlink2: onboard camera capture 

3 files changed, 59 insertions, 2 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 00d906dcd..ff1c070cb 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1652,6 +1652,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;
@@ -1695,6 +1697,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;
@@ -1702,7 +1708,7 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
-	warnx("data rate: %d bps, port: %s, baud: %d", _datarate, _device_name, (int)_baudrate);
+	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1765,6 +1771,13 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
 		break;
 
+	case MAVLINK_MODE_CAMERA:
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("ATTITUDE", 15.0f * rate_mult);
+		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
+		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 f743c2504..5a118a0ad 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 dc01935ca..a4a3c681f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1215,6 +1215,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(),
 	new MavlinkStreamSysStatus(),
@@ -1240,5 +1282,6 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamOpticalFlow(),
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
+	new MavlinkStreamCameraCapture(),
 	nullptr
 };