Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use SIM_VIB_FREQ for SITL vibrations

Browse Source 
this allows for testing of FFT on logs
This commit is contained in:
Andrew Tridgell 2019-01-07 20:32:40 +11:00
parent 9ca059ba16
commit 512c50a6c4
1 changed files with 26 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -65,10 +65,21 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance)
// add accel bias and noise
Vector3f accel_bias = instance==0?sitl->accel_bias.get():sitl->accel2_bias.get();
float xAccel = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
float yAccel = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
float zAccel = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
float xAccel = sitl->state.xAccel + accel_bias.x;
float yAccel = sitl->state.yAccel + accel_bias.y;
float zAccel = sitl->state.zAccel + accel_bias.z;
const Vector3f &vibe_freq = sitl->vibe_freq;
if (vibe_freq.is_zero()) {
xAccel += accel_noise * rand_float();
yAccel += accel_noise * rand_float();
zAccel += accel_noise * rand_float();
} else {
float t = AP_HAL::micros() * 1.0e-6;
xAccel += sinf(t * 2 * M_PI * vibe_freq.x) * accel_noise;
yAccel += sinf(t * 2 * M_PI * vibe_freq.y) * accel_noise;
zAccel += sinf(t * 2 * M_PI * vibe_freq.z) * accel_noise;
}
// correct for the acceleration due to the IMU position offset and angular acceleration
// correct for the centripetal acceleration
// only apply corrections to first accelerometer
@ -119,9 +130,17 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance)
float q = radians(sitl->state.pitchRate) + gyro_drift();
float r = radians(sitl->state.yawRate) + gyro_drift();
p += gyro_noise * rand_float();
q += gyro_noise * rand_float();
r += gyro_noise * rand_float();
const Vector3f &vibe_freq = sitl->vibe_freq;
if (vibe_freq.is_zero()) {
p += gyro_noise * rand_float();
q += gyro_noise * rand_float();
r += gyro_noise * rand_float();
} else {
float t = AP_HAL::micros() * 1.0e-6;
p += sinf(t * 2 * M_PI * vibe_freq.x) * gyro_noise;
q += sinf(t * 2 * M_PI * vibe_freq.y) * gyro_noise;
r += sinf(t * 2 * M_PI * vibe_freq.z) * gyro_noise;
}
Vector3f gyro = Vector3f(p, q, r);