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

DataFlash: Log EKF2 output predictor tracking data

Browse Source
This commit is contained in:
priseborough 2016-07-01 10:44:36 +10:00 committed by Andrew Tridgell
parent 253f744824
commit 00b66ddc07
2 changed files with 40 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
libraries/DataFlash/LogFile.cpp
View File

@ -1268,10 +1268,13 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
HAGL : (int16_t)(100*HAGL),
offset : (int16_t)(100*gndOffset),
RI : (int16_t)(100*rngInnov),
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr)
};
WriteBlock(&pkt5, sizeof(pkt5));
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr),
angErr : 0.0f,
velErr : 0.0f,
posErr : 0.0f
};
WriteBlock(&pkt5, sizeof(pkt5));
}
}
// only log EKF2 if enabled
@ -1411,31 +1414,34 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
WriteBlock(&pkt4, sizeof(pkt4));
// Write fifth EKF packet - take data from the primary instance
if (optFlowEnabled) {
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
float HAGL=0; // height above ground level
float rngInnov=0; // range finder innovations
float range=0; // measured range
float gndOffsetErr=0; // filter ground offset state error
ahrs.get_NavEKF2().getFlowDebug(-1,normInnov, gndOffset, flowInnovX, flowInnovY, auxFlowInnov, HAGL, rngInnov, range, gndOffsetErr);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
time_us : time_us,
normInnov : (uint8_t)(MIN(100*normInnov,255)),
FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY),
AFI : (int16_t)(1000*auxFlowInnov),
HAGL : (int16_t)(100*HAGL),
offset : (int16_t)(100*gndOffset),
RI : (int16_t)(100*rngInnov),
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr)
};
WriteBlock(&pkt5, sizeof(pkt5));
}
float normInnov=0; // normalised innovation variance ratio for optical flow observations fused by the main nav filter
float gndOffset=0; // estimated vertical position of the terrain relative to the nav filter zero datum
float flowInnovX=0, flowInnovY=0; // optical flow LOS rate vector innovations from the main nav filter
float auxFlowInnov=0; // optical flow LOS rate innovation from terrain offset estimator
float HAGL=0; // height above ground level
float rngInnov=0; // range finder innovations
float range=0; // measured range
float gndOffsetErr=0; // filter ground offset state error
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(0,predictorErrors);
struct log_EKF5 pkt5 = {
LOG_PACKET_HEADER_INIT(LOG_NKF5_MSG),
time_us : time_us,
normInnov : (uint8_t)(MIN(100*normInnov,255)),
FIX : (int16_t)(1000*flowInnovX),
FIY : (int16_t)(1000*flowInnovY),
AFI : (int16_t)(1000*auxFlowInnov),
HAGL : (int16_t)(100*HAGL),
offset : (int16_t)(100*gndOffset),
RI : (int16_t)(100*rngInnov),
meaRng : (uint16_t)(100*range),
errHAGL : (uint16_t)(100*gndOffsetErr),
angErr : (float)predictorErrors.x,
velErr : (float)predictorErrors.y,
posErr : (float)predictorErrors.z
};
WriteBlock(&pkt5, sizeof(pkt5));
// log innovations for the second IMU if enabled
if (ahrs.get_NavEKF2().activeCores() >= 2) {

7
libraries/DataFlash/LogStructure.h
View File

@ -352,6 +352,9 @@ struct PACKED log_EKF5 {
int16_t RI;
uint16_t meaRng;
uint16_t errHAGL;
float angErr;
float velErr;
float posErr;
};
struct PACKED log_Cmd {
@ -795,7 +798,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","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
"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), \
@ -805,7 +808,7 @@ Format characters in the format string for binary log messages
{ 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), \
"NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
"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" }, \
{ LOG_NKF7_MSG, sizeof(log_NKF2), \