2016-01-14 15:30:56 -04:00
# include "Sub.h"
2015-12-30 18:57:56 -04:00
/*
2017-03-09 13:25:54 -04:00
* control_auto . cpp
* Contains the mission , waypoint navigation and NAV_CMD item implementation
2015-12-30 18:57:56 -04:00
*
* While in the auto flight mode , navigation or do / now commands can be run .
* Code in this file implements the navigation commands
*/
// auto_init - initialise auto controller
2017-04-14 16:10:29 -03:00
bool Sub : : auto_init ( )
2015-12-30 18:57:56 -04:00
{
2017-04-14 16:10:29 -03:00
if ( ! position_ok ( ) | | mission . num_commands ( ) < 2 ) {
return false ;
}
auto_mode = Auto_Loiter ;
2015-12-30 18:57:56 -04:00
2017-04-14 16:10:29 -03:00
// stop ROI from carrying over from previous runs of the mission
// To-Do: reset the yaw as part of auto_wp_start when the previous command was not a wp command to remove the need for this special ROI check
if ( auto_yaw_mode = = AUTO_YAW_ROI ) {
set_auto_yaw_mode ( AUTO_YAW_HOLD ) ;
}
2015-12-30 18:57:56 -04:00
2017-04-14 16:10:29 -03:00
// initialise waypoint and spline controller
wp_nav . wp_and_spline_init ( ) ;
2015-12-30 18:57:56 -04:00
2017-04-14 16:10:29 -03:00
// clear guided limits
guided_limit_clear ( ) ;
2015-12-30 18:57:56 -04:00
2017-04-14 16:10:29 -03:00
// start/resume the mission (based on MIS_RESTART parameter)
mission . start_or_resume ( ) ;
return true ;
2015-12-30 18:57:56 -04:00
}
2017-04-07 00:21:37 -03:00
// auto_run - runs the appropriate auto controller
// according to the current auto_mode
// should be called at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub : : auto_run ( )
2015-12-30 18:57:56 -04:00
{
2017-04-07 00:21:37 -03:00
mission . update ( ) ;
2015-12-30 18:57:56 -04:00
// call the correct auto controller
switch ( auto_mode ) {
case Auto_WP :
case Auto_CircleMoveToEdge :
auto_wp_run ( ) ;
break ;
case Auto_Circle :
auto_circle_run ( ) ;
break ;
case Auto_Spline :
auto_spline_run ( ) ;
break ;
case Auto_NavGuided :
# if NAV_GUIDED == ENABLED
auto_nav_guided_run ( ) ;
# endif
break ;
case Auto_Loiter :
auto_loiter_run ( ) ;
break ;
2017-02-07 20:42:28 -04:00
case Auto_TerrainRecover :
2017-02-03 17:33:27 -04:00
auto_terrain_recover_run ( ) ;
break ;
2015-12-30 18:57:56 -04:00
}
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
2016-01-14 15:30:56 -04:00
void Sub : : auto_wp_start ( const Vector3f & destination )
2015-12-30 18:57:56 -04:00
{
auto_mode = Auto_WP ;
2016-05-03 01:45:37 -03:00
// initialise wpnav (no need to check return status because terrain data is not used)
2017-02-03 17:33:27 -04:00
wp_nav . set_wp_destination ( destination , false ) ;
2016-05-03 01:45:37 -03:00
2017-02-03 17:33:27 -04:00
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if ( auto_yaw_mode ! = AUTO_YAW_ROI ) {
set_auto_yaw_mode ( get_default_auto_yaw_mode ( false ) ) ;
}
2016-05-03 01:45:37 -03:00
}
// auto_wp_start - initialises waypoint controller to implement flying to a particular destination
void Sub : : auto_wp_start ( const Location_Class & dest_loc )
{
2017-02-03 17:33:27 -04:00
auto_mode = Auto_WP ;
2016-05-03 01:45:37 -03:00
2017-02-03 17:33:27 -04:00
// send target to waypoint controller
if ( ! wp_nav . set_wp_destination ( dest_loc ) ) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event ( ) ;
return ;
}
2015-12-30 18:57:56 -04:00
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if ( auto_yaw_mode ! = AUTO_YAW_ROI ) {
set_auto_yaw_mode ( get_default_auto_yaw_mode ( false ) ) ;
}
}
// auto_wp_run - runs the auto waypoint controller
// called by auto_run at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub : : auto_wp_run ( )
2015-12-30 18:57:56 -04:00
{
2017-04-13 16:34:58 -03:00
// if not armed set throttle to zero and exit immediately
2017-04-11 13:45:16 -03:00
if ( ! motors . armed ( ) ) {
2017-02-10 13:46:54 -04:00
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
2015-12-30 18:57:56 -04:00
// (of course it would be better if people just used take-off)
// call attitude controller
2017-02-10 13:46:54 -04:00
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
2016-04-05 00:17:39 -03:00
motors . set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2015-12-30 18:57:56 -04:00
attitude_control . set_throttle_out_unstabilized ( 0 , true , g . throttle_filt ) ;
2016-02-23 02:15:15 -04:00
2015-12-30 18:57:56 -04:00
return ;
}
// process pilot's yaw input
float target_yaw_rate = 0 ;
2017-04-14 16:58:54 -03:00
if ( ! failsafe . pilot_input ) {
2015-12-30 18:57:56 -04:00
// get pilot's desired yaw rate
2017-02-03 00:18:27 -04:00
target_yaw_rate = get_pilot_desired_yaw_rate ( channel_yaw - > get_control_in ( ) ) ;
2015-12-30 18:57:56 -04:00
if ( ! is_zero ( target_yaw_rate ) ) {
set_auto_yaw_mode ( AUTO_YAW_HOLD ) ;
}
}
2016-04-05 00:17:39 -03:00
// set motors to full range
motors . set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2015-12-30 18:57:56 -04:00
// run waypoint controller
2017-02-07 20:42:28 -04:00
// TODO logic for terrain tracking target going below fence limit
// TODO implement waypoint radius individually for each waypoint based on cmd.p2
// TODO fix auto yaw heading to switch to something appropriate when mission complete and switches to loiter
2016-05-03 01:45:37 -03:00
failsafe_terrain_set_status ( wp_nav . update_wpnav ( ) ) ;
2015-12-30 18:57:56 -04:00
2017-02-07 20:42:28 -04:00
///////////////////////
// update xy outputs //
float lateral_out , forward_out ;
translate_wpnav_rp ( lateral_out , forward_out ) ;
2017-02-03 17:33:27 -04:00
// Send to forward/lateral outputs
motors . set_lateral ( lateral_out ) ;
motors . set_forward ( forward_out ) ;
2017-02-07 20:42:28 -04:00
2015-12-30 18:57:56 -04:00
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control . update_z_controller ( ) ;
2017-02-03 17:33:27 -04:00
////////////////////////////
// update attitude output //
2017-02-07 20:42:28 -04:00
// get pilot desired lean angles
float target_roll , target_pitch ;
get_pilot_desired_lean_angles ( channel_roll - > get_control_in ( ) , channel_pitch - > get_control_in ( ) , target_roll , target_pitch , aparm . angle_max ) ;
2015-12-30 18:57:56 -04:00
// call attitude controller
if ( auto_yaw_mode = = AUTO_YAW_HOLD ) {
// roll & pitch from waypoint controller, yaw rate from pilot
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_euler_rate_yaw ( target_roll , target_pitch , target_yaw_rate ) ;
2017-02-03 17:33:27 -04:00
} else {
2015-12-30 18:57:56 -04:00
// roll, pitch from waypoint controller, yaw heading from auto_heading()
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_yaw ( target_roll , target_pitch , get_auto_heading ( ) , true ) ;
2015-12-30 18:57:56 -04:00
}
}
// auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
// seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE. If Straight or Spline the next_destination should be provided
2016-05-03 01:45:37 -03:00
void Sub : : auto_spline_start ( const Location_Class & destination , bool stopped_at_start ,
2017-02-03 17:33:27 -04:00
AC_WPNav : : spline_segment_end_type seg_end_type ,
const Location_Class & next_destination )
2015-12-30 18:57:56 -04:00
{
auto_mode = Auto_Spline ;
// initialise wpnav
2016-05-03 01:45:37 -03:00
if ( ! wp_nav . set_spline_destination ( destination , stopped_at_start , seg_end_type , next_destination ) ) {
2017-02-03 17:33:27 -04:00
// failure to set destination can only be because of missing terrain data
2016-05-22 21:50:44 -03:00
failsafe_terrain_on_event ( ) ;
return ;
2016-05-03 01:45:37 -03:00
}
2015-12-30 18:57:56 -04:00
// initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
if ( auto_yaw_mode ! = AUTO_YAW_ROI ) {
set_auto_yaw_mode ( get_default_auto_yaw_mode ( false ) ) ;
}
}
// auto_spline_run - runs the auto spline controller
// called by auto_run at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub : : auto_spline_run ( )
2015-12-30 18:57:56 -04:00
{
2017-04-13 16:34:58 -03:00
// if not armed set throttle to zero and exit immediately
if ( ! motors . armed ( ) ) {
2017-02-10 13:46:54 -04:00
// To-Do: reset waypoint origin to current location because vehicle is probably on the ground so we don't want it lurching left or right on take-off
2015-12-30 18:57:56 -04:00
// (of course it would be better if people just used take-off)
2017-02-10 13:46:54 -04:00
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
2015-12-30 18:57:56 -04:00
attitude_control . set_throttle_out_unstabilized ( 0 , true , g . throttle_filt ) ;
2016-04-05 00:17:39 -03:00
motors . set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2016-02-23 02:15:15 -04:00
2015-12-30 18:57:56 -04:00
return ;
}
// process pilot's yaw input
float target_yaw_rate = 0 ;
2017-04-14 16:58:54 -03:00
if ( ! failsafe . pilot_input ) {
2015-12-30 18:57:56 -04:00
// get pilot's desired yaw rat
2017-02-03 00:18:27 -04:00
target_yaw_rate = get_pilot_desired_yaw_rate ( channel_yaw - > get_control_in ( ) ) ;
2015-12-30 18:57:56 -04:00
if ( ! is_zero ( target_yaw_rate ) ) {
set_auto_yaw_mode ( AUTO_YAW_HOLD ) ;
}
}
2016-04-05 00:17:39 -03:00
// set motors to full range
motors . set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2015-12-30 18:57:56 -04:00
// run waypoint controller
wp_nav . update_spline ( ) ;
2017-04-13 17:38:28 -03:00
float lateral_out , forward_out ;
translate_wpnav_rp ( lateral_out , forward_out ) ;
// Send to forward/lateral outputs
motors . set_lateral ( lateral_out ) ;
motors . set_forward ( forward_out ) ;
2015-12-30 18:57:56 -04:00
// call z-axis position controller (wpnav should have already updated it's alt target)
pos_control . update_z_controller ( ) ;
2017-04-13 17:38:28 -03:00
// get pilot desired lean angles
float target_roll , target_pitch ;
get_pilot_desired_lean_angles ( channel_roll - > get_control_in ( ) , channel_pitch - > get_control_in ( ) , target_roll , target_pitch , aparm . angle_max ) ;
2015-12-30 18:57:56 -04:00
// call attitude controller
if ( auto_yaw_mode = = AUTO_YAW_HOLD ) {
// roll & pitch from waypoint controller, yaw rate from pilot
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_euler_rate_yaw ( target_roll , target_pitch , target_yaw_rate ) ;
2017-02-03 17:33:27 -04:00
} else {
2015-12-30 18:57:56 -04:00
// roll, pitch from waypoint controller, yaw heading from auto_heading()
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_yaw ( target_roll , target_pitch , get_auto_heading ( ) , true ) ;
2015-12-30 18:57:56 -04:00
}
}
// auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location
// we assume the caller has set the circle's circle with circle_nav.set_center()
// we assume the caller has performed all required GPS_ok checks
2016-05-03 01:45:37 -03:00
void Sub : : auto_circle_movetoedge_start ( const Location_Class & circle_center , float radius_m )
2015-12-30 18:57:56 -04:00
{
2017-02-03 17:33:27 -04:00
// convert location to vector from ekf origin
Vector3f circle_center_neu ;
if ( ! circle_center . get_vector_from_origin_NEU ( circle_center_neu ) ) {
// default to current position and log error
circle_center_neu = inertial_nav . get_position ( ) ;
Log_Write_Error ( ERROR_SUBSYSTEM_NAVIGATION , ERROR_CODE_FAILED_CIRCLE_INIT ) ;
}
circle_nav . set_center ( circle_center_neu ) ;
// set circle radius
if ( ! is_zero ( radius_m ) ) {
circle_nav . set_radius ( radius_m * 100.0f ) ;
}
// check our distance from edge of circle
Vector3f circle_edge_neu ;
circle_nav . get_closest_point_on_circle ( circle_edge_neu ) ;
float dist_to_edge = ( inertial_nav . get_position ( ) - circle_edge_neu ) . length ( ) ;
// if more than 3m then fly to edge
if ( dist_to_edge > 300.0f ) {
// set the state to move to the edge of the circle
auto_mode = Auto_CircleMoveToEdge ;
// convert circle_edge_neu to Location_Class
Location_Class circle_edge ( circle_edge_neu ) ;
// convert altitude to same as command
circle_edge . set_alt_cm ( circle_center . alt , circle_center . get_alt_frame ( ) ) ;
// initialise wpnav to move to edge of circle
if ( ! wp_nav . set_wp_destination ( circle_edge ) ) {
// failure to set destination can only be because of missing terrain data
failsafe_terrain_on_event ( ) ;
}
// if we are outside the circle, point at the edge, otherwise hold yaw
const Vector3f & curr_pos = inertial_nav . get_position ( ) ;
float dist_to_center = norm ( circle_center_neu . x - curr_pos . x , circle_center_neu . y - curr_pos . y ) ;
if ( dist_to_center > circle_nav . get_radius ( ) & & dist_to_center > 500 ) {
set_auto_yaw_mode ( get_default_auto_yaw_mode ( false ) ) ;
} else {
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
set_auto_yaw_mode ( AUTO_YAW_HOLD ) ;
}
} else {
auto_circle_start ( ) ;
}
2015-12-30 18:57:56 -04:00
}
// auto_circle_start - initialises controller to fly a circle in AUTO flight mode
2016-05-03 01:45:37 -03:00
// assumes that circle_nav object has already been initialised with circle center and radius
2016-01-14 15:30:56 -04:00
void Sub : : auto_circle_start ( )
2015-12-30 18:57:56 -04:00
{
auto_mode = Auto_Circle ;
// initialise circle controller
circle_nav . init ( circle_nav . get_center ( ) ) ;
}
// auto_circle_run - circle in AUTO flight mode
// called by auto_run at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub : : auto_circle_run ( )
2015-12-30 18:57:56 -04:00
{
// call circle controller
circle_nav . update ( ) ;
2017-03-04 15:41:26 -04:00
float lateral_out , forward_out ;
translate_circle_nav_rp ( lateral_out , forward_out ) ;
// Send to forward/lateral outputs
motors . set_lateral ( lateral_out ) ;
motors . set_forward ( forward_out ) ;
2015-12-30 18:57:56 -04:00
// call z-axis position controller
pos_control . update_z_controller ( ) ;
// roll & pitch from waypoint controller, yaw rate from pilot
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_yaw ( channel_roll - > get_control_in ( ) , channel_pitch - > get_control_in ( ) , circle_nav . get_yaw ( ) , true ) ;
2015-12-30 18:57:56 -04:00
}
# if NAV_GUIDED == ENABLED
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
2016-01-14 15:30:56 -04:00
void Sub : : auto_nav_guided_start ( )
2015-12-30 18:57:56 -04:00
{
auto_mode = Auto_NavGuided ;
// call regular guided flight mode initialisation
guided_init ( true ) ;
// initialise guided start time and position as reference for limit checking
guided_limit_init_time_and_pos ( ) ;
}
// auto_nav_guided_run - allows control by external navigation controller
// called by auto_run at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub : : auto_nav_guided_run ( )
2015-12-30 18:57:56 -04:00
{
// call regular guided flight mode run function
guided_run ( ) ;
}
# endif // NAV_GUIDED
// auto_loiter_start - initialises loitering in auto mode
// returns success/failure because this can be called by exit_mission
2016-01-14 15:30:56 -04:00
bool Sub : : auto_loiter_start ( )
2015-12-30 18:57:56 -04:00
{
// return failure if GPS is bad
if ( ! position_ok ( ) ) {
return false ;
}
auto_mode = Auto_Loiter ;
Vector3f origin = inertial_nav . get_position ( ) ;
// calculate stopping point
Vector3f stopping_point ;
pos_control . get_stopping_point_xy ( stopping_point ) ;
pos_control . get_stopping_point_z ( stopping_point ) ;
// initialise waypoint controller target to stopping point
wp_nav . set_wp_origin_and_destination ( origin , stopping_point ) ;
// hold yaw at current heading
set_auto_yaw_mode ( AUTO_YAW_HOLD ) ;
return true ;
}
// auto_loiter_run - loiter in AUTO flight mode
// called by auto_run at 100hz or more
2016-01-14 15:30:56 -04:00
void Sub : : auto_loiter_run ( )
2015-12-30 18:57:56 -04:00
{
2017-04-13 16:34:58 -03:00
// if not armed set throttle to zero and exit immediately
2017-04-11 13:45:16 -03:00
if ( ! motors . armed ( ) ) {
2017-02-03 17:33:27 -04:00
motors . set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
2017-02-10 13:46:54 -04:00
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
2015-12-30 18:57:56 -04:00
attitude_control . set_throttle_out_unstabilized ( 0 , true , g . throttle_filt ) ;
2016-02-23 02:15:15 -04:00
2015-12-30 18:57:56 -04:00
return ;
}
// accept pilot input of yaw
float target_yaw_rate = 0 ;
2017-04-14 16:58:54 -03:00
if ( ! failsafe . pilot_input ) {
2017-02-03 00:18:27 -04:00
target_yaw_rate = get_pilot_desired_yaw_rate ( channel_yaw - > get_control_in ( ) ) ;
2015-12-30 18:57:56 -04:00
}
2016-04-05 00:17:39 -03:00
// set motors to full range
motors . set_desired_spool_state ( AP_Motors : : DESIRED_THROTTLE_UNLIMITED ) ;
2016-05-03 01:45:37 -03:00
// run waypoint and z-axis position controller
failsafe_terrain_set_status ( wp_nav . update_wpnav ( ) ) ;
2017-02-07 20:42:28 -04:00
///////////////////////
// update xy outputs //
2017-02-03 17:33:27 -04:00
float lateral_out , forward_out ;
translate_wpnav_rp ( lateral_out , forward_out ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Send to forward/lateral outputs
motors . set_lateral ( lateral_out ) ;
motors . set_forward ( forward_out ) ;
2017-02-07 20:42:28 -04:00
// call z-axis position controller (wpnav should have already updated it's alt target)
2015-12-30 18:57:56 -04:00
pos_control . update_z_controller ( ) ;
2017-02-07 20:42:28 -04:00
// get pilot desired lean angles
float target_roll , target_pitch ;
get_pilot_desired_lean_angles ( channel_roll - > get_control_in ( ) , channel_pitch - > get_control_in ( ) , target_roll , target_pitch , aparm . angle_max ) ;
2017-02-03 17:33:27 -04:00
// roll & pitch from waypoint controller, yaw rate from pilot
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_euler_rate_yaw ( target_roll , target_pitch , target_yaw_rate ) ;
2015-12-30 18:57:56 -04:00
}
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
2016-01-14 15:30:56 -04:00
uint8_t Sub : : get_default_auto_yaw_mode ( bool rtl )
2015-12-30 18:57:56 -04:00
{
switch ( g . wp_yaw_behavior ) {
2017-02-03 17:33:27 -04:00
case WP_YAW_BEHAVIOR_NONE :
return AUTO_YAW_HOLD ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL :
if ( rtl ) {
return AUTO_YAW_HOLD ;
} else {
return AUTO_YAW_LOOK_AT_NEXT_WP ;
}
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case WP_YAW_BEHAVIOR_LOOK_AHEAD :
return AUTO_YAW_LOOK_AHEAD ;
break ;
2015-12-30 18:57:56 -04:00
2017-02-03 17:33:27 -04:00
case WP_YAW_BEHAVIOR_CORRECT_XTRACK :
return AUTO_YAW_CORRECT_XTRACK ;
break ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP :
default :
return AUTO_YAW_LOOK_AT_NEXT_WP ;
break ;
2015-12-30 18:57:56 -04:00
}
}
// set_auto_yaw_mode - sets the yaw mode for auto
2016-01-14 15:30:56 -04:00
void Sub : : set_auto_yaw_mode ( uint8_t yaw_mode )
2015-12-30 18:57:56 -04:00
{
// return immediately if no change
if ( auto_yaw_mode = = yaw_mode ) {
return ;
}
auto_yaw_mode = yaw_mode ;
// perform initialisation
switch ( auto_yaw_mode ) {
case AUTO_YAW_LOOK_AT_NEXT_WP :
// wpnav will initialise heading when wpnav's set_destination method is called
break ;
case AUTO_YAW_ROI :
// point towards a location held in yaw_look_at_WP
yaw_look_at_WP_bearing = ahrs . yaw_sensor ;
break ;
case AUTO_YAW_LOOK_AT_HEADING :
// keep heading pointing in the direction held in yaw_look_at_heading
// caller should set the yaw_look_at_heading
break ;
case AUTO_YAW_LOOK_AHEAD :
// Commanded Yaw to automatically look ahead.
yaw_look_ahead_bearing = ahrs . yaw_sensor ;
break ;
case AUTO_YAW_RESETTOARMEDYAW :
// initial_armed_bearing will be set during arming so no init required
break ;
}
}
// set_auto_yaw_look_at_heading - sets the yaw look at heading for auto mode
2016-01-14 15:30:56 -04:00
void Sub : : set_auto_yaw_look_at_heading ( float angle_deg , float turn_rate_dps , int8_t direction , uint8_t relative_angle )
2015-12-30 18:57:56 -04:00
{
// get current yaw target
int32_t curr_yaw_target = attitude_control . get_att_target_euler_cd ( ) . z ;
// get final angle, 1 = Relative, 0 = Absolute
if ( relative_angle = = 0 ) {
// absolute angle
yaw_look_at_heading = wrap_360_cd ( angle_deg * 100 ) ;
} else {
// relative angle
if ( direction < 0 ) {
angle_deg = - angle_deg ;
}
yaw_look_at_heading = wrap_360_cd ( ( angle_deg * 100 + curr_yaw_target ) ) ;
}
// get turn speed
2017-02-07 20:42:28 -04:00
// TODO actually implement this, right now, yaw_look_at_heading_slew is unused
// see AP_Float _slew_yaw in AC_AttitudeControl
2015-12-30 18:57:56 -04:00
if ( is_zero ( turn_rate_dps ) ) {
// default to regular auto slew rate
yaw_look_at_heading_slew = AUTO_YAW_SLEW_RATE ;
2017-02-03 17:33:27 -04:00
} else {
2015-12-30 18:57:56 -04:00
int32_t turn_rate = ( wrap_180_cd ( yaw_look_at_heading - curr_yaw_target ) / 100 ) / turn_rate_dps ;
yaw_look_at_heading_slew = constrain_int32 ( turn_rate , 1 , 360 ) ; // deg / sec
}
// set yaw mode
set_auto_yaw_mode ( AUTO_YAW_LOOK_AT_HEADING ) ;
// TO-DO: restore support for clockwise and counter clockwise rotation held in cmd.content.yaw.direction. 1 = clockwise, -1 = counterclockwise
}
// set_auto_yaw_roi - sets the yaw to look at roi for auto mode
2016-01-14 15:30:56 -04:00
void Sub : : set_auto_yaw_roi ( const Location & roi_location )
2015-12-30 18:57:56 -04:00
{
// if location is zero lat, lon and altitude turn off ROI
if ( roi_location . alt = = 0 & & roi_location . lat = = 0 & & roi_location . lng = = 0 ) {
// set auto yaw mode back to default assuming the active command is a waypoint command. A more sophisticated method is required to ensure we return to the proper yaw control for the active command
set_auto_yaw_mode ( get_default_auto_yaw_mode ( false ) ) ;
# if MOUNT == ENABLED
// switch off the camera tracking if enabled
if ( camera_mount . get_mode ( ) = = MAV_MOUNT_MODE_GPS_POINT ) {
camera_mount . set_mode_to_default ( ) ;
}
# endif // MOUNT == ENABLED
2017-02-03 17:33:27 -04:00
} else {
2015-12-30 18:57:56 -04:00
# if MOUNT == ENABLED
// check if mount type requires us to rotate the quad
2017-02-03 17:33:27 -04:00
if ( ! camera_mount . has_pan_control ( ) ) {
2015-12-30 18:57:56 -04:00
roi_WP = pv_location_to_vector ( roi_location ) ;
set_auto_yaw_mode ( AUTO_YAW_ROI ) ;
}
// send the command to the camera mount
camera_mount . set_roi_target ( roi_location ) ;
// TO-DO: expand handling of the do_nav_roi to support all modes of the MAVLink. Currently we only handle mode 4 (see below)
// 0: do nothing
// 1: point at next waypoint
// 2: point at a waypoint taken from WP# parameter (2nd parameter?)
// 3: point at a location given by alt, lon, lat parameters
// 4: point at a target given a target id (can't be implemented)
# else
// if we have no camera mount aim the quad at the location
roi_WP = pv_location_to_vector ( roi_location ) ;
set_auto_yaw_mode ( AUTO_YAW_ROI ) ;
# endif // MOUNT == ENABLED
}
}
// get_auto_heading - returns target heading depending upon auto_yaw_mode
// 100hz update rate
2018-06-28 08:09:40 -03:00
float Sub : : get_auto_heading ( )
2015-12-30 18:57:56 -04:00
{
2017-02-03 17:33:27 -04:00
switch ( auto_yaw_mode ) {
2015-12-30 18:57:56 -04:00
case AUTO_YAW_ROI :
// point towards a location held in roi_WP
return get_roi_yaw ( ) ;
break ;
case AUTO_YAW_LOOK_AT_HEADING :
// keep heading pointing in the direction held in yaw_look_at_heading with no pilot input allowed
return yaw_look_at_heading ;
break ;
case AUTO_YAW_LOOK_AHEAD :
// Commanded Yaw to automatically look ahead.
return get_look_ahead_yaw ( ) ;
break ;
case AUTO_YAW_RESETTOARMEDYAW :
// changes yaw to be same as when quad was armed
return initial_armed_bearing ;
break ;
2017-02-03 17:33:27 -04:00
case AUTO_YAW_CORRECT_XTRACK : {
2017-03-23 00:39:37 -03:00
// TODO return current yaw if not in appropriate mode
2017-02-03 17:33:27 -04:00
// Bearing of current track (centidegrees)
2017-12-03 17:08:08 -04:00
float track_bearing = get_bearing_cd ( wp_nav . get_wp_origin ( ) , wp_nav . get_wp_destination ( ) ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Bearing from current position towards intermediate position target (centidegrees)
2018-03-28 01:54:24 -03:00
float desired_angle = pos_control . get_bearing_to_target ( ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
float angle_error = wrap_180_cd ( desired_angle - track_bearing ) ;
float angle_limited = constrain_float ( angle_error , - g . xtrack_angle_limit * 100.0f , g . xtrack_angle_limit * 100.0f ) ;
return wrap_360_cd ( track_bearing + angle_limited ) ;
}
break ;
2017-02-07 20:42:28 -04:00
2015-12-30 18:57:56 -04:00
case AUTO_YAW_LOOK_AT_NEXT_WP :
default :
// point towards next waypoint.
2017-02-10 13:46:54 -04:00
// we don't use wp_bearing because we don't want the vehicle to turn too much during flight
2015-12-30 18:57:56 -04:00
return wp_nav . get_yaw ( ) ;
break ;
}
}
2017-02-07 20:42:28 -04:00
// Return true if it is possible to recover from a rangefinder failure
2017-02-03 17:33:27 -04:00
bool Sub : : auto_terrain_recover_start ( )
{
// Check rangefinder status to see if recovery is possible
2017-02-26 22:32:58 -04:00
switch ( rangefinder . status_orient ( ROTATION_PITCH_270 ) ) {
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
case RangeFinder : : RangeFinder_OutOfRangeLow :
case RangeFinder : : RangeFinder_OutOfRangeHigh :
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// RangeFinder_Good if just one valid sample was obtained recently, but ::rangefinder_state.alt_healthy
// requires several consecutive valid readings for wpnav to accept rangefinder data
case RangeFinder : : RangeFinder_Good :
auto_mode = Auto_TerrainRecover ;
break ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Not connected or no data
default :
return false ; // Rangefinder is not connected, or has stopped responding
}
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Initialize recovery timeout time
fs_terrain_recover_start_ms = AP_HAL : : millis ( ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Stop mission
mission . stop ( ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Reset xy target
2018-09-18 23:51:16 -03:00
loiter_nav . clear_pilot_desired_acceleration ( ) ;
2018-03-28 01:54:24 -03:00
loiter_nav . init_target ( ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Reset z axis controller
2017-04-14 15:16:12 -03:00
pos_control . relax_alt_hold_controllers ( motors . get_throttle_hover ( ) ) ;
2017-02-07 20:42:28 -04:00
// initialize vertical speeds and leash lengths
2018-09-20 02:44:52 -03:00
pos_control . set_max_speed_z ( wp_nav . get_speed_down ( ) , wp_nav . get_speed_up ( ) ) ;
pos_control . set_max_accel_z ( wp_nav . get_accel_z ( ) ) ;
2017-02-07 20:42:28 -04:00
// Reset vertical position and velocity targets
pos_control . set_alt_target ( inertial_nav . get_altitude ( ) ) ;
pos_control . set_desired_velocity_z ( inertial_nav . get_velocity_z ( ) ) ;
2017-07-08 22:51:41 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_WARNING , " Attempting auto failsafe recovery " ) ;
2017-02-03 17:33:27 -04:00
return true ;
2017-02-07 20:42:28 -04:00
}
// Attempt recovery from terrain failsafe
// If recovery is successful resume mission
// If recovery fails revert to failsafe action
2017-02-03 17:33:27 -04:00
void Sub : : auto_terrain_recover_run ( )
{
float target_climb_rate = 0 ;
static uint32_t rangefinder_recovery_ms = 0 ;
2017-02-07 20:42:28 -04:00
// if not armed set throttle to zero and exit immediately
2017-04-11 13:45:16 -03:00
if ( ! motors . armed ( ) ) {
2017-02-07 20:42:28 -04:00
motors . set_desired_spool_state ( AP_Motors : : DESIRED_SPIN_WHEN_ARMED ) ;
attitude_control . set_throttle_out_unstabilized ( 0 , true , g . throttle_filt ) ;
return ;
}
2017-02-26 22:32:58 -04:00
switch ( rangefinder . status_orient ( ROTATION_PITCH_270 ) ) {
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
case RangeFinder : : RangeFinder_OutOfRangeLow :
target_climb_rate = wp_nav . get_speed_up ( ) ;
rangefinder_recovery_ms = 0 ;
break ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
case RangeFinder : : RangeFinder_OutOfRangeHigh :
target_climb_rate = wp_nav . get_speed_down ( ) ;
rangefinder_recovery_ms = 0 ;
break ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
case RangeFinder : : RangeFinder_Good : // exit on success (recovered rangefinder data)
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
target_climb_rate = 0 ; // Attempt to hold current depth
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
if ( rangefinder_state . alt_healthy ) {
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Start timer as soon as rangefinder is healthy
if ( rangefinder_recovery_ms = = 0 ) {
rangefinder_recovery_ms = AP_HAL : : millis ( ) ;
2017-04-14 15:16:12 -03:00
pos_control . relax_alt_hold_controllers ( motors . get_throttle_hover ( ) ) ; // Reset alt hold targets
2017-02-03 17:33:27 -04:00
}
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// 1.5 seconds of healthy rangefinder means we can resume mission with terrain enabled
if ( AP_HAL : : millis ( ) > rangefinder_recovery_ms + 1500 ) {
2017-07-08 22:51:41 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Terrain failsafe recovery successful! " ) ;
2017-02-03 17:33:27 -04:00
failsafe_terrain_set_status ( true ) ; // Reset failsafe timers
failsafe . terrain = false ; // Clear flag
auto_mode = Auto_Loiter ; // Switch back to loiter for next iteration
mission . resume ( ) ; // Resume mission
rangefinder_recovery_ms = 0 ; // Reset for subsequent recoveries
}
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
}
break ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Not connected, or no data
default :
// Terrain failsafe recovery has failed, terrain data is not available
// and rangefinder is not connected, or has stopped responding
2017-07-08 22:51:41 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " Terrain failsafe recovery failure: No Rangefinder! " ) ;
2017-02-03 17:33:27 -04:00
failsafe_terrain_act ( ) ;
rangefinder_recovery_ms = 0 ;
return ;
}
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// exit on failure (timeout)
if ( AP_HAL : : millis ( ) > fs_terrain_recover_start_ms + FS_TERRAIN_RECOVER_TIMEOUT_MS ) {
// Recovery has failed, revert to failsafe action
2017-07-08 22:51:41 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_CRITICAL , " Terrain failsafe recovery timeout! " ) ;
2017-02-03 17:33:27 -04:00
failsafe_terrain_act ( ) ;
}
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// run loiter controller
2018-09-05 19:37:00 -03:00
loiter_nav . update ( ) ;
2017-02-07 20:42:28 -04:00
///////////////////////
// update xy targets //
2017-02-03 17:33:27 -04:00
float lateral_out , forward_out ;
translate_wpnav_rp ( lateral_out , forward_out ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// Send to forward/lateral outputs
motors . set_lateral ( lateral_out ) ;
motors . set_forward ( forward_out ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
/////////////////////
// update z target //
pos_control . set_alt_target_from_climb_rate_ff ( target_climb_rate , G_Dt , true ) ;
pos_control . update_z_controller ( ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
////////////////////////////
// update angular targets //
float target_roll = 0 ;
float target_pitch = 0 ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats
get_pilot_desired_lean_angles ( channel_roll - > get_control_in ( ) , channel_pitch - > get_control_in ( ) , target_roll , target_pitch , aparm . angle_max ) ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
float target_yaw_rate = 0 ;
2017-02-07 20:42:28 -04:00
2017-02-03 17:33:27 -04:00
// call attitude controller
2017-07-09 23:49:42 -03:00
attitude_control . input_euler_angle_roll_pitch_euler_rate_yaw ( target_roll , target_pitch , target_yaw_rate ) ;
2017-02-07 20:42:28 -04:00
}