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

DataFlash: Update NKF4 data logging

Browse Source 
Add roll/pitch error metric
Reduce normalised magnetometer vector with a vector length
This commit is contained in:
Paul Riseborough 2015-10-10 06:08:32 +11:00 committed by Randy Mackay
parent 20a3f9782e
commit a82c8b241f
2 changed files with 25 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/DataFlash/DataFlash.h
View File

@ -448,6 +448,23 @@ struct PACKED log_EKF4 {
uint16_t gps;
};
struct PACKED log_NKF4 {
LOG_PACKET_HEADER;
uint64_t time_us;
int16_t sqrtvarV;
int16_t sqrtvarP;
int16_t sqrtvarH;
int16_t sqrtvarM;
int16_t sqrtvarVT;
float tiltErr;
int8_t offsetNorth;
int8_t offsetEast;
uint8_t faults;
uint8_t timeouts;
uint16_t solution;
uint16_t gps;
};
struct PACKED log_EKF5 {
LOG_PACKET_HEADER;
uint64_t time_us;
@ -815,8 +832,8 @@ Format characters in the format string for binary log messages
"NKF2","Qbccccchhhhhh","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","QcccccccbbBBHH","TimeUS,SV,SP,SH,SMX,SMY,SMZ,SVT,OFN,EFE,FS,TS,SS,GPS" }, \
{ LOG_NKF4_MSG, sizeof(log_NKF4), \
"NKF4","QcccccfbbBBHH","TimeUS,SV,SP,SH,SM,SVT,errRP,OFN,EFE,FS,TS,SS,GPS" }, \
{ LOG_NKF5_MSG, sizeof(log_EKF5), \
"NKF5","QBhhhcccCC","TimeUS,normInnov,FIX,FIY,AFI,HAGL,offset,RI,meaRng,errHAGL" }, \
{ LOG_TERRAIN_MSG, sizeof(log_TERRAIN), \

10
libraries/DataFlash/LogFile.cpp
View File

@ -1380,20 +1380,22 @@ void DataFlash_Class::Log_Write_EKF2(AP_AHRS_NavEKF &ahrs, bool optFlowEnabled)
nav_filter_status solutionStatus {};
nav_gps_status gpsStatus {};
ahrs.get_NavEKF2().getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset);
float magLength = magVar.length();
ahrs.get_NavEKF2().getFilterFaults(faultStatus);
ahrs.get_NavEKF2().getFilterTimeouts(timeoutStatus);
ahrs.get_NavEKF2().getFilterStatus(solutionStatus);
ahrs.get_NavEKF2().getFilterGpsStatus(gpsStatus);
struct log_EKF4 pkt4 = {
float tiltError;
ahrs.get_NavEKF2().getTiltError(tiltError);
struct log_NKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_NKF4_MSG),
time_us : hal.scheduler->micros64(),
sqrtvarV : (int16_t)(100*velVar),
sqrtvarP : (int16_t)(100*posVar),
sqrtvarH : (int16_t)(100*hgtVar),
sqrtvarMX : (int16_t)(100*magVar.x),
sqrtvarMY : (int16_t)(100*magVar.y),
sqrtvarMZ : (int16_t)(100*magVar.z),
sqrtvarM : (int16_t)(100*magLength),
sqrtvarVT : (int16_t)(100*tasVar),
tiltErr : (float)tiltError,
offsetNorth : (int8_t)(offset.x),
offsetEast : (int8_t)(offset.y),
faults : (uint8_t)(faultStatus),