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

FW Position Control: bit of clean up after review

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-25 09:50:28 +01:00
parent 4fab9c8345
commit b0b8055f5c
4 changed files with 36 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+4
View File
@@ -714,6 +714,10 @@ TECS::tecs_status_publish(const hrt_abstime &now)
case TECS::ECL_TECS_MODE_ECO:
tecs_status.mode = tecs_status_s::TECS_MODE_ECO;
break;
default:
tecs_status.mode = tecs_status_s::TECS_MODE_NORMAL;
}
tecs_status.altitude_sp = _hgt_setpoint;
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+25 -32
View File
@@ -448,6 +448,21 @@ FixedwingPositionControl::getSpeedMode()
{
FW_SPEED_MODE_COMMANDED new_mode = static_cast<FW_SPEED_MODE_COMMANDED>(_param_fw_spd_mode_set.get());
const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - _param_fw_spdm_alt_er_u.get(),
_local_pos.ref_alt + _param_fw_alt_spdm_min.get());
const float altitude_amsl_max = _tecs.get_hgt_setpoint() + _param_fw_spdm_alt_er_o.get();
const bool altitude_conditions_met = _current_altitude <= altitude_amsl_max
&& _current_altitude >= altitude_amsl_min;
if (!altitude_conditions_met) {
// reset timer
_time_conditions_not_met = _local_pos.timestamp;
}
// add a 10s timeout after conditions where not met
const bool conditions_for_eco_dash_met = hrt_elapsed_time(&_time_conditions_not_met) > 10_s;
FW_SPEED_MODE speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL; //default;
switch (new_mode) {
@@ -456,7 +471,7 @@ FixedwingPositionControl::getSpeedMode()
break;
case ECO_CRUISE:
if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
if (conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO;
} else {
@@ -466,7 +481,7 @@ FixedwingPositionControl::getSpeedMode()
break;
case ECO_FULL:
if (_conditions_for_eco_dash_met) {
if (conditions_for_eco_dash_met) {
speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_ECO;
} else {
@@ -476,7 +491,7 @@ FixedwingPositionControl::getSpeedMode()
break;
case DASH_CRUISE:
if (_conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
if (conditions_for_eco_dash_met && _tecs.get_flight_phase() == tecs_status_s::TECS_FLIGHT_PHASE_LEVEL) {
speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH;
} else {
@@ -486,7 +501,7 @@ FixedwingPositionControl::getSpeedMode()
break;
case DASH_FULL:
if (_conditions_for_eco_dash_met) {
if (conditions_for_eco_dash_met) {
speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_DASH;
} else {
@@ -500,29 +515,8 @@ FixedwingPositionControl::getSpeedMode()
}
void
FixedwingPositionControl::check_eco_dash_allowed()
FixedwingPositionControl::resetEcoDashAllowed()
{
const float altitude_amsl_min = max(_tecs.get_hgt_setpoint() - _param_fw_spdm_alt_er_u.get(),
_local_pos.ref_alt + _param_fw_alt_spdm_min.get());
const float altitude_amsl_max = _tecs.get_hgt_setpoint() + _param_fw_spdm_alt_er_o.get();
const bool altitdue_conditions_met = _current_altitude <= altitude_amsl_max
&& _current_altitude >= altitude_amsl_min;
// add a 10s timeout after conditions where not met
_conditions_for_eco_dash_met = altitdue_conditions_met && hrt_elapsed_time(&_time_conditions_not_met) > 10_s;
if (!altitdue_conditions_met) {
// reset timer
_time_conditions_not_met = _local_pos.timestamp;
}
}
void
FixedwingPositionControl::resetAutoSpeedAdaptions()
{
_conditions_for_eco_dash_met = false;
_time_conditions_not_met = _local_pos.timestamp;
}
@@ -795,7 +789,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
/* get circle mode */
const bool was_circle_mode = _l1_control.circle_mode();
check_eco_dash_allowed();
FW_SPEED_MODE spd_mode = getSpeedMode();
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
@@ -2039,25 +2032,25 @@ FixedwingPositionControl::Run()
case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
resetAutoSpeedAdaptions();
resetEcoDashAllowed();
break;
}
case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
control_auto_descend(_local_pos.timestamp);
resetAutoSpeedAdaptions();
resetEcoDashAllowed();
break;
}
case FW_POSCTRL_MODE_MANUAL_POSITION: {
control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
resetAutoSpeedAdaptions();
resetEcoDashAllowed();
break;
}
case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
resetAutoSpeedAdaptions();
resetEcoDashAllowed();
break;
}
@@ -2068,7 +2061,7 @@ FixedwingPositionControl::Run()
reset_takeoff_state();
}
resetAutoSpeedAdaptions();
resetEcoDashAllowed();
_att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get());
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+2 -3
View File
@@ -282,7 +282,7 @@ private:
DASH_FULL
};
bool _conditions_for_eco_dash_met{false};
// bool _conditions_for_eco_dash_met{false};
hrt_abstime _time_conditions_not_met{0};
float _last_airspeed_setpoint{NAN};
@@ -376,10 +376,9 @@ private:
void publishOrbitStatus(const position_setpoint_s pos_sp);
void check_eco_dash_allowed();
FW_SPEED_MODE getSpeedMode();
void update_wind_mode();
void resetAutoSpeedAdaptions();
void resetEcoDashAllowed();
/*
* Call TECS : a wrapper function to call the TECS implementation
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+5 -5
View File
@@ -859,7 +859,7 @@ PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
* Wind-based airspeed scaling factor in Eco
*
* Multiplying this factor with the current absolute wind estimate gives the airspeed offset
* added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode. This helps to make the
* added to the setpoint (which is FW_AIRSPD_MIN) in Eco mode (see FW_SPD_MODE_SET). This helps to make the
* system more robust against disturbances in high wind.
*
* setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind
@@ -893,7 +893,7 @@ PARAM_DEFINE_INT32(FW_SPD_MODE_SET, 0);
/**
* Max altitude undershoot for Eco/Dash
*
* Eco/Dash mode is disabled if the current altitude is more than this value below the setpoint.
* Eco/Dash mode is disabled if the current altitude overshoots the setpoint by this value.
*
* @min 1.0
* @max 50.0
@@ -906,7 +906,7 @@ PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_U, 10.f);
/**
* Max altitude overshoot for Eco/Dash
*
* Eco/Dash mode is disabled if the current altitude is more than this value above the setpoint.
* Eco/Dash mode is disabled if the current altitude undershoots the setpoint by this value.
*
* @min 1.0
* @max 50.0
@@ -917,7 +917,7 @@ PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_U, 10.f);
PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_O, 20.f);
/**
* Min altitude for Eco/Dash
* Minimum height above home for Eco/Dash
*
* Eco/Dash mode can be enabled if above this relative altitude to home.
*
@@ -930,7 +930,7 @@ PARAM_DEFINE_FLOAT(FW_SPDM_ALT_ER_O, 20.f);
PARAM_DEFINE_FLOAT(FW_SPDM_ALT_MIN, 50.f);
/**
* Eco Mode: Speed <--> Altitude priority
* Speed <--> Altitude priority in Eco mode
*
* @min 0.0
* @max 2.0