Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Match handling of MAV_CMD_EXTERNAL_POSITION_ESTIMATE to common MAVLink dialect

Browse Source
This commit is contained in:
Marek S. Lukasiewicz 2024-03-06 18:37:46 +11:00 committed by Andrew Tridgell
parent cfd76b8dd9
commit 07681416f5
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -145,8 +145,8 @@ void NavEKF3_core::ResetPosition(resetDataSource posResetSource)
} }
#if EK3_FEATURE_POSITION_RESET #if EK3_FEATURE_POSITION_RESET
// Sets the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and uncertainty // Sets the EKF's NE horizontal position states and their corresponding variances from a supplied WGS-84 location and optionally uncertainty
// The altitude element of the location is not used. // The altitude element of the location is not used. If accuracy is not known should be passed as NaN.
// Returns true if the set was successful // Returns true if the set was successful
bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms) bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
{ {
@ -164,6 +164,11 @@ bool NavEKF3_core::setLatLng(const Location &loc, float posAccuracy, uint32_t ti
zeroRows(P,7,8); zeroRows(P,7,8);
zeroCols(P,7,8); zeroCols(P,7,8);
// handle unknown accuracy
if (isnan(posAccuracy)) {
posAccuracy = 0.0f; // will be ignored due to MAX below
}
// set the variances using the position measurement noise parameter // set the variances using the position measurement noise parameter
P[7][7] = P[8][8] = sq(MAX(posAccuracy,frontend->_gpsHorizPosNoise)); P[7][7] = P[8][8] = sq(MAX(posAccuracy,frontend->_gpsHorizPosNoise));