AP_Mission: handle common camera commands

libraries/AP_Mission/AP_Mission.cpp

@@ -268,6 +268,10 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
     case MAV_CMD_DO_SET_RELAY:
     case MAV_CMD_DO_REPEAT_SERVO:
     case MAV_CMD_DO_REPEAT_RELAY:
+    case MAV_CMD_DO_CONTROL_VIDEO:
+    case MAV_CMD_DO_DIGICAM_CONFIGURE:
+    case MAV_CMD_DO_DIGICAM_CONTROL:
+    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
         return true;
     default:
         return _cmd_verify_fn(cmd);
@@ -284,6 +288,11 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
     case MAV_CMD_DO_REPEAT_SERVO:
     case MAV_CMD_DO_REPEAT_RELAY:
         return start_command_do_servorelayevents(cmd);
+    case MAV_CMD_DO_CONTROL_VIDEO:
+    case MAV_CMD_DO_DIGICAM_CONFIGURE:
+    case MAV_CMD_DO_DIGICAM_CONTROL:
+    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+        return start_command_camera(cmd);
     default:
         return _cmd_start_fn(cmd);
     }

libraries/AP_Mission/AP_Mission.h

@@ -582,6 +582,7 @@ private:
     // mission items common to all vehicles:
     bool start_command_do_gripper(const AP_Mission::Mission_Command& cmd);
     bool start_command_do_servorelayevents(const AP_Mission::Mission_Command& cmd);
+    bool start_command_camera(const AP_Mission::Mission_Command& cmd);
 };
 
 namespace AP {

libraries/AP_Mission/AP_Mission_Commands.cpp

@@ -66,3 +66,47 @@ bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Com
 
     return true;
 }
+
+bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
+{
+    AP_Camera *camera = AP::camera();
+    if (camera == nullptr) {
+        return true;
+    }
+
+    switch (cmd.id) {
+    case MAV_CMD_DO_CONTROL_VIDEO:                      // Control on-board camera capturing. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|
+        break;
+
+    case MAV_CMD_DO_DIGICAM_CONFIGURE:                  // Mission command to configure an on-board camera controller system. |Modes: P, TV, AV, M, Etc| Shutter speed: Divisor number for one second| Aperture: F stop number| ISO number e.g. 80, 100, 200, Etc| Exposure type enumerator| Command Identity| Main engine cut-off time before camera trigger in seconds/10 (0 means no cut-off)|
+        camera->configure(
+            cmd.content.digicam_configure.shooting_mode,
+            cmd.content.digicam_configure.shutter_speed,
+            cmd.content.digicam_configure.aperture,
+            cmd.content.digicam_configure.ISO,
+            cmd.content.digicam_configure.exposure_type,
+            cmd.content.digicam_configure.cmd_id,
+            cmd.content.digicam_configure.engine_cutoff_time);
+        break;
+
+    case MAV_CMD_DO_DIGICAM_CONTROL:                    // Mission command to control an on-board camera controller system. |Session control e.g. show/hide lens| Zoom's absolute position| Zooming step value to offset zoom from the current position| Focus Locking, Unlocking or Re-locking| Shooting Command| Command Identity| Empty|
+        camera->control(
+            cmd.content.digicam_control.session,
+            cmd.content.digicam_control.zoom_pos,
+            cmd.content.digicam_control.zoom_step,
+            cmd.content.digicam_control.focus_lock,
+            cmd.content.digicam_control.shooting_cmd,
+            cmd.content.digicam_control.cmd_id);
+        break;
+
+    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+        camera->set_trigger_distance(cmd.content.cam_trigg_dist.meters);
+        break;
+
+    default:
+        return true;
+    }
+
+    return true;
+}
+