AP_NavEKF2: added logging of sensor data in EKF2

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -6,13 +6,14 @@
 #include "AP_NavEKF2_core.h"
 #include <AP_Vehicle/AP_Vehicle.h>
 #include <GCS_MAVLink/GCS.h>
+#include <DataFlash/DataFlash.h>
 
 /*
   parameter defaults for different types of vehicle. The
   APM_BUILD_DIRECTORY is taken from the main vehicle directory name
   where the code is built.
  */
-#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
+#if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_Replay)
 // copter defaults
 #define VELNE_NOISE_DEFAULT     0.5f
 #define VELD_NOISE_DEFAULT      0.7f
@@ -471,6 +472,33 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
     AP_Param::setup_object_defaults(this, var_info);
 }
 
+/*
+  see if we should log some sensor data
+ */
+void NavEKF2::check_log_write(void)
+{
+    if (!_enable_logging) {
+        return;
+    }
+    if (logging.log_compass) {
+        DataFlash_Class::instance()->Log_Write_Compass(*_ahrs->get_compass());
+        logging.log_compass = false;
+    }
+    if (logging.log_gps) {
+        DataFlash_Class::instance()->Log_Write_GPS(_ahrs->get_gps(), 0);
+        logging.log_gps = false;
+    }
+    if (logging.log_baro) {
+        DataFlash_Class::instance()->Log_Write_Baro(_baro);
+        logging.log_baro = false;
+    }
+    if (logging.log_imu) {
+        const AP_InertialSensor &ins = _ahrs->get_ins();
+        DataFlash_Class::instance()->Log_Write_IMUDT(ins);
+        logging.log_imu = false;
+    }
+}
+
 
 // Initialise the filter
 bool NavEKF2::InitialiseFilter(void)
@@ -527,6 +555,8 @@ bool NavEKF2::InitialiseFilter(void)
     for (uint8_t i=0; i<num_cores; i++) {
         ret &= core[i].InitialiseFilterBootstrap();
     }
+
+    check_log_write();
     return ret;
 }
 
@@ -565,6 +595,8 @@ void NavEKF2::UpdateFilter(void)
             }
         }
     }
+
+    check_log_write();
 }
 
 // Check basic filter health metrics and return a consolidated health status

libraries/AP_NavEKF2/AP_NavEKF2.h

@@ -52,6 +52,9 @@ public:
     // Update Filter States - this should be called whenever new IMU data is available
     void UpdateFilter(void);
 
+    // check if we should write log messages
+    void check_log_write(void);
+    
     // Check basic filter health metrics and return a consolidated health status
     bool healthy(void) const;
 
@@ -339,4 +342,11 @@ private:
     const float gndEffectBaroScaler;    // scaler applied to the barometer observation variance when ground effect mode is active
     const uint8_t gndGradientSigma;     // RMS terrain gradient percentage assumed by the terrain height estimation
     const uint8_t fusionTimeStep_ms;    // The minimum time interval between covariance predictions and measurement fusions in msec
+
+    struct {
+        bool log_compass:1;
+        bool log_gps:1;
+        bool log_baro:1;
+        bool log_imu:1;
+    } logging;
 };

libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp

@@ -9,7 +9,6 @@
 #include <AP_Vehicle/AP_Vehicle.h>
 
 #include <stdio.h>
-#include <DataFlash/DataFlash.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -157,6 +156,8 @@ void NavEKF2_core::readMagData()
     // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
     // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
     if (use_compass() && _ahrs->get_compass()->last_update_usec() - lastMagUpdate_us > 70000) {
+        frontend->logging.log_compass = true;
+
         // If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
         // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
         // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
@@ -431,6 +432,7 @@ void NavEKF2_core::readGpsData()
                 ResetVelocity();
             }
 
+            frontend->logging.log_gps = true;
         } else {
             // report GPS fix status
             gpsCheckStatus.bad_fix = true;
@@ -493,9 +495,7 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
 
     if (ins_index < ins.get_gyro_count()) {
         ins.get_delta_angle(ins_index,dAng);
-        if (frontend->_enable_logging && ins_index == 0) {
-            DataFlash_Class::instance()->Log_Write_IMUDT(ins);
-        }
+        frontend->logging.log_imu = true;
         return true;
     }
     return false;
@@ -512,6 +512,7 @@ void NavEKF2_core::readBaroData()
     // check to see if baro measurement has changed so we know if a new measurement has arrived
     // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
     if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) {
+        frontend->logging.log_baro = true;
 
         baroDataNew.hgt = frontend->_baro.get_altitude();