diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 5bab018a9d..c31865f97c 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -323,6 +323,8 @@ struct PACKED log_EKF4 { int16_t sqrtvarVT; int8_t offsetNorth; int8_t offsetEast; + uint8_t faults; + uint8_t divergeRate; }; struct PACKED log_Cmd { @@ -386,7 +388,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","Icccccccbb","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE" }, \ + "EKF4","IcccccccbbBB","TimeMS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,DS" }, \ { 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), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index f0c9004c8e..2c5d207194 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -945,7 +945,10 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) Vector3f magVar; float tasVar; Vector2f offset; + uint8_t faultStatus; + float deltaGyroBias; ahrs.get_NavEKF().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); + ahrs.get_NavEKF().getFilterFaults(faultStatus, deltaGyroBias); struct log_EKF4 pkt4 = { LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG), time_ms : hal.scheduler->millis(), @@ -957,7 +960,9 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) sqrtvarMZ : (int16_t)(100*magVar.z), sqrtvarVT : (int16_t)(100*tasVar), offsetNorth : (int8_t)(offset.x), - offsetEast : (int8_t)(offset.y) + offsetEast : (int8_t)(offset.y), + faults : (uint8_t)(faultStatus), + divergeRate : (uint8_t)(100*deltaGyroBias) }; WriteBlock(&pkt4, sizeof(pkt4)); }