diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index f6c376a6ef..5d4b09eb58 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -111,6 +111,12 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative yAccel += accel_noise * _rand_float(); zAccel += accel_noise * _rand_float(); + if (fabs(_sitl->accel_fail) > 1.0e-6) { + xAccel = _sitl->accel_fail; + yAccel = _sitl->accel_fail; + zAccel = _sitl->accel_fail; + } + p += gyro_noise * _rand_float(); q += gyro_noise * _rand_float(); r += gyro_noise * _rand_float(); diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp index 03cb7bbf7a..ace38d5cb9 100644 --- a/libraries/SITL/SITL.cpp +++ b/libraries/SITL/SITL.cpp @@ -46,6 +46,7 @@ const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { AP_GROUPINFO("GPS_HZ", 18, SITL, gps_hertz, 5), AP_GROUPINFO("BATT_VOLTAGE", 19, SITL, batt_voltage, 12.6), AP_GROUPINFO("ASPD_RND", 20, SITL, aspd_noise, 0.5), + AP_GROUPINFO("ACCEL_FAIL", 21, SITL, accel_fail, 0), AP_GROUPEND }; diff --git a/libraries/SITL/SITL.h b/libraries/SITL/SITL.h index 8054a0491f..fc9845bbe1 100644 --- a/libraries/SITL/SITL.h +++ b/libraries/SITL/SITL.h @@ -65,6 +65,7 @@ public: AP_Vector3f gps_glitch; // glitch offsets in lat, lon and altitude AP_Int8 gps_hertz; // GPS update rate in Hz AP_Float batt_voltage; // battery voltage base + AP_Float accel_fail; // accelerometer failure value // wind control AP_Float wind_speed;