Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Don't correct range finder for zero position offset

Browse Source
This commit is contained in:
priseborough 2016-10-15 09:18:25 +11:00 committed by Andrew Tridgell
parent 9a0ce1d5eb
commit f025c96e63
1 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -655,8 +655,10 @@ void NavEKF2_core::selectHeightForFusion()
// 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;
if (!posOffsetBody.is_zero()) {
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