2011-03-19 07:20:11 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2011-03-08 08:20:48 -04:00
2015-05-29 23:12:49 -03:00
# include "Copter.h"
2014-02-27 22:19:05 -04:00
// start_command - this function will be called when the ap_mission lib wishes to start a new command
2015-05-29 23:12:49 -03:00
bool Copter : : start_command ( const AP_Mission : : Mission_Command & cmd )
2014-02-27 22:19:05 -04:00
{
// To-Do: logging when new commands start/end
2014-10-16 21:37:49 -03:00
if ( should_log ( MASK_LOG_CMD ) ) {
2015-06-23 23:13:10 -03:00
DataFlash . Log_Write_Mission_Cmd ( mission , cmd ) ;
2014-02-27 22:19:05 -04:00
}
2011-03-02 22:32:50 -04:00
2014-02-27 22:19:05 -04:00
switch ( cmd . id ) {
///
/// navigation commands
///
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_TAKEOFF : // 22
2014-02-27 22:19:05 -04:00
do_takeoff ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_WAYPOINT : // 16 Navigate to Waypoint
2014-02-27 22:19:05 -04:00
do_nav_wp ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LAND : // 21 LAND to Waypoint
2014-02-27 22:19:05 -04:00
do_land ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LOITER_UNLIM : // 17 Loiter indefinitely
2014-02-27 22:19:05 -04:00
do_loiter_unlimited ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LOITER_TURNS : //18 Loiter N Times
2014-02-27 22:19:05 -04:00
do_circle ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LOITER_TIME : // 19
2014-02-27 22:19:05 -04:00
do_loiter_time ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_RETURN_TO_LAUNCH : //20
do_RTL ( ) ;
break ;
2011-04-02 21:47:20 -03:00
2014-03-27 04:05:49 -03:00
case MAV_CMD_NAV_SPLINE_WAYPOINT : // 82 Navigate to Waypoint using spline
do_spline_wp ( cmd ) ;
break ;
2014-05-23 02:29:08 -03:00
# if NAV_GUIDED == ENABLED
2014-10-13 05:41:48 -03:00
case MAV_CMD_NAV_GUIDED_ENABLE : // 92 accept navigation commands from external nav computer
do_nav_guided_enable ( cmd ) ;
2014-05-23 02:29:08 -03:00
break ;
# endif
2014-02-27 22:19:05 -04:00
//
// conditional commands
//
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_DELAY : // 112
2014-02-27 22:19:05 -04:00
do_wait_delay ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_DISTANCE : // 114
2014-02-27 22:19:05 -04:00
do_within_distance ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_CHANGE_ALT : // 113
2014-02-27 22:19:05 -04:00
do_change_alt ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_YAW : // 115
2014-02-27 22:19:05 -04:00
do_yaw ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2014-02-27 22:19:05 -04:00
///
/// do commands
///
2012-08-21 23:19:50 -03:00
case MAV_CMD_DO_CHANGE_SPEED : // 178
2014-02-27 22:19:05 -04:00
do_change_speed ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_DO_SET_HOME : // 179
2015-02-09 07:25:45 -04:00
do_set_home ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-02-24 01:56:59 -04:00
2014-01-20 01:05:31 -04:00
case MAV_CMD_DO_SET_SERVO :
2014-03-17 02:16:52 -03:00
ServoRelayEvents . do_set_servo ( cmd . content . servo . channel , cmd . content . servo . pwm ) ;
2012-08-21 23:19:50 -03:00
break ;
2014-01-20 01:05:31 -04:00
case MAV_CMD_DO_SET_RELAY :
2014-03-17 02:16:52 -03:00
ServoRelayEvents . do_set_relay ( cmd . content . relay . num , cmd . content . relay . state ) ;
2012-08-21 23:19:50 -03:00
break ;
2014-01-20 01:05:31 -04:00
case MAV_CMD_DO_REPEAT_SERVO :
2014-03-18 23:14:06 -03:00
ServoRelayEvents . 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 ) ;
2012-08-21 23:19:50 -03:00
break ;
2014-01-20 01:05:31 -04:00
case MAV_CMD_DO_REPEAT_RELAY :
2014-03-18 23:14:06 -03:00
ServoRelayEvents . do_repeat_relay ( cmd . content . repeat_relay . num , cmd . content . repeat_relay . repeat_count ,
cmd . content . repeat_relay . cycle_time * 1000.0f ) ;
2012-08-21 23:19:50 -03:00
break ;
2011-03-27 18:26:34 -03:00
2013-07-13 11:27:51 -03:00
case MAV_CMD_DO_SET_ROI : // 201
// point the copter and camera at a region of interest (ROI)
2014-02-27 22:19:05 -04:00
do_roi ( cmd ) ;
2013-07-13 11:27:51 -03:00
break ;
2015-03-21 08:59:50 -03:00
case MAV_CMD_DO_MOUNT_CONTROL : // 205
// point the camera to a specified angle
do_mount_control ( cmd ) ;
break ;
2012-07-10 19:39:13 -03:00
# if CAMERA == ENABLED
2012-08-21 23:19:50 -03:00
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 ;
2012-07-10 19:39:13 -03:00
2012-08-21 23:19:50 -03:00
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)|
2015-03-29 15:49:22 -03:00
do_digicam_configure ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2012-07-10 19:39:13 -03:00
2012-08-21 23:19: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|
2015-03-29 15:49:22 -03:00
do_digicam_control ( cmd ) ;
2012-08-21 23:19:50 -03:00
break ;
2013-10-11 07:34:23 -03:00
case MAV_CMD_DO_SET_CAM_TRIGG_DIST :
2014-03-17 02:16:52 -03:00
camera . set_trigger_distance ( cmd . content . cam_trigg_dist . meters ) ;
2013-10-11 07:34:23 -03:00
break ;
2012-07-10 19:39:13 -03:00
# endif
2014-04-03 05:53:27 -03:00
# if PARACHUTE == ENABLED
case MAV_CMD_DO_PARACHUTE : // Mission command to configure or release parachute
do_parachute ( cmd ) ;
break ;
# endif
2014-09-17 08:31:30 -03:00
# if EPM_ENABLED == ENABLED
case MAV_CMD_DO_GRIPPER : // Mission command to control EPM gripper
do_gripper ( cmd ) ;
break ;
# endif
2014-10-13 05:41:48 -03:00
# if NAV_GUIDED == ENABLED
case MAV_CMD_DO_GUIDED_LIMITS : // 220 accept guided mode limits
do_guided_limits ( cmd ) ;
break ;
# endif
2012-08-21 23:19:50 -03:00
default :
// do nothing with unrecognized MAVLink messages
break ;
}
2014-02-27 22:19:05 -04:00
// always return success
return true ;
2011-02-24 01:56:59 -04:00
}
2011-03-13 03:25:38 -03:00
/********************************************************************************/
// Verify command Handlers
/********************************************************************************/
2015-07-20 04:08:00 -03:00
// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run
// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode
bool Copter : : verify_command_callback ( const AP_Mission : : Mission_Command & cmd )
{
if ( control_mode = = AUTO ) {
bool cmd_complete = verify_command ( cmd ) ;
// send message to GCS
if ( cmd_complete ) {
2015-07-24 05:10:04 -03:00
gcs_send_mission_item_reached_message ( cmd . index ) ;
2015-07-20 04:08:00 -03:00
}
return cmd_complete ;
}
return false ;
}
2014-02-27 22:19:05 -04:00
// verify_command - this will be called repeatedly by ap_mission lib to ensure the active commands are progressing
// should return true once the active navigation command completes successfully
// called at 10hz or higher
2015-05-29 23:12:49 -03:00
bool Copter : : verify_command ( const AP_Mission : : Mission_Command & cmd )
2011-03-02 22:32:50 -04:00
{
2014-02-27 22:19:05 -04:00
switch ( cmd . id ) {
2011-02-24 01:56:59 -04:00
2014-02-27 22:19:05 -04:00
//
// navigation commands
//
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_TAKEOFF :
2015-07-20 04:08:00 -03:00
return verify_takeoff ( ) ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_WAYPOINT :
2015-07-20 04:08:00 -03:00
return verify_nav_wp ( cmd ) ;
2012-07-24 23:02:54 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LAND :
2015-07-20 04:08:00 -03:00
return verify_land ( ) ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LOITER_UNLIM :
2015-07-20 04:08:00 -03:00
return verify_loiter_unlimited ( ) ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LOITER_TURNS :
2015-07-20 04:08:00 -03:00
return verify_circle ( cmd ) ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_LOITER_TIME :
2015-07-20 04:08:00 -03:00
return verify_loiter_time ( ) ;
2011-03-13 03:25:38 -03:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_NAV_RETURN_TO_LAUNCH :
2015-07-20 04:08:00 -03:00
return verify_RTL ( ) ;
2011-02-24 01:56:59 -04:00
2014-03-27 04:05:49 -03:00
case MAV_CMD_NAV_SPLINE_WAYPOINT :
2015-07-20 04:08:00 -03:00
return verify_spline_wp ( cmd ) ;
2014-03-27 04:05:49 -03:00
2014-05-23 02:29:08 -03:00
# if NAV_GUIDED == ENABLED
2014-10-13 05:41:48 -03:00
case MAV_CMD_NAV_GUIDED_ENABLE :
2015-07-20 04:08:00 -03:00
return verify_nav_guided_enable ( cmd ) ;
2014-05-23 02:29:08 -03:00
# endif
2014-02-27 22:19:05 -04:00
///
/// conditional commands
///
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_DELAY :
2015-07-20 04:08:00 -03:00
return verify_wait_delay ( ) ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_DISTANCE :
2015-07-20 04:08:00 -03:00
return verify_within_distance ( ) ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_CHANGE_ALT :
2015-07-20 04:08:00 -03:00
return verify_change_alt ( ) ;
2011-02-24 01:56:59 -04:00
2012-08-21 23:19:50 -03:00
case MAV_CMD_CONDITION_YAW :
2015-07-20 04:08:00 -03:00
return verify_yaw ( ) ;
2011-03-13 03:25:38 -03:00
2014-04-03 05:53:27 -03:00
case MAV_CMD_DO_PARACHUTE :
// assume parachute was released successfully
2015-07-20 04:08:00 -03:00
return true ;
2014-04-03 05:53:27 -03:00
2012-08-21 23:19:50 -03:00
default :
2015-07-24 05:10:04 -03:00
// return true if we do not recognize the command so that we move on to the next command
2015-07-20 04:08:00 -03:00
return true ;
2012-08-21 23:19:50 -03:00
}
2011-02-24 01:56:59 -04:00
}
2014-02-27 22:19:05 -04:00
// exit_mission - function that is called once the mission completes
2015-05-29 23:12:49 -03:00
void Copter : : exit_mission ( )
2014-02-27 22:19:05 -04:00
{
2014-10-23 15:47:00 -03:00
// play a tone
AP_Notify : : events . mission_complete = 1 ;
2014-02-27 22:19:05 -04:00
// if we are not on the ground switch to loiter or land
if ( ! ap . land_complete ) {
// try to enter loiter but if that fails land
2014-10-12 05:06:51 -03:00
if ( ! auto_loiter_start ( ) ) {
2014-02-27 22:19:05 -04:00
set_mode ( LAND ) ;
}
} else {
# if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED
// disarm when the landing detector says we've landed and throttle is at minimum
2014-10-07 12:29:24 -03:00
if ( ap . throttle_zero | | failsafe . radio ) {
2014-02-27 22:19:05 -04:00
init_disarm_motors ( ) ;
}
# else
// if we've landed it's safe to disarm
init_disarm_motors ( ) ;
# endif
}
}
2011-03-02 22:32:50 -04:00
/********************************************************************************/
2011-04-26 02:15:25 -03:00
//
2011-03-02 22:32:50 -04:00
/********************************************************************************/
2012-11-29 08:08:19 -04:00
// do_RTL - start Return-to-Launch
2015-05-29 23:12:49 -03:00
void Copter : : do_RTL ( void )
2011-03-13 03:25:38 -03:00
{
2014-01-27 23:21:45 -04:00
// start rtl in auto flight mode
auto_rtl_start ( ) ;
2011-03-13 03:25:38 -03:00
}
2011-04-26 02:15:25 -03:00
/********************************************************************************/
// Nav (Must) commands
/********************************************************************************/
2012-12-09 02:50:50 -04:00
// do_takeoff - initiate takeoff navigation command
2015-05-29 23:12:49 -03:00
void Copter : : do_takeoff ( const AP_Mission : : Mission_Command & cmd )
2011-02-24 01:56:59 -04:00
{
2013-04-08 23:58:01 -03:00
// Set wp navigation target to safe altitude above current position
2014-02-27 22:19:05 -04:00
float takeoff_alt = cmd . content . location . alt ;
2015-11-27 13:11:58 -04:00
takeoff_alt = MAX ( takeoff_alt , current_loc . alt ) ;
takeoff_alt = MAX ( takeoff_alt , 100.0f ) ;
2014-01-24 22:37:58 -04:00
auto_takeoff_start ( takeoff_alt ) ;
2011-03-02 22:32:50 -04:00
}
2011-02-24 01:56:59 -04:00
2012-12-09 02:50:50 -04:00
// do_nav_wp - initiate move to next waypoint
2015-05-29 23:12:49 -03:00
void Copter : : do_nav_wp ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2014-04-16 08:32:49 -03:00
const Vector3f & curr_pos = inertial_nav . get_position ( ) ;
2014-11-04 22:39:28 -04:00
const Vector3f local_pos = pv_location_to_vector_with_default ( cmd . content . location , curr_pos ) ;
2014-04-16 08:32:49 -03:00
2013-04-08 23:58:01 -03:00
// this will be used to remember the time in millis after we reach or pass the WP.
2014-03-22 00:48:54 -03:00
loiter_time = 0 ;
2014-03-17 02:16:52 -03:00
// this is the delay, stored in seconds
2014-03-22 00:48:54 -03:00
loiter_time_max = abs ( cmd . p1 ) ;
2014-03-27 04:11:50 -03:00
// Set wp navigation target
auto_wp_start ( local_pos ) ;
// if no delay set the waypoint as "fast"
if ( loiter_time_max = = 0 ) {
wp_nav . set_fast_waypoint ( true ) ;
2013-05-08 12:18:36 -03:00
}
2011-03-13 03:25:38 -03:00
}
2012-12-09 02:50:50 -04:00
// do_land - initiate landing procedure
2015-05-29 23:12:49 -03:00
void Copter : : do_land ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2013-05-10 10:37:15 -03:00
// To-Do: check if we have already landed
// if location provided we fly to that location at current altitude
2014-02-27 22:19:05 -04:00
if ( cmd . content . location . lat ! = 0 | | cmd . content . location . lng ! = 0 ) {
2013-05-10 10:37:15 -03:00
// set state to fly to location
land_state = LAND_STATE_FLY_TO_LOCATION ;
// calculate and set desired location above landing target
2014-02-27 22:19:05 -04:00
Vector3f pos = pv_location_to_vector ( cmd . content . location ) ;
2015-02-10 08:28:30 -04:00
pos . z = inertial_nav . get_altitude ( ) ;
2014-01-24 22:37:58 -04:00
auto_wp_start ( pos ) ;
2013-02-18 01:58:24 -04:00
} else {
2013-05-10 10:37:15 -03:00
// set landing state
land_state = LAND_STATE_DESCENDING ;
2012-12-10 10:38:43 -04:00
2014-01-24 22:37:58 -04:00
// initialise landing controller
auto_land_start ( ) ;
2013-05-10 10:37:15 -03:00
}
2011-03-13 03:25:38 -03:00
}
2013-02-04 11:45:31 -04:00
// do_loiter_unlimited - start loitering with no end conditions
2013-02-18 01:58:24 -04:00
// note: caller should set yaw_mode
2015-05-29 23:12:49 -03:00
void Copter : : do_loiter_unlimited ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2013-07-10 23:35:54 -03:00
Vector3f target_pos ;
2013-04-08 23:58:01 -03:00
// get current position
2013-07-10 23:35:54 -03:00
Vector3f curr_pos = inertial_nav . get_position ( ) ;
2013-04-08 23:58:01 -03:00
2014-01-28 00:58:20 -04:00
// default to use position provided
2014-02-27 22:19:05 -04:00
target_pos = pv_location_to_vector ( cmd . content . location ) ;
2014-01-28 00:58:20 -04:00
2013-07-10 23:35:54 -03:00
// use current location if not provided
2014-02-27 22:19:05 -04:00
if ( cmd . content . location . lat = = 0 & & cmd . content . location . lng = = 0 ) {
2014-01-21 22:42:16 -04:00
wp_nav . get_wp_stopping_point_xy ( target_pos ) ;
2013-07-10 23:35:54 -03:00
}
2013-04-08 23:58:01 -03:00
// use current altitude if not provided
2014-01-28 00:58:20 -04:00
// To-Do: use z-axis stopping point instead of current alt
2014-02-27 22:19:05 -04:00
if ( cmd . content . location . alt = = 0 ) {
2013-07-10 23:35:54 -03:00
target_pos . z = curr_pos . z ;
2013-02-04 11:45:31 -04:00
}
2013-04-08 23:58:01 -03:00
// start way point navigator and provide it the desired location
2014-01-27 23:21:45 -04:00
auto_wp_start ( target_pos ) ;
2013-02-18 01:58:24 -04:00
}
2013-02-25 04:50:56 -04:00
// do_circle - initiate moving in a circle
2015-05-29 23:12:49 -03:00
void Copter : : do_circle ( const AP_Mission : : Mission_Command & cmd )
2013-02-18 01:58:24 -04:00
{
2014-01-27 10:44:36 -04:00
Vector3f curr_pos = inertial_nav . get_position ( ) ;
2014-02-27 22:19:05 -04:00
Vector3f circle_center = pv_location_to_vector ( cmd . content . location ) ;
2014-06-11 04:18:35 -03:00
uint8_t circle_radius_m = HIGHBYTE ( cmd . p1 ) ; // circle radius held in high byte of p1
2014-04-16 04:23:24 -03:00
bool move_to_edge_required = false ;
2013-04-08 23:58:01 -03:00
2014-01-27 10:44:36 -04:00
// set target altitude if not provided
2014-04-16 04:23:24 -03:00
if ( cmd . content . location . alt = = 0 ) {
2014-01-27 10:44:36 -04:00
circle_center . z = curr_pos . z ;
2014-04-16 04:23:24 -03:00
} else {
move_to_edge_required = true ;
2012-08-21 23:19:50 -03:00
}
2011-03-13 03:25:38 -03:00
2014-01-27 10:44:36 -04:00
// set lat/lon position if not provided
2014-04-16 04:23:24 -03:00
// To-Do: use previous command's destination if it was a straight line or spline waypoint command
2014-02-27 22:19:05 -04:00
if ( cmd . content . location . lat = = 0 & & cmd . content . location . lng = = 0 ) {
2014-01-27 10:44:36 -04:00
circle_center . x = curr_pos . x ;
circle_center . y = curr_pos . y ;
2014-04-16 04:23:24 -03:00
} else {
move_to_edge_required = true ;
2012-08-21 23:19:50 -03:00
}
2013-02-25 04:50:56 -04:00
2014-04-16 04:23:24 -03:00
// set circle controller's center
circle_nav . set_center ( circle_center ) ;
2013-02-25 04:50:56 -04:00
2014-06-11 04:18:35 -03:00
// set circle radius
if ( circle_radius_m ! = 0 ) {
circle_nav . set_radius ( ( float ) circle_radius_m * 100.0f ) ;
}
2014-04-16 04:23:24 -03:00
// check if we need to move to edge of circle
if ( move_to_edge_required ) {
// move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge
auto_circle_movetoedge_start ( ) ;
} else {
// start circling
auto_circle_start ( ) ;
}
2011-03-13 03:25:38 -03:00
}
2012-12-09 02:50:50 -04:00
// do_loiter_time - initiate loitering at a point for a given time period
2013-02-18 01:58:24 -04:00
// note: caller should set yaw_mode
2015-05-29 23:12:49 -03:00
void Copter : : do_loiter_time ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2013-07-10 23:35:54 -03:00
Vector3f target_pos ;
2013-04-08 23:58:01 -03:00
// get current position
2013-07-10 23:35:54 -03:00
Vector3f curr_pos = inertial_nav . get_position ( ) ;
2013-04-08 23:58:01 -03:00
2014-01-28 00:58:20 -04:00
// default to use position provided
2014-02-27 22:19:05 -04:00
target_pos = pv_location_to_vector ( cmd . content . location ) ;
2014-01-28 00:58:20 -04:00
2013-07-10 23:35:54 -03:00
// use current location if not provided
2014-02-27 22:19:05 -04:00
if ( cmd . content . location . lat = = 0 & & cmd . content . location . lng = = 0 ) {
2014-01-21 22:42:16 -04:00
wp_nav . get_wp_stopping_point_xy ( target_pos ) ;
2013-07-10 23:35:54 -03:00
}
2013-04-08 23:58:01 -03:00
// use current altitude if not provided
2014-02-27 22:19:05 -04:00
if ( cmd . content . location . alt = = 0 ) {
2013-07-10 23:35:54 -03:00
target_pos . z = curr_pos . z ;
2012-08-21 23:19:50 -03:00
}
2011-09-24 21:40:29 -03:00
2013-04-08 23:58:01 -03:00
// start way point navigator and provide it the desired location
2014-01-27 23:21:45 -04:00
auto_wp_start ( target_pos ) ;
2013-04-08 23:58:01 -03:00
// setup loiter timer
loiter_time = 0 ;
2014-03-03 01:38:32 -04:00
loiter_time_max = cmd . p1 ; // units are (seconds)
2011-03-13 03:25:38 -03:00
}
2014-03-27 04:05:49 -03:00
// do_spline_wp - initiate move to next waypoint
2015-05-29 23:12:49 -03:00
void Copter : : do_spline_wp ( const AP_Mission : : Mission_Command & cmd )
2014-03-27 04:05:49 -03:00
{
2014-11-04 22:39:28 -04:00
const Vector3f & curr_pos = inertial_nav . get_position ( ) ;
Vector3f local_pos = pv_location_to_vector_with_default ( cmd . content . location , curr_pos ) ;
2014-03-27 04:05:49 -03:00
// this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0 ;
// this is the delay, stored in seconds
loiter_time_max = abs ( cmd . p1 ) ;
// determine segment start and end type
bool stopped_at_start = true ;
AC_WPNav : : spline_segment_end_type seg_end_type = AC_WPNav : : SEGMENT_END_STOP ;
AP_Mission : : Mission_Command temp_cmd ;
2014-03-29 05:52:38 -03:00
Vector3f next_destination ; // end of next segment
2014-03-27 04:05:49 -03:00
// if previous command was a wp_nav command with no delay set stopped_at_start to false
// To-Do: move processing of delay into wp-nav controller to allow it to determine the stopped_at_start value itself?
uint16_t prev_cmd_idx = mission . get_prev_nav_cmd_index ( ) ;
if ( prev_cmd_idx ! = AP_MISSION_CMD_INDEX_NONE ) {
if ( mission . read_cmd_from_storage ( prev_cmd_idx , temp_cmd ) ) {
if ( ( temp_cmd . id = = MAV_CMD_NAV_WAYPOINT | | temp_cmd . id = = MAV_CMD_NAV_SPLINE_WAYPOINT ) & & temp_cmd . p1 = = 0 ) {
stopped_at_start = false ;
}
}
}
// if there is no delay at the end of this segment get next nav command
2014-03-29 05:52:38 -03:00
if ( cmd . p1 = = 0 & & mission . get_next_nav_cmd ( cmd . index + 1 , temp_cmd ) ) {
2014-03-27 04:05:49 -03:00
// if the next nav command is a waypoint set end type to spline or straight
if ( temp_cmd . id = = MAV_CMD_NAV_WAYPOINT ) {
seg_end_type = AC_WPNav : : SEGMENT_END_STRAIGHT ;
2014-11-04 22:39:28 -04:00
next_destination = pv_location_to_vector_with_default ( temp_cmd . content . location , local_pos ) ;
2014-03-27 04:05:49 -03:00
} else if ( temp_cmd . id = = MAV_CMD_NAV_SPLINE_WAYPOINT ) {
seg_end_type = AC_WPNav : : SEGMENT_END_SPLINE ;
2014-11-04 22:39:28 -04:00
next_destination = pv_location_to_vector_with_default ( temp_cmd . content . location , local_pos ) ;
2014-03-27 04:05:49 -03:00
}
}
// set spline navigation target
2014-03-29 05:52:38 -03:00
auto_spline_start ( local_pos , stopped_at_start , seg_end_type , next_destination ) ;
2014-03-27 04:05:49 -03:00
}
2014-05-23 02:29:08 -03:00
# if NAV_GUIDED == ENABLED
2014-10-13 05:41:48 -03:00
// do_nav_guided_enable - initiate accepting commands from external nav computer
2015-05-29 23:12:49 -03:00
void Copter : : do_nav_guided_enable ( const AP_Mission : : Mission_Command & cmd )
2014-05-23 02:29:08 -03:00
{
2014-10-13 05:41:48 -03:00
if ( cmd . p1 > 0 ) {
// initialise guided limits
guided_limit_init_time_and_pos ( ) ;
2014-05-23 02:29:08 -03:00
2014-10-13 05:41:48 -03:00
// set spline navigation target
auto_nav_guided_start ( ) ;
}
2014-05-23 02:29:08 -03:00
}
# endif // NAV_GUIDED
2014-04-03 05:53:27 -03:00
# if PARACHUTE == ENABLED
// do_parachute - configure or release parachute
2015-05-29 23:12:49 -03:00
void Copter : : do_parachute ( const AP_Mission : : Mission_Command & cmd )
2014-04-03 05:53:27 -03:00
{
switch ( cmd . p1 ) {
case PARACHUTE_DISABLE :
parachute . enabled ( false ) ;
Log_Write_Event ( DATA_PARACHUTE_DISABLED ) ;
break ;
case PARACHUTE_ENABLE :
parachute . enabled ( true ) ;
Log_Write_Event ( DATA_PARACHUTE_ENABLED ) ;
break ;
case PARACHUTE_RELEASE :
parachute_release ( ) ;
break ;
default :
2014-09-17 08:31:30 -03:00
// do nothing
break ;
}
}
# endif
# if EPM_ENABLED == ENABLED
// do_gripper - control EPM gripper
2015-05-29 23:12:49 -03:00
void Copter : : do_gripper ( const AP_Mission : : Mission_Command & cmd )
2014-09-17 08:31:30 -03:00
{
// Note: we ignore the gripper num parameter because we only support one gripper
switch ( cmd . content . gripper . action ) {
case GRIPPER_ACTION_RELEASE :
epm . release ( ) ;
Log_Write_Event ( DATA_EPM_RELEASE ) ;
break ;
case GRIPPER_ACTION_GRAB :
epm . grab ( ) ;
Log_Write_Event ( DATA_EPM_GRAB ) ;
break ;
default :
2014-04-03 05:53:27 -03:00
// do nothing
break ;
}
}
# endif
2014-10-13 05:41:48 -03:00
# if NAV_GUIDED == ENABLED
// do_guided_limits - pass guided limits to guided controller
2015-05-29 23:12:49 -03:00
void Copter : : do_guided_limits ( const AP_Mission : : Mission_Command & cmd )
2014-10-13 05:41:48 -03:00
{
guided_limit_set ( cmd . p1 * 1000 , // convert seconds to ms
cmd . content . guided_limits . alt_min * 100.0f , // convert meters to cm
cmd . content . guided_limits . alt_max * 100.0f , // convert meters to cm
cmd . content . guided_limits . horiz_max * 100.0f ) ; // convert meters to cm
}
# endif
2011-03-13 03:25:38 -03:00
/********************************************************************************/
2011-04-03 18:11:14 -03:00
// Verify Nav (Must) commands
2011-03-13 03:25:38 -03:00
/********************************************************************************/
2012-12-09 02:50:50 -04:00
// verify_takeoff - check if we have completed the takeoff
2015-05-29 23:12:49 -03:00
bool Copter : : verify_takeoff ( )
2011-03-02 22:32:50 -04:00
{
2013-04-08 23:58:01 -03:00
// have we reached our target altitude?
2014-01-19 10:35:55 -04:00
return wp_nav . reached_wp_destination ( ) ;
2011-03-02 22:32:50 -04:00
}
2012-11-23 02:57:49 -04:00
// verify_land - returns true if landing has been completed
2015-05-29 23:12:49 -03:00
bool Copter : : verify_land ( )
2012-01-29 02:00:05 -04:00
{
2013-05-10 10:37:15 -03:00
bool retval = false ;
switch ( land_state ) {
case LAND_STATE_FLY_TO_LOCATION :
// check if we've reached the location
2014-01-19 10:35:55 -04:00
if ( wp_nav . reached_wp_destination ( ) ) {
2013-05-10 10:37:15 -03:00
// get destination so we can use it for loiter target
2014-01-21 22:42:16 -04:00
Vector3f dest = wp_nav . get_wp_destination ( ) ;
2013-05-10 10:37:15 -03:00
2014-01-24 22:37:58 -04:00
// initialise landing controller
auto_land_start ( dest ) ;
2013-05-10 10:37:15 -03:00
// advance to next state
land_state = LAND_STATE_DESCENDING ;
}
break ;
case LAND_STATE_DESCENDING :
// rely on THROTTLE_LAND mode to correctly update landing status
retval = ap . land_complete ;
break ;
default :
// this should never happen
// TO-DO: log an error
retval = true ;
break ;
}
// true is returned if we've successfully landed
return retval ;
2011-03-02 22:32:50 -04:00
}
2012-12-09 02:50:50 -04:00
// verify_nav_wp - check if we have reached the next way point
2015-05-29 23:12:49 -03:00
bool Copter : : verify_nav_wp ( const AP_Mission : : Mission_Command & cmd )
2011-03-02 22:32:50 -04:00
{
2013-04-08 23:58:01 -03:00
// check if we have reached the waypoint
2014-01-19 10:35:55 -04:00
if ( ! wp_nav . reached_wp_destination ( ) ) {
2013-04-08 23:58:01 -03:00
return false ;
2012-08-21 23:19:50 -03:00
}
2014-10-23 15:47:00 -03:00
// play a tone
AP_Notify : : events . waypoint_complete = 1 ;
2013-04-08 23:58:01 -03:00
// start timer if necessary
if ( loiter_time = = 0 ) {
loiter_time = millis ( ) ;
2012-08-21 23:19:50 -03:00
}
2013-04-08 23:58:01 -03:00
// check if timer has run out
if ( ( ( millis ( ) - loiter_time ) / 1000 ) > = loiter_time_max ) {
2015-11-18 15:08:19 -04:00
gcs_send_text_fmt ( MAV_SEVERITY_INFO , " Reached command #%i " , cmd . index ) ;
2012-08-21 23:19:50 -03:00
return true ;
} else {
return false ;
}
2011-03-02 22:32:50 -04:00
}
2015-05-29 23:12:49 -03:00
bool Copter : : verify_loiter_unlimited ( )
2012-07-12 12:52:36 -03:00
{
2012-08-21 23:19:50 -03:00
return false ;
2012-07-12 12:52:36 -03:00
}
2011-03-02 22:32:50 -04:00
2012-12-09 02:50:50 -04:00
// verify_loiter_time - check if we have loitered long enough
2015-05-29 23:12:49 -03:00
bool Copter : : verify_loiter_time ( )
2011-03-02 22:32:50 -04:00
{
2013-04-08 23:58:01 -03:00
// return immediately if we haven't reached our destination
2014-01-19 10:35:55 -04:00
if ( ! wp_nav . reached_wp_destination ( ) ) {
2013-04-08 23:58:01 -03:00
return false ;
2012-08-21 23:19:50 -03:00
}
2013-04-08 23:58:01 -03:00
// start our loiter timer
if ( loiter_time = = 0 ) {
2012-08-21 23:19:50 -03:00
loiter_time = millis ( ) ;
}
2013-04-08 23:58:01 -03:00
// check if loiter timer has run out
2013-04-15 11:57:22 -03:00
return ( ( ( millis ( ) - loiter_time ) / 1000 ) > = loiter_time_max ) ;
2011-03-02 22:32:50 -04:00
}
2013-02-25 04:50:56 -04:00
// verify_circle - check if we have circled the point enough
2015-05-29 23:12:49 -03:00
bool Copter : : verify_circle ( const AP_Mission : : Mission_Command & cmd )
2011-09-21 17:19:36 -03:00
{
2014-04-16 04:23:24 -03:00
// check if we've reached the edge
if ( auto_mode = = Auto_CircleMoveToEdge ) {
if ( wp_nav . reached_wp_destination ( ) ) {
Vector3f curr_pos = inertial_nav . get_position ( ) ;
Vector3f circle_center = pv_location_to_vector ( cmd . content . location ) ;
// set target altitude if not provided
2015-05-04 23:34:21 -03:00
if ( is_zero ( circle_center . z ) ) {
2014-04-16 04:23:24 -03:00
circle_center . z = curr_pos . z ;
}
// set lat/lon position if not provided
if ( cmd . content . location . lat = = 0 & & cmd . content . location . lng = = 0 ) {
circle_center . x = curr_pos . x ;
circle_center . y = curr_pos . y ;
}
// start circling
auto_circle_start ( ) ;
}
return false ;
}
// check if we have completed circling
2016-02-25 13:13:02 -04:00
return fabsf ( circle_nav . get_angle_total ( ) / M_2PI ) > = LOWBYTE ( cmd . p1 ) ;
2011-09-21 17:19:36 -03:00
}
2012-11-29 08:08:19 -04:00
// verify_RTL - handles any state changes required to implement RTL
// do_RTL should have been called once first to initialise all variables
// returns true with RTL has completed successfully
2015-05-29 23:12:49 -03:00
bool Copter : : verify_RTL ( )
2011-03-02 22:32:50 -04:00
{
2015-05-17 22:37:32 -03:00
return ( rtl_state_complete & & ( rtl_state = = RTL_FinalDescent | | rtl_state = = RTL_Land ) ) ;
2011-03-02 22:32:50 -04:00
}
2014-03-27 04:05:49 -03:00
// verify_spline_wp - check if we have reached the next way point using spline
2015-05-29 23:12:49 -03:00
bool Copter : : verify_spline_wp ( const AP_Mission : : Mission_Command & cmd )
2014-03-27 04:05:49 -03:00
{
// check if we have reached the waypoint
if ( ! wp_nav . reached_wp_destination ( ) ) {
return false ;
}
// start timer if necessary
if ( loiter_time = = 0 ) {
loiter_time = millis ( ) ;
}
// check if timer has run out
if ( ( ( millis ( ) - loiter_time ) / 1000 ) > = loiter_time_max ) {
2015-11-18 15:08:19 -04:00
gcs_send_text_fmt ( MAV_SEVERITY_INFO , " Reached command #%i " , cmd . index ) ;
2014-03-27 04:05:49 -03:00
return true ;
} else {
return false ;
}
}
2014-05-23 02:29:08 -03:00
# if NAV_GUIDED == ENABLED
// verify_nav_guided - check if we have breached any limits
2015-05-29 23:12:49 -03:00
bool Copter : : verify_nav_guided_enable ( const AP_Mission : : Mission_Command & cmd )
2014-05-23 02:29:08 -03:00
{
2014-10-13 05:41:48 -03:00
// if disabling guided mode then immediately return true so we move to next command
if ( cmd . p1 = = 0 ) {
2014-05-23 02:29:08 -03:00
return true ;
}
2014-10-13 05:41:48 -03:00
// check time and position limits
return guided_limit_check ( ) ;
2014-05-23 02:29:08 -03:00
}
# endif // NAV_GUIDED
2011-03-02 22:32:50 -04:00
/********************************************************************************/
2011-04-03 18:11:14 -03:00
// Condition (May) commands
2011-03-02 22:32:50 -04:00
/********************************************************************************/
2015-05-29 23:12:49 -03:00
void Copter : : do_wait_delay ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2012-08-21 23:19:50 -03:00
condition_start = millis ( ) ;
2014-03-17 02:16:52 -03:00
condition_value = cmd . content . delay . seconds * 1000 ; // convert seconds to milliseconds
2011-03-13 03:25:38 -03:00
}
2015-05-29 23:12:49 -03:00
void Copter : : do_change_alt ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2013-04-08 23:58:01 -03:00
// To-Do: store desired altitude in a variable so that it can be verified later
2011-03-13 03:25:38 -03:00
}
2015-05-29 23:12:49 -03:00
void Copter : : do_within_distance ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2014-03-17 02:16:52 -03:00
condition_value = cmd . content . distance . meters * 100 ;
2011-03-13 03:25:38 -03:00
}
2015-05-29 23:12:49 -03:00
void Copter : : do_yaw ( const AP_Mission : : Mission_Command & cmd )
2011-03-02 22:32:50 -04:00
{
2014-06-13 06:17:35 -03:00
set_auto_yaw_look_at_heading (
cmd . content . yaw . angle_deg ,
cmd . content . yaw . turn_rate_dps ,
2014-09-26 00:20:40 -03:00
cmd . content . yaw . direction ,
2014-06-13 06:17:35 -03:00
cmd . content . yaw . relative_angle ) ;
2011-03-02 22:32:50 -04:00
}
2011-03-26 03:35:52 -03:00
2011-03-13 03:25:38 -03:00
/********************************************************************************/
// Verify Condition (May) commands
/********************************************************************************/
2015-05-29 23:12:49 -03:00
bool Copter : : verify_wait_delay ( )
2011-03-02 22:32:50 -04:00
{
2015-11-27 13:11:58 -04:00
if ( millis ( ) - condition_start > ( uint32_t ) MAX ( condition_value , 0 ) ) {
2012-08-21 23:19:50 -03:00
condition_value = 0 ;
return true ;
}
return false ;
2011-03-02 22:32:50 -04:00
}
2011-02-24 01:56:59 -04:00
2015-05-29 23:12:49 -03:00
bool Copter : : verify_change_alt ( )
2011-03-02 22:32:50 -04:00
{
2013-04-08 23:58:01 -03:00
// To-Do: use recorded target altitude to verify we have reached the target
return true ;
2011-02-24 01:56:59 -04:00
}
2015-05-29 23:12:49 -03:00
bool Copter : : verify_within_distance ( )
2011-03-02 22:32:50 -04:00
{
2014-04-17 10:22:05 -03:00
// update distance calculation
calc_wp_distance ( ) ;
2015-11-30 07:02:09 -04:00
if ( wp_distance < ( uint32_t ) MAX ( condition_value , 0 ) ) {
2012-08-21 23:19:50 -03:00
condition_value = 0 ;
return true ;
}
return false ;
2011-03-02 22:32:50 -04:00
}
2012-12-08 01:23:32 -04:00
// verify_yaw - return true if we have reached the desired heading
2015-05-29 23:12:49 -03:00
bool Copter : : verify_yaw ( )
2011-03-02 22:32:50 -04:00
{
2014-04-17 04:14:18 -03:00
// set yaw mode if it has been changed (the waypoint controller often retakes control of yaw as it executes a new waypoint command)
if ( auto_yaw_mode ! = AUTO_YAW_LOOK_AT_HEADING ) {
set_auto_yaw_mode ( AUTO_YAW_LOOK_AT_HEADING ) ;
}
// check if we are within 2 degrees of the target heading
if ( labs ( wrap_180_cd ( ahrs . yaw_sensor - yaw_look_at_heading ) ) < = 200 ) {
2012-08-21 23:19:50 -03:00
return true ;
} else {
return false ;
}
2011-03-02 22:32:50 -04:00
}
/********************************************************************************/
2011-04-03 18:11:14 -03:00
// Do (Now) commands
2011-03-02 22:32:50 -04:00
/********************************************************************************/
2013-06-01 23:25:35 -03:00
// do_guided - start guided mode
2015-05-29 23:12:49 -03:00
bool Copter : : do_guided ( const AP_Mission : : Mission_Command & cmd )
2013-06-01 23:25:35 -03:00
{
2014-06-02 06:06:11 -03:00
Vector3f pos_or_vel ; // target location or velocity
2014-05-26 04:00:25 -03:00
2014-05-14 16:11:28 -03:00
// only process guided waypoint if we are in guided mode
2014-05-23 03:06:44 -03:00
if ( control_mode ! = GUIDED & & ! ( control_mode = = AUTO & & auto_mode = = Auto_NavGuided ) ) {
2014-05-14 16:11:28 -03:00
return false ;
2013-06-01 23:25:35 -03:00
}
2014-05-26 04:00:25 -03:00
// switch to handle different commands
switch ( cmd . id ) {
case MAV_CMD_NAV_WAYPOINT :
// set wp_nav's destination
2014-06-02 06:06:11 -03:00
pos_or_vel = pv_location_to_vector ( cmd . content . location ) ;
guided_set_destination ( pos_or_vel ) ;
return true ;
break ;
2014-05-26 04:00:25 -03:00
case MAV_CMD_CONDITION_YAW :
do_yaw ( cmd ) ;
return true ;
break ;
default :
// reject unrecognised command
return false ;
break ;
}
2014-02-27 22:19:05 -04:00
return true ;
2013-06-01 23:25:35 -03:00
}
2015-05-29 23:12:49 -03:00
void Copter : : do_change_speed ( const AP_Mission : : Mission_Command & cmd )
2011-03-13 03:25:38 -03:00
{
2014-03-18 23:14:06 -03:00
if ( cmd . content . speed . target_ms > 0 ) {
2014-04-29 22:49:20 -03:00
wp_nav . set_speed_xy ( cmd . content . speed . target_ms * 100.0f ) ;
2014-03-18 23:14:06 -03:00
}
2011-03-13 03:25:38 -03:00
}
2015-05-29 23:12:49 -03:00
void Copter : : do_set_home ( const AP_Mission : : Mission_Command & cmd )
2011-03-02 22:32:50 -04:00
{
2015-02-09 07:25:45 -04:00
if ( cmd . p1 = = 1 | | ( cmd . content . location . lat = = 0 & & cmd . content . location . lng = = 0 & & cmd . content . location . alt = = 0 ) ) {
set_home_to_current_location ( ) ;
2012-08-21 23:19:50 -03:00
} else {
2015-02-09 07:25:45 -04:00
if ( ! far_from_EKF_origin ( cmd . content . location ) ) {
set_home ( cmd . content . location ) ;
}
2012-08-21 23:19:50 -03:00
}
2011-03-02 22:32:50 -04:00
}
2013-07-13 11:27:51 -03:00
// do_roi - starts actions required by MAV_CMD_NAV_ROI
// this involves either moving the camera to point at the ROI (region of interest)
// and possibly rotating the copter to point at the ROI if our mount type does not support a yaw feature
// TO-DO: add support for other features of MAV_CMD_DO_SET_ROI including pointing at a given waypoint
2015-05-29 23:12:49 -03:00
void Copter : : do_roi ( const AP_Mission : : Mission_Command & cmd )
2012-07-24 23:02:54 -03:00
{
2014-07-04 03:40:12 -03:00
set_auto_yaw_roi ( cmd . content . location ) ;
2012-07-24 23:02:54 -03:00
}
2012-12-06 10:48:30 -04:00
2015-03-29 15:49:22 -03:00
// do_digicam_configure Send Digicam Configure message with the camera library
2015-05-29 23:12:49 -03:00
void Copter : : do_digicam_configure ( const AP_Mission : : Mission_Command & cmd )
2015-03-29 15:49:22 -03:00
{
# if CAMERA == ENABLED
2015-09-06 17:43:13 -03:00
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 ) ;
2015-03-29 15:49:22 -03:00
# endif
}
// do_digicam_control Send Digicam Control message with the camera library
2015-05-29 23:12:49 -03:00
void Copter : : do_digicam_control ( const AP_Mission : : Mission_Command & cmd )
2015-03-29 15:49:22 -03:00
{
# if CAMERA == ENABLED
2016-01-28 18:32:15 -04:00
if ( 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 ) ) {
log_picture ( ) ;
}
2015-03-29 15:49:22 -03:00
# endif
}
2012-12-06 10:48:30 -04:00
// do_take_picture - take a picture with the camera library
2015-05-29 23:12:49 -03:00
void Copter : : do_take_picture ( )
2012-12-06 10:48:30 -04:00
{
2012-12-06 11:57:08 -04:00
# if CAMERA == ENABLED
2015-03-29 15:49:22 -03:00
camera . trigger_pic ( true ) ;
log_picture ( ) ;
# endif
}
// log_picture - log picture taken and send feedback to GCS
2015-05-29 23:12:49 -03:00
void Copter : : log_picture ( )
2015-03-29 15:49:22 -03:00
{
2016-01-19 01:26:14 -04:00
if ( ! camera . using_feedback_pin ( ) ) {
gcs_send_message ( MSG_CAMERA_FEEDBACK ) ;
if ( should_log ( MASK_LOG_CAMERA ) ) {
DataFlash . Log_Write_Camera ( ahrs , gps , current_loc ) ;
}
} else {
if ( should_log ( MASK_LOG_CAMERA ) ) {
DataFlash . Log_Write_Trigger ( ahrs , gps , current_loc ) ;
}
2012-12-06 11:57:08 -04:00
}
2012-12-06 10:48:30 -04:00
}
2015-03-21 08:59:50 -03:00
// point the camera to a specified angle
2015-05-29 23:12:49 -03:00
void Copter : : do_mount_control ( const AP_Mission : : Mission_Command & cmd )
2015-03-21 08:59:50 -03:00
{
# if MOUNT == ENABLED
camera_mount . set_angle_targets ( cmd . content . mount_control . roll , cmd . content . mount_control . pitch , cmd . content . mount_control . yaw ) ;
# endif
2016-01-19 01:26:14 -04:00
}