From 064a1068086634606a0603731cbf4f4472ed96a7 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 12 Oct 2016 08:30:01 +1100 Subject: [PATCH] AP_NavEKF2: Get IMU accelerometer body position offset data --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 ++ libraries/AP_NavEKF2/AP_NavEKF2_core.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index ceb556e265..e7e3396d93 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -276,8 +276,10 @@ void NavEKF2_core::readIMUData() // use the nominated imu or primary if not available if (ins.use_accel(imu_index)) { readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT); + accelPosOffset = ins.get_imu_pos_offset(imu_index); } else { readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT); + accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel()); } // Get delta angle data from primary gyro or primary if not available diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 10096ad68b..75fb262b9d 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -908,6 +908,8 @@ private: bool gndOffsetValid; // true when the ground offset state can still be considered valid Vector3f delAngBodyOF; // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement float delTimeOF; // time that delAngBodyOF is summed across + Vector3f accelPosOffset; // position of IMU accelerometer unit in body frame (m) + // Range finder float baroHgtOffset; // offset applied when when switching to use of Baro height