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

DataFlash: Update EKF2 data logging

Browse Source
This commit is contained in:
Paul Riseborough 2015-09-24 11:10:34 +10:00 committed by Andrew Tridgell
parent f77bdd90fc
commit 86ad1e6e66
2 changed files with 48 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
libraries/DataFlash/DataFlash.h
View File

@ -382,6 +382,23 @@ struct PACKED log_EKF2 {
int16_t magZ;
};
struct PACKED log_NKF2 {
LOG_PACKET_HEADER;
uint64_t time_us;
int8_t AZbias;
int16_t scaleX;
int16_t scaleY;
int16_t scaleZ;
int16_t windN;
int16_t windE;
int16_t magN;
int16_t magE;
int16_t magD;
int16_t magX;
int16_t magY;
int16_t magZ;
};
struct PACKED log_EKF3 {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -397,6 +414,22 @@ struct PACKED log_EKF3 {
int16_t innovVT;
};
struct PACKED log_NKF3 {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t innovVN;
int16_t innovVE;
int16_t innovVD;
int16_t innovPN;
int16_t innovPE;
int16_t innovPD;
int16_t innovMX;
int16_t innovMY;
int16_t innovMZ;
int16_t innovYaw;
int16_t innovVT;
};
struct PACKED log_EKF4 {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -786,10 +819,10 @@ Format characters in the format string for binary log messages
"EKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
{ LOG_NKF1_MSG, sizeof(log_EKF1), \
"NKF1","QccCffffffccc","TimeUS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_NKF2_MSG, sizeof(log_EKF2), \
"NKF2","Qbbbcchhhhhh","TimeUS,Ratio,AZ1bias,AZ2bias,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
{ LOG_NKF3_MSG, sizeof(log_EKF3), \
"NKF3","Qcccccchhhc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
{ LOG_NKF2_MSG, sizeof(log_NKF2), \
"NKF2","Qcccbcchhhhhh","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
{ LOG_NKF3_MSG, sizeof(log_NKF3), \
"NKF3","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ LOG_NKF4_MSG, sizeof(log_EKF4), \
"NKF4","QcccccccbbBBH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS" }, \
{ LOG_NKF5_MSG, sizeof(log_EKF5), \

20
libraries/DataFlash/LogFile.cpp
View File

@ -1314,22 +1314,23 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
WriteBlock(&pkt, sizeof(pkt));
// Write second EKF packet
float ratio = 0;
float az1bias = 0, az2bias = 0;
float azbias = 0;
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
ahrs.get_NavEKF2().getIMU1Weighting(ratio);
ahrs.get_NavEKF2().getAccelZBias(az1bias, az2bias);
Vector3f gyroScaleFactor;
ahrs.get_NavEKF2().getAccelZBias(azbias);
ahrs.get_NavEKF2().getWind(wind);
ahrs.get_NavEKF2().getMagNED(magNED);
ahrs.get_NavEKF2().getMagXYZ(magXYZ);
struct log_EKF2 pkt2 = {
ahrs.get_NavEKF2().getGyroScaleErrorPercentage(gyroScaleFactor);
struct log_NKF2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_NKF2_MSG),
time_us : hal.scheduler->micros64(),
Ratio : (int8_t)(100*ratio),
AZ1bias : (int8_t)(100*az1bias),
AZ2bias : (int8_t)(100*az2bias),
AZbias : (int8_t)(100*azbias),
scaleX : (int16_t)(100*gyroScaleFactor.x),
scaleY : (int16_t)(100*gyroScaleFactor.y),
scaleZ : (int16_t)(100*gyroScaleFactor.z),
windN : (int16_t)(100*wind.x),
windE : (int16_t)(100*wind.y),
magN : (int16_t)(magNED.x),
@ -1348,7 +1349,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
float tasInnov = 0;
float yawInnov = 0;
ahrs.get_NavEKF2().getInnovations(velInnov, posInnov, magInnov, tasInnov, yawInnov);
struct log_EKF3 pkt3 = {
struct log_NKF3 pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_NKF3_MSG),
time_us : hal.scheduler->micros64(),
innovVN : (int16_t)(100*velInnov.x),
@ -1360,6 +1361,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
innovMX : (int16_t)(magInnov.x),
innovMY : (int16_t)(magInnov.y),
innovMZ : (int16_t)(magInnov.z),
innovYaw : (int16_t)(100*degrees(tasInnov)),
innovVT : (int16_t)(100*tasInnov)
};
WriteBlock(&pkt3, sizeof(pkt3));