diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 39cdef5efd..c6abd5f7da 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -350,13 +350,13 @@ struct PACKED log_EKF4 { struct PACKED log_EKF5 { LOG_PACKET_HEADER; uint32_t time_ms; - float flowX; - float flowY; - float omegaX; - float omegaY; - float gndPos; + int16_t FIX; + int16_t FIY; + int16_t AFIX; + int16_t AFIY; + int16_t gndPos; uint8_t scaler; - uint8_t quality; + int16_t RI; uint16_t range; }; @@ -529,7 +529,7 @@ struct PACKED log_Esc { { LOG_ESC8_MSG, sizeof(log_Esc), \ "ESC8", "Icccc", "TimeMS,RPM,Volt,Curr,Temp" }, \ { LOG_EKF5_MSG, sizeof(log_EKF5), \ - "EKF5","IfffffBBc","TimeMS,flowX,flowY,omegaX,omegaY,gndPos,fScaler,flowQual,rng" } + "EKF5","IhhhhcBcC","TimeMS,FIX,FIY,AFIX,AFIY,gndPos,fScaler,RI,rng" } #if HAL_CPU_CLASS >= HAL_CPU_CLASS_75 #define LOG_COMMON_STRUCTURES LOG_BASE_STRUCTURES, LOG_EXTRA_STRUCTURES diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 663b4af6d4..fd7a240548 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -996,21 +996,21 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs) // Write fifth EKF packet float fscale; float gndPos; - float flowX, flowY; - float omegaX, omegaY; + float flowInnovX, flowInnovY; + float augFlowInnovX, augFlowInnovY; + float rngInnov; float range; - uint8_t flowQual; - ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowX, flowY, omegaX, omegaY, flowQual, range); + ahrs.get_NavEKF().getFlowDebug(fscale, gndPos, flowInnovX, flowInnovY, flowInnovY, augFlowInnovY, rngInnov, range); struct log_EKF5 pkt5 = { LOG_PACKET_HEADER_INIT(LOG_EKF5_MSG), time_ms : hal.scheduler->millis(), - flowX : (float)(flowX), - flowY : (float)(flowY), - omegaX : (float)(omegaX), - omegaY : (float)(omegaY), - gndPos : (float)(gndPos), + FIX : (int16_t)(1000*flowInnovX), + FIY : (int16_t)(1000*flowInnovY), + AFIX : (int16_t)(1000*flowInnovY), + AFIY : (int16_t)(1000*augFlowInnovY), + gndPos : (int16_t)(100*gndPos), scaler: (uint8_t)(100*fscale), - quality : (uint8_t)(flowQual), + RI : (int16_t)(100*rngInnov), range : (uint16_t)(100*range) }; WriteBlock(&pkt5, sizeof(pkt5));