aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF : Add fault and timeout logging

Browse Source
This commit is contained in:
priseborough 2014-11-01 18:04:18 +11:00 committed by Andrew Tridgell
parent f640ec30ff
commit 517026980b
2 changed files with 48 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -4376,24 +4376,44 @@ bool NavEKF::assume_zero_sideslip(void) const
/* /*
return the filter fault status as a bitmasked integer return the filter fault status as a bitmasked integer
0 = unassigned 0 = quaternions are NaN
1 = unassigned 1 = velocities are NaN
2 = badly conditioned X magnetometer fusion 2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion 3 = badly conditioned Y magnetometer fusion
4 = badly conditioned Z magnetometer fusion 5 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion 6 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion 7 = badly conditioned synthetic sideslip fusion
7 = unassigned 7 = filter is not initialised
return normalised delta gyro bias length used for divergence test
*/ */
void NavEKF::getFilterFaults(uint8_t &faults) const void NavEKF::getFilterFaults(uint8_t &faults) const
{ {
faults = (faultStatus.bad_xmag<<2 | faults = (state.quat.is_nan()<<0 |
state.velocity.is_nan()<<1 |
faultStatus.bad_xmag<<2 |
faultStatus.bad_ymag<<3 | faultStatus.bad_ymag<<3 |
faultStatus.bad_zmag<<4 | faultStatus.bad_zmag<<4 |
faultStatus.bad_airspeed<<5 | faultStatus.bad_airspeed<<5 |
faultStatus.bad_sideslip<<6); faultStatus.bad_sideslip<<6 |
!statesInitialised<<7);
} }
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
7 = unassigned
*/
void NavEKF::getFilterTimeouts(uint8_t &timeouts) const
{
timeouts = (posTimeout<<0 |
velTimeout<<1 |
hgtTimeout<<2 |
magTimeout<<3);
}
#endif // HAL_CPU_CLASS #endif // HAL_CPU_CLASS

26
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -167,18 +167,30 @@ public:
/* /*
return the filter fault status as a bitmasked integer return the filter fault status as a bitmasked integer
0 = unassigned 0 = quaternions are NaN
1 = unassigned 1 = velocities are NaN
2 = badly conditioned X magnetometer fusion 2 = badly conditioned X magnetometer fusion
3 = badly conditioned Y magnetometer fusion 3 = badly conditioned Y magnetometer fusion
4 = badly conditioned Z magnetometer fusion 5 = badly conditioned Z magnetometer fusion
5 = badly conditioned airspeed fusion 6 = badly conditioned airspeed fusion
6 = badly conditioned synthetic sideslip fusion 7 = badly conditioned synthetic sideslip fusion
7 = unassigned 7 = filter is not initialised
return normalised delta gyro bias length used for divergence test
*/ */
void getFilterFaults(uint8_t &faults) const; void getFilterFaults(uint8_t &faults) const;
/*
return filter timeout status as a bitmasked integer
0 = position measurement timeout
1 = velocity measurement timeout
2 = height measurement timeout
3 = magnetometer measurement timeout
5 = unassigned
6 = unassigned
7 = unassigned
7 = unassigned
*/
void getFilterTimeouts(uint8_t &timeouts) const;
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
private: private: