fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-10 12:11:36 +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
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;