Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Add EKF fault status logging

This commit is contained in:
priseborough 2014-05-14 18:18:28 +10:00 committed by Andrew Tridgell
parent ce8e1daa16
commit f01cc78d37
2 changed files with 9 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/DataFlash/DataFlash.h
View File

@ -323,6 +323,8 @@ struct PACKED log_EKF4 {
int16_t sqrtvarVT;
int8_t offsetNorth;
int8_t offsetEast;
uint8_t faults;
uint8_t divergeRate;
};
struct PACKED log_Cmd {
@ -386,7 +388,7 @@ struct PACKED log_Radio {
{ LOG_EKF3_MSG, sizeof(log_EKF3), \
"EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
{ LOG_EKF4_MSG, sizeof(log_EKF4), \
"EKF4","Icccccccbb","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE" }, \
"EKF4","IcccccccbbBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,DS" }, \
{ LOG_CMD_MSG, sizeof(log_Cmd), \
"CMD", "IHHHfffffff","TimeMS,CTot,CNum,CId,Prm1,Prm2,Prm3,Prm4,Lat,Lng,Alt" }, \
{ LOG_RADIO_MSG, sizeof(log_Radio), \

7
libraries/DataFlash/LogFile.cpp
View File

@ -945,7 +945,10 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
Vector3f magVar;
float tasVar;
Vector2f offset;
uint8_t faultStatus;
float deltaGyroBias;
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_NavEKF().getFilterFaults(faultStatus, deltaGyroBias);
struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
time_ms : hal.scheduler->millis(),
@ -957,7 +960,9 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
sqrtvarMZ : (int16_t)(100*magVar.z),
sqrtvarVT : (int16_t)(100*tasVar),
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y)
offsetEast : (int8_t)(offset.y),
faults : (uint8_t)(faultStatus),
divergeRate : (uint8_t)(100*deltaGyroBias)
};
WriteBlock(&pkt4, sizeof(pkt4));
}