diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 428d6d4daa..0e7bd27b6d 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -1420,7 +1420,10 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) beaconPosE : (int16_t)(100*beaconPosNED.y), beaconPosD : (int16_t)(100*beaconPosNED.z), offsetHigh : (int16_t)(100*bcnPosOffsetHigh), - offsetLow : (int16_t)(100*bcnPosOffsetLow) + offsetLow : (int16_t)(100*bcnPosOffsetLow), + posN : 0, + posE : 0, + posD : 0 }; WriteBlock(&pkt10, sizeof(pkt10)); } @@ -1695,7 +1698,8 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) Vector3f beaconPosNED; float bcnPosOffsetHigh; float bcnPosOffsetLow; - if (ahrs.get_NavEKF3().getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow)) { + Vector3f posNED; + if (ahrs.get_NavEKF3().getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow, posNED)) { if (rng > 0.0f) { struct log_RngBcnDebug pkt10 = { LOG_PACKET_HEADER_INIT(LOG_XKF10_MSG), @@ -1709,7 +1713,11 @@ void DataFlash_Class::Log_Write_EKF3(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled) beaconPosE : (int16_t)(100*beaconPosNED.y), beaconPosD : (int16_t)(100*beaconPosNED.z), offsetHigh : (int16_t)(100*bcnPosOffsetHigh), - offsetLow : (int16_t)(100*bcnPosOffsetLow) + offsetLow : (int16_t)(100*bcnPosOffsetLow), + posN : (int16_t)(100*posNED.x), + posE : (int16_t)(100*posNED.y), + posD : (int16_t)(100*posNED.z) + }; WriteBlock(&pkt10, sizeof(pkt10)); } diff --git a/libraries/DataFlash/LogStructure.h b/libraries/DataFlash/LogStructure.h index 9c90bbfb59..1e7ba2fcec 100644 --- a/libraries/DataFlash/LogStructure.h +++ b/libraries/DataFlash/LogStructure.h @@ -401,6 +401,9 @@ struct PACKED log_RngBcnDebug { int16_t beaconPosD; // beacon down position (cm) int16_t offsetHigh; // high estimate of vertical position offset of beacons rel to EKF origin (cm) int16_t offsetLow; // low estimate of vertical position offset of beacons rel to EKF origin (cm) + int16_t posN; // North position of receiver rel to EKF origin (cm) + int16_t posE; // East position of receiver rel to EKF origin (cm) + int16_t posD; // Down position of receiver rel to EKF origin (cm) }; struct PACKED log_Cmd { @@ -854,7 +857,7 @@ Format characters in the format string for binary log messages { LOG_NKF9_MSG, sizeof(log_NKF4), \ "NKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \ { LOG_NKF10_MSG, sizeof(log_RngBcnDebug), \ - "NKF0","QBccCCccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL" }, \ + "NKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD" }, \ { LOG_XKF1_MSG, sizeof(log_EKF1), \ "XKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \ { LOG_XKF2_MSG, sizeof(log_NKF2a), \ @@ -874,7 +877,7 @@ Format characters in the format string for binary log messages { LOG_XKF9_MSG, sizeof(log_NKF4), \ "XKF9","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \ { LOG_XKF10_MSG, sizeof(log_RngBcnDebug), \ - "XKF0","QBccCCccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL" }, \ + "XKF0","QBccCCcccccccc","TimeUS,ID,rng,innov,SIV,TR,BPN,BPE,BPD,OFH,OFL,OFN,OFE,OFD" }, \ { LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \ "TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \ { LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \