diff --git a/msg/TecsStatus.msg b/msg/TecsStatus.msg index 7c4c3a541c..90453f8511 100644 --- a/msg/TecsStatus.msg +++ b/msg/TecsStatus.msg @@ -33,3 +33,4 @@ uint8 TECS_MODE_LAND = 3 uint8 TECS_MODE_LAND_THROTTLELIM = 4 uint8 TECS_MODE_BAD_DESCENT = 5 uint8 TECS_MODE_CLIMBOUT = 6 +uint8 TECS_MODE_STALL_PREVENTION = 7 diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index de81e47b43..2580aed02b 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -344,7 +344,7 @@ TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const Alt void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, const Flag &flag) { if (!flag.detect_underspeed_enabled) { - _ratio_undersped = 0.0f; + _ratio_underspeed = 0.0f; return; } @@ -360,8 +360,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param ¶m, cons const float tas_fully_undersped = math::max(param.tas_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f); const float tas_starting_to_underspeed = math::max(param.tas_min - tas_error_bound, tas_fully_undersped); - _ratio_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) / - math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f); + _ratio_underspeed = 1.0f - math::constrain((input.tas - tas_fully_undersped) / + 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) @@ -371,8 +371,8 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co // Calculate the weight applied to control of specific kinetic energy error float pitch_speed_weight = constrain(param.pitch_speed_weight, 0.0f, 2.0f); - if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) { - pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight; + if (_ratio_underspeed > FLT_EPSILON && flag.airspeed_enabled) { + pitch_speed_weight = 2.0f * _ratio_underspeed + (1.0f - _ratio_underspeed) * pitch_speed_weight; } else if (!flag.airspeed_enabled) { pitch_speed_weight = 0.0f; @@ -540,7 +540,7 @@ void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit if (param.integrator_gain_throttle > FLT_EPSILON) { // underspeed conditions zero out integration float throttle_integ_input = (_getControlError(ste_rate) * param.integrator_gain_throttle) * dt * - STE_rate_to_throttle * (1.0f - _ratio_undersped); + STE_rate_to_throttle * (1.0f - _ratio_underspeed); // only allow integrator propagation into direction which unsaturates throttle if (_throttle_setpoint >= param.throttle_max) { @@ -603,7 +603,7 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C // ramp in max throttle setting with underspeediness value - throttle_setpoint = _ratio_undersped * param.throttle_max + (1.0f - _ratio_undersped) * throttle_setpoint; + throttle_setpoint = _ratio_underspeed * param.throttle_max + (1.0f - _ratio_underspeed) * throttle_setpoint; return constrain(throttle_setpoint, param.throttle_min, param.throttle_max); } @@ -614,23 +614,32 @@ void TECSControl::resetIntegrals() _throttle_integ_state = 0.0f; } +TECS::TECS() : + ModuleParams(nullptr) +{ + +} + float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas) { float new_setpoint{tas_setpoint}; - const float percent_undersped = _control.getRatioUndersped(); + const float percent_underspeed = _control.getRatioUnderspeed(); // Set the TAS demand to the minimum value if an underspeed or // or a uncontrolled descent condition exists to maximise climb rate - if (_uncommanded_descent_recovery) { + if (_stall_detection_percentage > FLT_EPSILON) { + new_setpoint = (1 - _stall_detection_percentage) * tas_setpoint + _stall_detection_percentage * tas_max; + + } else if (_uncommanded_descent_recovery) { new_setpoint = tas_min; - } else if (percent_undersped > FLT_EPSILON) { + } else if (percent_underspeed > FLT_EPSILON) { // TAS setpoint is reset from external setpoint every time tecs is called, so the interpolation is still // between current setpoint and mininimum airspeed here (it's not feeding the newly adjusted setpoint // from this line back into this method each time). // TODO: WOULD BE GOOD to "functionalize" this library a bit and remove many of these internal states to // avoid the fear of side effects in simple operations like this. - new_setpoint = tas_min * percent_undersped + (1.0f - percent_undersped) * tas_setpoint; + new_setpoint = tas_min * percent_underspeed + (1.0f - percent_underspeed) * tas_setpoint; } new_setpoint = constrain(new_setpoint, tas_min, tas_max); @@ -660,7 +669,7 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu const float SKE_error = SKE_setpoint - SKE_estimate; const float STE_error = SPE_error + SKE_error; - const bool underspeed_detected = _control.getRatioUndersped() > FLT_EPSILON; + const bool underspeed_detected = _control.getRatioUnderspeed() > FLT_EPSILON; // If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f) @@ -731,10 +740,16 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _control_param.tas_min = eas_to_tas * _equivalent_airspeed_min; _control_param.pitch_max = pitch_limit_max; _control_param.pitch_min = pitch_limit_min; - _control_param.throttle_trim = throttle_trim; _control_param.throttle_max = throttle_setpoint_max; _control_param.throttle_min = throttle_min; + if (throttle_trim > _control_param.throttle_trim / _stall_thr_trim_scale_to_prevent_stall) { + _stall_thr_trim_scale_to_prevent_stall = 1.0f; + + } + + _control_param.throttle_trim = throttle_trim * _stall_thr_trim_scale_to_prevent_stall; + if (dt < DT_MIN) { // Update intervall too small, do not update. Assume constant states/output in this case. return; @@ -766,6 +781,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set // TODO this function should not be in the module. Only give feedback that the airspeed can't be achieved. control_setpoint.tas_setpoint = _update_speed_setpoint(eas_to_tas * _equivalent_airspeed_min, eas_to_tas * _equivalent_airspeed_max, EAS_setpoint * eas_to_tas, eas_to_tas * eas.speed); + _update_altitude_setpoint(control_setpoint, altitude); const TECSControl::Input control_input{ .altitude = altitude, .altitude_rate = hgt_rate, @@ -778,17 +794,23 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed * eas_to_tas, control_setpoint.tas_setpoint); + // Detect stall + _detect_airspeedless_stall(dt, hgt_setpoint, altitude, pitch, hgt_rate); + // Update time stamps _update_timestamp = now; // Set TECS mode for next frame - if (_control.getRatioUndersped() > FLT_EPSILON) { + if (_control.getRatioUnderspeed() > FLT_EPSILON) { _tecs_mode = ECL_TECS_MODE_UNDERSPEED; } else if (_uncommanded_descent_recovery) { _tecs_mode = ECL_TECS_MODE_BAD_DESCENT; + } else if (_stall_detection_percentage > FLT_EPSILON) { + _tecs_mode = ECL_TECS_MODE_STALL_PREVENTION; + } else { // This is the default operation mode _tecs_mode = ECL_TECS_MODE_NORMAL; @@ -804,3 +826,78 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set } } +void TECS::_update_altitude_setpoint(TECSControl::Setpoint &control_setpoint, const float altitude) const +{ + if (_stall_detection_percentage > FLT_EPSILON) { + control_setpoint.altitude_reference.alt = (1.0f - _stall_detection_percentage) * control_setpoint.altitude_reference.alt + + _stall_detection_percentage * altitude; + control_setpoint.altitude_reference.alt_rate = (1.0f - _stall_detection_percentage) * + control_setpoint.altitude_reference.alt_rate; + control_setpoint.altitude_rate_setpoint = (1.0f - _stall_detection_percentage) * control_setpoint.altitude_rate_setpoint + - _stall_detection_percentage * _control_param.max_sink_rate; + } +} + +void TECS::_detect_airspeedless_stall(const float dt, const float altitude_sp, const float altitude, const float pitch, + const float altitude_rate) +{ + if (_param_fw_stall_prv.get()) { + // Checks to detect an aircraft stalling: a positive altitude change is given, + // while the pitch command is maxed and the AV is sinking. + // TODO could also add airspeed rate negative, would need to filter the speed_deriv_forward. + if (((altitude_sp - altitude) > 0.0f) && + (pitch > 0.0f) && + (altitude_rate < 0.0f) && + (fabsf(_control_param.pitch_max - _control.getPitchSetpoint()) < FLT_EPSILON)) { + if (_stall_state == STALL_MODE::NO_STALL) { + PX4_WARN("Entered stall condition"); + _stall_start_timestamp = hrt_absolute_time(); + _stall_state = STALL_MODE::MAYBE_STALL; + } + + } else if (_stall_state == STALL_MODE::MAYBE_STALL) { + PX4_WARN("Aborting stall"); + _stall_state = STALL_MODE::NO_STALL; + _stall_start_timestamp = 0ULL; + } + + // Only be sure to enter stall prevention if the stall state persists for a certain amount of time. + if (_stall_state == STALL_MODE::MAYBE_STALL + && hrt_elapsed_time(&_stall_start_timestamp) > math::max(_param_fw_stall_delay_ms.get() * 1000ULL, 0ULL)) { + PX4_WARN("Start stall reaction."); + _stall_state = STALL_MODE::STALL; + } + + // End stall if we pitched the nose downwards and flying nearly at max sinkrate. + if ((_stall_state == STALL_MODE::STALL) && (pitch < 0.0f) && (altitude_rate < (0.5f - _control_param.max_sink_rate))) { + PX4_WARN("Stall ending, speed increased."); + _stall_state = STALL_MODE::STALL_ENDING; + } + + if ((_stall_state == STALL_MODE::STALL_ENDING) && (_stall_detection_percentage < FLT_EPSILON)) { + PX4_WARN("Stall ended."); + _stall_state = STALL_MODE::NO_STALL; + _stall_start_timestamp = 0ULL; + _stall_thr_trim_scale_to_prevent_stall += math::max(_param_fw_stall_throttle_scale.get(), + 0.f); // Increase throttle trim to prevent future stall. + } + + switch (_stall_state) { + case STALL_MODE::STALL: { + _stall_detection_percentage = math::min(_stall_detection_percentage + dt / math::max(FLT_EPSILON, + _param_fw_stall_ramp_up.get()), 1.0f); + break; + } + + case STALL_MODE::STALL_ENDING: { + _stall_detection_percentage = math::max(_stall_detection_percentage - dt / math::max(FLT_EPSILON, + _param_fw_stall_ramp_up.get()), 0.0f); + break; + } + + default: { + _stall_detection_percentage = 0.0f; + } + } + } +} diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index e42242386a..1b7274c19c 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -299,7 +300,7 @@ public: * * @return Ratio of detected undersped [0,1]. */ - float getRatioUndersped() const {return _ratio_undersped;}; + float getRatioUnderspeed() const {return _ratio_underspeed;}; /** * @brief Get the throttle setpoint. * @@ -533,18 +534,19 @@ private: DebugOutput _debug_output; ///< Debug output. float _pitch_setpoint{0.0f}; ///< Controlled pitch setpoint [rad]. float _throttle_setpoint{0.0f}; ///< Controlled throttle setpoint [0,1]. - float _ratio_undersped{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1] + float _ratio_underspeed{0.0f}; ///< A continuous representation of how "undersped" the TAS is [0,1] float _ste_rate{0.0f}; ///< Specific total energy rate [m²/s³]. }; -class TECS +class TECS : public ModuleParams { public: enum ECL_TECS_MODE { ECL_TECS_MODE_NORMAL = 0, ECL_TECS_MODE_UNDERSPEED, ECL_TECS_MODE_BAD_DESCENT, - ECL_TECS_MODE_CLIMBOUT + ECL_TECS_MODE_CLIMBOUT, + ECL_TECS_MODE_STALL_PREVENTION }; struct DebugOutput { @@ -557,7 +559,7 @@ public: enum ECL_TECS_MODE tecs_mode; }; public: - TECS() = default; + TECS(); ~TECS() = default; // no copy, assignment, move, move assignment @@ -664,13 +666,21 @@ private: enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL}; ///< Current activated mode. - hrt_abstime _update_timestamp{0}; ///< last timestamp of the update function call. - + hrt_abstime _update_timestamp{0ULL}; ///< last timestamp of the update function call. + hrt_abstime _stall_start_timestamp{0ULL}; ///< timestamp when the stall condition was first achieved. + enum class STALL_MODE { + NO_STALL, + MAYBE_STALL, + STALL, + STALL_ENDING, + } _stall_state{STALL_MODE::NO_STALL}; ///< State machine of the stall detection algorithm + float _stall_thr_trim_scale_to_prevent_stall{1.0f}; ///< Scale applied to throttle trim to avoid future stall conditions. float _equivalent_airspeed_min{3.0f}; ///< equivalent airspeed demand lower limit (m/sec) float _equivalent_airspeed_max{30.0f}; ///< equivalent airspeed demand upper limit (m/sec) // controller mode logic bool _uncommanded_descent_recovery{false}; ///< true when a continuous descent caused by an unachievable airspeed demand has been detected + float _stall_detection_percentage{0.f}; ///< Continuous representation of how certain we are about a potential stall situation between [0 1] 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) @@ -737,5 +747,16 @@ private: */ void _detect_uncommanded_descent(float throttle_setpoint_max, float altitude, float altitude_setpoint, float tas, float tas_setpoint); + + void _detect_airspeedless_stall(float dt, float altitude_sp, float altitude, float pitch, float altitude_rate); + + void _update_altitude_setpoint(TECSControl::Setpoint &control_setpoint, float altitude) const; + + DEFINE_PARAMETERS( + (ParamBool) _param_fw_stall_prv, + (ParamInt) _param_fw_stall_delay_ms, + (ParamFloat) _param_fw_stall_throttle_scale, + (ParamFloat) _param_fw_stall_ramp_up + ) }; diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index 5098a0bb08..3203279a1a 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -567,6 +567,11 @@ void AirspeedModule::select_airspeed_and_publish() // we need to re-evaluate the sensors if we're currently not on a phyisical sensor or the current sensor got invalid bool airspeed_sensor_switching_necessary = false; + if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + _number_of_airspeed_sensors = 0; + _valid_airspeed_index = -1; + } + if (_prev_airspeed_index < airspeed_index::FIRST_SENSOR_INDEX) { airspeed_sensor_switching_necessary = true; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index ac2b97ffef..6c535d5de3 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -481,6 +481,10 @@ FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_air case TECS::ECL_TECS_MODE_CLIMBOUT: t.mode = tecs_status_s::TECS_MODE_CLIMBOUT; break; + + case TECS::ECL_TECS_MODE_STALL_PREVENTION: + t.mode = tecs_status_s::TECS_MODE_STALL_PREVENTION; + break; } t.altitude_sp = alt_sp; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 650d1239a0..1f96ccdd4d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -860,6 +860,65 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP, 3.0f); */ PARAM_DEFINE_FLOAT(FW_T_SINK_R_SP, 2.0f); +/** + * Stall detection and recovery + * + * Define a crude stall detection based on positive altitude change setpoint, while sinking + * with a positive pitch as well as a saturated pitch command. + * If stall is detected, ramp up a command to sink with maximum sink rate as well as maximum speed. + * + * @boolean + * @group FW TECS + */ +PARAM_DEFINE_INT32(FW_T_STALL_PRV, 0); + +/** + * Stall prevention throttle trim scale increase. + * + * + * If a stall condition is detected, the throttle trim is increased to prevent future stall condition. + * The throttle is scaled up by the amount defined in this parameter each time a stall has been detected. + * + * @unit ms + * @min 0 + * @max 5000 + * @group FW TECS + */ +PARAM_DEFINE_INT32(FW_T_STALL_DELAY, 100); + +/** + * Stall prevention throttle trim scale increase. + * + * + * If a stall condition is detected, the throttle trim is increased to prevent future stall condition. + * The throttle is scaled up by the amount defined in this parameter each time a stall has been detected. + * + * @unit % + * @min 0.01 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_STALL_TH_SC, 0.05f); + +/** + * Stall action ramp up + * + * + * If a stall condition is detected, a sink rate and an airspeed increase is commanded. to avoid sudden steps + * those commands are ramped up. This parameters defines the amount of time spend in stall, before the commands + * are completely ramped up. When stall is prevented. the commands are ramped down within the same time. + * + * @unit s + * @min 0.01 + * @max 2 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_T_STALL_RAMP, 0.2f); + /** * GPS failure loiter time *