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

DataFlash : Added logging of NavEKF innovations and variances

This commit is contained in:
Paul Riseborough 2014-01-31 09:33:12 +11:00 committed by Andrew Tridgell
parent 9bbddb2f66
commit 17e24521e8
2 changed files with 93 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/DataFlash/DataFlash.h
View File

@ -215,6 +215,7 @@ struct PACKED log_BARO {
float altitude;
float pressure;
int16_t temperature;
};
struct PACKED log_AHRS {
LOG_PACKET_HEADER;
@ -247,9 +248,9 @@ struct PACKED log_EKF1 {
float posN;
float posE;
float posD;
int8_t gyrX;
int8_t gyrY;
int8_t gyrZ;
int16_t gyrX;
int16_t gyrY;
int16_t gyrZ;
};
struct PACKED log_EKF2 {
@ -268,7 +269,35 @@ struct PACKED log_EKF2 {
int16_t magZ;
};
struct PACKED log_EKF3 {
LOG_PACKET_HEADER;
uint32_t time_ms;
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 innovVT;
};
struct PACKED log_EKF4 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t sqrtvarVN;
int16_t sqrtvarVE;
int16_t sqrtvarVD;
int16_t sqrtvarPN;
int16_t sqrtvarPE;
int16_t sqrtvarPD;
int16_t sqrtvarMX;
int16_t sqrtvarMY;
int16_t sqrtvarMZ;
int16_t sqrtvarVT;
};
#define LOG_COMMON_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
@ -290,15 +319,19 @@ struct PACKED log_EKF2 {
{ LOG_BARO_MSG, sizeof(log_BARO), \
"BARO", "Iffc", "TimeMS,Alt,Press,Temp" }, \
{ LOG_POWR_MSG, sizeof(log_POWR), \
"POWR","ICCH","TimeMS,Vcc,VServo,Flags" } \
"POWR","ICCH","TimeMS,Vcc,VServo,Flags" }, \
{ LOG_AHR2_MSG, sizeof(log_AHRS), \
"AHR2","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
{ LOG_SIMSTATE_MSG, sizeof(log_AHRS), \
"SIM","IccCfLL","TimeMS,Roll,Pitch,Yaw,Alt,Lat,Lng" }, \
{ LOG_EKF1_MSG, sizeof(log_EKF1), \
"EKF1","IccCffffffbbb","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \
"EKF1","IccCffffffccc","TimeMS,Roll,Pitch,Yaw,VN,VE,VD,PN,PE,PD,GX,GY,GZ" }, \
{ LOG_EKF2_MSG, sizeof(log_EKF2), \
"EKF2","Ibbbcchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }
"EKF2","Ibbbcchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }, \
{ LOG_EKF3_MSG, sizeof(log_EKF3), \
"EKF3","Icccccchhhc","TimeMS,IVN,IVE,IVD,IPN,IPE,IPD,IMX,IMY,IMZ,IVT" }, \
{ LOG_EKF4_MSG, sizeof(log_EKF4), \
"EKF4","Icccccchhhc","TimeMS,SVN,SVE,SVD,SPN,SPE,SPD,SMX,SMY,SMZ,SVT" }
// message types for common messages
#define LOG_FORMAT_MSG 128
@ -315,6 +348,8 @@ struct PACKED log_EKF2 {
#define LOG_SIMSTATE_MSG 139
#define LOG_EKF1_MSG 140
#define LOG_EKF2_MSG 141
#define LOG_EKF3_MSG 142
#define LOG_EKF4_MSG 143
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

59
libraries/DataFlash/LogFile.cpp
View File

@ -803,7 +803,7 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
return;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_AHRS2_MSG),
LOG_PACKET_HEADER_INIT(LOG_AHR2_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(degrees(euler.x)*100),
pitch : (int16_t)(degrees(euler.y)*100),
@ -816,9 +816,9 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
}
#if AP_AHRS_NAVEKF_AVAILABLE
// Write first EKF packet
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
{
// Write first EKF packet
Vector3f euler;
Vector3f posNED;
Vector3f velNED;
@ -841,12 +841,13 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
posN : (float)(posNED.x), // metres North
posE : (float)(posNED.y), // metres East
posD : (float)(posNED.z), // metres Down
gyrX : (int8_t)(gyroBias.x), // deg/min
gyrY : (int8_t)(gyroBias.y), // deg/min
gyrZ : (int8_t)(gyroBias.z) // deg/min
gyrX : (int8_t)(60*degrees(gyroBias.x)), // deg/min
gyrY : (int8_t)(60*degrees(gyroBias.y)), // deg/min
gyrZ : (int8_t)(60*degrees(gyroBias.z)) // deg/min
};
WriteBlock(&pkt, sizeof(pkt));
// Write second EKF packet
Vector3f accelBias;
Vector3f wind;
Vector3f magNED;
@ -871,6 +872,50 @@ void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
magZ : (int16_t)(magXYZ.z)
};
WriteBlock(&pkt2, sizeof(pkt2));
#endif
}
// Write third EKF packet
Vector3f velInnov;
Vector3f posInnov;
Vector3f magInnov;
float tasInnov;
ahrs.get_NavEKF().getInnovations(velInnov, posInnov, magInnov, tasInnov);
struct log_EKF3 pkt3 = {
LOG_PACKET_HEADER_INIT(LOG_EKF3_MSG),
time_ms : hal.scheduler->millis(),
innovVN : (int16_t)(100*velInnov.x),
innovVE : (int16_t)(100*velInnov.y),
innovVD : (int16_t)(100*velInnov.z),
innovPN : (int16_t)(100*posInnov.x),
innovPE : (int16_t)(100*posInnov.y),
innovPD : (int16_t)(100*posInnov.z),
innovMX : (int16_t)(magInnov.x),
innovMY : (int16_t)(magInnov.y),
innovMZ : (int16_t)(magInnov.z),
innovVT : (int16_t)(100*tasInnov)
};
WriteBlock(&pkt3, sizeof(pkt3));
// Write fourth EKF packet
Vector3f velVar;
Vector3f posVar;
Vector3f magVar;
float tasVar;
ahrs.get_NavEKF().getVariances(velVar, posVar, magVar, tasVar);
struct log_EKF4 pkt4 = {
LOG_PACKET_HEADER_INIT(LOG_EKF4_MSG),
time_ms : hal.scheduler->millis(),
sqrtvarVN : (int16_t)(100*sqrtf(velVar.x)),
sqrtvarVE : (int16_t)(100*sqrtf(velVar.y)),
sqrtvarVD : (int16_t)(100*sqrtf(velVar.z)),
sqrtvarPN : (int16_t)(100*sqrtf(posVar.x)),
sqrtvarPE : (int16_t)(100*sqrtf(posVar.y)),
sqrtvarPD : (int16_t)(100*sqrtf(posVar.z)),
sqrtvarMX : (int16_t)(sqrtf(magVar.x)),
sqrtvarMY : (int16_t)(sqrtf(magVar.y)),
sqrtvarMZ : (int16_t)(sqrtf(magVar.z)),
sqrtvarVT : (int16_t)(100*sqrtf(tasVar))
};
WriteBlock(&pkt4, sizeof(pkt4));
}
#endif