From d0ad56b98dd5dde5c4f0db8936faa2946662c496 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 8 Feb 2024 17:30:08 +0900 Subject: [PATCH] AP_Mission: support set-camera-source --- libraries/AP_Mission/AP_Mission.cpp | 16 ++++++++++++++++ libraries/AP_Mission/AP_Mission.h | 10 ++++++++++ libraries/AP_Mission/AP_Mission_Commands.cpp | 13 +++++++++++++ 3 files changed, 39 insertions(+) diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp index 2fd101c8fa..f0f5038e6a 100644 --- a/libraries/AP_Mission/AP_Mission.cpp +++ b/libraries/AP_Mission/AP_Mission.cpp @@ -386,6 +386,7 @@ bool AP_Mission::verify_command(const Mission_Command& cmd) case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_SET_CAMERA_ZOOM: case MAV_CMD_SET_CAMERA_FOCUS: + case MAV_CMD_SET_CAMERA_SOURCE: case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return true; @@ -433,6 +434,7 @@ bool AP_Mission::start_command(const Mission_Command& cmd) case MAV_CMD_IMAGE_STOP_CAPTURE: case MAV_CMD_SET_CAMERA_ZOOM: case MAV_CMD_SET_CAMERA_FOCUS: + case MAV_CMD_SET_CAMERA_SOURCE: case MAV_CMD_VIDEO_START_CAPTURE: case MAV_CMD_VIDEO_STOP_CAPTURE: return start_command_camera(cmd); @@ -1362,6 +1364,12 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_ cmd.content.set_camera_focus.focus_value = packet.param2; break; + case MAV_CMD_SET_CAMERA_SOURCE: + cmd.content.set_camera_source.instance = packet.param1; + cmd.content.set_camera_source.primary_source = packet.param2; + cmd.content.set_camera_source.secondary_source = packet.param3; + break; + case MAV_CMD_VIDEO_START_CAPTURE: cmd.content.video_start_capture.video_stream_id = packet.param1; break; @@ -1872,6 +1880,12 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c packet.param2 = cmd.content.set_camera_focus.focus_value; break; + case MAV_CMD_SET_CAMERA_SOURCE: + packet.param1 = cmd.content.set_camera_source.instance; + packet.param2 = cmd.content.set_camera_source.primary_source; + packet.param3 = cmd.content.set_camera_source.secondary_source; + break; + case MAV_CMD_VIDEO_START_CAPTURE: packet.param1 = cmd.content.video_start_capture.video_stream_id; break; @@ -2693,6 +2707,8 @@ const char *AP_Mission::Mission_Command::type() const return "SetCameraZoom"; case MAV_CMD_SET_CAMERA_FOCUS: return "SetCameraFocus"; + case MAV_CMD_SET_CAMERA_SOURCE: + return "SetCameraSource"; case MAV_CMD_VIDEO_START_CAPTURE: return "VideoStartCapture"; case MAV_CMD_VIDEO_STOP_CAPTURE: diff --git a/libraries/AP_Mission/AP_Mission.h b/libraries/AP_Mission/AP_Mission.h index c7204b315b..f332594c09 100644 --- a/libraries/AP_Mission/AP_Mission.h +++ b/libraries/AP_Mission/AP_Mission.h @@ -288,6 +288,13 @@ public: float focus_value; }; + // MAV_CMD_SET_CAMERA_SOURCE support + struct PACKED set_camera_source_Command { + uint8_t instance; + uint8_t primary_source; + uint8_t secondary_source; + }; + // MAV_CMD_VIDEO_START_CAPTURE support struct PACKED video_start_capture_Command { uint8_t video_stream_id; @@ -388,6 +395,9 @@ public: // MAV_CMD_SET_CAMERA_FOCUS support set_camera_focus_Command set_camera_focus; + // MAV_CMD_SET_CAMEARA_SOURCE support + set_camera_source_Command set_camera_source; + // MAV_CMD_VIDEO_START_CAPTURE support video_start_capture_Command video_start_capture; diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 21d6731b73..0ac95fa5c1 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -168,6 +168,19 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd) } return false; +#if AP_CAMERA_SET_CAMERA_SOURCE_ENABLED + case MAV_CMD_SET_CAMERA_SOURCE: + if (cmd.content.set_camera_source.instance == 0) { + // set lens for every backend + bool ret = false; + for (uint8_t i=0; iset_camera_source(i, (AP_Camera::CameraSource)cmd.content.set_camera_source.primary_source, (AP_Camera::CameraSource)cmd.content.set_camera_source.secondary_source); + } + return ret; + } + return camera->set_camera_source(cmd.content.set_camera_source.instance-1, (AP_Camera::CameraSource)cmd.content.set_camera_source.primary_source, (AP_Camera::CameraSource)cmd.content.set_camera_source.secondary_source); +#endif + case MAV_CMD_IMAGE_START_CAPTURE: // check if this is a single picture request (e.g. total images is 1 or interval and total images are zero) if ((cmd.content.image_start_capture.total_num_images == 1) ||