mirror of https://github.com/ArduPilot/ardupilot
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:
parent
d6569d6f79
commit
47e3d1af02
|
@ -135,8 +135,8 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
|
||||||
AP_GROUPINFO("_WIND_MAX", 22, AP_Airspeed, _wind_max, 0),
|
AP_GROUPINFO("_WIND_MAX", 22, AP_Airspeed, _wind_max, 0),
|
||||||
|
|
||||||
// @Param: _WIND_WARN
|
// @Param: _WIND_WARN
|
||||||
// @DisplayName: Airspeed and ground speed difference that gives a warning
|
// @DisplayName: Airspeed and GPS 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.
|
// @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.
|
// @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
|
||||||
// @Units: m/s
|
// @Units: m/s
|
||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
|
|
|
@ -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;
|
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;
|
const bool data_is_implausible = is_positive(_wind_max) && speed_diff > _wind_max;
|
||||||
// update health_probability with LowPassFilter
|
// update health_probability with LowPassFilter
|
||||||
if (data_is_implausible || data_is_inconsistent) {
|
if (data_is_implausible || data_is_inconsistent) {
|
||||||
|
|
Loading…
Reference in New Issue