fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 02:11:36 +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());
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
+1 -14
View File
@@ -285,17 +285,6 @@ private:
bool _conditions_for_eco_dash_met{false};
hrt_abstime _time_conditions_not_met{0};
// wind state
hrt_abstime _first_time_current_mode_detected{0}; ///< last time in normal wind mode
enum FW_WIND_MODE {
FW_WIND_MODE_LOW,
FW_WIND_MODE_NORMAL,
FW_WIND_MODE_HIGH,
} _fw_wind_mode_current{FW_WIND_MODE_NORMAL}; ///< used to make wind-based airspeed setpoint adaptions
FW_WIND_MODE _fw_wind_mode_detected_prev{FW_WIND_MODE_NORMAL};
float _last_airspeed_setpoint{NAN};
param_t _param_handle_airspeed_trans{PARAM_INVALID};
@@ -477,9 +466,7 @@ private:
(ParamFloat<px4::params::FW_TKO_PITCH_MIN>) _takeoff_pitch_min,
//Speed mode params
(ParamFloat<px4::params::FW_WIND_THLD_H>) _param_fw_wind_thld_h,
(ParamFloat<px4::params::FW_WIND_THLD_L>) _param_fw_wind_thld_l,
(ParamFloat<px4::params::FW_WIND_ARSP_OF>) _param_fw_wind_arsp_of,
(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
(ParamInt<px4::params::FW_SPD_MODE_SET>) _fw_spd_mode_set,
(ParamFloat<px4::params::FW_ALT_MIN>) fw_alt_min,
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+9 -41
View File
@@ -856,54 +856,22 @@ PARAM_DEFINE_INT32(FW_GPSF_LT, 30);
PARAM_DEFINE_FLOAT(FW_GPSF_R, 15.0f);
/**
* High wind threshold
* Wind-based airspeed scaling factor in Eco
*
* If the current wind estimate is above this threshold,the value of FW_WIND_ARSP_OF
* is added to the cruise airspeed setpoint.
* Set to zero number to disable wind-based airspeed setpoint up-scaling.
* 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
* system more robust against disturbances in high wind.
*
* @unit m/s
* @min 0.0
* @max 30
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_WIND_THLD_H, 0.0f);
/**
* Low wind threshold
* setpoint = FW_AIRSPD_MIN + FW_LND_AIRSPD_SC * wind
*
* If the current wind estimate is below this threshold for 60 seconds,the value of
* FW_WIND_ARSP_OF is subtracted from the cruise airspeed setpoint.
* Set to zero to disable wind-based airspeed setpoint down-scaling.
*
* @unit m/s
* @unit norm
* @min 0
* @max 30
* @decimal 1
* @increment 0.5
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_WIND_THLD_L, 0.0f);
/**
* Wind-based airspeed setpoint offset
*
* Airspeed setpoint offset that is added/subtracted to the cruise airspeed setpoint if wind-based setpoint
* adaption is enabled for either or both high and low winds (FW_WIND_THLD_H and FW_WIND_THLD_L).
* If in high wind condition, this offset is added to the cruise airspeed setpoint.
* If in low wind condition, this offset is subtracted from the cruise airspeed setpoint.
* Note: manual airspeed setpoints via sticks must be disabled for wind-based adaption to work (see FW_POS_STK_CONF).
*
* @unit m/s
* @min 0
* @max 10
* @decimal 1
* @max 2
* @decimal 2
* @increment 0.01
* @group FW TECS
*/
PARAM_DEFINE_FLOAT(FW_WIND_ARSP_OF, 1.0f);
PARAM_DEFINE_FLOAT(FW_WIND_ARSP_SC, 0.0f);
/**
* FW Speed mode setting