diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 20b05e37e3..97697f4a3e 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -76,7 +76,7 @@ void AP_Baro::calibrate() "for more than 500ms in AP_Baro::calibrate [1]\r\n")); } ground_pressure = get_pressure(); - ground_temperature = get_temperature(); + ground_temperature = get_calibration_temperature(); hal.scheduler->delay(20); } } @@ -93,7 +93,7 @@ void AP_Baro::calibrate() } } while (!_flags.healthy); ground_pressure = get_pressure(); - ground_temperature = get_temperature(); + ground_temperature = get_calibration_temperature(); hal.scheduler->delay(100); } @@ -111,7 +111,7 @@ void AP_Baro::calibrate() } while (!_flags.healthy); ground_pressure = (ground_pressure * 0.8f) + (get_pressure() * 0.2f); ground_temperature = (ground_temperature * 0.8f) + - (get_temperature() * 0.2f); + (get_calibration_temperature() * 0.2f); hal.scheduler->delay(100); } @@ -129,7 +129,13 @@ void AP_Baro::update_calibration() { float pressure = get_pressure(); _ground_pressure.set(pressure); - _ground_temperature.set(get_temperature()); + 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 altitude difference in meters between current pressure and a @@ -215,3 +221,33 @@ float AP_Baro::get_climb_rate(void) return _climb_rate_filter.slope() * 1.0e3f; } + +/* + set external temperature to be used for calibration (degrees C) + */ +void AP_Baro::set_external_temperature(float temperature) +{ + _external_temperature = temperature; + _last_external_temperature_ms = hal.scheduler->millis(); +} + +/* + get the temperature in degrees C to be used for calibration purposes + */ +float AP_Baro::get_calibration_temperature(void) const +{ + // if we have a recent external temperature then use it + if (_last_external_temperature_ms != 0 && hal.scheduler->millis() - _last_external_temperature_ms < 10000) { + return _external_temperature; + } + // if we don't have an external temperature then use the minimum + // of the barometer temperature and 25 degrees C. The reason for + // not just using the baro temperature is it tends to read high, + // often 30 degrees above the actual temperature. That means the + // EAS2TAS tends to be off by quite a large margin + float ret = get_temperature(); + if (ret > 25) { + ret = 25; + } + return ret; +} diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index 43859b71dd..3357695cbb 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -16,7 +16,8 @@ public: _altitude(0.0f), _last_altitude_EAS2TAS(0.0f), _EAS2TAS(0.0f), - _last_altitude_t(0) + _last_altitude_t(0), + _last_external_temperature_ms(0) { // initialise flags _flags.healthy = false; @@ -82,6 +83,11 @@ public: return _ground_pressure.get(); } + // set the temperature to be used for altitude calibration. This + // allows an external temperature source (such as a digital + // airspeed sensor) to be used as the temperature source + void set_external_temperature(float temperature); + // get last time sample was taken (in ms) uint32_t get_last_update() const { return _last_update; }; @@ -98,12 +104,18 @@ protected: uint8_t _pressure_samples; private: + // get the temperature to be used for altitude calibration + float get_calibration_temperature(void) const; + + AP_Float _ground_temperature; AP_Float _ground_pressure; AP_Int8 _alt_offset; float _altitude; float _last_altitude_EAS2TAS; float _EAS2TAS; + float _external_temperature; + uint32_t _last_external_temperature_ms; uint32_t _last_altitude_t; DerivativeFilterFloat_Size7 _climb_rate_filter; };