Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Mission: add camera zoom and focus support 

also image-start-capture, video-start-capture, video-stop-capture
This commit is contained in:
Randy Mackay 2023-03-08 20:28:43 +09:00
parent a3e3e5fd2d
commit 5b96998e62
3 changed files with 151 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
libraries/AP_Mission/AP_Mission.cpp
View File

@ -375,6 +375,11 @@ bool AP_Mission::verify_command(const Mission_Command& cmd)
case MAV_CMD_DO_SET_RESUME_REPEAT_DIST:
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
case MAV_CMD_JUMP_TAG:
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_SET_CAMERA_ZOOM:
case MAV_CMD_SET_CAMERA_FOCUS:
case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
return true;
default:
return _cmd_verify_fn(cmd);
@ -407,6 +412,11 @@ bool AP_Mission::start_command(const Mission_Command& cmd)
case MAV_CMD_DO_DIGICAM_CONFIGURE:
case MAV_CMD_DO_DIGICAM_CONTROL:
case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
case MAV_CMD_IMAGE_START_CAPTURE:
case MAV_CMD_SET_CAMERA_ZOOM:
case MAV_CMD_SET_CAMERA_FOCUS:
case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
return start_command_camera(cmd);
#endif
case MAV_CMD_DO_PARACHUTE:
@ -1292,6 +1302,30 @@ MAV_MISSION_RESULT AP_Mission::mavlink_int_to_mission_cmd(const mavlink_mission_
cmd.content.gimbal_manager_pitchyaw.gimbal_id = packet.z;
break;
case MAV_CMD_IMAGE_START_CAPTURE:
cmd.content.image_start_capture.interval_s = packet.param2;
cmd.content.image_start_capture.total_num_images = packet.param3;
cmd.content.image_start_capture.start_seq_number = packet.param4;
break;
case MAV_CMD_SET_CAMERA_ZOOM:
cmd.content.set_camera_zoom.zoom_type = packet.param1;
cmd.content.set_camera_zoom.zoom_value = packet.param2;
break;
case MAV_CMD_SET_CAMERA_FOCUS:
cmd.content.set_camera_focus.focus_type = packet.param1;
cmd.content.set_camera_focus.focus_value = packet.param2;
break;
case MAV_CMD_VIDEO_START_CAPTURE:
cmd.content.video_start_capture.video_stream_id = packet.param1;
break;
case MAV_CMD_VIDEO_STOP_CAPTURE:
cmd.content.video_stop_capture.video_stream_id = packet.param1;
break;
default:
// unrecognised command
return MAV_MISSION_UNSUPPORTED;
@ -1781,6 +1815,30 @@ bool AP_Mission::mission_cmd_to_mavlink_int(const AP_Mission::Mission_Command& c
packet.z = cmd.content.gimbal_manager_pitchyaw.gimbal_id;
break;
case MAV_CMD_IMAGE_START_CAPTURE:
packet.param2 = cmd.content.image_start_capture.interval_s;
packet.param3 = cmd.content.image_start_capture.total_num_images;
packet.param4 = cmd.content.image_start_capture.start_seq_number;
break;
case MAV_CMD_SET_CAMERA_ZOOM:
packet.param1 = cmd.content.set_camera_zoom.zoom_type;
packet.param2 = cmd.content.set_camera_zoom.zoom_value;
break;
case MAV_CMD_SET_CAMERA_FOCUS:
packet.param1 = cmd.content.set_camera_focus.focus_type;
packet.param2 = cmd.content.set_camera_focus.focus_value;
break;
case MAV_CMD_VIDEO_START_CAPTURE:
packet.param1 = cmd.content.video_start_capture.video_stream_id;
break;
case MAV_CMD_VIDEO_STOP_CAPTURE:
packet.param1 = cmd.content.video_stop_capture.video_stream_id;
break;
default:
// unrecognised command
return false;
@ -2584,6 +2642,16 @@ const char *AP_Mission::Mission_Command::type() const
return "PauseContinue";
case MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW:
return "GimbalPitchYaw";
case MAV_CMD_IMAGE_START_CAPTURE:
return "ImageStartCapture";
case MAV_CMD_SET_CAMERA_ZOOM:
return "SetCameraZoom";
case MAV_CMD_SET_CAMERA_FOCUS:
return "SetCameraFocus";
case MAV_CMD_VIDEO_START_CAPTURE:
return "VideoStartCapture";
case MAV_CMD_VIDEO_STOP_CAPTURE:
return "VideoStopCapture";
default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Mission command with ID %u has no string", id);

44
libraries/AP_Mission/AP_Mission.h
View File

@ -267,6 +267,35 @@ public:
uint8_t gimbal_id;
};
// MAV_CMD_IMAGE_START_CAPTURE support
struct PACKED image_start_capture_Command {
float interval_s;
uint16_t total_num_images;
uint16_t start_seq_number;
};
// MAV_CMD_SET_CAMERA_ZOOM support
struct PACKED set_camera_zoom_Command {
uint8_t zoom_type;
float zoom_value;
};
// MAV_CMD_SET_CAMERA_FOCUS support
struct PACKED set_camera_focus_Command {
uint8_t focus_type;
float focus_value;
};
// MAV_CMD_VIDEO_START_CAPTURE support
struct PACKED video_start_capture_Command {
uint8_t video_stream_id;
};
// MAV_CMD_VIDEO_STOP_CAPTURE support
struct PACKED video_stop_capture_Command {
uint8_t video_stream_id;
};
union Content {
// jump structure
Jump_Command jump;
@ -348,6 +377,21 @@ public:
// MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW
gimbal_manager_pitchyaw_Command gimbal_manager_pitchyaw;
// MAV_CMD_IMAGE_START_CAPTURE support
image_start_capture_Command image_start_capture;
// MAV_CMD_SET_CAMERA_ZOOM support
set_camera_zoom_Command set_camera_zoom;
// MAV_CMD_SET_CAMERA_FOCUS support
set_camera_focus_Command set_camera_focus;
// MAV_CMD_VIDEO_START_CAPTURE support
video_start_capture_Command video_start_capture;
// MAV_CMD_VIDEO_STOP_CAPTURE support
video_stop_capture_Command video_stop_capture;
// location
Location location{}; // Waypoint location
};

39
libraries/AP_Mission/AP_Mission_Commands.cpp
View File

@ -129,6 +129,45 @@ bool AP_Mission::start_command_camera(const AP_Mission::Mission_Command& cmd)
}
return true;
case MAV_CMD_SET_CAMERA_ZOOM:
if (cmd.content.set_camera_zoom.zoom_type == ZOOM_TYPE_CONTINUOUS) {
camera->set_zoom_step(cmd.content.set_camera_zoom.zoom_value);
return true;
}
return false;
case MAV_CMD_SET_CAMERA_FOCUS:
// accept any of the auto focus types
if ((cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_AUTO) ||
(cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_AUTO_SINGLE) ||
(cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_AUTO_CONTINUOUS)) {
camera->set_auto_focus();
return true;
}
// accept step or continuous manual focus
if (cmd.content.set_camera_focus.focus_type == FOCUS_TYPE_CONTINUOUS) {
camera->set_manual_focus_step(cmd.content.set_camera_focus.focus_value);
return true;
}
return false;
case MAV_CMD_IMAGE_START_CAPTURE:
camera->take_picture();
return true;
case MAV_CMD_VIDEO_START_CAPTURE:
case MAV_CMD_VIDEO_STOP_CAPTURE:
{
const bool start_recording = (cmd.id == MAV_CMD_VIDEO_START_CAPTURE);
if (cmd.content.video_start_capture.video_stream_id == 0) {
// stream id of zero interpreted as primary camera
return camera->record_video(start_recording);
} else {
// non-zero stream id is converted to camera instance
return camera->record_video(cmd.content.video_start_capture.video_stream_id - 1, start_recording);
}
}
default:
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("Unhandled camera case");