DataFlash: moved EKF logging to common code

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"

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
+}
+