diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index d7b3a5ea08..92ccc0ab3c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -1481,9 +1481,9 @@ void NavEKF3::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 NavEKF3::getFilterFaults(int8_t instance, uint16_t &faults) const @@ -1502,10 +1502,10 @@ void NavEKF3::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 NavEKF3::getFilterTimeouts(int8_t instance, uint8_t &timeouts) const { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 95971e74dc..0aaf1b5872 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -327,9 +327,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; @@ -341,10 +341,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_NavEKF3/AP_NavEKF3_Outputs.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp index e9ccaa0b56..ac80d637ae 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp @@ -492,9 +492,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 NavEKF3_core::getFilterFaults(uint16_t &faults) const