Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

DataFlash: Publish the EKF2 magnetometer selection index

This commit is contained in:
Paul Riseborough 2015-11-08 21:28:00 +11:00 committed by Andrew Tridgell
parent f00b1ff22d
commit 88cc1e2ffe
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/DataFlash/DataFlash.h
View File

@ -395,6 +395,7 @@ struct PACKED log_NKF2 {
int16_t magX;
int16_t magY;
int16_t magZ;
uint8_t index;
};
struct PACKED log_EKF3 {
@ -828,7 +829,7 @@ Format characters in the format string for binary log messages
{ 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), \
"NKF2","Qbccccchhhhhh","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
"NKF2","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ 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_NKF4), \
@ -838,7 +839,7 @@ Format characters in the format string for binary log messages
{ 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), \
"NKF7","Qbccccchhhhhh","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
"NKF7","QbccccchhhhhhB","TimeUS,AZbias,GSX,GSY,GSZ,VWN,VWE,MN,ME,MD,MX,MY,MZ,MI" }, \
{ LOG_NKF8_MSG, sizeof(log_NKF3), \
"NKF8","Qcccccchhhcc","TimeUS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IYAW,IVT" }, \
{ LOG_NKF9_MSG, sizeof(log_NKF4), \

8
libraries/DataFlash/LogFile.cpp
View File

@ -1304,6 +1304,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
Vector3f magNED;
Vector3f magXYZ;
Vector3f gyroScaleFactor;
uint8_t magIndex = ahrs.get_NavEKF2().getActiveMag(0);
ahrs.get_NavEKF2().getAccelZBias(0,azbias);
ahrs.get_NavEKF2().getWind(0,wind);
ahrs.get_NavEKF2().getMagNED(0,magNED);
@ -1323,7 +1324,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
magD : (int16_t)(magNED.z),
magX : (int16_t)(magXYZ.x),
magY : (int16_t)(magXYZ.y),
magZ : (int16_t)(magXYZ.z)
magZ : (int16_t)(magXYZ.z),
index : (uint8_t)(magIndex)
};
WriteBlock(&pkt2, sizeof(pkt2));
@ -1449,6 +1451,7 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
ahrs.get_NavEKF2().getMagNED(1,magNED);
ahrs.get_NavEKF2().getMagXYZ(1,magXYZ);
ahrs.get_NavEKF2().getGyroScaleErrorPercentage(1,gyroScaleFactor);
magIndex = ahrs.get_NavEKF2().getActiveMag(1);
struct log_NKF2 pkt7 = {
LOG_PACKET_HEADER_INIT(LOG_NKF7_MSG),
time_us : hal.scheduler->micros64(),
@ -1463,7 +1466,8 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
magD : (int16_t)(magNED.z),
magX : (int16_t)(magXYZ.x),
magY : (int16_t)(magXYZ.y),
magZ : (int16_t)(magXYZ.z)
magZ : (int16_t)(magXYZ.z),
index : (uint8_t)(magIndex)
};
WriteBlock(&pkt7, sizeof(pkt7));