mirror of https://github.com/ArduPilot/ardupilot
AP_NavEKF2: use err info from ext nav interface
This commit is contained in:
parent
f92d539b41
commit
e64c92b322
|
@ -788,6 +788,7 @@ void NavEKF2_core::fuseEulerYaw()
|
||||||
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
|
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
|
||||||
} else if (extNavUsedForYaw) {
|
} else if (extNavUsedForYaw) {
|
||||||
// Get the yaw angle from the external vision data
|
// Get the yaw angle from the external vision data
|
||||||
|
R_YAW = sq(extNavDataDelayed.angErr);
|
||||||
extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
|
extNavDataDelayed.quat.to_euler(euler321.x, euler321.y, euler321.z);
|
||||||
measured_yaw = euler321.z;
|
measured_yaw = euler321.z;
|
||||||
} else {
|
} else {
|
||||||
|
@ -847,6 +848,7 @@ void NavEKF2_core::fuseEulerYaw()
|
||||||
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
|
measured_yaw = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
|
||||||
} else if (extNavUsedForYaw) {
|
} else if (extNavUsedForYaw) {
|
||||||
// Get the yaw angle from the external vision data
|
// Get the yaw angle from the external vision data
|
||||||
|
R_YAW = sq(extNavDataDelayed.angErr);
|
||||||
euler312 = extNavDataDelayed.quat.to_vector312();
|
euler312 = extNavDataDelayed.quat.to_vector312();
|
||||||
measured_yaw = euler312.z;
|
measured_yaw = euler312.z;
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -909,11 +909,7 @@ void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat,
|
||||||
|
|
||||||
extNavDataNew.pos = pos;
|
extNavDataNew.pos = pos;
|
||||||
extNavDataNew.quat = quat;
|
extNavDataNew.quat = quat;
|
||||||
if (posErr > 0) {
|
extNavDataNew.posErr = posErr;
|
||||||
extNavDataNew.posErr = posErr;
|
|
||||||
} else {
|
|
||||||
extNavDataNew.posErr = frontend->_gpsHorizPosNoise;
|
|
||||||
}
|
|
||||||
extNavDataNew.angErr = angErr;
|
extNavDataNew.angErr = angErr;
|
||||||
timeStamp_ms = timeStamp_ms - delay_ms;
|
timeStamp_ms = timeStamp_ms - delay_ms;
|
||||||
// Correct for the average intersampling delay due to the filter updaterate
|
// Correct for the average intersampling delay due to the filter updaterate
|
||||||
|
|
Loading…
Reference in New Issue