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

AP_NavEKF2: Correct range finder data for body frame position offset

This commit is contained in:
priseborough 2016-10-12 09:26:51 +11:00 committed by Andrew Tridgell
parent e884e9cc6e
commit 16f021c466
1 changed files with 9 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -650,6 +650,15 @@ void NavEKF2_core::selectHeightForFusion()
readRangeFinder();
rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
// correct range data for the body frame position offset relative to the IMU
// the corrected reading is the reading that would have been taken if the sensor was
// co-located with the IMU
if (rangeDataToFuse) {
Vector3f posOffsetBody = rangeDataDelayed.body_offset - accelPosOffset;
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
}
// read baro height data from the sensor and check for new data in the buffer
readBaroData();
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);