Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

EKF3: allow alt from other sources when using ExtNav #15602

Merged
merged 3 commits into from
Oct 20, 2020
Merged

EKF3: allow alt from other sources when using ExtNav #15602

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
435ef3f
AP_NavEKF3: minor restructure of how fusePosData and fuseVelData are set
rmackay9 Jul 4, 2020
ed7c45c
AP_NavEKF3: allow alt from other sources when using ExtNav
rmackay9 Jul 4, 2020
d82e9e2
AP_NavEKF3: consolidate extNav height source timeouts
rmackay9 Oct 20, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 9 additions & 8 deletions libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -407,6 +407,10 @@ void NavEKF3_core::SelectVelPosFusion()
// get data that has now fallen behind the fusion time horizon
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);

// initialise all possible data we may fuse
fusePosData = false;
fuseVelData = false;

// Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS != 3)) {

Expand All @@ -430,22 +434,15 @@ void NavEKF3_core::SelectVelPosFusion()
velPosObs[3] = gpsDataDelayed.pos.x;
velPosObs[4] = gpsDataDelayed.pos.y;
} else if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3)) {
// use external nav system for position
// use external nav system for horizontal position
extNavUsedForPos = true;
activeHgtSource = HGT_SOURCE_EXTNAV;
fuseVelData = false;
fuseHgtData = true;
fusePosData = true;

// correct for external navigation sensor position
CorrectExtNavForSensorOffset(extNavDataDelayed.pos);

velPosObs[3] = extNavDataDelayed.pos.x;
velPosObs[4] = extNavDataDelayed.pos.y;
velPosObs[5] = extNavDataDelayed.pos.z;
} else {
fuseVelData = false;
fusePosData = false;
}

// fuse external navigation velocity data if available
Expand Down Expand Up @@ -1046,6 +1043,8 @@ void NavEKF3_core::selectHeightForFusion()
activeHgtSource = HGT_SOURCE_GPS;
} else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
activeHgtSource = HGT_SOURCE_BCN;
} else if ((frontend->_altSource == 4) && ((imuSampleTime_ms - extNavMeasTime_ms) < 500)) {
rmackay9 marked this conversation as resolved.
Show resolved Hide resolved
activeHgtSource = HGT_SOURCE_EXTNAV;
}

// Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
Expand Down Expand Up @@ -1086,7 +1085,9 @@ void NavEKF3_core::selectHeightForFusion()
// Select the height measurement source
if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
hgtMea = -extNavDataDelayed.pos.z;
velPosObs[5] = -hgtMea;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));
fuseHgtData = true;
} else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
// using range finder data
// correct for tilt using a flat earth model
Expand Down