From 71d3847bfc1f182946011ce9b1f23743121e9ac2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 3 Mar 2012 11:54:19 +1100 Subject: [PATCH] SITL: added a drift_level multiplier this makes it easier to experiment with different gyro drift levels --- libraries/Desktop/support/sitl_adc.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/Desktop/support/sitl_adc.h b/libraries/Desktop/support/sitl_adc.h index e0624bd724..e92fded895 100644 --- a/libraries/Desktop/support/sitl_adc.h +++ b/libraries/Desktop/support/sitl_adc.h @@ -10,6 +10,7 @@ #include static const float vibration_level = 2.0; +static const float drift_level = 1.0; // order zgyro, xgyro, ygyro, temp, xacc, yacc, zacc, aspd static const float noise_scale[8] = { 150, 150, 150, 0, 400, 400, 400, 0 }; static const float noise_offset[8]= { 0, 0, 0, 0, 0, 0, 0, 0 }; @@ -25,11 +26,11 @@ static double rand_float(void) static inline float gyro_drift(uint8_t chan) { - if (drift_rate[chan] == 0) { + if (drift_rate[chan] * drift_level == 0.0) { return 0; } extern long unsigned int micros(void); - double period = 10*drift_rate[chan]; + double period = 10*drift_rate[chan] * drift_level; double minutes = fmod(micros() / 60.0e6, period); if (minutes < period/2) { return minutes; @@ -57,7 +58,7 @@ static inline float noise_generator(uint8_t chan) } } if (noise_count == 0) { - return rand_float() * base_noise; + return rand_float() * base_noise * vibration_level; } return gyro_drift(chan) + noise/noise_count; }