diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 58d85376b3..6bd921e3c1 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -66,7 +66,8 @@ AP_Baro::AP_Baro() : _last_altitude_EAS2TAS(0.0f), _EAS2TAS(0.0f), _external_temperature(0.0f), - _last_external_temperature_ms(0) + _last_external_temperature_ms(0), + _hil_mode(false) { memset(sensors, 0, sizeof(sensors)); @@ -289,8 +290,10 @@ void AP_Baro::init(void) */ void AP_Baro::update(void) { - for (uint8_t i=0; i<_num_drivers; i++) { - drivers[i]->update(); + if (!_hil_mode) { + for (uint8_t i=0; i<_num_drivers; i++) { + drivers[i]->update(); + } } // consider a sensor as healthy if it has had an update in the diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index ca79ad162d..91f9bee174 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -121,6 +121,9 @@ public: // return number of registered sensors uint8_t num_instances(void) const { return _num_sensors; } + // enable HIL mode + void set_hil_mode(void) { _hil_mode = true; } + private: // how many drivers do we have? uint8_t _num_drivers; @@ -150,6 +153,7 @@ private: float _external_temperature; uint32_t _last_external_temperature_ms; DerivativeFilterFloat_Size7 _climb_rate_filter; + bool _hil_mode:1; void SimpleAtmosphere(const float alt, float &sigma, float &delta, float &theta); };