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

AP_NavEKF3: fix extnav vel reset

This commit is contained in:
chobits 2020-07-16 18:03:29 +08:00 committed by Randy Mackay
parent 3f581d7a20
commit 564244ce9a
2 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -370,9 +370,14 @@ void NavEKF3_core::setAidingMode()
} else if (readyToUseExtNav()) {
// we are commencing aiding using external nav
posResetSource = EXTNAV;
velResetSource = DEFAULT;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index);
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m)",(unsigned)imu_index,(double)extNavDataDelayed.pos.x,(double)extNavDataDelayed.pos.y,(double)extNavDataDelayed.pos.z);
if (useExtNavVel) {
velResetSource = EXTNAV;
gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initial vel NED = %3.1f,%3.1f,%3.1f (m/s)",(unsigned)imu_index,(double)extNavVelDelayed.vel.x,(double)extNavVelDelayed.vel.y,(double)extNavVelDelayed.vel.z);
} else {
velResetSource = DEFAULT;
}
// handle height reset as special case
hgtMea = -extNavDataDelayed.pos.z;
posDownObsNoise = sq(constrain_float(extNavDataDelayed.posErr, 0.1f, 10.0f));

16
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -45,11 +45,12 @@ void NavEKF3_core::ResetVelocity(void)
// clear the timeout flags and counters
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && posResetSource == DEFAULT) || velResetSource == EXTNAV) {
} else if ((imuSampleTime_ms - extNavVelMeasTime_ms < 250 && velResetSource == DEFAULT) || velResetSource == EXTNAV) {
// use external nav data as the 2nd preference
// already corrected for sensor position
stateStruct.velocity = extNavVelDelayed.vel;
P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
stateStruct.velocity.x = extNavVelDelayed.vel.x;
stateStruct.velocity.y = extNavVelDelayed.vel.y;
P[5][5] = P[4][4] = sq(extNavVelDelayed.err);
velTimeout = false;
lastVelPassTime_ms = imuSampleTime_ms;
} else {
@ -264,7 +265,7 @@ void NavEKF3_core::ResetHeight(void)
if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 && !frontend->inhibitGpsVertVelUse) {
stateStruct.velocity.z = gpsDataNew.vel.z;
} else if (inFlight && useExtNavVel && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
stateStruct.velocity.z = extNavVelNew.vel.z;
stateStruct.velocity.z = extNavVelDelayed.vel.z;
} else if (onGround) {
stateStruct.velocity.z = 0.0f;
}
@ -280,8 +281,11 @@ void NavEKF3_core::ResetHeight(void)
zeroCols(P,6,6);
// set the variances to the measurement variance
P[6][6] = sq(frontend->_gpsVertVelNoise);
if (useExtNavVel) {
P[6][6] = sq(extNavVelDelayed.err);
} else {
P[6][6] = sq(frontend->_gpsVertVelNoise);
}
}
// Zero the EKF height datum