diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index de81e47b43..de5f4cd73d 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -288,7 +288,7 @@ TECSControl::STERateLimit TECSControl::_calculateTotalEnergyRateLimit(const Para TECSControl::STERateLimit limit; // Calculate the specific total energy rate limits from the max throttle limits limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G; - limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G; + limit.STE_rate_min = - math::max(param.min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G; return limit; } diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index e42242386a..78f02c419a 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -135,7 +135,7 @@ public: float jerk_max; ///< Magnitude of the maximum jerk allowed [m/s³]. float vert_accel_limit; ///< Magnitude of the maximum vertical acceleration allowed [m/s²]. float max_climb_rate; ///< Climb rate produced by max allowed throttle [m/s]. - float max_sink_rate; ///< Maximum safe sink rate [m/s]. + float max_sink_rate; ///< Maximum sink rate (with min throttle, max speed) [m/s]. }; public: @@ -189,7 +189,8 @@ public: */ struct Param { // Vehicle specific params - float max_sink_rate; ///< Maximum safe sink rate [m/s]. + float max_sink_rate; ///< Maximum sink rate (with min throttle and max speed) [m/s]. + float min_sink_rate; ///< Minimum sink rate (with min throttle and trim speed) [m/s]. float max_climb_rate; ///< Climb rate produced by max allowed throttle [m/s]. 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]. @@ -610,7 +611,8 @@ public: void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;}; void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; }; - void set_max_sink_rate(float sink_rate) { _control_param.max_sink_rate = sink_rate; _reference_param.max_sink_rate = sink_rate; }; + void set_max_sink_rate(float max_sink_rate) { _control_param.max_sink_rate = max_sink_rate; _reference_param.max_sink_rate = max_sink_rate; }; + void set_min_sink_rate(float min_sink_rate) { _control_param.min_sink_rate = min_sink_rate; }; 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; }; @@ -696,8 +698,9 @@ private: }; /// Control parameters. TECSControl::Param _control_param{ - .max_sink_rate = 2.0f, - .max_climb_rate = 2.0f, + .max_sink_rate = 5.0f, + .min_sink_rate = 2.0f, + .max_climb_rate = 5.0f, .vert_accel_limit = 0.0f, .equivalent_airspeed_trim = 15.0f, .tas_min = 3.0f, diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index d626e29585..4c4dfcb61a 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -119,6 +119,7 @@ FixedwingPositionControl::parameters_update() // TECS parameters _tecs.set_max_climb_rate(_param_fw_t_clmb_max.get()); _tecs.set_max_sink_rate(_param_fw_t_sink_max.get()); + _tecs.set_min_sink_rate(_param_fw_t_sink_min.get()); _tecs.set_speed_weight(_param_fw_t_spdweight.get()); _tecs.set_equivalent_airspeed_trim(_param_fw_airspd_trim.get()); _tecs.set_equivalent_airspeed_min(_param_fw_airspd_min.get());