// return altitude difference in meters between current pressure and a
// given base_pressure in Pascal
float AP_Baro::get_altitude_difference(float base_pressure, float pressure) const
{
float ret;
#if HAL_CPU_CLASS <= HAL_CPU_CLASS_16
// on slower CPUs use a less exact, but faster, calculation
float scaling = base_pressure / pressure;
float temp = get_calibration_temperature() + 273.15f;
ret = logf(scaling) * temp * 29.271267f;
#else
// on faster CPUs use a more exact calculation
float scaling = pressure / base_pressure;
float temp = get_calibration_temperature() + 273.15f;
// This is an exact calculation that is within +-2.5m of the standard atmosphere tables
// in the troposphere (up to 11,000 m amsl).
ret = 153.8462f * temp * (1.0f - expf(0.190259f * logf(scaling)));
#endif
return ret;
}
/**
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
float pressure = get_pressure();
_ground_pressure.set(pressure);
float last_temperature = _ground_temperature;
_ground_temperature.set(get_calibration_temperature());
if (fabsf(last_temperature - _ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
// 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)
{
float altitude = get_altitude();
if ((fabsf(altitude - _last_altitude_EAS2TAS) < 100.0f) && !is_zero(_EAS2TAS)) {
// not enough change to require re-calculating
return _EAS2TAS;
}
float tempK = get_calibration_temperature() + 273.15f - 0.0065f * altitude;
_EAS2TAS = safe_sqrt(1.225f / ((float)get_pressure() / (287.26f * tempK)));
_last_altitude_EAS2TAS = altitude;
return _EAS2TAS;
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set_and_notify(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set_and_notify(get_calibration_temperature(i));
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
}
/*
update the barometer calibration
this updates the baro ground calibration to the current values. It
can be used before arming to keep the baro well calibrated
*/
void AP_Baro::update_calibration()
{
for (uint8_t i=0; i<_num_sensors; i++) {
if (healthy(i)) {
sensors[i].ground_pressure.set(get_pressure(i));
}
float last_temperature = sensors[i].ground_temperature;
sensors[i].ground_temperature.set(get_calibration_temperature(i));
// don't notify the GCS too rapidly or we flood the link
uint32_t now = AP_HAL::millis();
if (now - _last_notify_ms > 10000) {
sensors[i].ground_pressure.notify();
sensors[i].ground_temperature.notify();
_last_notify_ms = now;
}
if (fabsf(last_temperature - sensors[i].ground_temperature) > 3) {
// reset _EAS2TAS to force it to recalculate. This happens
// when a digital airspeed sensor comes online
_EAS2TAS = 0;
}
}
}
// calibrate the barometer. This must be called at least once before
// the altitude() or climb_rate() interfaces can be used
void AP_Baro::calibrate()
{
float ground_pressure = 0;
float ground_temperature = 0;
// reset the altitude offset when we calibrate. The altitude
// offset is supposed to be for within a flight
_alt_offset.set_and_save(0);
{
uint32_t tstart = hal.scheduler->millis();
while (ground_pressure == 0 || !_flags.healthy) {
read(); // Get initial data from absolute pressure sensor
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [1]\r\n"));
}
ground_pressure = get_pressure();
ground_temperature = get_calibration_temperature();
hal.scheduler->delay(20);
}
}
// let the barometer settle for a full second after startup
// the MS5611 reads quite a long way off for the first second,
// leading to about 1m of error if we don't wait
for (uint8_t i = 0; i < 10; i++) {
uint32_t tstart = hal.scheduler->millis();
do {
read();
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [2]\r\n"));
}
} while (!_flags.healthy);
ground_pressure = get_pressure();
ground_temperature = get_calibration_temperature();
hal.scheduler->delay(100);
}
// now average over 5 values for the ground pressure and
// temperature settings
for (uint16_t i = 0; i < 5; i++) {
uint32_t tstart = hal.scheduler->millis();
do {
read();
if (hal.scheduler->millis() - tstart > 500) {
hal.scheduler->panic(PSTR("PANIC: AP_Baro::read unsuccessful "
"for more than 500ms in AP_Baro::calibrate [3]\r\n"));
}
} while (!_flags.healthy);
ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f);
ground_temperature = (ground_temperature * 0.8f) +
(get_calibration_temperature() * 0.2f);
hal.scheduler->delay(100);
}
_ground_pressure.set_and_save(ground_pressure);
_ground_temperature.set_and_save(ground_temperature);
}
