From 987d2611097b40eef08c51b136e6a66dcd71aeb6 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 6 Nov 2015 14:04:53 +1100 Subject: [PATCH] DataFlash: Update logging of EKF2 primary core index Changes made in response to review comments --- libraries/DataFlash/DataFlash.h | 4 ++-- libraries/DataFlash/LogFile.cpp | 5 ++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index 54d86983e4..55560cf2c7 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -461,7 +461,7 @@ struct PACKED log_NKF4 { uint8_t timeouts; uint16_t solution; uint16_t gps; - uint8_t primary; + int8_t primary; }; struct PACKED log_EKF5 { @@ -832,7 +832,7 @@ Format characters in the format string for binary log messages { LOG_NKF3_MSG, sizeof(log_NKF3), \ "NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \ { LOG_NKF4_MSG, sizeof(log_NKF4), \ - "NKF4","QcccccfbbBBHHB","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \ + "NKF4","QcccccfbbBBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \ { LOG_NKF5_MSG, sizeof(log_EKF5), \ "NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index e0a7eda042..090f0bbb6b 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1367,8 +1367,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) ahrs.get_NavEKF2().getFilterGpsStatus(gpsStatus); float tiltError; ahrs.get_NavEKF2().getTiltError(tiltError); - uint8_t primaryIndex; - ahrs.get_NavEKF2().getPrimaryCoreIndex(primaryIndex); + uint8_t primaryIndex = ahrs.get_NavEKF2().getPrimaryCoreIndex(); struct log_NKF4 pkt4 = { LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG), time_us : hal.scheduler->micros64(), @@ -1384,7 +1383,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) timeouts : (uint8_t)(timeoutStatus), solution : (uint16_t)(solutionStatus.value), gps : (uint16_t)(gpsStatus.value), - primary : (uint8_t)primaryIndex + primary : (int8_t)primaryIndex }; WriteBlock(&pkt4, sizeof(pkt4));