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

DataFlash: Use a unique struct for EKF2 logging

Browse Source
This commit is contained in:
priseborough 2016-07-04 08:11:03 +10:00 committed by Andrew Tridgell
parent 772c719df3
commit cc7caa27bc
2 changed files with 17 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/DataFlash/LogFile.cpp
View File

@ -1270,9 +1270,6 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
RI : (int16_t)(100*rngInnov),
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr),
angErr : 0.0f,
velErr : 0.0f,
posErr : 0.0f
};
WriteBlock(&pkt5, sizeof(pkt5));
}
@ -1425,7 +1422,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
Vector3f predictorErrors; // output predictor angle, velocity and position tracking error
ahrs.get_NavEKF2().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
ahrs.get_NavEKF2().getOutputTrackingError(-1,predictorErrors);
struct log_EKF5 pkt5 = {
struct log_NKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
time_us : time_us,
normInnov : (uint8_t)(MIN(100*normInnov,255)),

18
libraries/DataFlash/LogStructure.h
View File

@ -352,6 +352,20 @@ struct PACKED log_EKF5 {
int16_t RI;
uint16_t meaRng;
uint16_t errHAGL;
};
struct PACKED log_NKF5 {
LOG_PACKET_HEADER;
uint64_t time_us;
uint8_t normInnov;
int16_t FIX;
int16_t FIY;
int16_t AFI;
int16_t HAGL;
int16_t offset;
int16_t RI;
uint16_t meaRng;
uint16_t errHAGL;
float angErr;
float velErr;
float posErr;
@ -798,7 +812,7 @@ Format characters in the format string for binary log messages
{ LOG_EKF4_MSG, sizeof(log_EKF4), \
"EKF4","QcccccccbbHBHH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,OFE,FS,TS,SS,GPS" }, \
{ LOG_EKF5_MSG, sizeof(log_EKF5), \
"EKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,_,_,_" }, \
"EKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr" }, \
{ LOG_NKF1_MSG, sizeof(log_EKF1), \
"NKF1","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_NKF2_MSG, sizeof(log_NKF2), \
@ -807,7 +821,7 @@ Format characters in the format string for binary log messages
"NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ LOG_NKF4_MSG, sizeof(log_NKF4), \
"NKF4","QcccccfbbHBHHb","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,OFE,FS,TS,SS,GPS,PI" }, \
{ LOG_NKF5_MSG, sizeof(log_EKF5), \
{ LOG_NKF5_MSG, sizeof(log_NKF5), \
"NKF5","QBhhhcccCCfff","TimeUS,NI,FIX,FIY,AFI,HAGL,offset,RI,rng,Herr,eAng,eVel,ePos" }, \
{ LOG_NKF6_MSG, sizeof(log_EKF1), \
"NKF6","QccCfffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,dPD,PN,PE,PD,GX,GY,GZ" }, \