diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 9bd5b15458..1d77c68ef9 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -1333,9 +1333,9 @@ void NavEKF2::setTerrainHgtStable(bool val) 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion 3 = badly conditioned Y magnetometer fusion - 5 = badly conditioned Z magnetometer fusion - 6 = badly conditioned airspeed fusion - 7 = badly conditioned synthetic sideslip fusion + 4 = badly conditioned Z magnetometer fusion + 5 = badly conditioned airspeed fusion + 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const @@ -1354,10 +1354,10 @@ void NavEKF2::getFilterFaults(int8_t instance, uint16_t &faults) const 1 = velocity measurement timeout 2 = height measurement timeout 3 = magnetometer measurement timeout + 4 = unassigned 5 = unassigned 6 = unassigned 7 = unassigned - 7 = unassigned */ void NavEKF2::getFilterTimeouts(int8_t instance, uint8_t &timeouts) const { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h index 04c1c70632..c7f62b9267 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2.h @@ -256,9 +256,9 @@ public: 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion 3 = badly conditioned Y magnetometer fusion - 5 = badly conditioned Z magnetometer fusion - 6 = badly conditioned airspeed fusion - 7 = badly conditioned synthetic sideslip fusion + 4 = badly conditioned Z magnetometer fusion + 5 = badly conditioned airspeed fusion + 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ void getFilterFaults(int8_t instance, uint16_t &faults) const; @@ -270,10 +270,10 @@ public: 1 = velocity measurement timeout 2 = height measurement timeout 3 = magnetometer measurement timeout + 4 = unassigned 5 = unassigned 6 = unassigned 7 = unassigned - 7 = unassigned */ void getFilterTimeouts(int8_t instance, uint8_t &timeouts) const; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp index 5387d8e424..e566f97e7a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Outputs.cpp @@ -484,9 +484,9 @@ return the filter fault status as a bitmasked integer 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion 3 = badly conditioned Y magnetometer fusion - 5 = badly conditioned Z magnetometer fusion - 6 = badly conditioned airspeed fusion - 7 = badly conditioned synthetic sideslip fusion + 4 = badly conditioned Z magnetometer fusion + 5 = badly conditioned airspeed fusion + 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ void NavEKF2_core::getFilterFaults(uint16_t &faults) const @@ -645,4 +645,4 @@ bool NavEKF2_core::getDataEKFGSF(float &yaw_composite, float &yaw_composite_vari return yawEstimator->getLogData(yaw_composite, yaw_composite_variance, yaw, innov_VN, innov_VE, weight); } return false; -} \ No newline at end of file +} diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index f83acfa54b..163b246331 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -246,9 +246,9 @@ public: 1 = velocities are NaN 2 = badly conditioned X magnetometer fusion 3 = badly conditioned Y magnetometer fusion - 5 = badly conditioned Z magnetometer fusion - 6 = badly conditioned airspeed fusion - 7 = badly conditioned synthetic sideslip fusion + 4 = badly conditioned Z magnetometer fusion + 5 = badly conditioned airspeed fusion + 6 = badly conditioned synthetic sideslip fusion 7 = filter is not initialised */ void getFilterFaults(uint16_t &faults) const;