mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-09 07:30:35 +08:00
fw_tecs: Support tighter altitude tracking during low-height flight (#23519)
* fw_tecs: Support tighter altitude tracking during low-height flight. Added FW_T_THR_LOW_HGT defining low-height flight threshold * tecs: Applied smoothed-out altitude TC transition to landings * fw_tecs: modified tighter altitude control for low-height implementation * addressed PR comments * addressed PR comments --------- Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Alvaro Fernandez
@@ -80,6 +80,9 @@ FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
|
||||
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
|
||||
_roll_slew_rate.setForcedValue(0.f);
|
||||
|
||||
_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
|
||||
_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
|
||||
|
||||
}
|
||||
|
||||
FixedwingPositionControl::~FixedwingPositionControl()
|
||||
@@ -419,6 +422,7 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air
|
||||
|
||||
tecs_status.altitude_sp = alt_sp;
|
||||
tecs_status.altitude_reference = debug_output.altitude_reference;
|
||||
tecs_status.altitude_time_constant = _tecs.get_altitude_error_time_constant();
|
||||
tecs_status.height_rate_reference = debug_output.height_rate_reference;
|
||||
tecs_status.height_rate_direct = debug_output.height_rate_direct;
|
||||
tecs_status.height_rate_setpoint = debug_output.control.altitude_rate_control;
|
||||
@@ -940,8 +944,9 @@ 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")
|
||||
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(),
|
||||
@@ -950,7 +955,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);
|
||||
|
||||
const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
|
||||
const float yaw_body = 0.f;
|
||||
@@ -976,6 +982,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
|
||||
const float descend_rate = -0.5f;
|
||||
const bool disable_underspeed_handling = false;
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
_current_altitude,
|
||||
_performance_model.getCalibratedTrimAirspeed(),
|
||||
@@ -985,6 +993,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,
|
||||
disable_underspeed_handling,
|
||||
descend_rate);
|
||||
|
||||
@@ -1125,6 +1134,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
||||
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
position_sp_alt,
|
||||
target_airspeed,
|
||||
@@ -1133,7 +1144,9 @@ 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);
|
||||
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
@@ -1178,6 +1191,8 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
|
||||
float yaw_body = _yaw;
|
||||
const bool disable_underspeed_handling = false;
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
position_sp_alt,
|
||||
target_airspeed,
|
||||
@@ -1187,6 +1202,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,
|
||||
disable_underspeed_handling,
|
||||
pos_sp_curr.vz);
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
@@ -1200,6 +1216,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};
|
||||
|
||||
@@ -1248,6 +1265,8 @@ 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};
|
||||
|
||||
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
|
||||
@@ -1255,7 +1274,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
|
||||
// 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.
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
|
||||
// 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());
|
||||
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
|
||||
@@ -1289,7 +1311,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
roll_body = 0.0f;
|
||||
}
|
||||
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
is_low_height = true; // In low-height flight, TECS will control altitude tighter
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
@@ -1300,7 +1322,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);
|
||||
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
@@ -1350,6 +1373,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
|
||||
tecs_fw_thr_max = _param_fw_thr_max.get();
|
||||
}
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
target_airspeed,
|
||||
@@ -1358,7 +1383,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
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
@@ -1388,7 +1414,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;
|
||||
|
||||
@@ -1425,6 +1450,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
|
||||
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
target_airspeed,
|
||||
@@ -1433,7 +1460,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);
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
@@ -1471,6 +1499,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
adjusted_min_airspeed = takeoff_airspeed;
|
||||
}
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
if (_runway_takeoff.runwayTakeoffEnabled()) {
|
||||
if (!_runway_takeoff.isInitialized()) {
|
||||
_runway_takeoff.init(now, _yaw, global_position);
|
||||
@@ -1559,6 +1589,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_param_fw_thr_max.get(),
|
||||
_param_sinkrate_target.get(),
|
||||
_performance_model.getMaximumClimbRate(_air_density),
|
||||
is_low_height,
|
||||
disable_underspeed_handling);
|
||||
|
||||
_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
|
||||
@@ -1646,6 +1677,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
max_takeoff_throttle,
|
||||
_param_sinkrate_target.get(),
|
||||
_performance_model.getMaximumClimbRate(_air_density),
|
||||
is_low_height,
|
||||
disable_underspeed_handling);
|
||||
|
||||
if (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) {
|
||||
@@ -1709,8 +1741,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
|
||||
float target_airspeed = adapt_airspeed_setpoint(control_interval, airspeed_land, adjusted_min_airspeed,
|
||||
ground_speed);
|
||||
// Enable tighter altitude control for landings
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
|
||||
|
||||
// now handle position
|
||||
const Vector2f local_position{_local_pos.x, _local_pos.y};
|
||||
@@ -1758,6 +1789,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
|
||||
@@ -1834,6 +1868,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
throttle_max,
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get(),
|
||||
is_low_height,
|
||||
disable_underspeed_handling,
|
||||
height_rate_setpoint);
|
||||
|
||||
@@ -1894,7 +1929,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 */
|
||||
|
||||
@@ -1947,8 +1983,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);
|
||||
|
||||
// Enable tighter altitude control for landings
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
|
||||
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);
|
||||
@@ -1972,6 +2006,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
|
||||
float roll_body;
|
||||
float yaw_body;
|
||||
@@ -2051,6 +2088,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
throttle_max,
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get(),
|
||||
is_low_height,
|
||||
disable_underspeed_handling,
|
||||
height_rate_setpoint);
|
||||
|
||||
@@ -2108,6 +2146,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,
|
||||
disable_underspeed_handling,
|
||||
-glide_slope_sink_rate); // heightrate = -sinkrate
|
||||
|
||||
@@ -2165,6 +2204,9 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
|
||||
throttle_max = 0.0f;
|
||||
}
|
||||
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
const bool disable_underspeed_handling = false;
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
@@ -2176,6 +2218,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
|
||||
throttle_max,
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get(),
|
||||
is_low_height,
|
||||
disable_underspeed_handling,
|
||||
height_rate_sp);
|
||||
|
||||
@@ -2267,8 +2310,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
|
||||
}
|
||||
}
|
||||
|
||||
const bool is_low_height = checkLowHeightConditions();
|
||||
|
||||
const bool disable_underspeed_handling = false;
|
||||
|
||||
|
||||
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,
|
||||
@@ -2278,6 +2324,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
|
||||
throttle_max,
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get(),
|
||||
is_low_height,
|
||||
disable_underspeed_handling,
|
||||
height_rate_sp);
|
||||
|
||||
@@ -2302,6 +2349,8 @@ 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)
|
||||
{
|
||||
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);
|
||||
|
||||
@@ -2332,7 +2381,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());
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
@@ -2555,7 +2605,6 @@ FixedwingPositionControl::Run()
|
||||
|
||||
// restore nominal TECS parameters in case changed intermittently (e.g. in landing handling)
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
|
||||
// restore lateral-directional guidance parameters (changed in takeoff mode)
|
||||
_npfg.setPeriod(_param_npfg_period.get());
|
||||
@@ -2687,6 +2736,8 @@ FixedwingPositionControl::Run()
|
||||
_roll_slew_rate.setForcedValue(_roll);
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Publish estimate of level flight
|
||||
_flight_phase_estimation_pub.get().timestamp = hrt_absolute_time();
|
||||
_flight_phase_estimation_pub.update();
|
||||
@@ -2755,7 +2806,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
|
||||
@@ -2775,6 +2826,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
|
||||
/* No underspeed protection in landing mode */
|
||||
_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
|
||||
|
||||
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.
|
||||
const float airspeed_rate_estimate = 0.f;
|
||||
@@ -2897,6 +2950,27 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
|
||||
}
|
||||
}
|
||||
|
||||
bool FixedwingPositionControl::checkLowHeightConditions()
|
||||
{
|
||||
// Are conditions for low-height
|
||||
return _param_fw_t_thr_low_hgt.get() >= 0.f && _local_pos.dist_bottom_valid
|
||||
&& _local_pos.dist_bottom < _param_fw_t_thr_low_hgt.get();
|
||||
}
|
||||
|
||||
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 low-height conditions satisfied, compute target time constant for altitude tracking
|
||||
alt_tracking_tc *= _param_fw_thrtc_sc.get();
|
||||
}
|
||||
|
||||
_tecs_alt_time_const_slew_rate.update(alt_tracking_tc, dt);
|
||||
_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());
|
||||
}
|
||||
|
||||
Vector2f
|
||||
FixedwingPositionControl::calculateTouchdownPosition(const float control_interval, const Vector2f &local_land_position)
|
||||
{
|
||||
|
||||
Reference in New Issue
Block a user