2018-10-23 01:45:34 -03:00
# include "AP_Mission.h"
# include <GCS_MAVLink/GCS.h>
2019-04-04 08:07:45 -03:00
# include <AP_Camera/AP_Camera.h>
2018-10-23 01:45:34 -03:00
# include <AP_Gripper/AP_Gripper.h>
2019-01-31 20:16:42 -04:00
# include <AP_Parachute/AP_Parachute.h>
2018-10-23 03:11:47 -03:00
# include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
2020-07-02 20:04:27 -03:00
# include <AC_Sprayer/AC_Sprayer.h>
2021-02-25 21:08:22 -04:00
# include <AP_Scripting/AP_Scripting.h>
2022-03-03 23:29:47 -04:00
# include <RC_Channel/RC_Channel.h>
2022-06-02 09:23:47 -03:00
# include <AP_Mount/AP_Mount.h>
2018-10-23 01:45:34 -03:00
2021-02-17 21:01:41 -04:00
bool AP_Mission : : start_command_do_aux_function ( const AP_Mission : : Mission_Command & cmd )
{
const RC_Channel : : AUX_FUNC function = ( RC_Channel : : AUX_FUNC ) cmd . content . auxfunction . function ;
const RC_Channel : : AuxSwitchPos pos = ( RC_Channel : : AuxSwitchPos ) cmd . content . auxfunction . switchpos ;
// sanity check the switch position. Could map from the mavlink
// enumeration if we were really keen
switch ( pos ) {
case RC_Channel : : AuxSwitchPos : : HIGH :
case RC_Channel : : AuxSwitchPos : : MIDDLE :
case RC_Channel : : AuxSwitchPos : : LOW :
break ;
default :
return false ;
}
2021-04-28 06:13:52 -03:00
rc ( ) . run_aux_function ( function , pos , RC_Channel : : AuxFuncTriggerSource : : MISSION ) ;
2021-02-17 21:01:41 -04:00
return true ;
}
2022-09-20 04:37:48 -03:00
# if AP_GRIPPER_ENABLED
2018-10-23 01:45:34 -03:00
bool AP_Mission : : start_command_do_gripper ( const AP_Mission : : Mission_Command & cmd )
{
AP_Gripper * gripper = AP : : gripper ( ) ;
if ( gripper = = nullptr ) {
2018-10-29 22:56:06 -03:00
return false ;
2018-10-23 01:45:34 -03:00
}
// Note: we ignore the gripper num parameter because we only
// support one gripper
switch ( cmd . content . gripper . action ) {
case GRIPPER_ACTION_RELEASE :
gripper - > release ( ) ;
// Log_Write_Event(DATA_GRIPPER_RELEASE);
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Gripper Released " ) ;
2018-10-29 22:56:06 -03:00
return true ;
2018-10-23 01:45:34 -03:00
case GRIPPER_ACTION_GRAB :
gripper - > grab ( ) ;
// Log_Write_Event(DATA_GRIPPER_GRAB);
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Gripper Grabbed " ) ;
2018-10-29 22:56:06 -03:00
return true ;
2018-10-23 01:45:34 -03:00
default :
2018-10-29 22:56:06 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL : : panic ( " Unhandled gripper case " ) ;
# endif
return false ;
2018-10-23 01:45:34 -03:00
}
}
2022-09-20 04:37:48 -03:00
# endif // AP_GRIPPER_ENABLED
2018-10-23 03:11:47 -03:00
2023-06-06 05:05:06 -03:00
# if AP_SERVORELAYEVENTS_ENABLED
2018-10-23 03:11:47 -03:00
bool AP_Mission : : start_command_do_servorelayevents ( const AP_Mission : : Mission_Command & cmd )
{
AP_ServoRelayEvents * sre = AP : : servorelayevents ( ) ;
if ( sre = = nullptr ) {
2018-10-29 22:56:06 -03:00
return false ;
2018-10-23 03:11:47 -03:00
}
switch ( cmd . id ) {
case MAV_CMD_DO_SET_SERVO :
2020-06-12 18:25:55 -03:00
return sre - > do_set_servo ( cmd . content . servo . channel , cmd . content . servo . pwm ) ;
2018-10-23 03:11:47 -03:00
2023-06-06 05:05:06 -03:00
# if AP_RELAY_ENABLED
2018-10-23 03:11:47 -03:00
case MAV_CMD_DO_SET_RELAY :
2020-06-12 18:25:55 -03:00
return sre - > do_set_relay ( cmd . content . relay . num , cmd . content . relay . state ) ;
2023-06-06 05:05:06 -03:00
# endif
2018-10-23 03:11:47 -03:00
case MAV_CMD_DO_REPEAT_SERVO :
2020-06-12 18:25:55 -03:00
return sre - > do_repeat_servo ( cmd . content . repeat_servo . channel ,
cmd . content . repeat_servo . pwm ,
cmd . content . repeat_servo . repeat_count ,
cmd . content . repeat_servo . cycle_time * 1000.0f ) ;
2018-10-23 03:11:47 -03:00
2023-06-06 05:05:06 -03:00
# if AP_RELAY_ENABLED
2018-10-23 03:11:47 -03:00
case MAV_CMD_DO_REPEAT_RELAY :
2020-06-12 18:25:55 -03:00
return sre - > do_repeat_relay ( cmd . content . repeat_relay . num ,
cmd . content . repeat_relay . repeat_count ,
cmd . content . repeat_relay . cycle_time * 1000.0f ) ;
2023-06-06 05:05:06 -03:00
# endif
2018-10-23 03:11:47 -03:00
default :
2018-10-29 22:56:06 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL : : panic ( " Unhandled servo/relay case " ) ;
# endif
return false ;
2018-10-23 03:11:47 -03:00
}
}
2023-06-06 05:05:06 -03:00
# endif // AP_SERVORELAYEVENTS_ENABLED
2018-10-23 04:03:50 -03:00
2022-06-02 05:28:26 -03:00
# if AP_CAMERA_ENABLED
2018-10-23 04:03:50 -03:00
bool AP_Mission : : start_command_camera ( const AP_Mission : : Mission_Command & cmd )
{
AP_Camera * camera = AP : : camera ( ) ;
if ( camera = = nullptr ) {
2018-10-29 22:56:06 -03:00
return false ;
2018-10-23 04:03:50 -03:00
}
switch ( cmd . id ) {
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 ) ;
2018-10-29 22:56:06 -03:00
return true ;
2018-10-23 04:03:50 -03:00
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 ) ;
2018-10-29 22:56:06 -03:00
return true ;
2018-10-23 04:03:50 -03:00
case MAV_CMD_DO_SET_CAM_TRIGG_DIST :
camera - > set_trigger_distance ( cmd . content . cam_trigg_dist . meters ) ;
2020-03-23 20:55:51 -03:00
if ( cmd . content . cam_trigg_dist . trigger = = 1 ) {
camera - > take_picture ( ) ;
}
2018-10-29 22:56:06 -03:00
return true ;
2018-10-23 04:03:50 -03:00
2023-03-08 07:28:43 -04:00
case MAV_CMD_SET_CAMERA_ZOOM :
if ( cmd . content . set_camera_zoom . zoom_type = = ZOOM_TYPE_CONTINUOUS ) {
2023-04-18 22:06:46 -03:00
return camera - > set_zoom ( ZoomType : : RATE , cmd . content . set_camera_zoom . zoom_value ) ;
2023-04-12 09:37:02 -03:00
}
if ( cmd . content . set_camera_zoom . zoom_type = = ZOOM_TYPE_RANGE ) {
2023-04-18 22:06:46 -03:00
return camera - > set_zoom ( ZoomType : : PCT , cmd . content . set_camera_zoom . zoom_value ) ;
2023-03-08 07:28:43 -04:00
}
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 ) ) {
2023-04-24 09:23:47 -03:00
return camera - > set_focus ( FocusType : : AUTO , 0 ) ;
2023-03-08 07:28:43 -04:00
}
2023-04-24 09:23:47 -03:00
// accept continuous manual focus
2023-03-08 07:28:43 -04:00
if ( cmd . content . set_camera_focus . focus_type = = FOCUS_TYPE_CONTINUOUS ) {
2023-04-24 09:23:47 -03:00
return camera - > set_focus ( FocusType : : RATE , cmd . content . set_camera_focus . focus_value ) ;
}
// accept range manual focus
if ( cmd . content . set_camera_focus . focus_type = = FOCUS_TYPE_RANGE ) {
return camera - > set_focus ( FocusType : : PCT , cmd . content . set_camera_focus . focus_value ) ;
2023-03-08 07:28:43 -04:00
}
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 ) ;
}
}
2018-10-23 04:03:50 -03:00
default :
2018-10-29 22:56:06 -03:00
# if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL : : panic ( " Unhandled camera case " ) ;
# endif
return false ;
2018-10-23 04:03:50 -03:00
}
}
2022-06-02 05:28:26 -03:00
# endif
2018-10-23 04:03:50 -03:00
2019-01-31 20:16:42 -04:00
bool AP_Mission : : start_command_parachute ( const AP_Mission : : Mission_Command & cmd )
{
2020-01-17 18:57:33 -04:00
# if HAL_PARACHUTE_ENABLED
2019-01-31 20:16:42 -04:00
AP_Parachute * parachute = AP : : parachute ( ) ;
if ( parachute = = nullptr ) {
return false ;
}
switch ( cmd . p1 ) {
case PARACHUTE_DISABLE :
parachute - > enabled ( false ) ;
break ;
case PARACHUTE_ENABLE :
parachute - > enabled ( true ) ;
break ;
case PARACHUTE_RELEASE :
parachute - > release ( ) ;
break ;
default :
// do nothing
return false ;
}
return true ;
2020-01-17 18:57:33 -04:00
# else
return false ;
# endif // HAL_PARACHUTE_ENABLED
2019-01-31 20:16:42 -04:00
}
2020-02-24 18:43:20 -04:00
bool AP_Mission : : command_do_set_repeat_dist ( const AP_Mission : : Mission_Command & cmd )
{
_repeat_dist = cmd . p1 ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Resume repeat dist set to %u m " , _repeat_dist ) ;
return true ;
}
2020-07-02 20:04:27 -03:00
bool AP_Mission : : start_command_do_sprayer ( const AP_Mission : : Mission_Command & cmd )
{
# if HAL_SPRAYER_ENABLED
AC_Sprayer * sprayer = AP : : sprayer ( ) ;
if ( sprayer = = nullptr ) {
return false ;
}
if ( cmd . p1 = = 1 ) {
sprayer - > run ( true ) ;
} else {
sprayer - > run ( false ) ;
}
return true ;
# else
return false ;
# endif // HAL_SPRAYER_ENABLED
}
2021-02-25 21:08:22 -04:00
bool AP_Mission : : start_command_do_scripting ( const AP_Mission : : Mission_Command & cmd )
{
2021-11-15 01:08:28 -04:00
# if AP_SCRIPTING_ENABLED
2021-02-25 21:08:22 -04:00
AP_Scripting * scripting = AP_Scripting : : get_singleton ( ) ;
if ( scripting = = nullptr ) {
return false ;
}
scripting - > handle_mission_command ( cmd ) ;
return true ;
# else
return false ;
2021-11-15 01:08:28 -04:00
# endif // AP_SCRIPTING_ENABLED
2021-02-25 21:08:22 -04:00
}
2022-06-02 09:23:47 -03:00
bool AP_Mission : : start_command_do_gimbal_manager_pitchyaw ( const AP_Mission : : Mission_Command & cmd )
{
# if HAL_MOUNT_ENABLED
AP_Mount * mount = AP : : mount ( ) ;
if ( mount = = nullptr ) {
return false ;
}
2022-09-02 02:33:21 -03:00
// check gimbal device id. 0 is primary, 1 is 1st gimbal, 2 is 2nd gimbal, etc
2023-03-02 00:03:02 -04:00
uint8_t gimbal_instance = mount - > get_primary_instance ( ) ;
2022-09-02 02:33:21 -03:00
if ( cmd . content . gimbal_manager_pitchyaw . gimbal_id > 0 ) {
gimbal_instance = cmd . content . gimbal_manager_pitchyaw . gimbal_id - 1 ;
}
2022-06-02 09:23:47 -03:00
// check flags for change to RETRACT
if ( ( cmd . content . gimbal_manager_pitchyaw . flags & GIMBAL_MANAGER_FLAGS_RETRACT ) > 0 ) {
2022-09-02 02:33:21 -03:00
mount - > set_mode ( gimbal_instance , MAV_MOUNT_MODE_RETRACT ) ;
2022-06-02 09:23:47 -03:00
return true ;
}
// check flags for change to NEUTRAL
if ( ( cmd . content . gimbal_manager_pitchyaw . flags & GIMBAL_MANAGER_FLAGS_NEUTRAL ) > 0 ) {
2022-09-02 02:33:21 -03:00
mount - > set_mode ( gimbal_instance , MAV_MOUNT_MODE_NEUTRAL ) ;
2022-06-02 09:23:47 -03:00
return true ;
}
2022-06-20 08:51:35 -03:00
// handle angle target
const bool pitch_angle_valid = ! isnan ( cmd . content . gimbal_manager_pitchyaw . pitch_angle_deg ) & & ( fabsf ( cmd . content . gimbal_manager_pitchyaw . pitch_angle_deg ) < = 90 ) ;
const bool yaw_angle_valid = ! isnan ( cmd . content . gimbal_manager_pitchyaw . yaw_angle_deg ) & & ( fabsf ( cmd . content . gimbal_manager_pitchyaw . yaw_angle_deg ) < = 360 ) ;
if ( pitch_angle_valid & & yaw_angle_valid ) {
2022-09-02 02:33:21 -03:00
mount - > set_angle_target ( gimbal_instance , 0 , cmd . content . gimbal_manager_pitchyaw . pitch_angle_deg , cmd . content . gimbal_manager_pitchyaw . yaw_angle_deg , cmd . content . gimbal_manager_pitchyaw . flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK ) ;
2022-06-20 08:51:35 -03:00
return true ;
}
// handle rate target
if ( ! isnan ( cmd . content . gimbal_manager_pitchyaw . pitch_rate_degs ) & & ! isnan ( cmd . content . gimbal_manager_pitchyaw . yaw_rate_degs ) ) {
2022-09-02 02:33:21 -03:00
mount - > set_rate_target ( gimbal_instance , 0 , cmd . content . gimbal_manager_pitchyaw . pitch_rate_degs , cmd . content . gimbal_manager_pitchyaw . yaw_rate_degs , cmd . content . gimbal_manager_pitchyaw . flags & GIMBAL_MANAGER_FLAGS_YAW_LOCK ) ;
2022-06-02 09:23:47 -03:00
return true ;
}
2022-09-07 12:42:38 -03:00
# endif // HAL_MOUNT_ENABLED
2022-06-02 09:23:47 -03:00
// if we got this far then message is not handled
return false ;
}