From 9ccf08a0f8581b847af1c5e101f99895d0a0c57e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 7 Mar 2023 15:39:52 +0900 Subject: [PATCH] AP_Camera: add handle_command_long --- libraries/AP_Camera/AP_Camera.cpp | 21 +++++++++++++++++++++ libraries/AP_Camera/AP_Camera.h | 1 + 2 files changed, 22 insertions(+) diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp index 0cd824cf97..174400fe9a 100644 --- a/libraries/AP_Camera/AP_Camera.cpp +++ b/libraries/AP_Camera/AP_Camera.cpp @@ -204,6 +204,27 @@ void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t & } } +// handle command_long mavlink messages +MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet) +{ + switch (packet.command) { + case MAV_CMD_DO_DIGICAM_CONFIGURE: + configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7); + return MAV_RESULT_ACCEPTED; + case MAV_CMD_DO_DIGICAM_CONTROL: + control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6); + return MAV_RESULT_ACCEPTED; + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + set_trigger_distance(packet.param1); + if (is_equal(packet.param3, 1.0f)) { + take_picture(); + } + return MAV_RESULT_ACCEPTED; + default: + return MAV_RESULT_UNSUPPORTED; + } +} + // set camera trigger distance in a mission void AP_Camera::set_trigger_distance(uint8_t instance, float distance_m) { diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h index c7a6cd11bb..274e9f8696 100644 --- a/libraries/AP_Camera/AP_Camera.h +++ b/libraries/AP_Camera/AP_Camera.h @@ -73,6 +73,7 @@ public: // MAVLink methods void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg); + MAV_RESULT handle_command_long(const mavlink_command_long_t &packet); void send_feedback(mavlink_channel_t chan) const; // configure camera