Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: fixed getLLH alt for local origin height 

this fixes a bug introduced in #21834

this fix in #21834 was correct for getPosD, but should not have been
applied to getLLH

this caused cruise mode in plane to descend/ascend by the difference
between the public and local origins on mode entry

fixes #21984
This commit is contained in:
Andrew Tridgell 2022-10-22 20:01:14 +11:00
parent 4682336743
commit 8e8ceb25ea
3 changed files with 21 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -2007,8 +2007,8 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
// Record the position delta between current core and new primary core and the timestamp of the core change // Record the position delta between current core and new primary core and the timestamp of the core change
// Add current delta in case it hasn't been consumed yet // Add current delta in case it hasn't been consumed yet
core[old_primary].getPosD(posDownOldPrimary); core[old_primary].getPosD_local(posDownOldPrimary);
core[new_primary].getPosD(posDownNewPrimary); core[new_primary].getPosD_local(posDownNewPrimary);
pos_down_reset_data.core_delta = posDownNewPrimary - posDownOldPrimary + pos_down_reset_data.core_delta; pos_down_reset_data.core_delta = posDownNewPrimary - posDownOldPrimary + pos_down_reset_data.core_delta;
pos_down_reset_data.last_primary_change = imuSampleTime_us / 1000; pos_down_reset_data.last_primary_change = imuSampleTime_us / 1000;
pos_down_reset_data.core_changed = true; pos_down_reset_data.core_changed = true;

21
libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp
View File

@ -252,9 +252,9 @@ bool NavEKF3_core::getPosNE(Vector2f &posNE) const
return false; return false;
} }
// Write the last calculated D position of the body frame origin relative to the EKF origin (m). // Write the last calculated D position of the body frame origin relative to the EKF local origin
// Return true if the estimate is valid // Return true if the estimate is valid
bool NavEKF3_core::getPosD(float &posD) const bool NavEKF3_core::getPosD_local(float &posD) const
{ {
// The EKF always has a height estimate regardless of mode of operation // The EKF always has a height estimate regardless of mode of operation
// Correct for the IMU offset (EKF calculations are at the IMU) // Correct for the IMU offset (EKF calculations are at the IMU)
@ -268,15 +268,24 @@ bool NavEKF3_core::getPosD(float &posD) const
posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt; posD = outputDataNew.position.z + posOffsetNED.z + 0.01f * (float)EKF_origin.alt - (float)ekfGpsRefHgt;
} }
// Return the current height solution status
return filterStatus.flags.vert_pos;
}
// Write the last calculated D position of the body frame origin relative to the public origin
// Return true if the estimate is valid
bool NavEKF3_core::getPosD(float &posD) const
{
bool ret = getPosD_local(posD);
// adjust posD for difference between our origin and the public_origin // adjust posD for difference between our origin and the public_origin
Location local_origin; Location local_origin;
if (getOriginLLH(local_origin)) { if (getOriginLLH(local_origin)) {
posD += (public_origin.alt - local_origin.alt) * 0.01; posD += (public_origin.alt - local_origin.alt) * 0.01;
} }
// Return the current height solution status return ret;
return filterStatus.flags.vert_pos;
} }
// return the estimated height of body frame origin above ground level // return the estimated height of body frame origin above ground level
@ -296,7 +305,7 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
Location origin; Location origin;
if (getOriginLLH(origin)) { if (getOriginLLH(origin)) {
float posD; float posD;
if(getPosD(posD) && PV_AidingMode != AID_NONE) { if (getPosD_local(posD) && PV_AidingMode != AID_NONE) {
// Altitude returned is an absolute altitude relative to the WGS-84 spherioid // Altitude returned is an absolute altitude relative to the WGS-84 spherioid
loc.set_alt_cm(origin.alt - posD*100.0, 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) { if (filterStatus.flags.horiz_pos_abs || filterStatus.flags.horiz_pos_rel) {

5
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -138,7 +138,10 @@ public:
// If false returned, do not use for flight control // If false returned, do not use for flight control
bool getPosNE(Vector2f &posNE) const; bool getPosNE(Vector2f &posNE) const;
// Write the last calculated D position relative to the reference point (m). // get position D from local origin
bool getPosD_local(float &posD) const;
// Write the last calculated D position relative to the public origin
// If a calculated solution is not available, use the best available data and return false // If a calculated solution is not available, use the best available data and return false
// If false returned, do not use for flight control // If false returned, do not use for flight control
bool getPosD(float &posD) const; bool getPosD(float &posD) const;