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

FW Position Control: add airspeed offset based on wind in Eco mode

Browse Source 
setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2021-11-25 09:50:28 +01:00
parent 8c42b7178d
commit 024777ec7c
3 changed files with 23 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+13 -42
View File
@@ -364,6 +364,19 @@ FixedwingPositionControl::get_cruise_airspeed_setpoint(const hrt_abstime &now, c
if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_ECO) {
airspeed_setpoint = _param_fw_airspd_min.get();
// Adapt cruise airspeed setpoint based on wind estimate (disable in airspeed-less mode)
if (_airspeed_valid && _param_fw_wind_arsp_sc.get() > FLT_EPSILON) {
wind_s wind_estimate;
if (_wind_sub.update(&wind_estimate)) {
const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
if (PX4_ISFINITE(wind.length())) {
airspeed_setpoint += _param_fw_wind_arsp_sc.get() * wind.length();
}
}
}
} else if (_speed_mode_current == FW_SPEED_MODE::FW_SPEED_MODE_DASH) {
airspeed_setpoint = _param_fw_airspd_max.get();
}
@@ -504,47 +517,6 @@ FixedwingPositionControl::resetAutoSpeedAdaptions()
_speed_mode_current = FW_SPEED_MODE::FW_SPEED_MODE_NORMAL;
}
void
FixedwingPositionControl::update_wind_mode()
{
/* If the corresponding wind threshold values are set, the wind state will change if these values are
* passed for a certain amount of time. The wind state is then used to increase airspeed (high wind),
* resp. decrease (low wind). It takes longer to move to a lower wind state than to a higher,
* as in general higher airspeed is safer. */
wind_s wind_estimate;
if (_wind_sub.update(&wind_estimate)) {
const matrix::Vector2f wind(wind_estimate.windspeed_north, wind_estimate.windspeed_east);
FW_WIND_MODE fw_wind_mode_detected(FW_WIND_MODE_NORMAL);
if (_param_fw_wind_thld_h.get() > FLT_EPSILON && wind.length() > _param_fw_wind_thld_h.get()) {
fw_wind_mode_detected = FW_WIND_MODE_HIGH;
} else if (_param_fw_wind_thld_l.get() > FLT_EPSILON && wind.length() < _param_fw_wind_thld_l.get()) {
fw_wind_mode_detected = FW_WIND_MODE_LOW;
}
if (fw_wind_mode_detected != _fw_wind_mode_detected_prev) {
_first_time_current_mode_detected = hrt_absolute_time();
}
const float min_time_detection_mode_down = 60.0f; // min time to switch to lower wind state
const float time_since_new_mode_detected = hrt_elapsed_time(&_first_time_current_mode_detected) * 1e-6f;
// immediately switch to higher wind mode after detection, but require minimum time within lower wind to switch to lower mode
if (fw_wind_mode_detected > _fw_wind_mode_current) {
_fw_wind_mode_current = fw_wind_mode_detected;
} else if (fw_wind_mode_detected < _fw_wind_mode_current
&& time_since_new_mode_detected > min_time_detection_mode_down) {
_fw_wind_mode_current = fw_wind_mode_detected;
}
_fw_wind_mode_detected_prev = fw_wind_mode_detected;
}
}
void
FixedwingPositionControl::status_publish()
{
@@ -816,7 +788,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
check_eco_dash_allowed();
updateSpeedMode();
update_wind_mode();
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_param_fw_t_spdweight.get());