Skip to content

Commit 0f0d595

Browse files
eufrizzeufrizz
committed
AP_NavEKF3: Fix up EK3_FEATURE_EXTERNAL_NAV conditional compilation
1 parent 401d8a3 commit 0f0d595

File tree

2 files changed

+50
-46
lines changed

2 files changed

+50
-46
lines changed

‎libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

Lines changed: 46 additions & 42 deletions
Original file line numberDiff line numberDiff line change
@@ -512,7 +512,7 @@ void NavEKF3_core::SelectVelPosFusion()
512512
}
513513

514514
const AP_NavEKF_Source::SourceXY posxy_source = frontend->sources.getPosXYSource();
515-
#if EK3_FEATURE_EXTERNAL_NAV
515+
#if EK3_FEATURE_EXTERNAL_NAV
516516
// Check for data at the fusion time horizon
517517
extNavDataToFuse = (posxy_source == AP_NavEKF_Source::SourceXY::EXTNAV) && storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
518518
if (extNavDataToFuse) {
@@ -713,7 +713,9 @@ void NavEKF3_core::FuseVelPosNED()
713713
R_OBS[4] = R_OBS[0];
714714
for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
715715
} else {
716+
#if EK3_FEATURE_EXTERNAL_NAV
716717
const bool extNavUsedForVel = extNavVelToFuse && frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV);
718+
#endif
717719
if (fuseVelData)
718720
{
719721
#if EK3_FEATURE_EXTERNAL_NAV
@@ -784,57 +786,56 @@ void NavEKF3_core::FuseVelPosNED()
784786
// For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS performance
785787
// plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
786788
ftype obs_data_chk;
789+
#if EK3_FEATURE_EXTERNAL_NAV
790+
if (extNavUsedForVel) {
791+
obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
792+
} else
793+
#endif
787794
if (gpsDataToFuse)
788795
{
789796
obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
790-
}
791-
#if EK3_FEATURE_EXTERNAL_NAV
792-
else if (extNavUsedForVel) {
793-
obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
794797
} else {
795798
// TODO (EF): No horizontal vel this update, default value
796799
obs_data_chk = 5.0f;
797800
}
798-
#endif
799801
R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
800802

801803
}
802804

803805
R_OBS[5] = posDownObsNoise;
804806
for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
805807

806-
// TODO (EF): extra indentation for cleaner git diff, remove
807-
// if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
808-
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
809-
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
810-
if (gpsDataDelayed.have_vz && fuseVelData && (frontend->sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) {
811-
// calculate innovations for height and vertical GPS vel measurements
812-
const ftype hgtErr = stateStruct.position.z - velPosObs[5];
813-
const ftype velDErr = stateStruct.velocity.z - velPosObs[2];
814-
// Check if they are the same sign and both more than 3-sigma out of bounds
815-
// Step the test threshold up in stages from 1 to 2 to 3 sigma after exiting
816-
// from a previous bad IMU event so that a subsequent error is caught more quickly.
817-
const uint32_t timeSinceLastBadIMU_ms = imuSampleTime_ms - badIMUdata_ms;
818-
float R_gain;
819-
if (timeSinceLastBadIMU_ms > (BAD_IMU_DATA_HOLD_MS * 2)) {
820-
R_gain = 9.0F;
821-
} else if (timeSinceLastBadIMU_ms > ((BAD_IMU_DATA_HOLD_MS * 3) / 2)) {
822-
R_gain = 4.0F;
823-
} else {
824-
R_gain = 1.0F;
825-
}
826-
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > R_gain * R_OBS[5]) && (sq(velDErr) >R_gain * R_OBS[2])) {
827-
badIMUdata_ms = imuSampleTime_ms;
828-
} else {
829-
goodIMUdata_ms = imuSampleTime_ms;
830-
}
831-
if (timeSinceLastBadIMU_ms < BAD_IMU_DATA_HOLD_MS) {
832-
badIMUdata = true;
833-
stateStruct.velocity.z = gpsDataDelayed.vel.z;
834-
} else {
835-
badIMUdata = false;
836-
}
808+
// If vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
809+
// innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
810+
// the accelerometers and we should disable the GPS and barometer innovation consistency checks.
811+
if (gpsDataDelayed.have_vz && fuseVelData && (frontend->sources.getPosZSource() != AP_NavEKF_Source::SourceZ::GPS)) {
812+
// calculate innovations for height and vertical GPS vel measurements
813+
const ftype hgtErr = stateStruct.position.z - velPosObs[5];
814+
const ftype velDErr = stateStruct.velocity.z - velPosObs[2];
815+
// Check if they are the same sign and both more than 3-sigma out of bounds
816+
// Step the test threshold up in stages from 1 to 2 to 3 sigma after exiting
817+
// from a previous bad IMU event so that a subsequent error is caught more quickly.
818+
const uint32_t timeSinceLastBadIMU_ms = imuSampleTime_ms - badIMUdata_ms;
819+
float R_gain;
820+
if (timeSinceLastBadIMU_ms > (BAD_IMU_DATA_HOLD_MS * 2)) {
821+
R_gain = 9.0F;
822+
} else if (timeSinceLastBadIMU_ms > ((BAD_IMU_DATA_HOLD_MS * 3) / 2)) {
823+
R_gain = 4.0F;
824+
} else {
825+
R_gain = 1.0F;
826+
}
827+
if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > R_gain * R_OBS[5]) && (sq(velDErr) >R_gain * R_OBS[2])) {
828+
badIMUdata_ms = imuSampleTime_ms;
829+
} else {
830+
goodIMUdata_ms = imuSampleTime_ms;
831+
}
832+
if (timeSinceLastBadIMU_ms < BAD_IMU_DATA_HOLD_MS) {
833+
badIMUdata = true;
834+
stateStruct.velocity.z = gpsDataDelayed.vel.z;
835+
} else {
836+
badIMUdata = false;
837837
}
838+
}
838839

839840
// Test horizontal position measurements
840841
if (fusePosData) {
@@ -871,11 +872,13 @@ void NavEKF3_core::FuseVelPosNED()
871872
// if timed out or outside the specified uncertainty radius, reset to the external sensor
872873
// if velocity drift is being constrained, dont reset until gps passes quality checks
873874
bool posVarianceIsTooLarge = false;
875+
#if EK3_FEATURE_EXTERNAL_NAV
874876
if (extNavDataToFuse)
875877
{
876878
posVarianceIsTooLarge = (P[8][8] + P[7][7]) > sq(ftype(extNavDataDelayed.posErr));
877-
}
878-
else if (gpsDataToFuse)
879+
} else
880+
#endif
881+
if (gpsDataToFuse)
879882
{
880883
posVarianceIsTooLarge = (frontend->_gpsGlitchRadiusMax > 0) && (P[8][8] + P[7][7]) > sq(ftype(frontend->_gpsGlitchRadiusMax));
881884
}
@@ -896,17 +899,18 @@ void NavEKF3_core::FuseVelPosNED()
896899
// Reset the position variances and corresponding covariances to a value that will pass the checks
897900
zeroRows(P,7,8);
898901
zeroCols(P,7,8);
902+
#if EK3_FEATURE_EXTERNAL_NAV
899903
if (extNavDataToFuse)
900904
{
901905
P[7][7] = sq(ftype(0.5f*extNavDataDelayed.posErr));
902-
}
903-
else if (gpsDataToFuse)
906+
} else
907+
#endif
908+
if (gpsDataToFuse)
904909
{
905910
P[7][7] = sq(ftype(0.5f*frontend->_gpsGlitchRadiusMax));
906911
}
907912
else
908913
{
909-
// (EF): Doesn't seem to trigger
910914
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "reset pos variance w/o extnav or GPS");
911915
}
912916
P[8][8] = P[7][7];

‎libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1897,9 +1897,9 @@ void NavEKF3_core::ConstrainVariances()
18971897
#if EK3_FEATURE_EXTERNAL_NAV
18981898
if (useExtNavVel) {
18991899
P[6][6] = fmaxF(P[6][6], sq(extNavVelDelayed.err));
1900-
} else if (gpsDataToFuse)
1900+
} else
19011901
#endif
1902-
{
1902+
if (gpsDataToFuse) {
19031903
P[6][6] = fmaxF(P[6][6], sq(frontend->_gpsVertVelNoise));
19041904
}
19051905
else {
@@ -1922,9 +1922,9 @@ void NavEKF3_core::ConstrainVariances()
19221922
#if EK3_FEATURE_EXTERNAL_NAV
19231923
if (useExtNavVel) {
19241924
P[6][6] = sq(extNavVelDelayed.err);
1925-
} else if (gpsDataToFuse)
1925+
} else
19261926
#endif
1927-
{
1927+
if (gpsDataToFuse) {
19281928
P[6][6] = sq(frontend->_gpsVertVelNoise);
19291929
}
19301930
else {

0 commit comments

Comments
 (0)