aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: store and use lastKnownPositionD

Browse Source 
When returning our last-known-position we were supplying lat/lng but not
altitude.

This usually really doesn't matter as this result generally isn't used,
especially for altitude.  OTOH, it may prevent a bug into the future.
This commit is contained in:
Peter Barker 2022-04-08 10:52:08 +10:00 committed by Peter Barker
parent 252f2ac6f5
commit 0665f9c32b
4 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -381,6 +381,9 @@ void NavEKF3_core::setAidingMode()
meaHgtAtTakeOff = baroDataDelayed.hgt;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct.position.z = -meaHgtAtTakeOff;
// store the current height to be used to keep reporting
// the last known position
lastKnownPositionD = stateStruct.position.z;
// reset relative aiding sensor fusion activity status
flowFusionActive = false;
bodyVelFusionActive = false;

3
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -279,7 +279,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
float posD;
if(getPosD(posD) && PV_AidingMode != AID_NONE) {
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid
loc.set_alt_cm(origin.alt - posD*100, Location::AltFrame::ABSOLUTE);
loc.set_alt_cm(origin.alt - posD*100.0, Location::AltFrame::ABSOLUTE);
if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {
// The EKF is able to provide a position estimate
loc.lat = EKF_origin.lat;
@ -309,6 +309,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
loc.lng = EKF_origin.lng;
loc.offset(lastKnownPositionNE.x + posOffsetNED.x,
lastKnownPositionNE.y + posOffsetNED.y);
loc.alt = EKF_origin.alt - lastKnownPositionD*100.0;
return false;
}
}

1
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -237,6 +237,7 @@ void NavEKF3_core::InitialiseVariables()
dt = 0;
velDotNEDfilt.zero();
lastKnownPositionNE.zero();
lastKnownPositionD = 0;
prevTnb.zero();
memset(&P[0][0], 0, sizeof(P));
memset(&KH[0][0], 0, sizeof(KH));

1
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -1047,6 +1047,7 @@ private:
uint32_t lastSynthYawTime_ms; // time stamp when yaw observation was last fused (msec)
uint32_t ekfStartTime_ms; // time the EKF was started (msec)
Vector2F lastKnownPositionNE; // last known position
float lastKnownPositionD; // last known height
uint32_t lastLaunchAccelTime_ms;
ftype velTestRatio; // sum of squares of GPS velocity innovation divided by fail threshold
ftype posTestRatio; // sum of squares of GPS position innovation divided by fail threshold