diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b3148c4a7c..d25821b949 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -394,6 +394,10 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co } else if (!flag.airspeed_enabled) { pitch_speed_weight = 0.0f; + } else if (param.fast_descend > FLT_EPSILON) { + // pitch loop controls the airspeed to max + pitch_speed_weight = 1.f + param.fast_descend; + } // don't allow any weight to be larger than one, as it has the same effect as reducing the control @@ -405,8 +409,7 @@ 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)}; ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)}; @@ -514,10 +517,17 @@ 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)}; + float throttle_setpoint{param.throttle_min}; + + if (1.f - param.fast_descend < FLT_EPSILON) { + // During fast descend, we control airspeed over the pitch control loop and give minimal thrust. + throttle_setpoint = param.throttle_min; + + } else { + _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag); + throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag); + } // Rate limit the throttle demand if (fabsf(param.throttle_slewrate) > FLT_EPSILON) { @@ -651,6 +661,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 +716,11 @@ 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. */ + _setFastDescend(hgt_setpoint, altitude); + _control_param.fast_descend = _fast_descend; + // Update airspeedfilter submodule const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed, .equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas}; @@ -712,15 +728,25 @@ 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 (1.f - _fast_descend < FLT_EPSILON) { + // Reset the altitude reference model, while we are in fast descend. + 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(); control_setpoint.altitude_rate_setpoint_direct = _altitude_reference_model.getHeightRateSetpointDirect(); - control_setpoint.tas_setpoint = eas_to_tas * EAS_setpoint; + control_setpoint.tas_setpoint = _control_param.tas_max * _fast_descend + (1 - _fast_descend) * eas_to_tas * + EAS_setpoint; const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, @@ -740,3 +766,17 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _update_timestamp = now; } +void TECS::_setFastDescend(const float alt_setpoint, const float alt) +{ + if (_control_flag.airspeed_enabled && (_fast_descend_alt_err > FLT_EPSILON) + && ((alt_setpoint + _fast_descend_alt_err) < alt)) { + _fast_descend = 1.f; + + } else if ((_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { + // Were in fast descend, scale it down. up until 5m above target altitude + _fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f); + + } else { + _fast_descend = 0.f; + } +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 8749ac0a21..64881fb17f 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] @@ -233,6 +234,8 @@ public: float load_factor_correction; ///< Gain from normal load factor increase to total energy rate demand [m²/s³]. float load_factor; ///< Additional normal load factor. + + float fast_descend; }; /** @@ -393,7 +396,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 +407,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 +416,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. */ @@ -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,10 @@ 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]. + float _fast_descend{0.f}; ///< Value for fast descend in [0,1]. continuous value used to flatten the high speed value out when close to target altitude. 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 +705,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, @@ -716,11 +725,20 @@ private: .throttle_slewrate = 0.0f, .load_factor_correction = 0.0f, .load_factor = 1.0f, + .fast_descend = 0.f }; TECSControl::Flag _control_flag{ .airspeed_enabled = false, .detect_underspeed_enabled = false, }; + + /** + * @brief Set fast descend value + * + * @param alt_setpoint is the altitude setpoint + * @param alt is the current altitude + */ + void _setFastDescend(float alt_setpoint, float alt); }; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index c4ce648381..a1286db402 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 5089021685..eb42160483 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -964,6 +964,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 *