From ea508f1b8018f1c59e4898b010e557480867bffc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 9 May 2016 12:26:57 +1000
Subject: [PATCH] AP_NavEKF2: use LOG_REPLAY and EK2_LOG_MASK parameters

---
 libraries/AP_NavEKF2/AP_NavEKF2.cpp | 20 +++++++++++++-------
 libraries/AP_NavEKF2/AP_NavEKF2.h   |  5 +++--
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
index 7c0963e071..6d4ddd68e3 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp
@@ -428,12 +428,12 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Units: m/s
     AP_GROUPINFO("NOAID_NOISE", 35, NavEKF2, _noaidHorizNoise, 10.0f),
 
-    // @Param: LOGGING
-    // @DisplayName: Enable EKF sensor logging
-    // @Description: This enables EKF sensor logging for use by Replay
-    // @Values: 0:Disabled,1:Enabled
+    // @Param: LOG_MASK
+    // @DisplayName: EKF sensor logging IMU mask
+    // @Description: This sets the IMU mask of sensors to do full logging for
+    // @Values: 0:Disabled,1:FirstIMU,3:FirstAndSecondIMU,7:AllIMUs
     // @User: Advanced
-    AP_GROUPINFO("LOGGING", 36, NavEKF2, _enable_logging, 0),
+    AP_GROUPINFO("LOG_MASK", 36, NavEKF2, _logging_mask, 1),
     
     AP_GROUPEND
 };
@@ -477,7 +477,7 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) :
  */
 void NavEKF2::check_log_write(void)
 {
-    if (!_enable_logging) {
+    if (!have_ekf_logging()) {
         return;
     }
     if (logging.log_compass) {
@@ -494,7 +494,7 @@ void NavEKF2::check_log_write(void)
     }
     if (logging.log_imu) {
         const AP_InertialSensor &ins = _ahrs->get_ins();
-        DataFlash_Class::instance()->Log_Write_IMUDT(ins, imuSampleTime_us);
+        DataFlash_Class::instance()->Log_Write_IMUDT(ins, imuSampleTime_us, _logging_mask.get());
         logging.log_imu = false;
     }
 
@@ -511,6 +511,12 @@ bool NavEKF2::InitialiseFilter(void)
     }
 
     imuSampleTime_us = AP_HAL::micros64();
+
+    // see if we will be doing logging
+    DataFlash_Class *dataflash = DataFlash_Class::instance();
+    if (dataflash != nullptr) {
+        logging.enabled = dataflash->log_replay();
+    }
     
     if (core == nullptr) {
 
diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.h b/libraries/AP_NavEKF2/AP_NavEKF2.h
index 79922ef1ce..b80e511107 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2.h
+++ b/libraries/AP_NavEKF2/AP_NavEKF2.h
@@ -269,7 +269,7 @@ public:
     void set_enable(bool enable) { _enable.set(enable); }
 
     // are we doing sensor logging inside the EKF?
-    bool have_ekf_logging(void) const { return _enable_logging != 0; }
+    bool have_ekf_logging(void) const { return logging.enabled && _logging_mask != 0; }
     
 private:
     uint8_t num_cores; // number of allocated cores
@@ -316,7 +316,7 @@ private:
     AP_Int8 _imuMask;               // Bitmask of IMUs to instantiate EKF2 for
     AP_Int16 _gpsCheckScaler;       // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
     AP_Float _noaidHorizNoise;      // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
-    AP_Int8 _enable_logging;        // Enable logging for Replay
+    AP_Int8 _logging_mask;          // mask of IMUs to log
 
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration
@@ -347,6 +347,7 @@ private:
     const uint8_t fusionTimeStep_ms;    // The minimum time interval between covariance predictions and measurement fusions in msec
 
     struct {
+        bool enabled:1;
         bool log_compass:1;
         bool log_gps:1;
         bool log_baro:1;