Browse Source

SITL: add some minimal noise when motors are off

this actually improves the gyro calibration
master
Andrew Tridgell 13 years ago
parent
commit
f03ba86d9d
  1. 21
      libraries/Desktop/support/sitl_adc.cpp

21
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) { if (motors_on) {
// only add accel/gyro noise when motors are on // add extra noise when the motors are on
xAccel += sitl.accel_noise * rand_float(); accel_noise += sitl.accel_noise;
yAccel += sitl.accel_noise * rand_float(); gyro_noise += ToRad(sitl.gyro_noise);
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();
} }
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(); p += gyro_drift();
q += gyro_drift(); q += gyro_drift();

Loading…
Cancel
Save