diff --git a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp index 6e5410833c..74966579cb 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp @@ -38,6 +38,11 @@ void SITL_State::_update_barometer(float altitude) return; } + if (_sitl->baro_disable) { + // barometer is disabled + return; + } + // 80Hz, to match the real APM2 barometer uint32_t now = hal.scheduler->millis(); if ((now - last_update) < 12) { diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index ef29fec129..532ce3b7ec 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -55,6 +55,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { AP_GROUPINFO("SONAR_RND", 24, SITL, sonar_noise, 0), AP_GROUPINFO("RC_FAIL", 25, SITL, rc_fail, 0), AP_GROUPINFO("GPS2_ENABLE", 26, SITL, gps2_enable, 0), + AP_GROUPINFO("BARO_DISABLE", 27, SITL, baro_disable, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index e3e08f3923..4433cd776d 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -75,6 +75,7 @@ public: AP_Float batt_voltage; // battery voltage base AP_Float accel_fail; // accelerometer failure value AP_Int8 rc_fail; // fail RC input + AP_Int8 baro_disable; // disable simulated barometer // wind control AP_Float wind_speed;