diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index ae6835dc5249..056b56100bbe 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -2,6 +2,7 @@ uint64 timestamp # time since system start (microseconds) float32 altitude_sp # Altitude setpoint AMSL [m] float32 altitude_reference # Altitude setpoint reference AMSL [m] +float32 altitude_time_constant # Time constant of the altitude tracker [s] float32 height_rate_reference # Height rate setpoint reference [m/s] float32 height_rate_direct # Direct height rate setpoint from velocity reference generator [m/s] float32 height_rate_setpoint # Height rate setpoint [m/s] diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index f033bf72dabf..4f178680cb25 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -647,6 +647,14 @@ class TECS float get_pitch_setpoint() {return _control.getPitchSetpoint();} float get_throttle_setpoint() {return _control.getThrottleSetpoint();} + /** + * Returns the altitude tracking time constant + */ + float get_altitude_error_time_constant() const + { + return 1.0f / math::max(_control_param.altitude_error_gain, 0.01f); + } + uint64_t timestamp() { return _update_timestamp; } float get_underspeed_ratio() { return _control.getRatioUndersped(); } diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 7858fe2c1c5f..033a7cdd9cdb 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -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; @@ -939,8 +943,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(), @@ -949,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); const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle const float yaw_body = 0.f; @@ -975,6 +981,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(), @@ -984,6 +992,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); @@ -1120,6 +1129,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, @@ -1128,7 +1139,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); @@ -1173,6 +1186,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, @@ -1182,6 +1197,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(); @@ -1195,6 +1211,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}; @@ -1243,6 +1260,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 @@ -1250,7 +1269,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(); @@ -1284,7 +1306,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, @@ -1295,7 +1317,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(); @@ -1345,6 +1368,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, @@ -1353,7 +1378,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 @@ -1383,7 +1409,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; @@ -1420,6 +1445,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, @@ -1428,7 +1455,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(); @@ -1466,6 +1494,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); @@ -1554,6 +1584,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 @@ -1641,6 +1672,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) { @@ -1704,8 +1736,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}; @@ -1753,6 +1784,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 @@ -1829,6 +1863,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); @@ -1889,7 +1924,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 */ @@ -1942,8 +1978,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); @@ -1967,6 +2001,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; @@ -2046,6 +2083,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); @@ -2103,6 +2141,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 @@ -2160,6 +2199,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, @@ -2171,6 +2213,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); @@ -2262,8 +2305,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, @@ -2273,6 +2319,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); @@ -2297,6 +2344,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); @@ -2327,7 +2376,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(); @@ -2550,7 +2600,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()); @@ -2682,6 +2731,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(); @@ -2750,7 +2801,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 @@ -2770,6 +2821,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; @@ -2892,6 +2945,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) { diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index 84300ae1b409..f481f66777c0 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -175,6 +175,8 @@ static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s; // [-] Can-run threshold needed to trigger the roll-constraining failsafe warning static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f; +// [s] slew rate with which we change altitude time constant +static constexpr float TECS_ALT_TIME_CONST_SLEW_RATE = 1.0f; class FixedwingPositionControl final : public ModuleBase, public ModuleParams, public px4::WorkItem @@ -401,6 +403,9 @@ class FixedwingPositionControl final : public ModuleBase _tecs_alt_time_const_slew_rate; + // VTOL / TRANSITION bool _is_vtol_tailsitter{false}; matrix::Vector2d _transition_waypoint{(double)NAN, (double)NAN}; @@ -761,12 +766,13 @@ class FixedwingPositionControl final : public ModuleBase) _param_fw_lnd_fl_pmin, (ParamFloat) _param_fw_lnd_flalt, (ParamFloat) _param_fw_thrtc_sc, + (ParamFloat) _param_fw_t_thr_low_hgt, (ParamBool) _param_fw_lnd_earlycfg, (ParamInt) _param_fw_lnd_useter, diff --git a/src/modules/fw_pos_control/fw_path_navigation_params.c b/src/modules/fw_pos_control/fw_path_navigation_params.c index ac2ab276af41..f0c8683386a8 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -412,10 +412,9 @@ PARAM_DEFINE_FLOAT(FW_LND_FL_PMAX, 15.0f); PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f); /** - * Altitude time constant factor for landing + * Altitude time constant factor for landing and low-height flight * - * During landing, the TECS altitude time constant (FW_T_ALT_TC) - * is multiplied by this value. + * The TECS altitude time constant (FW_T_ALT_TC) is multiplied by this value. * * @unit * @min 0.2 @@ -426,6 +425,24 @@ PARAM_DEFINE_FLOAT(FW_LND_AIRSPD, -1.f); */ PARAM_DEFINE_FLOAT(FW_LND_THRTC_SC, 1.0f); +/** + * Low-height threshold for tighter altitude tracking + * + * Defines the height (distance to bottom) threshold below which tighter altitude + * tracking gets enabled (see FW_LND_THRTC_SC). Below this height, TECS smoothly + * (1 sec / sec) transitions the altitude tracking time constant from FW_T_ALT_TC + * to FW_LND_THRTC_SC*FW_T_ALT_TC. + * + * If equal to -1, low-height traking is disabled. + * + * @unit m + * @min -1 + * @decimal 0 + * @increment 1 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_THR_LOW_HGT, -1.f); + /* * TECS parameters *