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

Dataflash : Improved logging of EKF sensor health checks 

This changes the definition sof the messages in the EKF4 message to make it more useful
The values shown now relate directly to how much margin there is before a measurement
is rejected, enabling better tuning of error trap parameters
This commit is contained in:
priseborough 2014-04-01 10:05:07 +11:00 committed by Andrew Tridgell
parent 04036d7777
commit 09c5ba1aed
2 changed files with 20 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/DataFlash/DataFlash.h
View File

@ -314,16 +314,15 @@ struct PACKED log_EKF3 {
struct PACKED log_EKF4 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t sqrtvarVN;
int16_t sqrtvarVE;
int16_t sqrtvarVD;
int16_t sqrtvarPN;
int16_t sqrtvarPE;
int16_t sqrtvarPD;
int16_t sqrtvarV;
int16_t sqrtvarP;
int16_t sqrtvarH;
int16_t sqrtvarMX;
int16_t sqrtvarMY;
int16_t sqrtvarMZ;
int16_t sqrtvarVT;
int8_t offsetNorth;
int8_t offsetEast;
};
struct PACKED log_Cmd {
@ -387,7 +386,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","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }, \
"EKF4","Icccccccbb","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE" }, \
{ 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), \

27
libraries/DataFlash/LogFile.cpp
View File

@ -937,24 +937,25 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
WriteBlock(&pkt3, sizeof(pkt3));
// Write fourth EKF packet
Vector3f velVar;
Vector3f posVar;
float velVar;
float posVar;
float hgtVar;
Vector3f magVar;
float tasVar;
ahrs.get_NavEKF().getVariances(velVar, posVar, magVar, tasVar);
Vector2f offset;
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
time_ms : hal.scheduler->millis(),
sqrtvarVN : (int16_t)(100*sqrtf(velVar.x)),
sqrtvarVE : (int16_t)(100*sqrtf(velVar.y)),
sqrtvarVD : (int16_t)(100*sqrtf(velVar.z)),
sqrtvarPN : (int16_t)(100*sqrtf(posVar.x)),
sqrtvarPE : (int16_t)(100*sqrtf(posVar.y)),
sqrtvarPD : (int16_t)(100*sqrtf(posVar.z)),
sqrtvarMX : (int16_t)(sqrtf(magVar.x)),
sqrtvarMY : (int16_t)(sqrtf(magVar.y)),
sqrtvarMZ : (int16_t)(sqrtf(magVar.z)),
sqrtvarVT : (int16_t)(100*sqrtf(tasVar))
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarMX : (int16_t)(100*magVar.x),
sqrtvarMY : (int16_t)(100*magVar.y),
sqrtvarMZ : (int16_t)(100*magVar.z),
sqrtvarVT : (int16_t)(100*tasVar),
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y)
};
WriteBlock(&pkt4, sizeof(pkt4));
}