zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

DataFlash: Add logging of EKF GPS check status

master
Paul Riseborough 9 years ago committed by Randy Mackay
parent
5177746c00
commit
ba6387f206
2 changed files with 11 additions and 4 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      libraries/DataFlash/DataFlash.h
  2. 10
      libraries/DataFlash/LogFile.cpp

5
libraries/DataFlash/DataFlash.h
Unescape View File

@ -445,6 +445,7 @@ struct PACKED log_EKF4 {
uint8_t faults; uint8_t faults;
uint8_t timeouts; uint8_t timeouts;
uint16_t solution; uint16_t solution;
uint16_t gps;
}; };
struct PACKED log_EKF5 { struct PACKED log_EKF5 {
@ -805,7 +806,7 @@ Format characters in the format string for binary log messages
{ LOG_EKF3_MSG, sizeof(log_EKF3), \ { LOG_EKF3_MSG, sizeof(log_EKF3), \
"EKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \ "EKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
{ LOG_EKF4_MSG, sizeof(log_EKF4), \ { LOG_EKF4_MSG, sizeof(log_EKF4), \
"EKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \ "EKF4","QcccccccbbBBHH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS,GPS" }, \
{ LOG_EKF5_MSG, sizeof(log_EKF5), \ { LOG_EKF5_MSG, sizeof(log_EKF5), \
"EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ "EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
{ LOG_NKF1_MSG, sizeof(log_EKF1), \ { LOG_NKF1_MSG, sizeof(log_EKF1), \
@ -815,7 +816,7 @@ Format characters in the format string for binary log messages
{ LOG_NKF3_MSG, sizeof(log_NKF3), \ { LOG_NKF3_MSG, sizeof(log_NKF3), \
"NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \ "NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ LOG_NKF4_MSG, sizeof(log_EKF4), \ { LOG_NKF4_MSG, sizeof(log_EKF4), \
"NKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \ "NKF4","QcccccccbbBBHH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS,GPS" }, \
{ LOG_NKF5_MSG, sizeof(log_EKF5), \ { LOG_NKF5_MSG, sizeof(log_EKF5), \
"NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ "NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \

10
libraries/DataFlash/LogFile.cpp
Unescape View File

@ -1225,10 +1225,12 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
Vector2f offset; Vector2f offset;
uint8_t faultStatus, timeoutStatus; uint8_t faultStatus, timeoutStatus;
nav_filter_status solutionStatus; nav_filter_status solutionStatus;
nav_gps_status gpsStatus {};
ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_NavEKF().getFilterFaults(faultStatus); ahrs.get_NavEKF().getFilterFaults(faultStatus);
ahrs.get_NavEKF().getFilterTimeouts(timeoutStatus); ahrs.get_NavEKF().getFilterTimeouts(timeoutStatus);
ahrs.get_NavEKF().getFilterStatus(solutionStatus); ahrs.get_NavEKF().getFilterStatus(solutionStatus);
ahrs.get_NavEKF().getFilterGpsStatus(gpsStatus);
struct log_EKF4 pkt4 = { struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG), LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
time_us : hal.scheduler->micros64(), time_us : hal.scheduler->micros64(),
@ -1243,7 +1245,8 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
offsetEast : (int8_t)(offset.y), offsetEast : (int8_t)(offset.y),
faults : (uint8_t)(faultStatus), faults : (uint8_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus), timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value) solution : (uint16_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value)
}; };
WriteBlock(&pkt4, sizeof(pkt4)); WriteBlock(&pkt4, sizeof(pkt4));
@ -1375,10 +1378,12 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
Vector2f offset; Vector2f offset;
uint8_t faultStatus=0, timeoutStatus=0; uint8_t faultStatus=0, timeoutStatus=0;
nav_filter_status solutionStatus {}; nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {};
ahrs.get_NavEKF2().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); ahrs.get_NavEKF2().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
ahrs.get_NavEKF2().getFilterFaults(faultStatus); ahrs.get_NavEKF2().getFilterFaults(faultStatus);
ahrs.get_NavEKF2().getFilterTimeouts(timeoutStatus); ahrs.get_NavEKF2().getFilterTimeouts(timeoutStatus);
ahrs.get_NavEKF2().getFilterStatus(solutionStatus); ahrs.get_NavEKF2().getFilterStatus(solutionStatus);
ahrs.get_NavEKF2().getFilterGpsStatus(gpsStatus);
struct log_EKF4 pkt4 = { struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG), LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
time_us : hal.scheduler->micros64(), time_us : hal.scheduler->micros64(),
@ -1393,7 +1398,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
offsetEast : (int8_t)(offset.y), offsetEast : (int8_t)(offset.y),
faults : (uint8_t)(faultStatus), faults : (uint8_t)(faultStatus),
timeouts : (uint8_t)(timeoutStatus), timeouts : (uint8_t)(timeoutStatus),
solution : (uint16_t)(solutionStatus.value) solution : (uint16_t)(solutionStatus.value),
gps : (uint16_t)(gpsStatus.value)
}; };
WriteBlock(&pkt4, sizeof(pkt4)); WriteBlock(&pkt4, sizeof(pkt4));

Write Preview
Loading…
Cancel
Save