From 470f5c456260267af1b7a4513909bb1c8134e065 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 19 Oct 2016 07:41:26 +1100 Subject: [PATCH] AP_InertialSensor: Correct SITL IMU1 accel for position in body frame --- .../AP_InertialSensor_SITL.cpp | 20 +++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp index 6a9ce7b24a..7b18ae781c 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_SITL.cpp @@ -73,6 +73,26 @@ void AP_InertialSensor_SITL::timer_update(void) float yAccel2 = sitl->state.yAccel + accel2_noise * rand_float(); float zAccel2 = sitl->state.zAccel + accel2_noise * rand_float(); + // correct for the acceleration due to the IMU position offset and angular acceleration + // correct for the centripetal acceleration + // only apply corrections to first accelerometer + Vector3f pos_offset = sitl->imu_pos_offset; + if (!pos_offset.is_zero()) { + // calculate sensed acceleration due to lever arm effect + // Note: the % operator has been overloaded to provide a cross product + Vector3f angular_accel = Vector3f(radians(sitl->state.angAccel.x) , radians(sitl->state.angAccel.y) , radians(sitl->state.angAccel.z)); + Vector3f lever_arm_accel = angular_accel % pos_offset; + + // calculate sensed acceleration due to centripetal acceleration + Vector3f angular_rate = Vector3f(radians(sitl->state.rollRate), radians(sitl->state.pitchRate), radians(sitl->state.yawRate)); + Vector3f centripetal_accel = angular_rate % (angular_rate % pos_offset); + + // apply corrections + xAccel1 += lever_arm_accel.x + centripetal_accel.x; + yAccel1 += lever_arm_accel.y + centripetal_accel.y; + zAccel1 += lever_arm_accel.z + centripetal_accel.z; + } + if (fabsf(sitl->accel_fail) > 1.0e-6f) { xAccel1 = sitl->accel_fail; yAccel1 = sitl->accel_fail;