Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -04:00
Code Issues Packages Projects Releases Wiki Activity

DataFlash: log range beacon fusion data

Browse Source
This commit is contained in:
priseborough 2016-11-23 21:04:51 +09:00 committed by Randy Mackay
parent bbdf3982c6
commit 431a74e0b4
2 changed files with 49 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
libraries/DataFlash/LogFile.cpp
View File

@ -1383,6 +1383,37 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
};
WriteBlock(&pkt9, sizeof(pkt9));
}
// write range beacon fusion debug packet if the range value is non-zero
if (ahrs.get_beacon() != nullptr) {
uint8_t ID;
float rng;
float innovVar;
float innov;
float testRatio;
Vector3f beaconPosNED;
float bcnPosOffsetHigh;
float bcnPosOffsetLow;
if (ahrs.get_NavEKF2().getRangeBeaconDebug(-1, ID, rng, innov, innovVar, testRatio, beaconPosNED, bcnPosOffsetHigh, bcnPosOffsetLow)) {
if (rng > 0.0f) {
struct log_RngBcnDebug pkt10 = {
LOG_PACKET_HEADER_INIT(LOG_NKF10_MSG),
time_us : time_us,
ID : (uint8_t)ID,
rng : (int16_t)(100*rng),
innov : (int16_t)(100*innov),
sqrtInnovVar : (uint16_t)(100*safe_sqrt(innovVar)),
testRatio : (uint16_t)(100*testRatio),
beaconPosN : (int16_t)(100*beaconPosNED.x),
beaconPosE : (int16_t)(100*beaconPosNED.y),
beaconPosD : (int16_t)(100*beaconPosNED.z),
offsetHigh : (int16_t)(100*bcnPosOffsetHigh),
offsetLow : (int16_t)(100*bcnPosOffsetLow)
};
WriteBlock(&pkt10, sizeof(pkt10));
}
}
}
}
#endif

18
libraries/DataFlash/LogStructure.h
View File

@ -371,6 +371,21 @@ struct PACKED log_NKF5 {
float posErr;
};
struct PACKED log_RngBcnDebug {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t ID; // beacon identifier
int16_t rng; // beacon range (cm)
int16_t innov; // beacon range innovation (cm)
uint16_t sqrtInnovVar; // sqrt of beacon range innovation variance (cm)
uint16_t testRatio; // beacon range innovation consistency test ratio *100
int16_t beaconPosN; // beacon north position (cm)
int16_t beaconPosE; // beacon east position (cm)
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)
};
struct PACKED log_Cmd {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -821,6 +836,8 @@ Format characters in the format string for binary log messages
"NKF8","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ 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" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \
"TERR","QBLLHffHH","TimeUS,Status,Lat,Lng,Spacing,TerrH,CHeight,Pending,Loaded" }, \
{ LOG_GPS_UBX1_MSG, sizeof(log_Ubx1), \
@ -1000,6 +1017,7 @@ enum LogMessages {
LOG_NKF7_MSG,
LOG_NKF8_MSG,
LOG_NKF9_MSG,
LOG_NKF10_MSG,
LOG_DF_MAV_STATS,
LOG_MSG_SBPHEALTH,