diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 53d850c9be..a118f09ad5 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -136,6 +136,11 @@ float AP_Baro::get_altitude(void) { float scaling, temp; + if (_ground_pressure == 0) { + // called before initialisation + return 0; + } + if (_last_altitude_t == _last_update) { // no new information return _altitude + _alt_offset; diff --git a/libraries/AP_Baro/AP_Baro_HIL.cpp b/libraries/AP_Baro/AP_Baro_HIL.cpp index 9692d0bc4b..a3bb827ced 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.cpp +++ b/libraries/AP_Baro/AP_Baro_HIL.cpp @@ -3,6 +3,7 @@ #include #include "AP_Baro_HIL.h" #include + extern const AP_HAL::HAL& hal; // Public Methods ////////////////////////////////////////////////////////////// @@ -21,8 +22,8 @@ uint8_t AP_Baro_HIL::read() if (_count != 0) { hal.scheduler->suspend_timer_procs(); result = 1; - Press = ((float)_pressure_sum) / _count; - Temp = ((float)_temperature_sum) / _count; + Press = _pressure_sum / _count; + Temp = _temperature_sum / _count; _pressure_samples = _count; _count = 0; _pressure_sum = 0; diff --git a/libraries/AP_Baro/AP_Baro_HIL.h b/libraries/AP_Baro/AP_Baro_HIL.h index 9607f63944..4e34279912 100644 --- a/libraries/AP_Baro/AP_Baro_HIL.h +++ b/libraries/AP_Baro/AP_Baro_HIL.h @@ -11,8 +11,8 @@ private: uint8_t BMP085_State; float Temp; float Press; - int32_t _pressure_sum; - int32_t _temperature_sum; + float _pressure_sum; + float _temperature_sum; volatile uint8_t _count; public: