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

DataFlash: add logging for EKF body frame odometry fusion

This commit is contained in:
priseborough 2017-03-22 14:46:33 +11:00 committed by Randy Mackay
parent 90cd46f4d7
commit d2c89443df
2 changed files with 32 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/DataFlash/LogFile.cpp
View File

@ -1729,6 +1729,24 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
WriteBlock(&pkt10, sizeof(pkt10));
}
}
// write debug data for body frame odometry fusion
Vector3f velBodyInnov,velBodyInnovVar;
static uint32_t lastUpdateTime_ms = 0;
uint32_t updateTime_ms = ahrs.get_NavEKF3().getBodyFrameOdomDebug(-1, velBodyInnov, velBodyInnovVar);
if (updateTime_ms > lastUpdateTime_ms) {
struct log_ekfBodyOdomDebug pkt11 = {
LOG_PACKET_HEADER_INIT(LOG_XKFD_MSG),
time_us : time_us,
velInnovX : velBodyInnov.x,
velInnovY : velBodyInnov.y,
velInnovZ : velBodyInnov.z,
velInnovVarX : velBodyInnovVar.x,
velInnovVarY : velBodyInnovVar.y,
velInnovVarZ : velBodyInnovVar.z
};
WriteBlock(&pkt11, sizeof(pkt11));
updateTime_ms = lastUpdateTime_ms;
}
}
#endif

14
libraries/DataFlash/LogStructure.h
View File

@ -418,6 +418,17 @@ struct PACKED log_RngBcnDebug {
int16_t posD; // Down position of receiver rel to EKF origin (cm)
};
struct PACKED log_ekfBodyOdomDebug {
LOG_PACKET_HEADER;
uint64_t time_us;
float velInnovX;
float velInnovY;
float velInnovZ;
float velInnovVarX;
float velInnovVarY;
float velInnovVarZ;
};
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -936,6 +947,8 @@ Format characters in the format string for binary log messages
"XKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD" }, \
{ LOG_XKQ1_MSG, sizeof(log_Quaternion), "XKQ1", QUAT_FMT, QUAT_LABELS }, \
{ LOG_XKQ2_MSG, sizeof(log_Quaternion), "XKQ2", QUAT_FMT, QUAT_LABELS }, \
{ LOG_XKFD_MSG, sizeof(log_ekfBodyOdomDebug), \
"XKFD","Qffffff","TimeUS,IX,IY,IZ,IVX,IVY,IVZ" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
@ -1135,6 +1148,7 @@ enum LogMessages {
LOG_XKF10_MSG,
LOG_XKQ1_MSG,
LOG_XKQ2_MSG,
LOG_XKFD_MSG,
LOG_DF_MAV_STATS,
LOG_MSG_SBPHEALTH,