fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 04:31:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW Position Control: add Eco mode in TECS and separate Eco climbrate

Browse Source 
Separate settings in Eco mode (beside airspeed):
- FW_T_CLMB_R_SP_E (slower climb for more efficiency)
- FW_T_ALT_TC_E (larger to reduce motor action due to altitude errors)
- FW_T_SPDWEIGHT_E (more weight to airspeed to reduce motor action due to altitude errors)

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-25 09:50:28 +01:00
parent 024777ec7c
commit 55809e809c
6 changed files with 77 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
msg/tecs_status.msg
+1
View File
@@ -6,6 +6,7 @@ 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_ECO = 7
float32 altitude_sp
src/lib/tecs/TECS.cpp
+14 -1
View File
@@ -586,7 +586,7 @@ void TECS::_update_STE_rate_lim()
void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max,
float target_climbrate, float target_sinkrate, float hgt_rate_sp)
float target_climbrate, float target_sinkrate, float hgt_rate_sp, bool eco_mode_enabled)
{
// Calculate the time since last update (seconds)
uint64_t now = hrt_absolute_time();
@@ -623,6 +623,8 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
// Detect an uncommanded descent caused by an unachievable airspeed demand
_detect_uncommanded_descent();
_eco_mode_enabled = eco_mode_enabled;
// Calculate the demanded true airspeed
_update_speed_setpoint();
@@ -652,6 +654,9 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
} else if (_climbout_mode_active) {
_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
} else if (_eco_mode_enabled) {
_tecs_mode = ECL_TECS_MODE_ECO;
} else {
// This is the default operation mode
_tecs_mode = ECL_TECS_MODE_NORMAL;
@@ -662,6 +667,11 @@ void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_set
void TECS::_update_speed_height_weights()
{
if (_eco_mode_enabled) {
_pitch_speed_weight = _pitch_speed_weight_eco;
_height_error_gain = _height_error_gain_eco;
}
// Calculate the weight applied to control of specific kinetic energy error
_SKE_weighting = constrain(_pitch_speed_weight, 0.0f, 2.0f);
@@ -699,6 +709,9 @@ TECS::tecs_status_publish(const hrt_abstime &now)
case TECS::ECL_TECS_MODE_CLIMBOUT:
tecs_status.mode = tecs_status_s::TECS_MODE_CLIMBOUT;
break;
case TECS::ECL_TECS_MODE_ECO:
tecs_status.mode = tecs_status_s::TECS_MODE_ECO;
}
tecs_status.altitude_sp = _hgt_setpoint;
src/lib/tecs/TECS.hpp
+11 -2
View File
@@ -94,7 +94,8 @@ public:
void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint,
float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout,
float throttle_min, float throttle_setpoint_max, float throttle_cruise,
float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN);
float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate, float hgt_rate_sp = NAN,
bool eco_mode_enabled = false);
void reset_state() { _states_initialized = false; }
@@ -102,7 +103,8 @@ public:
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_ECO
};
void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; }
@@ -140,6 +142,9 @@ public:
void set_seb_rate_ff_gain(float ff_gain) { _SEB_rate_ff = ff_gain; }
// eco mode settings
void set_speed_weight_eco(float weight_eco) { _pitch_speed_weight_eco = weight_eco; }
void set_height_error_time_constant_eco(float time_const_eco) { _height_error_gain_eco = 1.0f / math::max(time_const_eco, 0.1f); }
// getters
float get_throttle_setpoint() { return _last_throttle_setpoint; }
@@ -210,6 +215,9 @@ private:
float _speed_derivative_time_const{0.01f}; ///< speed derivative filter time constant (s)
float _SEB_rate_ff{1.0f};
float _height_error_gain_eco{0.2f}; ///< in eco mode: height error inverse time constant [1/s]
float _pitch_speed_weight_eco{1.0f}; ///< in eco mode: speed control weighting used by pitch demand calculation
// complimentary filter states
float _vert_vel_state{0.0f}; ///< complimentary filter state - height rate (m/sec)
float _vert_pos_state{0.0f}; ///< complimentary filter state - height (m)
@@ -280,6 +288,7 @@ private:
bool _airspeed_enabled{false}; ///< true when airspeed use has been enabled
bool _states_initialized{false}; ///< true when TECS states have been iniitalized
bool _in_air{false}; ///< true when the vehicle is flying
bool _eco_mode_enabled{false};
// flight phase
int _flight_phase{tecs_status_s::TECS_FLIGHT_PHASE_LEVEL};
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+10 -2
View File
@@ -131,6 +131,9 @@ FixedwingPositionControl::parameters_update()
_tecs.set_speed_derivative_time_constant(_param_tas_rate_time_const.get());
_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
_tecs.set_speed_weight_eco(_param_fw_t_spdweight_eco.get());
_tecs.set_height_error_time_constant_eco(_param_fw_t_h_error_tc_eco.get());
// Landing slope
/* check if negative value for 2/3 of flare altitude is set for throttle cut */
@@ -662,7 +665,8 @@ FixedwingPositionControl::getManualHeightRateSetpoint()
} else if (_manual_control_setpoint_altitude < - deadBand) {
/* pitching up */
float pitch = -(_manual_control_setpoint_altitude + deadBand) / factor;
const float climb_rate_target = _param_climbrate_target.get();
const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ?
_param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get();
height_rate_setpoint = pitch * climb_rate_target;
}
@@ -2261,6 +2265,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
}
}
const float climb_rate_target = _speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO ?
_param_fw_t_clmb_r_sp_eco.get() : _param_climbrate_target.get();
_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
_current_altitude, alt_sp,
airspeed_sp, _airspeed, _eas2tas,
@@ -2269,7 +2276,8 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
throttle_min, throttle_max, throttle_cruise,
pitch_min_rad - radians(_param_fw_psp_off.get()),
pitch_max_rad - radians(_param_fw_psp_off.get()),
_param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
climb_rate_target, _param_sinkrate_target.get(), hgt_rate_sp,
_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO);
}
void FixedwingPositionControl::publishOrbitStatus(const position_setpoint_s pos_sp)
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+4 -1
View File
@@ -471,7 +471,10 @@ private:
(ParamInt<px4::params::FW_SPD_MODE_SET>) _fw_spd_mode_set,
(ParamFloat<px4::params::FW_ALT_MIN>) fw_alt_min,
(ParamFloat<px4::params::FW_ALT_ERR_U>) fw_alt_err_u,
(ParamFloat<px4::params::FW_ALT_ERR_O>) fw_alt_err_o
(ParamFloat<px4::params::FW_ALT_ERR_O>) fw_alt_err_o,
(ParamFloat<px4::params::FW_T_ALT_TC_E>) _param_fw_t_h_error_tc_eco,
(ParamFloat<px4::params::FW_T_SPDWEIGHT_E>) _param_fw_t_spdweight_eco,
(ParamFloat<px4::params::FW_T_CLMB_R_SP_E>) _param_fw_t_clmb_r_sp_eco
)
};
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+37
View File
@@ -928,3 +928,40 @@ PARAM_DEFINE_FLOAT(FW_ALT_ERR_O, 20.f);
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_ALT_MIN, 50.f);
/**
* Eco Mode: Speed <--> Altitude priority
*
* @min 0.0
* @max 2.0
* @decimal 1
* @increment 1.0
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT_E, 1.8f);
/**
* Eco Mode: Altitude error time constant.
*
* Normally larger than FW_T_ALT_TC to have looser altitude control and in turn less throttle changes.
*
* @min 2.0
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_ALT_TC_E, 10.0f);
/**
* Eco target climbrate.
*
* The rate at which the vehicle will climb in autonomous modes to achieve altitude setpoints in Eco mode.
*
* @unit m/s
* @min 0.5
* @max 15
* @decimal 2
* @increment 0.01
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_T_CLMB_R_SP_E, 2.0f);