fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

tecs: use FW_T_SINK_MIN for STE_rate_min (#21190)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-02-24 15:46:41 +01:00 committed by GitHub
parent 33b54f7c57
commit 837095b9a8
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 10 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/tecs/TECS.cpp
View File

@ -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;
}

13
src/lib/tecs/TECS.hpp
View File

@ -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,

1
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -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());