2018-03-30 02:49:46 -03:00
# include "Copter.h"
2019-05-09 23:18:49 -03:00
Mode : : AutoYaw Mode : : auto_yaw ;
2018-03-30 02:49:46 -03:00
// roi_yaw - returns heading towards location held in roi
2021-02-05 05:55:45 -04:00
float Mode : : AutoYaw : : roi_yaw ( ) const
2018-03-30 02:49:46 -03:00
{
2021-10-20 05:29:57 -03:00
return get_bearing_cd ( copter . inertial_nav . get_position_xy_cm ( ) , roi . xy ( ) ) ;
2018-03-30 02:49:46 -03:00
}
2024-04-09 03:48:42 -03:00
// returns a yaw in degrees, direction of vehicle travel:
2019-05-09 23:18:49 -03:00
float Mode : : AutoYaw : : look_ahead_yaw ( )
2018-03-30 02:49:46 -03:00
{
2021-10-20 05:29:57 -03:00
const Vector3f & vel = copter . inertial_nav . get_velocity_neu_cms ( ) ;
const float speed_sq = vel . xy ( ) . length_squared ( ) ;
2018-03-30 02:49:46 -03:00
// Commanded Yaw to automatically look ahead.
2021-10-20 05:29:57 -03:00
if ( copter . position_ok ( ) & & ( speed_sq > ( YAW_LOOK_AHEAD_MIN_SPEED * YAW_LOOK_AHEAD_MIN_SPEED ) ) ) {
2024-04-09 03:48:42 -03:00
_look_ahead_yaw = degrees ( atan2f ( vel . y , vel . x ) ) ;
2018-03-30 02:49:46 -03:00
}
return _look_ahead_yaw ;
}
2019-05-09 23:18:49 -03:00
void Mode : : AutoYaw : : set_mode_to_default ( bool rtl )
2018-03-30 02:49:46 -03:00
{
set_mode ( default_mode ( rtl ) ) ;
}
// default_mode - returns auto_yaw.mode() based on WP_YAW_BEHAVIOR parameter
// set rtl parameter to true if this is during an RTL
2022-09-24 11:12:33 -03:00
Mode : : AutoYaw : : Mode Mode : : AutoYaw : : default_mode ( bool rtl ) const
2018-03-30 02:49:46 -03:00
{
switch ( copter . g . wp_yaw_behavior ) {
case WP_YAW_BEHAVIOR_NONE :
2022-09-24 11:12:33 -03:00
return Mode : : HOLD ;
2018-03-30 02:49:46 -03:00
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP_EXCEPT_RTL :
if ( rtl ) {
2022-09-24 11:12:33 -03:00
return Mode : : HOLD ;
2018-03-30 02:49:46 -03:00
} else {
2022-09-24 11:12:33 -03:00
return Mode : : LOOK_AT_NEXT_WP ;
2018-03-30 02:49:46 -03:00
}
case WP_YAW_BEHAVIOR_LOOK_AHEAD :
2022-09-24 11:12:33 -03:00
return Mode : : LOOK_AHEAD ;
2018-03-30 02:49:46 -03:00
case WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP :
default :
2022-09-24 11:12:33 -03:00
return Mode : : LOOK_AT_NEXT_WP ;
2018-03-30 02:49:46 -03:00
}
}
// set_mode - sets the yaw mode for auto
2022-09-24 11:12:33 -03:00
void Mode : : AutoYaw : : set_mode ( Mode yaw_mode )
2018-03-30 02:49:46 -03:00
{
// return immediately if no change
if ( _mode = = yaw_mode ) {
return ;
}
2022-04-14 06:58:27 -03:00
_last_mode = _mode ;
2018-03-30 02:49:46 -03:00
_mode = yaw_mode ;
// perform initialisation
switch ( _mode ) {
2022-09-24 11:12:33 -03:00
case Mode : : HOLD :
2021-09-24 21:54:05 -03:00
break ;
2022-09-24 11:12:33 -03:00
case Mode : : LOOK_AT_NEXT_WP :
2018-03-30 02:49:46 -03:00
// wpnav will initialise heading when wpnav's set_destination method is called
break ;
2022-09-24 11:12:33 -03:00
case Mode : : ROI :
2018-03-30 02:49:46 -03:00
// look ahead until we know otherwise
break ;
2022-09-24 11:12:33 -03:00
case Mode : : FIXED :
2018-03-30 02:49:46 -03:00
// keep heading pointing in the direction held in fixed_yaw
// caller should set the fixed_yaw
break ;
2022-09-24 11:12:33 -03:00
case Mode : : LOOK_AHEAD :
2018-03-30 02:49:46 -03:00
// Commanded Yaw to automatically look ahead.
2024-04-09 03:48:42 -03:00
_look_ahead_yaw = copter . ahrs . yaw_sensor * 0.01 ; // cdeg -> deg
2018-03-30 02:49:46 -03:00
break ;
2022-09-24 11:12:33 -03:00
case Mode : : RESETTOARMEDYAW :
2018-03-30 02:49:46 -03:00
// initial_armed_bearing will be set during arming so no init required
break ;
2022-09-24 11:12:33 -03:00
case Mode : : ANGLE_RATE :
2021-09-24 21:54:05 -03:00
break ;
2022-09-24 11:12:33 -03:00
case Mode : : RATE :
2018-03-30 02:49:46 -03:00
// initialise target yaw rate to zero
2023-03-04 07:02:41 -04:00
_yaw_rate_cds = 0.0 ;
2018-03-30 02:49:46 -03:00
break ;
2020-04-21 08:50:25 -03:00
2022-09-24 11:12:33 -03:00
case Mode : : CIRCLE :
case Mode : : PILOT_RATE :
2022-04-14 06:58:27 -03:00
case Mode : : WEATHERVANE :
2020-04-21 08:50:25 -03:00
// no initialisation required
break ;
2018-03-30 02:49:46 -03:00
}
}
// set_fixed_yaw - sets the yaw look at heading for auto mode
2021-07-09 05:44:07 -03:00
void Mode : : AutoYaw : : set_fixed_yaw ( float angle_deg , float turn_rate_ds , int8_t direction , bool relative_angle )
2018-03-30 02:49:46 -03:00
{
2021-07-09 22:25:52 -03:00
_last_update_ms = millis ( ) ;
2021-07-09 05:44:07 -03:00
2018-03-30 02:49:46 -03:00
// calculate final angle as relative to vehicle heading or absolute
2021-07-09 05:44:07 -03:00
if ( relative_angle ) {
2024-06-16 22:30:41 -03:00
if ( _mode = = Mode : : HOLD ) {
_yaw_angle_cd = copter . ahrs . yaw_sensor ;
}
2021-07-09 22:25:52 -03:00
_fixed_yaw_offset_cd = angle_deg * 100.0 * ( direction > = 0 ? 1.0 : - 1.0 ) ;
2018-03-30 02:49:46 -03:00
} else {
2021-07-09 05:44:07 -03:00
// absolute angle
_fixed_yaw_offset_cd = wrap_180_cd ( angle_deg * 100.0 - _yaw_angle_cd ) ;
2023-11-27 03:20:37 -04:00
if ( direction < 0 & & is_positive ( _fixed_yaw_offset_cd ) ) {
2021-07-09 05:44:07 -03:00
_fixed_yaw_offset_cd - = 36000.0 ;
2023-11-27 03:20:37 -04:00
} else if ( direction > 0 & & is_negative ( _fixed_yaw_offset_cd ) ) {
2021-07-09 05:44:07 -03:00
_fixed_yaw_offset_cd + = 36000.0 ;
2018-03-30 02:49:46 -03:00
}
}
// get turn speed
2021-07-09 05:44:07 -03:00
if ( ! is_positive ( turn_rate_ds ) ) {
// default to default slew rate
2022-01-27 17:04:03 -04:00
_fixed_yaw_slewrate_cds = copter . attitude_control - > get_slew_yaw_max_degs ( ) * 100.0 ;
2018-03-30 02:49:46 -03:00
} else {
2022-01-27 17:04:03 -04:00
_fixed_yaw_slewrate_cds = MIN ( copter . attitude_control - > get_slew_yaw_max_degs ( ) , turn_rate_ds ) * 100.0 ;
2018-03-30 02:49:46 -03:00
}
// set yaw mode
2022-09-24 11:12:33 -03:00
set_mode ( Mode : : FIXED ) ;
2021-07-09 05:44:07 -03:00
}
2018-03-30 02:49:46 -03:00
2021-07-09 05:44:07 -03:00
// set_fixed_yaw - sets the yaw look at heading for auto mode
void Mode : : AutoYaw : : set_yaw_angle_rate ( float yaw_angle_d , float yaw_rate_ds )
{
2021-07-09 22:25:52 -03:00
_last_update_ms = millis ( ) ;
2021-07-09 05:44:07 -03:00
_yaw_angle_cd = yaw_angle_d * 100.0 ;
_yaw_rate_cds = yaw_rate_ds * 100.0 ;
// set yaw mode
2022-09-24 11:12:33 -03:00
set_mode ( Mode : : ANGLE_RATE ) ;
2018-03-30 02:49:46 -03:00
}
// set_roi - sets the yaw to look at roi for auto mode
2019-05-09 23:18:49 -03:00
void Mode : : AutoYaw : : set_roi ( const Location & roi_location )
2018-03-30 02:49:46 -03: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
auto_yaw . set_mode_to_default ( false ) ;
2020-07-24 14:02:40 -03:00
# if HAL_MOUNT_ENABLED
2018-03-30 02:49:46 -03:00
// switch off the camera tracking if enabled
2023-10-16 01:10:04 -03:00
copter . camera_mount . clear_roi_target ( ) ;
2020-07-24 14:02:40 -03:00
# endif // HAL_MOUNT_ENABLED
2018-03-30 02:49:46 -03:00
} else {
2020-07-24 14:02:40 -03:00
# if HAL_MOUNT_ENABLED
2018-03-30 02:49:46 -03:00
// check if mount type requires us to rotate the quad
if ( ! copter . camera_mount . has_pan_control ( ) ) {
2019-02-04 18:08:19 -04:00
if ( roi_location . get_vector_from_origin_NEU ( roi ) ) {
2022-09-24 11:12:33 -03:00
auto_yaw . set_mode ( Mode : : ROI ) ;
2019-02-04 18:08:19 -04:00
}
2018-03-30 02:49:46 -03:00
}
// send the command to the camera mount
copter . 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
2019-02-04 18:08:19 -04:00
if ( roi_location . get_vector_from_origin_NEU ( roi ) ) {
2022-09-24 11:12:33 -03:00
auto_yaw . set_mode ( Mode : : ROI ) ;
2019-02-04 18:08:19 -04:00
}
2020-07-24 14:02:40 -03:00
# endif // HAL_MOUNT_ENABLED
2018-03-30 02:49:46 -03:00
}
}
// set auto yaw rate in centi-degrees per second
2019-05-09 23:18:49 -03:00
void Mode : : AutoYaw : : set_rate ( float turn_rate_cds )
2018-03-30 02:49:46 -03:00
{
2022-09-24 11:12:33 -03:00
set_mode ( Mode : : RATE ) ;
2021-07-09 05:44:07 -03:00
_yaw_rate_cds = turn_rate_cds ;
2018-03-30 02:49:46 -03:00
}
2022-09-24 11:12:33 -03:00
// return true if fixed yaw target has been reached
bool Mode : : AutoYaw : : reached_fixed_yaw_target ( )
{
if ( mode ( ) ! = Mode : : FIXED ) {
// should not happen, not in the right mode
return true ;
}
if ( ! is_zero ( _fixed_yaw_offset_cd ) ) {
// still slewing yaw target
return false ;
}
// Within 2 deg of target
return ( fabsf ( wrap_180_cd ( copter . ahrs . yaw_sensor - _yaw_angle_cd ) ) < = 200 ) ;
}
2023-03-04 07:02:41 -04:00
// yaw_cd - returns target heading depending upon auto_yaw.mode()
float Mode : : AutoYaw : : yaw_cd ( )
2018-03-30 02:49:46 -03:00
{
switch ( _mode ) {
2022-09-24 11:12:33 -03:00
case Mode : : ROI :
2018-03-30 02:49:46 -03:00
// point towards a location held in roi
2023-03-04 07:02:41 -04:00
_yaw_angle_cd = roi_yaw ( ) ;
break ;
2018-03-30 02:49:46 -03:00
2022-09-24 11:12:33 -03:00
case Mode : : FIXED : {
2018-03-30 02:49:46 -03:00
// keep heading pointing in the direction held in fixed_yaw
// with no pilot input allowed
2021-07-09 22:25:52 -03:00
const uint32_t now_ms = millis ( ) ;
2021-07-14 01:29:57 -03:00
float dt = ( now_ms - _last_update_ms ) * 0.001 ;
2021-07-09 22:25:52 -03:00
_last_update_ms = now_ms ;
2021-07-09 05:44:07 -03:00
float yaw_angle_step = constrain_float ( _fixed_yaw_offset_cd , - dt * _fixed_yaw_slewrate_cds , dt * _fixed_yaw_slewrate_cds ) ;
_fixed_yaw_offset_cd - = yaw_angle_step ;
_yaw_angle_cd + = yaw_angle_step ;
2023-03-04 07:02:41 -04:00
break ;
2021-07-09 05:44:07 -03:00
}
2018-03-30 02:49:46 -03:00
2022-09-24 11:12:33 -03:00
case Mode : : LOOK_AHEAD :
2018-03-30 02:49:46 -03:00
// Commanded Yaw to automatically look ahead.
2024-04-09 03:48:42 -03:00
_yaw_angle_cd = look_ahead_yaw ( ) * 100.0 ;
2023-03-04 07:02:41 -04:00
break ;
2018-03-30 02:49:46 -03:00
2022-09-24 11:12:33 -03:00
case Mode : : RESETTOARMEDYAW :
2018-03-30 02:49:46 -03:00
// changes yaw to be same as when quad was armed
2023-03-04 07:02:41 -04:00
_yaw_angle_cd = copter . initial_armed_bearing ;
break ;
2018-03-30 02:49:46 -03:00
2022-09-24 11:12:33 -03:00
case Mode : : CIRCLE :
2020-10-01 19:30:30 -03:00
# if MODE_CIRCLE_ENABLED
2020-04-21 08:50:25 -03:00
if ( copter . circle_nav - > is_active ( ) ) {
2023-03-04 07:02:41 -04:00
_yaw_angle_cd = copter . circle_nav - > get_yaw ( ) ;
2020-04-21 08:50:25 -03:00
}
2020-10-01 19:30:30 -03:00
# endif
2023-03-04 07:02:41 -04:00
break ;
2020-04-21 08:50:25 -03:00
2022-09-24 11:12:33 -03:00
case Mode : : ANGLE_RATE : {
2021-07-09 22:25:52 -03:00
const uint32_t now_ms = millis ( ) ;
2021-07-14 01:29:57 -03:00
float dt = ( now_ms - _last_update_ms ) * 0.001 ;
2021-07-09 22:25:52 -03:00
_last_update_ms = now_ms ;
2021-07-09 05:44:07 -03:00
_yaw_angle_cd + = _yaw_rate_cds * dt ;
2023-03-04 07:02:41 -04:00
break ;
2021-07-09 05:44:07 -03:00
}
2023-03-04 07:02:41 -04:00
case Mode : : RATE :
case Mode : : WEATHERVANE :
case Mode : : PILOT_RATE :
_yaw_angle_cd = copter . attitude_control - > get_att_target_euler_cd ( ) . z ;
break ;
2022-09-24 11:12:33 -03:00
case Mode : : LOOK_AT_NEXT_WP :
2018-03-30 02:49:46 -03:00
default :
// point towards next waypoint.
// we don't use wp_bearing because we don't want the copter to turn too much during flight
2023-03-04 07:02:41 -04:00
_yaw_angle_cd = copter . pos_control - > get_yaw_cd ( ) ;
break ;
2018-03-30 02:49:46 -03:00
}
2023-03-04 07:02:41 -04:00
return _yaw_angle_cd ;
2018-03-30 02:49:46 -03:00
}
// returns yaw rate normally set by SET_POSITION_TARGET mavlink
// messages (positive is clockwise, negative is counter clockwise)
2023-03-04 07:02:41 -04:00
float Mode : : AutoYaw : : rate_cds ( )
2018-03-30 02:49:46 -03:00
{
2021-01-19 22:50:48 -04:00
switch ( _mode ) {
2022-09-24 11:12:33 -03:00
case Mode : : HOLD :
case Mode : : ROI :
case Mode : : FIXED :
case Mode : : LOOK_AHEAD :
case Mode : : RESETTOARMEDYAW :
case Mode : : CIRCLE :
2023-03-04 07:02:41 -04:00
_yaw_rate_cds = 0.0f ;
break ;
2021-01-19 22:50:48 -04:00
2022-09-24 11:12:33 -03:00
case Mode : : LOOK_AT_NEXT_WP :
2023-03-04 07:02:41 -04:00
_yaw_rate_cds = copter . pos_control - > get_yaw_rate_cds ( ) ;
break ;
2022-09-24 11:12:33 -03:00
case Mode : : PILOT_RATE :
2023-03-04 07:02:41 -04:00
_yaw_rate_cds = _pilot_yaw_rate_cds ;
break ;
case Mode : : ANGLE_RATE :
case Mode : : RATE :
case Mode : : WEATHERVANE :
break ;
2018-03-30 02:49:46 -03:00
}
// return zero turn rate (this should never happen)
2023-03-04 07:02:41 -04:00
return _yaw_rate_cds ;
2018-03-30 02:49:46 -03:00
}
2022-09-24 11:12:33 -03:00
AC_AttitudeControl : : HeadingCommand Mode : : AutoYaw : : get_heading ( )
{
// process pilot's yaw input
_pilot_yaw_rate_cds = 0.0 ;
if ( ! copter . failsafe . radio & & copter . flightmode - > use_pilot_yaw ( ) ) {
// get pilot's desired yaw rate
2024-09-19 19:33:02 -03:00
_pilot_yaw_rate_cds = copter . flightmode - > get_pilot_desired_yaw_rate ( ) ;
2022-09-24 11:12:33 -03:00
if ( ! is_zero ( _pilot_yaw_rate_cds ) ) {
auto_yaw . set_mode ( AutoYaw : : Mode : : PILOT_RATE ) ;
}
} else if ( auto_yaw . mode ( ) = = AutoYaw : : Mode : : PILOT_RATE ) {
// RC failsafe, or disabled make sure not in pilot control
auto_yaw . set_mode ( AutoYaw : : Mode : : HOLD ) ;
}
2024-09-04 00:48:37 -03:00
# if WEATHERVANE_ENABLED
2022-04-14 06:58:27 -03:00
update_weathervane ( _pilot_yaw_rate_cds ) ;
# endif
2022-09-24 11:12:33 -03:00
AC_AttitudeControl : : HeadingCommand heading ;
2023-03-04 07:02:41 -04:00
heading . yaw_angle_cd = auto_yaw . yaw_cd ( ) ;
2022-09-24 11:12:33 -03:00
heading . yaw_rate_cds = auto_yaw . rate_cds ( ) ;
switch ( auto_yaw . mode ( ) ) {
case Mode : : HOLD :
case Mode : : RATE :
case Mode : : PILOT_RATE :
2022-04-14 06:58:27 -03:00
case Mode : : WEATHERVANE :
2022-09-24 11:12:33 -03:00
heading . heading_mode = AC_AttitudeControl : : HeadingMode : : Rate_Only ;
break ;
case Mode : : LOOK_AT_NEXT_WP :
case Mode : : ROI :
case Mode : : FIXED :
case Mode : : LOOK_AHEAD :
case Mode : : RESETTOARMEDYAW :
case Mode : : ANGLE_RATE :
case Mode : : CIRCLE :
heading . heading_mode = AC_AttitudeControl : : HeadingMode : : Angle_And_Rate ;
break ;
}
return heading ;
}
2022-04-14 06:58:27 -03:00
// handle the interface to the weathervane library
// pilot_yaw can be an angle or a rate or rcin from yaw channel. It just needs to represent a pilot's request to yaw the vehicle to enable pilot overrides.
2024-09-04 00:48:37 -03:00
# if WEATHERVANE_ENABLED
2022-04-14 06:58:27 -03:00
void Mode : : AutoYaw : : update_weathervane ( const int16_t pilot_yaw_cds )
{
if ( ! copter . flightmode - > allows_weathervaning ( ) ) {
return ;
}
float yaw_rate_cds ;
if ( copter . g2 . weathervane . get_yaw_out ( yaw_rate_cds , pilot_yaw_cds , copter . flightmode - > get_alt_above_ground_cm ( ) * 0.01 ,
2022-12-14 04:54:12 -04:00
copter . pos_control - > get_roll_cd ( ) - copter . attitude_control - > get_roll_trim_cd ( ) ,
2022-04-14 06:58:27 -03:00
copter . pos_control - > get_pitch_cd ( ) ,
copter . flightmode - > is_taking_off ( ) ,
copter . flightmode - > is_landing ( ) ) ) {
set_mode ( Mode : : WEATHERVANE ) ;
_yaw_rate_cds = yaw_rate_cds ;
return ;
}
// if the weathervane controller has previously been activated we need to ensure we return control back to what was previously set
if ( mode ( ) = = Mode : : WEATHERVANE ) {
_yaw_rate_cds = 0.0 ;
if ( _last_mode = = Mode : : HOLD ) {
set_mode_to_default ( false ) ;
} else {
set_mode ( _last_mode ) ;
}
}
}
2024-09-04 00:48:37 -03:00
# endif // WEATHERVANE_ENABLED