Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Add logging for EKF3 state variances

Browse Source
This commit is contained in:
priseborough 2017-05-21 12:13:08 +10:00 committed by Francisco Ferreira
parent 2b97d0f5c9
commit 4c08622a7c
2 changed files with 66 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
libraries/DataFlash/LogFile.cpp
View File

@ -1279,7 +1279,6 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
};
WriteBlock(&pktq1, sizeof(pktq1));
// log innovations for the second IMU if enabled
if (ahrs.get_NavEKF2().activeCores() >= 2) {
// Write 6th EKF packet
@ -1772,6 +1771,49 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
updateTime_ms = lastUpdateTime_ms;
}
// log state variances every 0.49s
static uint32_t lastEkfStateVarLogTime_ms = 0;
if (AP_HAL::millis() - lastEkfStateVarLogTime_ms > 490) {
lastEkfStateVarLogTime_ms = AP_HAL::millis();
float stateVar[24];
ahrs.get_NavEKF3().getStateVariances(-1, stateVar);
struct log_ekfStateVar pktv1 = {
LOG_PACKET_HEADER_INIT(LOG_XKV1_MSG),
time_us : time_us,
v00 : stateVar[0],
v01 : stateVar[1],
v02 : stateVar[2],
v03 : stateVar[3],
v04 : stateVar[4],
v05 : stateVar[5],
v06 : stateVar[6],
v07 : stateVar[7],
v08 : stateVar[8],
v09 : stateVar[9],
v10 : stateVar[10],
v11 : stateVar[11]
};
WriteBlock(&pktv1, sizeof(pktv1));
struct log_ekfStateVar pktv2 = {
LOG_PACKET_HEADER_INIT(LOG_XKV2_MSG),
time_us : time_us,
v00 : stateVar[12],
v01 : stateVar[13],
v02 : stateVar[14],
v03 : stateVar[15],
v04 : stateVar[16],
v05 : stateVar[17],
v06 : stateVar[18],
v07 : stateVar[19],
v08 : stateVar[20],
v09 : stateVar[21],
v10 : stateVar[22],
v11 : stateVar[23]
};
WriteBlock(&pktv2, sizeof(pktv2));
}
// log EKF timing statistics every 5s
static uint32_t lastTimingLogTime_ms = 0;
if (AP_HAL::millis() - lastTimingLogTime_ms > 5000) {

23
libraries/DataFlash/LogStructure.h
View File

@ -444,6 +444,23 @@ struct PACKED log_ekfBodyOdomDebug {
float velInnovVarZ;
};
struct PACKED log_ekfStateVar {
LOG_PACKET_HEADER;
uint64_t time_us;
float v00;
float v01;
float v02;
float v03;
float v04;
float v05;
float v06;
float v07;
float v08;
float v09;
float v10;
float v11;
};
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -1006,6 +1023,10 @@ Format characters in the format string for binary log messages
{ 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_XKV1_MSG, sizeof(log_ekfStateVar), \
"XKV1","Qffffffffffff","TimeUS,V00,V01,V02,V03,V04,V05,V06,V07,V08,V09,V10,V11" }, \
{ LOG_XKV2_MSG, sizeof(log_ekfStateVar), \
"XKV2","Qffffffffffff","TimeUS,V12,V13,V14,V15,V16,V17,V18,V19,V20,V21,V22,V23" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
@ -1210,6 +1231,8 @@ enum LogMessages {
LOG_XKQ1_MSG,
LOG_XKQ2_MSG,
LOG_XKFD_MSG,
LOG_XKV1_MSG,
LOG_XKV2_MSG,
LOG_DF_MAV_STATS,
LOG_MSG_SBPHEALTH,