diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index c6abd5f7da..54a3c8044a 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -345,6 +345,7 @@ struct PACKED log_EKF4 { int8_t offsetEast; uint8_t faults; uint8_t staticmode; + uint8_t timeouts; }; struct PACKED log_EKF5 { @@ -503,7 +504,7 @@ struct PACKED log_Esc { { 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","IcccccccbbBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,StaticMode" }, \ + "EKF4","IcccccccbbBBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,StaticMode,TS" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","IBLLHffHH","TimeMS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \ { LOG_UBX1_MSG, sizeof(log_Ubx1), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index fd7a240548..1b1ba9d032 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -974,8 +974,10 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) float tasVar; Vector2f offset; uint8_t faultStatus; + uint8_t timeoutStatus; ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); ahrs.get_NavEKF().getFilterFaults(faultStatus); + ahrs.get_NavEKF().getFilterTimeouts(timeoutStatus); struct log_EKF4 pkt4 = { LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG), time_ms : hal.scheduler->millis(), @@ -989,7 +991,8 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) offsetNorth : (int8_t)(offset.x), offsetEast : (int8_t)(offset.y), faults : (uint8_t)(faultStatus), - staticmode : (uint8_t)(ahrs.get_NavEKF().getStaticMode()) + staticmode : (uint8_t)(ahrs.get_NavEKF().getStaticMode()), + timeouts : (uint8_t)(timeoutStatus) }; WriteBlock(&pkt4, sizeof(pkt4));