AP_Mission: support set-camera-source

rmackay9 · Mar 25, 2024 · 3a57094 · 3a57094
1 parent a6a5cde
commit 3a57094
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 0 deletions.
diff --git a/libraries/AP_Mission/AP_Mission.cpp b/libraries/AP_Mission/AP_Mission.cpp
@@ -387,6 +387,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;
@@ -434,6 +435,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);
@@ -1357,6 +1359,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;
@@ -1865,6 +1873,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;
@@ -2686,6 +2700,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
@@ -288,6 +288,13 @@ class AP_Mission
         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 @@ class AP_Mission
         // 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
@@ -165,6 +165,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; i<AP_CAMERA_MAX_INSTANCES; i++) {
+                ret |= camera->set_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) ||