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

DataFlash: moved EKF logging to common code

This commit is contained in:
Andrew Tridgell 2014-01-04 11:51:22 +11:00
parent 6a9189a6a5
commit 45177df354
2 changed files with 105 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/DataFlash/DataFlash.h
View File

@ -54,6 +54,9 @@ public:
void Log_Write_Baro(AP_Baro &baro);
void Log_Write_Power(void);
void Log_Write_AHRS2(AP_AHRS &ahrs);
#if AP_AHRS_NAVEKF_AVAILABLE
void Log_Write_EKF(AP_AHRS_NavEKF &ahrs);
#endif
void Log_Write_Message(const char *message);
void Log_Write_Message_P(const prog_char_t *message);
@ -232,6 +235,41 @@ struct PACKED log_POWR {
uint16_t flags;
};
struct PACKED log_EKF1 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int16_t roll;
int16_t pitch;
uint16_t yaw;
float velN;
float velE;
float velD;
float posN;
float posE;
float posD;
int8_t gyrX;
int8_t gyrY;
int8_t gyrZ;
};
struct PACKED log_EKF2 {
LOG_PACKET_HEADER;
uint32_t time_ms;
int8_t accX;
int8_t accY;
int8_t accZ;
int16_t windN;
int16_t windE;
int16_t magN;
int16_t magE;
int16_t magD;
int16_t magX;
int16_t magY;
int16_t magZ;
};
#define LOG_COMMON_STRUCTURES \
{ LOG_FORMAT_MSG, sizeof(log_Format), \
"FMT", "BBnNZ", "Type,Length,Name,Format,Columns" }, \
@ -256,7 +294,11 @@ struct PACKED log_POWR {
{ 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" }
"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" }, \
{ LOG_EKF2_MSG, sizeof(log_EKF2), \
"EKF2","Ibbbcchhhhhh","TimeMS,AX,AY,AZ,VWN,VWE,MN,ME,MD,MX,MY,MZ" }
// message types for common messages
#define LOG_FORMAT_MSG 128
@ -271,6 +313,8 @@ struct PACKED log_POWR {
#define LOG_POWR_MSG 137
#define LOG_AHR2_MSG 138
#define LOG_SIMSTATE_MSG 139
#define LOG_EKF1_MSG 140
#define LOG_EKF2_MSG 141
#include "DataFlash_Block.h"
#include "DataFlash_File.h"

61
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_AHR2_MSG),
LOG_PACKET_HEADER_INIT(LOG_AHRS2_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(degrees(euler.x)*100),
pitch : (int16_t)(degrees(euler.y)*100),
@ -815,3 +815,62 @@ void DataFlash_Class::Log_Write_AHRS2(AP_AHRS &ahrs)
WriteBlock(&pkt, sizeof(pkt));
}
#if AP_AHRS_NAVEKF_AVAILABLE
// Write first EKF packet
void DataFlash_Class::Log_Write_EKF(AP_AHRS_NavEKF &ahrs)
{
Vector3f euler;
Vector3f posNED;
Vector3f velNED;
Vector3f dAngBias;
Vector3f dVelBias;
Vector3f gyroBias;
ahrs.get_NavEKF().getEulerAngles(euler);
ahrs.get_NavEKF().getVelNED(velNED);
ahrs.get_NavEKF().getPosNED(posNED);
ahrs.get_NavEKF().getGyroBias(gyroBias);
struct log_EKF1 pkt = {
LOG_PACKET_HEADER_INIT(LOG_EKF1_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(100*degrees(euler.x)), // roll angle (centi-deg)
pitch : (int16_t)(100*degrees(euler.y)), // pitch angle (centi-deg)
yaw : (uint16_t)wrap_360_cd(100*degrees(euler.z)), // yaw angle (centi-deg)
velN : (float)(velNED.x), // velocity North (m/s)
velE : (float)(velNED.y), // velocity East (m/s)
velD : (float)(velNED.z), // velocity Down (m/s)
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
};
WriteBlock(&pkt, sizeof(pkt));
Vector3f accelBias;
Vector3f wind;
Vector3f magNED;
Vector3f magXYZ;
ahrs.get_NavEKF().getAccelBias(accelBias);
ahrs.get_NavEKF().getWind(wind);
ahrs.get_NavEKF().getMagNED(magNED);
ahrs.get_NavEKF().getMagXYZ(magXYZ);
struct log_EKF2 pkt2 = {
LOG_PACKET_HEADER_INIT(LOG_EKF2_MSG),
time_ms : hal.scheduler->millis(),
accX : (int8_t)(100*accelBias.x),
accY : (int8_t)(100*accelBias.y),
accZ : (int8_t)(100*accelBias.z),
windN : (int16_t)(100*wind.x),
windE : (int16_t)(100*wind.y),
magN : (int16_t)(magNED.x),
magE : (int16_t)(magNED.y),
magD : (int16_t)(magNED.z),
magX : (int16_t)(magXYZ.x),
magY : (int16_t)(magXYZ.y),
magZ : (int16_t)(magXYZ.z)
};
WriteBlock(&pkt2, sizeof(pkt2));
#endif
}