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

AP_InertialSensor: use SIM_GYR_SCALE in SITL

Browse Source
This commit is contained in:
Andrew Tridgell 2016-01-19 15:29:08 +11:00
parent a828db792e
commit 24d8610013
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp
View File

@ -101,6 +101,16 @@ void AP_InertialSensor_SITL::timer_update(void)
Vector3f gyro0 = Vector3f(p1, q1, r1) + _imu.get_gyro_offsets(0);
Vector3f gyro1 = Vector3f(p2, q2, r2) + _imu.get_gyro_offsets(1);
// add in gyro scaling
Vector3f scale = sitl->gyro_scale;
gyro0.x *= (1 + scale.x*0.01);
gyro0.y *= (1 + scale.y*0.01);
gyro0.z *= (1 + scale.z*0.01);
gyro1.x *= (1 + scale.x*0.01);
gyro1.y *= (1 + scale.y*0.01);
gyro1.z *= (1 + scale.z*0.01);
_notify_new_gyro_raw_sample(gyro_instance[0], gyro0);
_notify_new_gyro_raw_sample(gyro_instance[1], gyro1);
}