mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: added get_EAS2TAS()
this provides the scaling factor between equivalent and true airspeed based on altitude
This commit is contained in:
parent
61361dac9b
commit
7bdb098e77
|
@ -131,6 +131,22 @@ float AP_Baro::get_altitude(void)
|
|||
return _altitude;
|
||||
}
|
||||
|
||||
// return current scale factor that converts from equivalent to true airspeed
|
||||
// valid for altitudes up to 10km AMSL
|
||||
// assumes standard atmosphere lapse rate
|
||||
float AP_Baro::get_EAS2TAS(void)
|
||||
{
|
||||
if ((abs(_altitude - _last_altitude_EAS2TAS) < 100.0f) && (_EAS2TAS != 0.0f)) {
|
||||
// not enough change to require re-calculating
|
||||
return _EAS2TAS;
|
||||
}
|
||||
|
||||
float tempK = ((float)_ground_temperature) + 273.15f - 0.0065f * _altitude;
|
||||
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
|
||||
_last_altitude_EAS2TAS = _altitude;
|
||||
return _EAS2TAS;
|
||||
}
|
||||
|
||||
// return current climb_rate estimeate relative to time that calibrate()
|
||||
// was called. Returns climb rate in meters/s, positive means up
|
||||
// note that this relies on read() being called regularly to get new data
|
||||
|
|
|
@ -36,6 +36,9 @@ public:
|
|||
// of the last calibrate() call
|
||||
float get_altitude(void);
|
||||
|
||||
// get scale factor required to convert equivalent to true airspeed
|
||||
float get_EAS2TAS(void);
|
||||
|
||||
// return how many pressure samples were used to obtain
|
||||
// the last pressure reading
|
||||
uint8_t get_pressure_samples(void) {
|
||||
|
@ -67,6 +70,8 @@ private:
|
|||
AP_Float _ground_temperature;
|
||||
AP_Float _ground_pressure;
|
||||
float _altitude;
|
||||
float _last_altitude_EAS2TAS;
|
||||
float _EAS2TAS;
|
||||
uint32_t _last_altitude_t;
|
||||
DerivativeFilterFloat_Size7 _climb_rate_filter;
|
||||
};
|
||||
|
|
Loading…
Reference in New Issue