aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

DataFlash: log 3-state range beacon estimator states for EKF3

Browse Source
This commit is contained in:
priseborough 2016-12-17 21:00:57 +11:00 committed by Randy Mackay
parent 92b8c33b19
commit a02a84560f
2 changed files with 16 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/DataFlash/LogFile.cpp
View File

@ -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));
}

7
libraries/DataFlash/LogStructure.h
View File

@ -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), \