mirror of https://github.com/ArduPilot/ardupilot
AP_Airspeed : Fixes bug that caused airspeed calibration to be sent a zero airspeed
This bug resulted in the airspeed ratio going to the maximum value of 4 and staying there. This could lead to a very slow flying model and a stall.
This commit is contained in:
parent
5a92ce4af6
commit
71de4ddcd0
|
@ -170,6 +170,7 @@ void AP_Airspeed::read(void)
|
|||
}
|
||||
airspeed_pressure = get_pressure();
|
||||
airspeed_pressure = max(airspeed_pressure - _offset, 0);
|
||||
_last_pressure = airspeed_pressure;
|
||||
_raw_airspeed = sqrtf(airspeed_pressure * _ratio);
|
||||
_airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed;
|
||||
}
|
||||
|
|
|
@ -106,7 +106,7 @@ public:
|
|||
// return the differential pressure in Pascal for the last
|
||||
// airspeed reading. Used by the calibration code
|
||||
float get_differential_pressure(void) const {
|
||||
return max(_last_pressure - _offset, 0);
|
||||
return max(_last_pressure, 0);
|
||||
}
|
||||
|
||||
// set the apparent to true airspeed ratio
|
||||
|
|
Loading…
Reference in New Issue