2015-05-13 03:09:36 -03:00
# include "Plane.h"
2013-04-11 21:25:46 -03:00
/*
reset the total loiter angle
*/
2015-05-13 03:09:36 -03:00
void Plane : : loiter_angle_reset ( void )
2013-04-11 21:25:46 -03:00
{
2013-04-15 08:31:11 -03:00
loiter . sum_cd = 0 ;
loiter . total_cd = 0 ;
2017-02-21 07:56:32 -04:00
loiter . reached_target_alt = false ;
loiter . unable_to_acheive_target_alt = false ;
2013-04-11 21:25:46 -03:00
}
/*
update the total angle we have covered in a loiter . Used to support
commands to do N circles of loiter
*/
2015-05-13 03:09:36 -03:00
void Plane : : loiter_angle_update ( void )
2013-04-11 21:25:46 -03:00
{
2017-02-21 07:56:32 -04:00
static const int32_t lap_check_interval_cd = 3 * 36000 ;
const int32_t target_bearing_cd = nav_controller - > target_bearing_cd ( ) ;
2013-04-11 21:25:46 -03:00
int32_t loiter_delta_cd ;
2020-07-31 06:12:33 -03:00
const bool reached_target = reached_loiter_target ( ) ;
2017-02-21 07:56:32 -04:00
2020-07-31 06:12:33 -03:00
if ( loiter . sum_cd = = 0 & & ! reached_target ) {
2016-03-10 00:43:28 -04:00
// we don't start summing until we are doing the real loiter
loiter_delta_cd = 0 ;
} else if ( loiter . sum_cd = = 0 ) {
2013-04-15 08:31:11 -03:00
// use 1 cd for initial delta
loiter_delta_cd = 1 ;
2017-02-21 07:56:32 -04:00
loiter . start_lap_alt_cm = current_loc . alt ;
loiter . next_sum_lap_cd = lap_check_interval_cd ;
2013-04-11 21:25:46 -03:00
} else {
loiter_delta_cd = target_bearing_cd - loiter . old_target_bearing_cd ;
}
2017-02-21 07:56:32 -04:00
2013-04-11 21:25:46 -03:00
loiter . old_target_bearing_cd = target_bearing_cd ;
loiter_delta_cd = wrap_180_cd ( loiter_delta_cd ) ;
2014-05-01 07:32:34 -03:00
loiter . sum_cd + = loiter_delta_cd * loiter . direction ;
2017-02-21 07:56:32 -04:00
2020-07-31 06:12:33 -03:00
bool reached_target_alt = false ;
if ( reached_target ) {
// once we reach the position target we start checking the
// altitude target
bool terrain_status_ok = false ;
# if AP_TERRAIN_AVAILABLE
/*
if doing terrain following then we check against terrain
target , fetch the terrain information
*/
float altitude_agl = 0 ;
if ( target_altitude . terrain_following ) {
if ( terrain . status ( ) = = AP_Terrain : : TerrainStatusOK & &
terrain . height_above_terrain ( altitude_agl , true ) ) {
terrain_status_ok = true ;
}
}
if ( terrain_status_ok & &
2023-06-14 12:26:50 -03:00
fabsF ( altitude_agl - target_altitude . terrain_alt_cm * 0.01 ) < 5 ) {
2020-07-31 06:12:33 -03:00
reached_target_alt = true ;
} else
# endif
if ( ! terrain_status_ok & & labs ( current_loc . alt - target_altitude . amsl_cm ) < 500 ) {
reached_target_alt = true ;
}
}
if ( reached_target_alt ) {
2017-02-21 07:56:32 -04:00
loiter . reached_target_alt = true ;
loiter . unable_to_acheive_target_alt = false ;
loiter . next_sum_lap_cd = loiter . sum_cd + lap_check_interval_cd ;
} else if ( ! loiter . reached_target_alt & & labs ( loiter . sum_cd ) > = loiter . next_sum_lap_cd ) {
2023-10-11 04:41:54 -03:00
// check every few laps for scenario where up/downward inhibit you from loitering up/down for too long
2017-02-21 07:56:32 -04:00
loiter . unable_to_acheive_target_alt = labs ( current_loc . alt - loiter . start_lap_alt_cm ) < 500 ;
loiter . start_lap_alt_cm = current_loc . alt ;
loiter . next_sum_lap_cd + = lap_check_interval_cd ;
}
2013-04-11 21:25:46 -03:00
}
2011-09-08 22:29:39 -03:00
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************
2015-05-13 03:09:36 -03:00
void Plane : : navigate ( )
2011-09-08 22:29:39 -03:00
{
2012-08-16 21:50:15 -03:00
// do not navigate with corrupt data
// ---------------------------------
if ( ! have_position ) {
return ;
}
2016-10-04 08:39:30 -03:00
if ( next_WP_loc . lat = = 0 & & next_WP_loc . lng = = 0 ) {
2012-08-16 21:50:15 -03:00
return ;
}
2023-08-02 20:45:27 -03:00
check_home_alt_change ( ) ;
2012-08-16 21:50:15 -03:00
// waypoint distance from plane
// ----------------------------
2019-02-24 20:11:45 -04:00
auto_state . wp_distance = current_loc . get_distance ( next_WP_loc ) ;
2019-04-12 05:23:04 -03:00
auto_state . wp_proportion = current_loc . line_path_proportion ( prev_WP_loc , next_WP_loc ) ;
2021-11-12 13:53:28 -04:00
TECS_controller . set_path_proportion ( auto_state . wp_proportion ) ;
2012-08-16 21:50:15 -03:00
2013-04-11 21:25:46 -03:00
// update total loiter angle
loiter_angle_update ( ) ;
2012-08-16 21:50:15 -03:00
2013-04-12 07:27:56 -03:00
// control mode specific updates to navigation demands
// ---------------------------------------------------
2020-08-19 05:19:01 -03:00
control_mode - > navigate ( ) ;
2011-09-08 22:29:39 -03:00
}
2021-09-10 03:28:21 -03:00
// method intended for use in calc_airspeed_errors only
float Plane : : mode_auto_target_airspeed_cm ( )
{
# if HAL_QUADPLANE_ENABLED
2022-06-30 18:43:06 -03:00
if ( quadplane . landing_with_fixed_wing_spiral_approach ( ) & &
2024-08-06 21:58:28 -03:00
( ( vtol_approach_s . approach_stage = = VTOLApproach : : Stage : : APPROACH_LINE ) | |
( vtol_approach_s . approach_stage = = VTOLApproach : : Stage : : VTOL_LANDING ) ) ) {
2021-11-12 13:53:28 -04:00
const float land_airspeed = TECS_controller . get_land_airspeed ( ) ;
2021-09-10 03:28:21 -03:00
if ( is_positive ( land_airspeed ) ) {
return land_airspeed * 100 ;
}
// fallover to normal airspeed
2024-01-17 22:34:22 -04:00
return aparm . airspeed_cruise * 100 ;
2021-09-10 03:28:21 -03:00
}
if ( quadplane . in_vtol_land_approach ( ) ) {
return quadplane . get_land_airspeed ( ) * 100 ;
}
# endif
// normal AUTO mode and new_airspeed variable was set by
// DO_CHANGE_SPEED command while in AUTO mode
if ( new_airspeed_cm > 0 ) {
return new_airspeed_cm ;
}
// fallover to normal airspeed
2024-01-17 22:34:22 -04:00
return aparm . airspeed_cruise * 100 ;
2021-09-10 03:28:21 -03:00
}
2015-05-13 03:09:36 -03:00
void Plane : : calc_airspeed_errors ( )
2011-09-08 22:29:39 -03:00
{
2022-01-02 03:41:27 -04:00
// Get the airspeed_estimate, update smoothed airspeed estimate
// NOTE: we use the airspeed estimate function not direct sensor
// as TECS may be using synthetic airspeed
2022-12-04 16:53:13 -04:00
float airspeed_measured = 0.1 ;
2022-01-02 03:41:27 -04:00
if ( ahrs . airspeed_estimate ( airspeed_measured ) ) {
2022-12-04 16:53:13 -04:00
smoothed_airspeed = MAX ( 0.1 , smoothed_airspeed * 0.8f + airspeed_measured * 0.2f ) ;
2022-01-02 03:41:27 -04:00
}
// low pass filter speed scaler, with 1Hz cutoff, at 10Hz
const float speed_scaler = calc_speed_scaler ( ) ;
const float cutoff_Hz = 2.0 ;
const float dt = 0.1 ;
surface_speed_scaler + = calc_lowpass_alpha_dt ( dt , cutoff_Hz ) * ( speed_scaler - surface_speed_scaler ) ;
2021-02-23 12:32:06 -04:00
2011-12-09 19:40:56 -04:00
2018-09-01 20:16:50 -03:00
// FBW_B/cruise airspeed target
2019-01-15 13:46:13 -04:00
if ( ! failsafe . rc_failsafe & & ( control_mode = = & mode_fbwb | | control_mode = = & mode_cruise ) ) {
2023-04-26 00:27:13 -03:00
if ( flight_option_enabled ( FlightOptions : : CRUISE_TRIM_AIRSPEED ) ) {
2024-01-17 22:34:22 -04:00
target_airspeed_cm = aparm . airspeed_cruise * 100 ;
2023-04-26 00:27:13 -03:00
} else if ( flight_option_enabled ( FlightOptions : : CRUISE_TRIM_THROTTLE ) ) {
2018-09-01 21:01:32 -03:00
float control_min = 0.0f ;
float control_mid = 0.0f ;
const float control_max = channel_throttle - > get_range ( ) ;
2018-11-09 18:38:43 -04:00
const float control_in = get_throttle_input ( ) ;
2018-09-01 21:01:32 -03:00
switch ( channel_throttle - > get_type ( ) ) {
2022-02-25 02:01:06 -04:00
case RC_Channel : : ControlType : : ANGLE :
2018-09-01 21:01:32 -03:00
control_min = - control_max ;
break ;
2022-02-25 02:01:06 -04:00
case RC_Channel : : ControlType : : RANGE :
2018-09-01 21:01:32 -03:00
control_mid = channel_throttle - > get_control_mid ( ) ;
break ;
}
if ( control_in < = control_mid ) {
2024-01-17 22:34:22 -04:00
target_airspeed_cm = linear_interpolate ( aparm . airspeed_min * 100 , aparm . airspeed_cruise * 100 ,
2018-09-01 21:01:32 -03:00
control_in ,
control_min , control_mid ) ;
} else {
2024-01-17 22:34:22 -04:00
target_airspeed_cm = linear_interpolate ( aparm . airspeed_cruise * 100 , aparm . airspeed_max * 100 ,
2018-09-01 21:01:32 -03:00
control_in ,
control_mid , control_max ) ;
}
} else {
target_airspeed_cm = ( ( int32_t ) ( aparm . airspeed_max - aparm . airspeed_min ) *
2018-11-09 18:38:43 -04:00
get_throttle_input ( ) ) + ( ( int32_t ) aparm . airspeed_min * 100 ) ;
2018-09-01 21:01:32 -03:00
}
2024-08-01 00:07:38 -03:00
# if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
2023-10-11 04:41:54 -03:00
} else if ( control_mode = = & mode_guided & & guided_state . target_airspeed_cm > 0.0 ) { // if offboard guided speed change cmd not set, then this section is skipped
2020-05-16 01:02:01 -03:00
// offboard airspeed demanded
uint32_t now = AP_HAL : : millis ( ) ;
float delta = 1e-3 f * ( now - guided_state . target_airspeed_time_ms ) ;
guided_state . target_airspeed_time_ms = now ;
float delta_amt = 100 * delta * guided_state . target_airspeed_accel ;
target_airspeed_cm + = delta_amt ;
//target_airspeed_cm recalculated then clamped to between MIN airspeed and MAX airspeed by constrain_float
if ( is_positive ( guided_state . target_airspeed_accel ) ) {
target_airspeed_cm = constrain_float ( MIN ( guided_state . target_airspeed_cm , target_airspeed_cm ) , aparm . airspeed_min * 100 , aparm . airspeed_max * 100 ) ;
} else {
target_airspeed_cm = constrain_float ( MAX ( guided_state . target_airspeed_cm , target_airspeed_cm ) , aparm . airspeed_min * 100 , aparm . airspeed_max * 100 ) ;
}
2011-12-09 19:40:56 -04:00
2024-08-01 00:07:38 -03:00
# endif // AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
2021-02-23 12:32:06 -04:00
2020-07-10 11:14:23 -03:00
# if HAL_SOARING_ENABLED
} else if ( g2 . soaring_controller . is_active ( ) & & g2 . soaring_controller . get_throttle_suppressed ( ) ) {
if ( control_mode = = & mode_thermal ) {
float arspd = g2 . soaring_controller . get_thermalling_target_airspeed ( ) ;
if ( arspd > 0 ) {
target_airspeed_cm = arspd * 100 ;
} else {
2024-01-17 22:34:22 -04:00
target_airspeed_cm = aparm . airspeed_cruise * 100 ;
2020-07-10 11:14:23 -03:00
}
} else if ( control_mode = = & mode_auto ) {
float arspd = g2 . soaring_controller . get_cruising_target_airspeed ( ) ;
if ( arspd > 0 ) {
target_airspeed_cm = arspd * 100 ;
} else {
2024-01-17 22:34:22 -04:00
target_airspeed_cm = aparm . airspeed_cruise * 100 ;
2020-07-10 11:14:23 -03:00
}
}
# endif
2022-09-29 20:10:41 -03:00
} else if ( flight_stage = = AP_FixedWing : : FlightStage : : LAND ) {
2016-11-30 16:10:10 -04:00
// Landing airspeed target
2017-01-10 15:47:31 -04:00
target_airspeed_cm = landing . get_target_airspeed_cm ( ) ;
2021-02-23 12:32:06 -04:00
} else if ( control_mode = = & mode_guided & & new_airspeed_cm > 0 ) { //DO_CHANGE_SPEED overrides onboard guided speed commands, user would have re-enter guided mode to revert
target_airspeed_cm = new_airspeed_cm ;
} else if ( control_mode = = & mode_auto ) {
2021-09-10 03:28:21 -03:00
target_airspeed_cm = mode_auto_target_airspeed_cm ( ) ;
# if HAL_QUADPLANE_ENABLED
2021-05-18 20:20:48 -03:00
} else if ( control_mode = = & mode_qrtl & & quadplane . in_vtol_land_approach ( ) ) {
target_airspeed_cm = quadplane . get_land_airspeed ( ) * 100 ;
2021-09-10 03:28:21 -03:00
# endif
2016-11-30 16:10:10 -04:00
} else {
2021-02-23 12:32:06 -04:00
// Normal airspeed target for all other cases
2024-01-17 22:34:22 -04:00
target_airspeed_cm = aparm . airspeed_cruise * 100 ;
2016-04-13 13:12:26 -03:00
}
2011-12-09 19:40:56 -04:00
// Set target to current airspeed + ground speed undershoot,
// but only when this is faster than the target airspeed commanded
// above.
2020-12-23 01:25:35 -04:00
if ( control_mode - > does_auto_throttle ( ) & &
2023-06-02 03:08:20 -03:00
groundspeed_undershoot_is_valid & &
control_mode ! = & mode_circle ) {
float EAS_undershoot = ( int32_t ) ( ( float ) groundspeed_undershoot / ahrs . get_EAS2TAS ( ) ) ;
int32_t min_gnd_target_airspeed = airspeed_measured * 100 + EAS_undershoot ;
2017-07-31 06:58:19 -03:00
if ( min_gnd_target_airspeed > target_airspeed_cm ) {
2012-08-07 03:05:51 -03:00
target_airspeed_cm = min_gnd_target_airspeed ;
2017-07-31 06:58:19 -03:00
}
2011-12-09 19:40:56 -04:00
}
2020-05-16 01:02:01 -03:00
// when using the special GUIDED mode features for slew control, don't allow airspeed nudging as it doesn't play nicely.
2024-08-01 00:07:38 -03:00
# if AP_PLANE_OFFBOARD_GUIDED_SLEW_ENABLED
2021-02-23 12:32:06 -04:00
if ( control_mode = = & mode_guided & & ! is_zero ( guided_state . target_airspeed_cm ) & & ( airspeed_nudge_cm ! = 0 ) ) {
2020-05-16 01:02:01 -03:00
airspeed_nudge_cm = 0 ; //airspeed_nudge_cm forced to zero
}
# endif
2011-12-09 19:40:56 -04:00
// Bump up the target airspeed based on throttle nudging
2020-12-22 20:47:58 -04:00
if ( control_mode - > allows_throttle_nudging ( ) & & airspeed_nudge_cm > 0 ) {
2012-08-07 03:05:51 -03:00
target_airspeed_cm + = airspeed_nudge_cm ;
2011-12-09 19:40:56 -04:00
}
// Apply airspeed limit
2021-04-13 17:53:07 -03:00
target_airspeed_cm = constrain_int32 ( target_airspeed_cm , aparm . airspeed_min * 100 , aparm . airspeed_max * 100 ) ;
2011-12-09 19:40:56 -04:00
2014-03-19 17:57:27 -03:00
// use the TECS view of the target airspeed for reporting, to take
// account of the landing speed
2021-11-12 13:53:28 -04:00
airspeed_error = TECS_controller . get_target_airspeed ( ) - airspeed_measured ;
2011-12-09 19:40:56 -04:00
}
2015-05-13 03:09:36 -03:00
void Plane : : calc_gndspeed_undershoot ( )
2011-12-09 19:40:56 -04:00
{
2018-11-16 17:51:12 -04:00
// Use the component of ground speed in the forward direction
// This prevents flyaway if wind takes plane backwards
2023-06-02 03:08:20 -03:00
Vector3f velNED ;
if ( ahrs . have_inertial_nav ( ) & & ahrs . get_velocity_NED ( velNED ) ) {
2018-11-16 17:51:12 -04:00
const Matrix3f & rotMat = ahrs . get_rotation_body_to_ned ( ) ;
Vector2f yawVect = Vector2f ( rotMat . a . x , rotMat . b . x ) ;
2017-10-30 01:19:01 -03:00
if ( ! yawVect . is_zero ( ) ) {
yawVect . normalize ( ) ;
2023-06-02 03:08:20 -03:00
float gndSpdFwd = yawVect * velNED . xy ( ) ;
2024-01-18 01:53:11 -04:00
groundspeed_undershoot_is_valid = aparm . min_groundspeed > 0 ;
groundspeed_undershoot = groundspeed_undershoot_is_valid ? ( aparm . min_groundspeed * 100 - gndSpdFwd * 100 ) : 0 ;
2017-10-30 01:19:01 -03:00
}
2018-01-09 20:19:47 -04:00
} else {
2023-06-02 03:08:20 -03:00
groundspeed_undershoot_is_valid = false ;
2018-11-16 17:51:12 -04:00
groundspeed_undershoot = 0 ;
2012-09-11 00:37:25 -03:00
}
2011-09-08 22:29:39 -03:00
}
2021-09-10 03:28:21 -03:00
// method intended to be used by update_loiter
void Plane : : update_loiter_update_nav ( uint16_t radius )
2011-09-08 22:29:39 -03:00
{
2021-09-10 03:28:21 -03:00
# if HAL_QUADPLANE_ENABLED
2016-05-05 22:28:26 -03:00
if ( loiter . start_time_ms ! = 0 & &
2016-09-30 19:35:58 -03:00
quadplane . guided_mode_enabled ( ) ) {
2016-05-11 02:57:41 -03:00
if ( ! auto_state . vtol_loiter ) {
auto_state . vtol_loiter = true ;
// reset loiter start time, so we don't consider the point
// reached till we get much closer
loiter . start_time_ms = 0 ;
quadplane . guided_start ( ) ;
}
2021-09-10 03:28:21 -03:00
return ;
}
# endif
# if HAL_QUADPLANE_ENABLED
const bool quadplane_qrtl_switch = ( control_mode = = & mode_rtl & & quadplane . available ( ) & & quadplane . rtl_mode = = QuadPlane : : RTL_MODE : : SWITCH_QRTL ) ;
# else
const bool quadplane_qrtl_switch = false ;
# endif
if ( ( loiter . start_time_ms = = 0 & &
( control_mode = = & mode_auto | | control_mode = = & mode_guided ) & &
auto_state . crosstrack & &
current_loc . get_distance ( next_WP_loc ) > radius * 3 ) | |
quadplane_qrtl_switch ) {
2017-10-30 02:24:32 -03:00
/*
if never reached loiter point and using crosstrack and somewhat far away from loiter point
navigate to it like in auto - mode for normal crosstrack behavior
we also use direct waypoint navigation if we are a quadplane
that is going to be switching to QRTL when it gets within
RTL_RADIUS
*/
2015-09-16 06:03:28 -03:00
nav_controller - > update_waypoint ( prev_WP_loc , next_WP_loc ) ;
2021-09-10 03:28:21 -03:00
return ;
2015-09-16 06:03:28 -03:00
}
2021-09-10 03:28:21 -03:00
nav_controller - > update_loiter ( next_WP_loc , radius , loiter . direction ) ;
}
void Plane : : update_loiter ( uint16_t radius )
{
if ( radius < = 1 ) {
// if radius is <=1 then use the general loiter radius. if it's small, use default
radius = ( abs ( aparm . loiter_radius ) < = 1 ) ? LOITER_RADIUS_DEFAULT : abs ( aparm . loiter_radius ) ;
if ( next_WP_loc . loiter_ccw = = 1 ) {
loiter . direction = - 1 ;
} else {
loiter . direction = ( aparm . loiter_radius < 0 ) ? - 1 : 1 ;
}
}
2023-05-15 21:18:12 -03:00
// the radius actually being used by the controller is required by other functions
loiter . radius = ( float ) radius ;
2021-09-10 03:28:21 -03:00
update_loiter_update_nav ( radius ) ;
2015-09-16 06:03:28 -03:00
if ( loiter . start_time_ms = = 0 ) {
2016-05-07 21:40:42 -03:00
if ( reached_loiter_target ( ) | |
2016-03-17 23:47:34 -03:00
auto_state . wp_proportion > 1 ) {
2015-09-16 06:03:28 -03:00
// we've reached the target, start the timer
loiter . start_time_ms = millis ( ) ;
2020-09-22 14:00:39 -03:00
if ( control_mode - > is_guided_mode ( ) ) {
2015-12-21 13:48:30 -04:00
// starting a loiter in GUIDED means we just reached the target point
2017-07-07 22:55:12 -03:00
gcs ( ) . send_mission_item_reached_message ( 0 ) ;
2015-12-21 13:48:30 -04:00
}
2021-09-10 03:28:21 -03:00
# if HAL_QUADPLANE_ENABLED
2016-09-30 19:35:58 -03:00
if ( quadplane . guided_mode_enabled ( ) ) {
2016-05-05 22:28:26 -03:00
quadplane . guided_start ( ) ;
}
2021-09-10 03:28:21 -03:00
# endif
2015-09-16 06:03:28 -03:00
}
}
2011-09-08 22:29:39 -03:00
}
2013-07-13 07:05:53 -03:00
/*
2022-05-10 09:53:10 -03:00
handle speed and height control in FBWB , CRUISE , and optionally , LOITER mode .
2013-07-13 07:05:53 -03:00
In this mode the elevator is used to change target altitude . The
throttle is used to change target airspeed or throttle
*/
2015-05-13 03:09:36 -03:00
void Plane : : update_fbwb_speed_height ( void )
2013-07-13 07:05:53 -03:00
{
2017-05-09 05:05:36 -03:00
uint32_t now = micros ( ) ;
if ( now - target_altitude . last_elev_check_us > = 100000 ) {
// we don't run this on every loop as it would give too small granularity on quadplanes at 300Hz, and
// give below 1cm altitude change, which would result in no climb or descent
float dt = ( now - target_altitude . last_elev_check_us ) * 1.0e-6 ;
dt = constrain_float ( dt , 0.1 , 0.15 ) ;
target_altitude . last_elev_check_us = now ;
2021-02-23 12:32:06 -04:00
2022-03-11 21:03:13 -04:00
float elevator_input = channel_pitch - > get_control_in ( ) * ( 1 / 4500.0 ) ;
2021-02-23 12:32:06 -04:00
2017-05-09 05:05:36 -03:00
if ( g . flybywire_elev_reverse ) {
elevator_input = - elevator_input ;
}
2024-04-12 16:03:16 -03:00
bool input_stop_climb = ! is_positive ( elevator_input ) & & is_positive ( target_altitude . last_elevator_input ) ;
bool input_stop_descent = ! is_negative ( elevator_input ) & & is_negative ( target_altitude . last_elevator_input ) ;
if ( input_stop_climb | | input_stop_descent ) {
// user elevator input reached or passed zero, lock in the current altitude
2017-05-09 05:05:36 -03:00
set_target_altitude_current ( ) ;
}
2019-08-22 21:17:14 -03:00
2024-04-20 09:08:45 -03:00
float climb_rate = g . flybywire_climb_rate * elevator_input ;
climb_rate = constrain_float ( climb_rate , - TECS_controller . get_max_sinkrate ( ) , TECS_controller . get_max_climbrate ( ) ) ;
int32_t alt_change_cm = climb_rate * dt * 100 ;
2024-04-12 16:03:16 -03:00
change_target_altitude ( alt_change_cm ) ;
2020-09-23 05:16:45 -03:00
# if HAL_SOARING_ENABLED
2020-09-07 08:52:43 -03:00
if ( g2 . soaring_controller . is_active ( ) ) {
if ( g2 . soaring_controller . get_throttle_suppressed ( ) ) {
// we're in soaring mode with throttle suppressed
set_target_altitude_current ( ) ;
} else {
2020-09-10 06:24:19 -03:00
// we're in soaring mode climbing back to altitude. Set target to SOAR_ALT_CUTOFF plus 10m to ensure we positively climb
// through SOAR_ALT_CUTOFF, thus triggering throttle suppression and return to glide.
2020-09-07 08:52:43 -03:00
target_altitude . amsl_cm = 100 * plane . g2 . soaring_controller . get_alt_cutoff ( ) + 1000 + AP : : ahrs ( ) . get_home ( ) . alt ;
}
2019-08-22 21:17:14 -03:00
}
# endif
2021-02-23 12:32:06 -04:00
2017-05-09 05:05:36 -03:00
target_altitude . last_elevator_input = elevator_input ;
2013-07-13 07:05:53 -03:00
}
2021-02-23 12:32:06 -04:00
2022-01-28 11:31:33 -04:00
check_fbwb_altitude ( ) ;
2014-07-24 03:21:30 -03:00
2013-07-13 07:05:53 -03:00
calc_throttle ( ) ;
calc_nav_pitch ( ) ;
}
2014-06-04 20:35:09 -03:00
/*
calculate the turn angle for the next leg of the mission
*/
2015-05-13 03:09:36 -03:00
void Plane : : setup_turn_angle ( void )
2014-06-04 20:35:09 -03:00
{
int32_t next_ground_course_cd = mission . get_next_ground_course_cd ( - 1 ) ;
if ( next_ground_course_cd = = - 1 ) {
// the mission library can't determine a turn angle, assume 90 degrees
auto_state . next_turn_angle = 90.0f ;
} else {
// get the heading of the current leg
2019-04-05 10:02:44 -03:00
int32_t ground_course_cd = prev_WP_loc . get_bearing_to ( next_WP_loc ) ;
2014-06-04 20:35:09 -03:00
// work out the angle we need to turn through
auto_state . next_turn_angle = wrap_180_cd ( next_ground_course_cd - ground_course_cd ) * 0.01f ;
}
2021-02-23 12:32:06 -04:00
}
2014-06-04 20:35:09 -03:00
2016-05-07 21:40:42 -03:00
/*
see if we have reached our loiter target
*/
bool Plane : : reached_loiter_target ( void )
{
2021-09-10 03:28:21 -03:00
# if HAL_QUADPLANE_ENABLED
2016-05-07 21:40:42 -03:00
if ( quadplane . in_vtol_auto ( ) ) {
return auto_state . wp_distance < 3 ;
}
2021-09-10 03:28:21 -03:00
# endif
2016-05-07 21:40:42 -03:00
return nav_controller - > reached_loiter_target ( ) ;
}