Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: Improve ARSPD_WIND_MAX behavior by handling 3D speed 

* The previous method negated earth-frame vertical velocity
* In a steep dive on a plane, the 2D ground speed assumption of speed breaks down
* Use 3D speed always
* If only a 2D fix is avialable, then vertical velocity should be 0

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-04-15 11:31:10 -06:00 committed by Peter Barker
parent d6569d6f79
commit 47e3d1af02
2 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -135,8 +135,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
AP_GROUPINFO("_WIND_MAX", 22, AP_Airspeed, _wind_max, 0),
// @Param: _WIND_WARN
// @DisplayName: Airspeed and ground speed difference that gives a warning
// @Description: If the difference between airspeed and ground speed is greater than this value the sensor will issue a warning. If 0 ARSPD_WIND_MAX is used.
// @DisplayName: Airspeed and GPS speed difference that gives a warning
// @Description: If the difference between airspeed and GPS speed is greater than this value the sensor will issue a warning. If 0 ARSPD_WIND_MAX is used.
// @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
// @Units: m/s
// @User: Advanced

5
libraries/AP_Airspeed/AP_Airspeed_Health.cpp
View File

@ -64,8 +64,9 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
}
data_is_inconsistent = state[i].failures.test_ratio > gate_size;
}
const float speed_diff = fabsf(state[i].airspeed-gps.ground_speed());
const auto gps_speed = gps.velocity().length();
const float speed_diff = fabsf(state[i].airspeed-gps_speed);
const bool data_is_implausible = is_positive(_wind_max) && speed_diff > _wind_max;
// update health_probability with LowPassFilter
if (data_is_implausible || data_is_inconsistent) {