From 5f1f61982905ac06b27c110e343ef04c590c6b27 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 28 Jul 2014 17:41:19 +0900 Subject: [PATCH] HAL_AVR_SITL: integrate ACC_BIAS and BARO_GLITCH Adjust simulated accelerometer values and barometer altitude --- libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp | 3 +++ libraries/AP_HAL_AVR_SITL/sitl_ins.cpp | 8 +++++--- 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp index 74966579cb..ff5e91719a 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_barometer.cpp @@ -53,6 +53,9 @@ void SITL_State::_update_barometer(float altitude) sim_alt += _sitl->baro_drift * now / 1000; sim_alt += _sitl->baro_noise * _rand_float(); + // add baro glitch + sim_alt += _sitl->baro_glitch; + _barometer->setHIL(sim_alt); } diff --git a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp index e1358fdd93..bc0db89d0c 100644 --- a/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp +++ b/libraries/AP_HAL_AVR_SITL/sitl_ins.cpp @@ -149,9 +149,11 @@ void SITL_State::_update_ins(float roll, float pitch, float yaw, // Relative accel_noise += _sitl->accel_noise; gyro_noise += ToRad(_sitl->gyro_noise); } - float xAccel1 = xAccel + accel_noise * _rand_float(); - float yAccel1 = yAccel + accel_noise * _rand_float(); - float zAccel1 = zAccel + accel_noise * _rand_float(); + // get accel bias (add only to first accelerometer) + Vector3f accel_bias = _sitl->accel_bias.get(); + float xAccel1 = xAccel + accel_noise * _rand_float() + accel_bias.x; + float yAccel1 = yAccel + accel_noise * _rand_float() + accel_bias.y; + float zAccel1 = zAccel + accel_noise * _rand_float() + accel_bias.z; float xAccel2 = xAccel + accel_noise * _rand_float(); float yAccel2 = yAccel + accel_noise * _rand_float();