AP_Mission: support set-camera-lens

rmackay9 · Feb 10, 2024 · 171c4c2 · 171c4c2
1 parent 53abecc
commit 171c4c2
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 0 deletions.
diff --git 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_LENS:
     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_LENS:
     case MAV_CMD_VIDEO_START_CAPTURE:
     case MAV_CMD_VIDEO_STOP_CAPTURE:
         return start_command_camera(cmd);
@@ -1362,6 +1364,11 @@ 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_LENS:
+        cmd.content.set_camera_lens.instance = packet.param1;
+        cmd.content.set_camera_lens.lens = packet.param2;
+        break;
+
     case MAV_CMD_VIDEO_START_CAPTURE:
         cmd.content.video_start_capture.video_stream_id = packet.param1;
         break;
@@ -1872,6 +1879,11 @@ 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_LENS:
+        packet.param1 = cmd.content.set_camera_lens.instance;
+        packet.param2 = cmd.content.set_camera_lens.lens;
+        break;
+
     case MAV_CMD_VIDEO_START_CAPTURE:
         packet.param1 = cmd.content.video_start_capture.video_stream_id;
         break;
@@ -2693,6 +2705,8 @@ const char *AP_Mission::Mission_Command::type() const
         return "SetCameraZoom";
     case MAV_CMD_SET_CAMERA_FOCUS:
         return "SetCameraFocus";
+    case MAV_CMD_SET_CAMERA_LENS:
+        return "SetCameraLens";
     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,12 @@ class AP_Mission
         float focus_value;
     };
 
+    // MAV_CMD_SET_CAMERA_LENS support
+    struct PACKED set_camera_lens_Command {
+        uint8_t instance;
+        uint8_t lens;
+    };
+
     // MAV_CMD_VIDEO_START_CAPTURE support
     struct PACKED video_start_capture_Command {
         uint8_t video_stream_id;
@@ -388,6 +394,9 @@ class AP_Mission
         // MAV_CMD_SET_CAMERA_FOCUS support
         set_camera_focus_Command set_camera_focus;
 
+        // MAV_CMD_SET_CAMEARA_LENS support
+        set_camera_lens_Command set_camera_lens;
+
         // 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
@@ -168,6 +168,17 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
         }
         return false;
 
+    case MAV_CMD_SET_CAMERA_LENS:
+        if (cmd.content.set_camera_lens.instance == 0) {
+            // set lens for every backend
+            bool ret = false;
+            for (uint8_t i=0; i<AP_CAMERA_MAX_INSTANCES; i++) {
+                ret |= camera->set_lens(i, cmd.content.set_camera_lens.lens);
+            }
+            return ret;
+        }
+        return camera->set_lens(cmd.content.set_camera_lens.instance-1, cmd.content.set_camera_lens.lens);
+
     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) ||