Skip to content

Commit

Permalink
fw_tecs: modified tighter altitude control for low-height implementation
Browse files Browse the repository at this point in the history
  • Loading branch information
@oravla5
oravla5 committed Aug 13, 2024
1 parent 8befc06 commit 9ad8384
Show file tree
Hide file tree
Showing 2 changed files with 77 additions and 28 deletions.
94 changes: 72 additions & 22 deletions src/modules/fw_pos_control/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -942,8 +942,10 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
void
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
{
// only control altitude and airspeed ("fixed-bank loiter")
// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

// only control altitude and airspeed ("fixed-bank loiter")
tecs_update_pitch_throttle(control_interval,
_current_altitude,
_performance_model.getCalibratedTrimAirspeed(),
Expand All @@ -952,7 +954,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);

_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
_att_sp.yaw_body = 0.f;
Expand All @@ -975,6 +978,9 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
// but not letting it drift too far away.
const float descend_rate = -0.5f;

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
_current_altitude,
_performance_model.getCalibratedTrimAirspeed(),
Expand All @@ -984,6 +990,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
is_low_height,
false,
descend_rate);

Expand Down Expand Up @@ -1118,6 +1125,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co

_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
position_sp_alt,
target_airspeed,
Expand All @@ -1126,7 +1136,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
tecs_fw_thr_min,
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);
}

void
Expand Down Expand Up @@ -1167,6 +1178,9 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co

_att_sp.yaw_body = _yaw;

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
position_sp_alt,
target_airspeed,
Expand All @@ -1176,6 +1190,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
is_low_height,
false,
pos_sp_curr.vz);
}
Expand All @@ -1185,6 +1200,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
const position_setpoint_s &pos_sp_next)
{

Vector2d curr_wp{0, 0};
Vector2d prev_wp{0, 0};

Expand Down Expand Up @@ -1233,6 +1249,9 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
Vector2f curr_wp_local{_global_local_proj_ref.project(curr_wp(0), curr_wp(1))};
Vector2f vehicle_to_loiter_center{curr_wp_local - curr_pos_local};

// check if generic low-height flight conditions are satisfied
bool is_low_height = checkLowHeightConditions();

const bool close_to_circle = vehicle_to_loiter_center.norm() < loiter_radius + _npfg.switchDistance(500);

if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && _position_setpoint_next_valid
Expand All @@ -1241,7 +1260,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
// landing airspeed and potentially tighter altitude control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.

_is_low_height = true; // In low-height flight, TECS will control altitude tighter
// Just before landing, enforce low-height flight conditions for tighter altitude control
is_low_height = true;

airspeed_sp = (_param_fw_lnd_airspd.get() > FLT_EPSILON) ? _param_fw_lnd_airspd.get() :
_performance_model.getMinimumCalibratedAirspeed(getLoadFactor());
Expand Down Expand Up @@ -1276,7 +1296,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
_att_sp.roll_body = 0.0f;
}

_is_low_height = true; // In low-height flight, TECS will control altitude tighter
is_low_height = true; // In low-height flight, TECS will control altitude tighter
}

tecs_update_pitch_throttle(control_interval,
Expand All @@ -1287,7 +1307,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
tecs_fw_thr_min,
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);
}

#ifdef CONFIG_FIGURE_OF_EIGHT
Expand Down Expand Up @@ -1332,6 +1353,9 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
tecs_fw_thr_max = _param_fw_thr_max.get();
}

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
pos_sp_curr.alt,
target_airspeed,
Expand All @@ -1340,7 +1364,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
tecs_fw_thr_min,
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);

// Yaw
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
Expand All @@ -1366,7 +1391,6 @@ void
FixedwingPositionControl::control_auto_path(const float control_interval, const Vector2d &curr_pos,
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_curr)
{

float tecs_fw_thr_min;
float tecs_fw_thr_max;

Expand Down Expand Up @@ -1403,6 +1427,9 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const

_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
pos_sp_curr.alt,
target_airspeed,
Expand All @@ -1411,7 +1438,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
tecs_fw_thr_min,
tecs_fw_thr_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);

_att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max);
_att_sp.pitch_body = get_tecs_pitch();
Expand Down Expand Up @@ -1446,6 +1474,9 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
adjusted_min_airspeed = takeoff_airspeed;
}

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

if (_runway_takeoff.runwayTakeoffEnabled()) {
if (!_runway_takeoff.isInitialized()) {
_runway_takeoff.init(now, _yaw, global_position);
Expand Down Expand Up @@ -1531,7 +1562,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_performance_model.getMaximumClimbRate(_air_density));
_performance_model.getMaximumClimbRate(_air_density),
is_low_height);

_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation

Expand Down Expand Up @@ -1609,7 +1641,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
_param_fw_thr_min.get(),
max_takeoff_throttle,
_param_sinkrate_target.get(),
_performance_model.getMaximumClimbRate(_air_density));
_performance_model.getMaximumClimbRate(_air_density),
is_low_height);

if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
// explicitly set idle throttle until motors are enabled
Expand Down Expand Up @@ -1664,7 +1697,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
ground_speed);

_is_low_height = true; // In low-height flight, TECS will control altitude tighter

// now handle position
const Vector2f local_position{_local_pos.x, _local_pos.y};
Expand Down Expand Up @@ -1712,6 +1744,9 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
// flare at the maximum of the altitude determined by the time before touchdown and a minimum flare altitude
const float flare_rel_alt = math::max(_param_fw_lnd_fl_time.get() * _local_pos.vz, _param_fw_lnd_flalt.get());

// During landing, enforce low-height flight conditions for tighter altitude control
const bool is_low_height = true;

// the terrain estimate (if enabled) is always used to determine the flaring altitude
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) {
// flare and land with minimal speed
Expand Down Expand Up @@ -1787,6 +1822,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
is_low_height,
true,
height_rate_setpoint);

Expand Down Expand Up @@ -1841,7 +1877,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
desired_max_sinkrate,
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);

/* set the attitude and throttle commands */

Expand Down Expand Up @@ -1891,7 +1928,6 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
ground_speed);

_is_low_height = true; // In low-height flight, TECS will control altitude tighter

const Vector2f local_position{_local_pos.x, _local_pos.y};
Vector2f local_landing_orbit_center = _global_local_proj_ref.project(pos_sp_curr.lat, pos_sp_curr.lon);
Expand All @@ -1915,6 +1951,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
loiter_radius = _param_nav_loiter_rad.get();
}

// During landing, enforce low-height flight conditions for tighter altitude control
const bool is_low_height = true;

// the terrain estimate (if enabled) is always used to determine the flaring altitude
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) {
// flare and land with minimal speed
Expand Down Expand Up @@ -1989,6 +2028,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
is_low_height,
true,
height_rate_setpoint);

Expand Down Expand Up @@ -2044,6 +2084,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
_param_fw_thr_max.get(),
desired_max_sinkrate,
_param_climbrate_target.get(),
is_low_height,
false,
-glide_slope_sink_rate); // heightrate = -sinkrate

Expand Down Expand Up @@ -2097,6 +2138,9 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
throttle_max = 0.0f;
}

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
_current_altitude,
calibrated_airspeed_sp,
Expand All @@ -2106,6 +2150,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
is_low_height,
false,
height_rate_sp);

Expand Down Expand Up @@ -2192,6 +2237,9 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
}
}

// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

tecs_update_pitch_throttle(control_interval,
_current_altitude, // TODO: check if this is really what we want.. or if we want to lock the altitude.
calibrated_airspeed_sp,
Expand All @@ -2201,6 +2249,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
throttle_max,
_param_sinkrate_target.get(),
_param_climbrate_target.get(),
is_low_height,
false,
height_rate_sp);

Expand All @@ -2221,6 +2270,9 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
const position_setpoint_s &pos_sp_curr)
{
// check if generic low-height flight conditions are satisfied
const bool is_low_height = checkLowHeightConditions();

float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed,
_performance_model.getMinimumCalibratedAirspeed(getLoadFactor()), ground_speed);

Expand Down Expand Up @@ -2251,7 +2303,8 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
_param_fw_thr_min.get(),
_param_fw_thr_max.get(),
_param_sinkrate_target.get(),
_param_climbrate_target.get());
_param_climbrate_target.get(),
is_low_height);

_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
_att_sp.pitch_body = get_tecs_pitch();
Expand Down Expand Up @@ -2469,9 +2522,6 @@ FixedwingPositionControl::Run()

update_in_air_states(_local_pos.timestamp);

// check if generic low-height flight conditions are satisfied
_is_low_height = checkLowHeightConditions();

// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
_tecs.set_speed_weight(_param_fw_t_spdweight.get());

Expand Down Expand Up @@ -2671,7 +2721,7 @@ FixedwingPositionControl::reset_landing_state()
void
FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
float pitch_min_rad, float pitch_max_rad, float throttle_min, float throttle_max,
const float desired_max_sinkrate, const float desired_max_climbrate,
const float desired_max_sinkrate, const float desired_max_climbrate, const bool is_low_height,
bool disable_underspeed_detection, float hgt_rate_sp)
{
// do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition
Expand All @@ -2692,7 +2742,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);

/* Update altitude time constant */
updateTECSAltitudeTimeConstant(control_interval);
updateTECSAltitudeTimeConstant(is_low_height, control_interval);

// HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases
// when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0.
Expand Down Expand Up @@ -2823,12 +2873,12 @@ bool FixedwingPositionControl::checkLowHeightConditions()
&& _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
}

void FixedwingPositionControl::updateTECSAltitudeTimeConstant(const float dt)
void FixedwingPositionControl::updateTECSAltitudeTimeConstant(const bool is_low_height, const float dt)
{
// Target time constant for the TECS altitude tracker
float alt_tracking_tc = _param_fw_t_h_error_tc.get();

if (_is_low_height) {
if (is_low_height) {
// If low-height conditions satisfied, compute target time constant for altitude tracking
alt_tracking_tc *= _param_fw_thrtc_sc.get();
}
Expand Down
Loading

0 comments on commit 9ad8384

Please sign in to comment.