From 45177df35445ae4949d0494d390f7d74b4850dfc Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 4 Jan 2014 11:51:22 +1100 Subject: [PATCH] DataFlash: moved EKF logging to common code --- libraries/DataFlash/DataFlash.h | 46 ++++++++++++++++++++++++- libraries/DataFlash/LogFile.cpp | 61 ++++++++++++++++++++++++++++++++- 2 files changed, 105 insertions(+), 2 deletions(-) diff --git a/libraries/DataFlash/DataFlash.h b/libraries/DataFlash/DataFlash.h index f280fcd40d..0930ee0a66 100644 --- a/libraries/DataFlash/DataFlash.h +++ b/libraries/DataFlash/DataFlash.h @@ -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" diff --git a/libraries/DataFlash/LogFile.cpp b/libraries/DataFlash/LogFile.cpp index 585122444e..802215bca9 100644 --- a/libraries/DataFlash/LogFile.cpp +++ b/libraries/DataFlash/LogFile.cpp @@ -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 +} +