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_NavEKF3: Adjust sensor height when EK3_OGN_HGT_MASK bit 2 is set

Browse Source
This commit is contained in:
Paul Riseborough 2024-05-21 20:26:26 +10:00 committed by Randy Mackay
parent 65a97e13c9
commit f0f74c8595
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -685,8 +685,10 @@ void NavEKF3_core::readGpsData()
gpsDataNew.lat = gpsloc.lat; gpsDataNew.lat = gpsloc.lat;
gpsDataNew.lng = gpsloc.lng; gpsDataNew.lng = gpsloc.lng;
if ((frontend->_originHgtMode & (1<<2)) == 0) { if ((frontend->_originHgtMode & (1<<2)) == 0) {
// the height adjustment to match GPS is being achieved by adjusting the origin height
gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt); gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
} else { } else {
// the height adjustment to match GPS is being achieved by adjusting the measurements
gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt); gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
} }
storedGPS.push(gpsDataNew); storedGPS.push(gpsDataNew);

9
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -1273,6 +1273,11 @@ void NavEKF3_core::selectHeightForFusion()
hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd); hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
// correct for terrain position relative to datum // correct for terrain position relative to datum
hgtMea -= terrainState; hgtMea -= terrainState;
// correct sensor so that local position height adjusts to match GPS
if (frontend->_originHgtMode & (1 << 1) && frontend->_originHgtMode & (1 << 2)) {
// offset has to be applied to the measurement, not the NED origin
hgtMea += (float)(ekfGpsRefHgt - 0.01 * (double)EKF_origin.alt);
}
velPosObs[5] = -hgtMea; velPosObs[5] = -hgtMea;
// enable fusion // enable fusion
fuseHgtData = true; fuseHgtData = true;
@ -1299,6 +1304,10 @@ void NavEKF3_core::selectHeightForFusion()
} else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) { } else if (baroDataToFuse && (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO)) {
// using Baro data // using Baro data
hgtMea = baroDataDelayed.hgt - baroHgtOffset; hgtMea = baroDataDelayed.hgt - baroHgtOffset;
// correct sensor so that local position height adjusts to match GPS
if (frontend->_originHgtMode & (1 << 0) && frontend->_originHgtMode & (1 << 2)) {
hgtMea += (float)(ekfGpsRefHgt - 0.01 * (double)EKF_origin.alt);
}
// enable fusion // enable fusion
fuseHgtData = true; fuseHgtData = true;
// set the observation noise // set the observation noise