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

AP_NavEKF2: use err info from ext nav interface

This commit is contained in:
chobits 2020-06-04 10:53:00 +08:00 committed by Randy Mackay
parent f92d539b41
commit e64c92b322
2 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -788,6 +788,7 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
} else if (extNavUsedForYaw) {
// Get the yaw angle from the external vision data
R_YAW = sq(extNavDataDelayed.angErr);
extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
measured_yaw = euler321.z;
} else {
@ -847,6 +848,7 @@ void NavEKF2_core::fuseEulerYaw()
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
} else if (extNavUsedForYaw) {
// Get the yaw angle from the external vision data
R_YAW = sq(extNavDataDelayed.angErr);
euler312 = extNavDataDelayed.quat.to_vector312();
measured_yaw = euler312.z;
} else {

6
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -909,11 +909,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
extNavDataNew.pos = pos;
extNavDataNew.quat = quat;
if (posErr > 0) {
extNavDataNew.posErr = posErr;
} else {
extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
}
extNavDataNew.posErr = posErr;
extNavDataNew.angErr = angErr;
timeStamp_ms = timeStamp_ms - delay_ms;
// Correct for the average intersampling delay due to the filter updaterate