diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b0d9c43b20..867df64caa 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -239,7 +239,7 @@ void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param _detectUnderspeed(input, param, flag); - const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; + const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)}; _pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag); @@ -381,7 +381,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, cons math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f); } -TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag) +TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Input &input, const Param ¶m, + const Flag &flag) { SpecificEnergyWeighting weight; @@ -394,6 +395,10 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co } else if (!flag.airspeed_enabled) { pitch_speed_weight = 0.0f; + } else if (flag.fast_descend) { + // pitch loop controls the airspeed to max + pitch_speed_weight = 2.0; + } // don't allow any weight to be larger than one, as it has the same effect as reducing the control @@ -405,10 +410,9 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co } void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates, - const Param ¶m, - const Flag &flag) + const Param ¶m, const Flag &flag) { - const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)}; + const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(input, param, flag)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; _calcPitchControlUpdate(dt, input, seb_rate, param); @@ -514,20 +518,25 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate; _ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const); _ste_rate_estimate_filter.update(STE_rate_estimate_raw); - ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)}; - _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); - float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)}; - // Rate limit the throttle demand - if (fabsf(param.throttle_slewrate) > FLT_EPSILON) { - const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate; - throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit, - _throttle_setpoint + throttle_increment_limit); + if (flag.fast_descend) { + _throttle_setpoint = param.throttle_min; + + } else { + _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); + float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)}; + + // Rate limit the throttle demand + if (fabsf(param.throttle_slewrate) > FLT_EPSILON) { + const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate; + throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit, + _throttle_setpoint + throttle_increment_limit); + } + + _throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max); } - _throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max); - // Debug output _debug_output.total_energy_rate_estimate = ste_rate.estimate; _debug_output.total_energy_rate_sp = ste_rate.setpoint; @@ -651,6 +660,7 @@ void TECS::initControlParams(float target_climbrate, float target_sinkrate, floa _reference_param.target_sinkrate = target_sinkrate; // Control _control_param.tas_min = eas_to_tas * _equivalent_airspeed_min; + _control_param.tas_max = eas_to_tas * _equivalent_airspeed_max; _control_param.pitch_max = pitch_limit_max; _control_param.pitch_min = pitch_limit_min; _control_param.throttle_trim = throttle_trim; @@ -705,6 +715,10 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas); } else { + /* Check if we want to fast descend. On fast descend, we set the throttle to min, and use the altitude control + loop to control the speed to the maximum airspeed. */ + const bool is_fast_descend{_checkFastDescend(hgt_setpoint, altitude)}; + // Update airspeedfilter submodule const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed, .equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas}; @@ -712,10 +726,19 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled); // Update Reference model submodule - const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, - .alt_rate = hgt_rate_sp}; + if (is_fast_descend) { + // Reset the altitude reference model. + const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ + .alt = altitude, + .alt_rate = hgt_rate}; + _altitude_reference_model.initialize(init_state); - _altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param); + } else { + const TECSAltitudeReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint, + .alt_rate = hgt_rate_sp}; + + _altitude_reference_model.update(dt, setpoint, altitude, hgt_rate, _reference_param); + } TECSControl::Setpoint control_setpoint; control_setpoint.altitude_reference = _altitude_reference_model.getAltitudeReference(); @@ -727,6 +750,12 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set .tas = eas_to_tas * _airspeed_filter.getState().speed, .tas_rate = eas_to_tas * _airspeed_filter.getState().speed_rate}; + if (is_fast_descend) { + control_setpoint.tas_setpoint = _control_param.tas_max; + } + + _control_flag.fast_descend = is_fast_descend; + _control.update(dt, control_setpoint, control_input, _control_param, _control_flag); } @@ -740,3 +769,13 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _update_timestamp = now; } +bool TECS::_checkFastDescend(const float alt_setpoint, const float alt) +{ + bool ret_val{false}; + + if ((_fast_descend_alt_err > FLT_EPSILON) && ((alt_setpoint + _fast_descend_alt_err) < alt)) { + ret_val = true; + } + + return ret_val; +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 8749ac0a21..9192e29668 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -202,6 +202,7 @@ public: float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²]. float equivalent_airspeed_trim; ///< Equivalent cruise airspeed for airspeed less mode [m/s]. float tas_min; ///< True airspeed demand lower limit [m/s]. + float tas_max; ///< True airspeed demand upper limit [m/s]. float pitch_max; ///< Maximum pitch angle allowed in [rad]. float pitch_min; ///< Minimal pitch angle allowed in [rad]. float throttle_trim; ///< Normalized throttle required to fly level at calibrated airspeed setpoint [0,1] @@ -278,6 +279,7 @@ public: struct Flag { bool airspeed_enabled; ///< Flag if the airspeed sensor is enabled. bool detect_underspeed_enabled; ///< Flag if underspeed detection is enabled. + bool fast_descend; ///< Flag if descending with higher airspeed is allowed. }; public: TECSControl() = default; @@ -393,7 +395,7 @@ private: * @brief calculate airspeed control proportional output. * * @param setpoint is the control setpoints. - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @param flag is the control flags. * @return controlled airspeed rate setpoint in [m/s²]. @@ -404,7 +406,7 @@ private: * @brief calculate altitude control proportional output. * * @param setpoint is the control setpoints. - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @return controlled altitude rate setpoint in [m/s]. */ @@ -413,14 +415,14 @@ private: * @brief Calculate specific energy rates. * * @param control_setpoint is the controlles altitude and airspeed rate setpoints. - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @return Specific energy rates in [m²/s³]. */ SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const; /** * @brief Detect underspeed. * - * @param input is the current input measurment of the UAS. + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @param flag is the control flags. */ @@ -428,11 +430,12 @@ private: /** * @brief Update specific energy balance weights. * + * @param input is the current input measurement of the UAS. * @param param is the control parameters. * @param flag is the control flags. * @return Weights used for the specific energy balance. */ - SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param ¶m, const Flag &flag); + SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Input &input, const Param ¶m, const Flag &flag); /** * @brief Calculate pitch control. * @@ -602,9 +605,11 @@ public: void set_max_climb_rate(float climb_rate) { _control_param.max_climb_rate = climb_rate; _reference_param.max_climb_rate = climb_rate; }; void set_altitude_rate_ff(float altitude_rate_ff) { _control_param.altitude_setpoint_gain_ff = altitude_rate_ff; }; - void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f);; }; + void set_altitude_error_time_constant(float time_const) { _control_param.altitude_error_gain = 1.0f / math::max(time_const, 0.1f); }; + void set_fast_descend_altitude_error(float altitude_error) { _fast_descend_alt_err = altitude_error; }; void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; } + void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; } void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; } void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; } @@ -665,7 +670,9 @@ private: hrt_abstime _update_timestamp{0}; ///< last timestamp of the update function call. - float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) + float _equivalent_airspeed_min{10.0f}; ///< equivalent airspeed demand lower limit (m/sec) + float _equivalent_airspeed_max{20.0f}; ///< equivalent airspeed demand upper limit (m/sec) + float _fast_descend_alt_err{-1.f}; ///< Altitude difference between current altitude to altitude setpoint needed to descend with higher airspeed [m]. static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) @@ -697,6 +704,7 @@ private: .vert_accel_limit = 0.0f, .equivalent_airspeed_trim = 15.0f, .tas_min = 10.0f, + .tas_max = 20.0f, .pitch_max = 0.5f, .pitch_min = -0.5f, .throttle_trim = 0.0f, @@ -722,5 +730,14 @@ private: .airspeed_enabled = false, .detect_underspeed_enabled = false, }; + + /** + * @brief Check if fast descend should be used + * + * @param alt_setpoint is the altitude setpoint + * @param alt is the + * @return true if fast descend should be enabled + */ + bool _checkFastDescend(float alt_setpoint, float alt); }; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5672e7f446..4cc970aa62 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -129,6 +129,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_equivalent_airspeed_trim(_performance_model.getCalibratedTrimAirspeed()); _tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); + _tecs.set_equivalent_airspeed_max(_performance_model.getMaximumCalibratedAirspeed()); _tecs.set_throttle_damp(_param_fw_t_thr_damping.get()); _tecs.set_integrator_gain_throttle(_param_fw_t_thr_integ.get()); _tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get()); @@ -137,6 +138,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get()); _tecs.set_pitch_damping(_param_fw_t_ptch_damp.get()); _tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get()); + _tecs.set_fast_descend_altitude_error(_param_fw_t_fast_alt_err.get()); _tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get()); _tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get()); _tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get()); diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index e1593916bc..ad30e75de4 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -957,6 +957,7 @@ private: (ParamFloat) _param_fw_t_hrate_ff, (ParamFloat) _param_fw_t_h_error_tc, + (ParamFloat) _param_fw_t_fast_alt_err, (ParamFloat) _param_fw_t_thr_integ, (ParamFloat) _param_fw_t_I_gain_pit, (ParamFloat) _param_fw_t_ptch_damp, 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 a636d8d757..33ee50a8df 100644 --- a/src/modules/fw_pos_control/fw_path_navigation_params.c +++ b/src/modules/fw_pos_control/fw_path_navigation_params.c @@ -633,6 +633,15 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.1f); */ PARAM_DEFINE_FLOAT(FW_T_ALT_TC, 5.0f); +/** + * Minimum altitude error needed to descend with max airspeed. A negative value disables fast descend. + * + * @min -1.0 + * @decimal 0 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_F_ALT_ERR, -1.0f); + /** * Height rate feed forward *