From 8fe3475b41b76ecf07aa6cd1d73196c17b4c8ebe Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Mar 2014 20:12:12 +0100 Subject: [PATCH 1/4] 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 }; From 952fd1ec6cbc615d6bd9213061dcce0f9cf8c5f0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 11:38:41 +0100 Subject: [PATCH 2/4] mavlink: camera mode with rate multiplier --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 2457a7cae8..dca248dc96 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,8 +1773,8 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 20.0f); - configure_stream("GLOBAL_POSITION_INT", 20.0f); + configure_stream("ATTITUDE", 20.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 20.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break; From a989dd6eec5ec15e6387769d8f657a29d68fabff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 14:23:23 +0100 Subject: [PATCH 3/4] mavlink: correct verbose info, bytes not bits --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dca248dc96..576b3ddec8 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1709,7 +1709,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); From bf69a7b6473adde22b3220b6e72c0d065c897bc5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Mar 2014 14:26:45 +0100 Subject: [PATCH 4/4] mavlink: camera mode rate is now correct --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 576b3ddec8..a36ad0bf6b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1773,8 +1773,8 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_CAMERA: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 20.0f * rate_mult); - configure_stream("GLOBAL_POSITION_INT", 20.0f * rate_mult); + configure_stream("ATTITUDE", 15.0f * rate_mult); + configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult); configure_stream("CAMERA_CAPTURE", 1.0f); break;