diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index e4d50b4fd733..b3a79241be99 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -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(), @@ -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; @@ -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(), @@ -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); @@ -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, @@ -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 @@ -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, @@ -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); } @@ -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}; @@ -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 @@ -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()); @@ -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, @@ -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 @@ -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, @@ -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 @@ -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; @@ -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, @@ -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(); @@ -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); @@ -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 @@ -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 @@ -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}; @@ -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 @@ -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); @@ -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 */ @@ -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); @@ -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 @@ -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); @@ -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 @@ -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, @@ -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); @@ -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, @@ -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); @@ -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); @@ -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(); @@ -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()); @@ -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 @@ -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. @@ -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(); } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index c6967ca5733e..ae96d2c597e1 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -289,8 +289,6 @@ class FixedwingPositionControl final : public ModuleBase