AP_Baro: minor improvements to Baro HIL code health check

This commit is contained in:
Andrew Tridgell 2014-07-22 18:07:48 +10:00
parent 0df2dd9092
commit a458f1bf5f
2 changed files with 12 additions and 7 deletions

View File

@ -125,7 +125,8 @@ void AP_Baro::calibrate()
*/
void AP_Baro::update_calibration()
{
_ground_pressure.set(get_pressure());
float pressure = get_pressure();
_ground_pressure.set(pressure);
_ground_temperature.set(get_temperature());
}
@ -166,7 +167,8 @@ float AP_Baro::get_altitude(void)
return _altitude + _alt_offset;
}
_altitude = get_altitude_difference(_ground_pressure, get_pressure());
float pressure = get_pressure();
_altitude = get_altitude_difference(_ground_pressure, pressure);
_last_altitude_t = _last_update;

View File

@ -9,6 +9,7 @@ extern const AP_HAL::HAL& hal;
// Public Methods //////////////////////////////////////////////////////////////
bool AP_Baro_HIL::init()
{
healthy = false;
return true;
}
@ -35,12 +36,14 @@ uint8_t AP_Baro_HIL::read()
void AP_Baro_HIL::setHIL(float pressure, float temperature)
{
if (pressure > 0) {
_count = 1;
_pressure_sum = pressure;
_temperature_sum = temperature;
_last_update = hal.scheduler->millis();
healthy = true;
}
}
// ==========================================================================