diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b67b8b2082..2a68e135d9 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -570,17 +570,21 @@ float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const C // Specific total energy rate = _STE_rate_max is achieved when throttle is set to _throttle_setpoint_max // Specific total energy rate = 0 at cruise throttle // Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min + + // assume airspeed and density-independent delta_throttle to sink/climb rate mapping + // TODO: include air density for thrust mappings + const float throttle_increase_max_STE_rate = limit.STE_rate_max * (param.throttle_max - param.throttle_trim); + const float throttle_decrease_min_STE_rate = limit.STE_rate_min * (param.throttle_trim - param.throttle_min); + float throttle_predicted = 0.0f; if (ste_rate.setpoint >= FLT_EPSILON) { // throttle is between trim and maximum - throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_max * - (param.throttle_max - param.throttle_trim); + throttle_predicted = param.throttle_trim_adjusted + ste_rate.setpoint / throttle_increase_max_STE_rate; } else { // throttle is between trim and minimum - throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min * - (param.throttle_min - param.throttle_trim); + throttle_predicted = param.throttle_trim_adjusted - ste_rate.setpoint / throttle_decrease_min_STE_rate; } @@ -667,8 +671,8 @@ void TECS::initialize(const float altitude, const float altitude_rate, const flo void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, float throttle_min, float throttle_setpoint_max, - float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, - const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp) + float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate, + float target_sinkrate, const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp) { // Calculate the time since last update (seconds) @@ -684,6 +688,7 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _control_param.pitch_max = pitch_limit_max; _control_param.pitch_min = pitch_limit_min; _control_param.throttle_trim = throttle_trim; + _control_param.throttle_trim_adjusted = throttle_trim_adjusted; _control_param.throttle_max = throttle_setpoint_max; _control_param.throttle_min = throttle_min; diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 360c85cc1d..a7b80d3657 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -197,7 +197,8 @@ public: float tas_min; ///< True airpeed demand lower 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 given eas. + float throttle_trim; ///< Normalized throttle required to fly level at trim airspeed and sea level + float throttle_trim_adjusted; ///< Trim throttle adjusted for airspeed, load factor and air density float throttle_max; ///< Normalized throttle upper limit. float throttle_min; ///< Normalized throttle lower limit. @@ -578,8 +579,8 @@ public: */ void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, float throttle_min, float throttle_setpoint_max, - float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, - float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN); + float throttle_trim, float throttle_trim_adjusted, float pitch_limit_min, float pitch_limit_max, float target_climbrate, + float target_sinkrate, float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN); /** * @brief Initialize the control loop @@ -695,6 +696,7 @@ private: .pitch_max = 5.0f, .pitch_min = -5.0f, .throttle_trim = 0.0f, + .throttle_trim_adjusted = 0.f, .throttle_max = 1.0f, .throttle_min = 0.1f, .altitude_error_gain = 0.2f, diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp index d517dc7fc1..9fa4832ae8 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.cpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.cpp @@ -2458,9 +2458,25 @@ FixedwingPositionControl::reset_landing_state() } } -float FixedwingPositionControl::compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, - float throttle_max) +float FixedwingPositionControl::calculateTrimThrottle(float throttle_min, + float throttle_max, float airspeed_sp) { + float throttle_trim = + _param_fw_thr_trim.get(); // throttle required for level flight at trim airspeed, at sea level (standard atmosphere) + + // Drag modelling (parasite drag): calculate mapping airspeed-->throttle, assuming a linear relation with different gradient + // above and below trim. This is tunable thorugh FW_THR_ASPD_MIN and FW_THR_ASPD_MAX. + if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_min.get() > FLT_EPSILON + && airspeed_sp < _param_fw_airspd_trim.get()) { + throttle_trim = _param_fw_thr_trim.get() - (_param_fw_thr_trim.get() - _param_fw_thr_aspd_min.get()) / + (_param_fw_airspd_trim.get() - _param_fw_airspd_min.get()) * (_param_fw_airspd_trim.get() - airspeed_sp); + + } else if (PX4_ISFINITE(airspeed_sp) && _param_fw_thr_aspd_max.get() > FLT_EPSILON + && airspeed_sp > _param_fw_airspd_trim.get()) { + throttle_trim = _param_fw_thr_trim.get() + (_param_fw_thr_aspd_max.get() - _param_fw_thr_trim.get()) / + (_param_fw_airspd_max.get() - _param_fw_airspd_trim.get()) * (airspeed_sp - _param_fw_airspd_trim.get()); + } + float weight_ratio = 1.0f; if (_param_weight_base.get() > FLT_EPSILON && _param_weight_gross.get() > FLT_EPSILON) { @@ -2550,8 +2566,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva } /* update TECS vehicle state estimates */ - const float throttle_trim_comp = compensateTrimThrottleForDensityAndWeight(_param_fw_thr_trim.get(), throttle_min, - throttle_max); + const float throttle_trim_adjusted = calculateTrimThrottle(throttle_min, + throttle_max, airspeed_sp); // HOTFIX: the airspeed rate estimate using acceleration in body-forward direction has shown to lead to high biases // when flying tight turns. It's in this case much safer to just set the estimated airspeed rate to 0. @@ -2565,7 +2581,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva _eas2tas, throttle_min, throttle_max, - throttle_trim_comp, + _param_fw_thr_trim.get(), + throttle_trim_adjusted, pitch_min_rad - radians(_param_fw_psp_off.get()), pitch_max_rad - radians(_param_fw_psp_off.get()), desired_max_climbrate, @@ -2574,7 +2591,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva -_local_pos.vz, hgt_rate_sp); - tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_comp); + tecs_status_publish(alt_sp, airspeed_sp, airspeed_rate_estimate, throttle_trim_adjusted); } float diff --git a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp index 7f27717206..c67e1a5d06 100644 --- a/src/modules/fw_path_navigation/FixedwingPositionControl.hpp +++ b/src/modules/fw_path_navigation/FixedwingPositionControl.hpp @@ -689,14 +689,15 @@ private: void set_control_mode_current(const hrt_abstime &now); /** - * @brief Compensate trim throttle for air density and vehicle weight. + * @brief Estimate trim throttle for air density, vehicle weight and current airspeed * - * @param trim throttle required at sea level during standard conditions. * @param throttle_min Minimum allowed trim throttle. * @param throttle_max Maximum allowed trim throttle. - * @return Trim throttle compensated for air density and vehicle weight. + * @param airspeed_sp Current airspeed setpoint (CAS) [m/s] + * @return Estimated trim throttle */ - float compensateTrimThrottleForDensityAndWeight(float throttle_trim, float throttle_min, float throttle_max); + float calculateTrimThrottle(float throttle_min, float throttle_max, + float airspeed_sp); void publishOrbitStatus(const position_setpoint_s pos_sp); @@ -906,6 +907,9 @@ private: (ParamFloat) _param_speed_rate_standard_dev, (ParamFloat) _param_process_noise_standard_dev, + (ParamFloat) _param_fw_thr_aspd_min, + (ParamFloat) _param_fw_thr_aspd_max, + (ParamFloat) _param_fw_thr_trim, (ParamFloat) _param_fw_thr_idle, (ParamFloat) _param_fw_thr_max, diff --git a/src/modules/fw_path_navigation/fw_path_navigation_params.c b/src/modules/fw_path_navigation/fw_path_navigation_params.c index 9e2792c928..48fea5fe69 100644 --- a/src/modules/fw_path_navigation/fw_path_navigation_params.c +++ b/src/modules/fw_path_navigation/fw_path_navigation_params.c @@ -1063,3 +1063,33 @@ PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f); * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f); + +/** + * Throttle at min airspeed + * + * Required throttle for level flight at minimum airspeed FW_AIRSPD_MIN (sea level, standard atmosphere) + * + * Set to 0 to disable mapping of airspeed to trim throttle below FW_AIRSPD_TRIM. + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_THR_ASPD_MIN, 0.f); + +/** + * Throttle at max airspeed + * + * Required throttle for level flight at maximum airspeed FW_AIRSPD_MAX (sea level, standard atmosphere) + * + * Set to 0 to disable mapping of airspeed to trim throttle. + * + * @min 0 + * @max 1 + * @decimal 2 + * @increment 0.01 + * @group FW TECS + */ +PARAM_DEFINE_FLOAT(FW_THR_ASPD_MAX, 0.f);