aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: use the new noise model

Browse Source 
use the new noise model at a low level for master until we sort out
DCM noise handling
This commit is contained in:
Andrew Tridgell 2012-03-02 18:29:27 +11:00
parent 3745dfd59f
commit dd9065123c
1 changed files with 34 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/Desktop/support/sitl_adc.h
View File

@ -9,7 +9,12 @@
#include <stdlib.h> #include <stdlib.h>
#include <math.h> #include <math.h>
static float noise_scale[8] = { 240, 400, 500, 200, 400, 400, 2000, 200 }; static const float vibration_level = 0.1;
// 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 };
static const float drift_rate[8] = { 0.7, 1.0, 0.5, 0, 0, 0, 0, 0 };
static const float base_noise = 2;
// generate a random float between -1 and 1 // generate a random float between -1 and 1
static double rand_float(void) static double rand_float(void)
@ -18,25 +23,43 @@ static double rand_float(void)
return (ret - 1.0e6) / 1.0e6; return (ret - 1.0e6) / 1.0e6;
} }
static inline float gyro_drift(uint8_t chan)
{
if (drift_rate[chan] == 0) {
return 0;
}
extern long unsigned int micros(void);
double period = 10*drift_rate[chan];
double minutes = fmod(micros() / 60.0e6, period);
if (minutes < period/2) {
return minutes;
}
return period - minutes;
}
static inline float noise_generator(uint8_t chan) static inline float noise_generator(uint8_t chan)
{ {
extern float sitl_motor_speed[4]; extern float sitl_motor_speed[4];
extern long unsigned int micros(void);
uint8_t i; uint8_t i;
float noise = 0; float noise = 0;
uint8_t noise_count=0; uint8_t noise_count=0;
double t = micros() / 1.0e6; extern long unsigned int micros(void);
for (i=0; i<4; i++) { for (i=0; i<4; i++) {
if (sitl_motor_speed[i] > 0.0) { if (sitl_motor_speed[i] > 0.0) {
float n = rand_float() * noise_scale[chan]; float n = rand_float() * noise_scale[chan] * vibration_level;
noise += sin(fmod(t * sitl_motor_speed[i] * 2 * 3.14 + i, 2*3.14)) * n; //double t = micros() / 1.0e6;
//float freq = (rand_float() + 1.0) * sitl_motor_speed[i];
//noise += sin(fmod(t * freq * 2 * M_PI + i,
//2*M_PI)) * n;
noise += n + noise_offset[chan];
noise_count++; noise_count++;
} }
} }
if (noise_count == 0) { if (noise_count == 0) {
return 0; return rand_float() * base_noise;
} }
return noise/noise_count; return gyro_drift(chan) + noise/noise_count;
} }
// this implements the UDR2 register // this implements the UDR2 register
@ -75,11 +98,10 @@ struct ADC_UDR2 {
} }
next_analog = channels[chan]; next_analog = channels[chan];
idx = 1; idx = 1;
next_value = (unsigned)(next_analog + noise_generator(chan) + 0.5); next_analog += noise_generator(chan) + 0.5;
if (next_value >= 0x1000) { if (next_analog > 0xFFF) next_analog = 0xFFF;
next_value = 0xFFF; if (next_analog < 0) next_analog = 0;
} next_value = ((unsigned)next_analog) << 3;
next_value = (next_value << 3);
return *this; return *this;
} }