Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: remove unused isDeltaYaw param from resetQuatStateYawOnly

Browse Source
This commit is contained in:
Andrew Tridgell 2020-04-23 18:40:50 +10:00
parent 197d31e9cc
commit 080f555b16
3 changed files with 5 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -187,7 +187,7 @@ void NavEKF3_core::realignYawGPS()
}
// keep roll and pitch and reset yaw
resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f)), false);
resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f)));
// reset the velocity and position states as they will be inaccurate due to bad yaw
velResetSource = GPS;
@ -1489,7 +1489,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(15.0f))) {
// keep roll and pitch and reset yaw
resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, false);
resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF);
// record the emergency reset event
EKFGSF_yaw_reset_request_ms = 0;
@ -1521,7 +1521,7 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
}
void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw)
void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance)
{
Quaternion quatBeforeReset = stateStruct.quat;
Vector3f angleErrVarVec = calcRotVecVariances();
@ -1533,16 +1533,10 @@ void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, bool isDe
if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) {
// rolled more than pitched so use 321 rotation order
stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
if (isDeltaYaw) {
yaw = wrap_PI(yaw + eulerAngles.z);
}
stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, yaw);
} else {
// pitched more than rolled so use 312 rotation order
eulerAngles = stateStruct.quat.to_vector312();
if (isDeltaYaw) {
yaw = wrap_PI(yaw + eulerAngles.z);
}
stateStruct.quat.from_vector312(eulerAngles.x, eulerAngles.y, yaw);
}

2
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -1657,7 +1657,7 @@ void NavEKF3_core::setYawFromMag()
float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
// update quaternion states and covariances
resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)), false);
resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)));
}
// update quaternion, mag field states and associated variances using magnetomer and declination data

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -903,8 +903,7 @@ private:
// reset the quaternion state covariances using the supplied yaw variance
// yaw : new yaw angle (rad)
// yaw_variance : variance of new yaw angle (rad^2)
// isDeltaYaw : true when the yaw should be added to the existing yaw angle
void resetQuatStateYawOnly(float yaw, float yawVariance, bool isDeltaYaw);
void resetQuatStateYawOnly(float yaw, float yawVariance);
// attempt to reset the yaw to the EKF-GSF value
// returns false if unsuccessful