From c05c495cc8f7e71e0a23bcc4059feb06298257d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 5 Jun 2017 15:37:07 +1000 Subject: [PATCH] AP_InertialSensor: support rotated IMUs in SITL --- libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index f8b2a6a3ee..5c594d01d5 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -94,6 +94,8 @@ void AP_InertialSensor_SITL::generate_accel(uint8_t instance) Vector3f accel = Vector3f(xAccel, yAccel, zAccel) + _imu.get_accel_offsets(instance); + _rotate_and_correct_accel(accel_instance[instance], accel); + _notify_new_accel_raw_sample(accel_instance[instance], accel, AP_HAL::micros64()); } @@ -126,6 +128,8 @@ void AP_InertialSensor_SITL::generate_gyro(uint8_t instance) gyro.y *= (1 + scale.y*0.01); gyro.z *= (1 + scale.z*0.01); + _rotate_and_correct_gyro(gyro_instance[instance], gyro); + _notify_new_gyro_raw_sample(gyro_instance[instance], gyro, AP_HAL::micros64()); }