Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: use ACC2_BIAS

This commit is contained in:
Andrew Tridgell 2016-12-18 15:53:57 +11:00
parent 65c3d0e060
commit a30745903a
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -61,15 +61,16 @@ void AP_InertialSensor_SITL::timer_update(void)
gyro_noise += ToRad(sitl->gyro_noise); gyro_noise += ToRad(sitl->gyro_noise);
} }
// get accel bias (add only to first accelerometer) // add accel bias and noise
Vector3f accel_bias = sitl->accel_bias.get(); Vector3f accel_bias = sitl->accel_bias.get();
float xAccel1 = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x; float xAccel1 = sitl->state.xAccel + accel_noise * rand_float() + accel_bias.x;
float yAccel1 = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y; float yAccel1 = sitl->state.yAccel + accel_noise * rand_float() + accel_bias.y;
float zAccel1 = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z; float zAccel1 = sitl->state.zAccel + accel_noise * rand_float() + accel_bias.z;
float xAccel2 = sitl->state.xAccel + accel2_noise * rand_float(); accel_bias = sitl->accel2_bias.get();
float yAccel2 = sitl->state.yAccel + accel2_noise * rand_float(); float xAccel2 = sitl->state.xAccel + accel2_noise * rand_float() + accel_bias.x;
float zAccel2 = sitl->state.zAccel + accel2_noise * rand_float(); float yAccel2 = sitl->state.yAccel + accel2_noise * rand_float() + accel_bias.y;
float zAccel2 = sitl->state.zAccel + accel2_noise * rand_float() + accel_bias.z;
// correct for the acceleration due to the IMU position offset and angular acceleration // correct for the acceleration due to the IMU position offset and angular acceleration
// correct for the centripetal acceleration // correct for the centripetal acceleration