From f03ba86d9d7063a388ebe56c58151eefb101eabf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jul 2012 11:04:54 +1000 Subject: [PATCH] SITL: add some minimal noise when motors are off this actually improves the gyro calibration --- libraries/Desktop/support/sitl_adc.cpp | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/libraries/Desktop/support/sitl_adc.cpp b/libraries/Desktop/support/sitl_adc.cpp index a017d03667..e71e0b5c8f 100644 --- a/libraries/Desktop/support/sitl_adc.cpp +++ b/libraries/Desktop/support/sitl_adc.cpp @@ -104,16 +104,21 @@ void sitl_update_adc(float roll, float pitch, float yaw, // Relative to earth } } + // minimum noise levels are 2 bits + float accel_noise = _accel_scale*2; + float gyro_noise = _gyro_gain_y*2; if (motors_on) { - // only add accel/gyro noise when motors are on - xAccel += sitl.accel_noise * rand_float(); - yAccel += sitl.accel_noise * rand_float(); - zAccel += sitl.accel_noise * rand_float(); - - p += ToRad(sitl.gyro_noise) * rand_float(); - q += ToRad(sitl.gyro_noise) * rand_float(); - r += ToRad(sitl.gyro_noise) * rand_float(); + // add extra noise when the motors are on + accel_noise += sitl.accel_noise; + gyro_noise += ToRad(sitl.gyro_noise); } + xAccel += accel_noise * rand_float(); + yAccel += accel_noise * rand_float(); + zAccel += accel_noise * rand_float(); + + p += gyro_noise * rand_float(); + q += gyro_noise * rand_float(); + r += gyro_noise * rand_float(); p += gyro_drift(); q += gyro_drift();