Replay: generate dataflash logs in Replay

this allows the calculated EKF flight path to be compared with
different parameters by using replay

Tools/Replay/LogReader.cpp

@@ -18,7 +18,7 @@
 
 extern const AP_HAL::HAL& hal;
 
-LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed) :
+LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash) :
     vehicle(VEHICLE_UNKNOWN),
     fd(-1),
     ahrs(_ahrs),
@@ -27,6 +27,7 @@ LogReader::LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro
     compass(_compass),
     gps(_gps),
     airspeed(_airspeed),
+    dataflash(_dataflash),
     accel_mask(7),
     gyro_mask(7)
 {}
@@ -352,6 +353,7 @@ bool LogReader::update(uint8_t &type)
             ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
             ahrs.set_fly_forward(true);
         }
+        dataflash.Log_Write_Message(msg.msg);
         break;
     }
 
@@ -369,6 +371,7 @@ bool LogReader::update(uint8_t &type)
         if (accel_mask & 1) {
             ins.set_accel(0, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
         }
+        dataflash.Log_Write_IMU(ins);
         break;
     }
 
@@ -386,6 +389,7 @@ bool LogReader::update(uint8_t &type)
         if (accel_mask & 2) {
             ins.set_accel(1, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
         }
+        dataflash.Log_Write_IMU(ins);
         break;
     }
 
@@ -403,6 +407,7 @@ bool LogReader::update(uint8_t &type)
         if (accel_mask & 4) {
             ins.set_accel(2, Vector3f(msg.accel_x, msg.accel_y, msg.accel_z));
         }
+        dataflash.Log_Write_IMU(ins);
         break;
     }
 
@@ -434,6 +439,7 @@ bool LogReader::update(uint8_t &type)
             ground_alt_cm = msg.altitude;
         }
         rel_altitude = msg.rel_altitude*0.01f;
+        dataflash.Log_Write_GPS(gps, 0, rel_altitude);
         break;
     }
 
@@ -464,6 +470,7 @@ bool LogReader::update(uint8_t &type)
         if (msg.status == 3 && ground_alt_cm == 0) {
             ground_alt_cm = msg.altitude;
         }
+        dataflash.Log_Write_GPS(gps, 1, rel_altitude);
         break;
     }
 
@@ -494,6 +501,7 @@ bool LogReader::update(uint8_t &type)
         }
         wait_timestamp(msg.timestamp);
         baro.setHIL(msg.pressure, msg.temperature*0.01f);
+        dataflash.Log_Write_Baro(baro);
         break;
     }
 

Tools/Replay/LogReader.h

@@ -20,7 +20,7 @@ enum log_messages {
 class LogReader 
 {
 public:
-    LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed);
+    LogReader(AP_AHRS &_ahrs, AP_InertialSensor &_ins, AP_Baro_HIL &_baro, AP_Compass_HIL &_compass, AP_GPS &_gps, AP_Airspeed &_airspeed, DataFlash_Class &_dataflash);
     bool open_log(const char *logfile);
     bool update(uint8_t &type);
     bool wait_type(uint8_t type);
@@ -48,6 +48,7 @@ private:
     AP_Compass_HIL &compass;
     AP_GPS &gps;
     AP_Airspeed &airspeed;
+    DataFlash_Class &dataflash;
 
     uint8_t accel_mask;
     uint8_t gyro_mask;

Tools/Replay/Replay.pde

@@ -75,6 +75,7 @@ static Baro_Glitch baro_glitch(barometer);
 static AP_InertialNav inertial_nav(ahrs, barometer, gps_glitch, baro_glitch);
 static AP_Vehicle::FixedWing aparm;
 static AP_Airspeed airspeed(aparm);
+static DataFlash_File dataflash("logs");
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
 SITL sitl;
@@ -82,7 +83,7 @@ SITL sitl;
 
 static const NavEKF &NavEKF = ahrs.get_NavEKF();
 
-static LogReader LogReader(ahrs, ins, barometer, compass, gps, airspeed);
+static LogReader LogReader(ahrs, ins, barometer, compass, gps, airspeed, dataflash);
 
 static FILE *plotf;
 static FILE *plotf2;
@@ -103,6 +104,10 @@ static struct {
     float value;
 } user_parameters[100];
 
+static const struct LogStructure log_structure[] PROGMEM = {
+    LOG_COMMON_STRUCTURES
+};
+
 // setup the var_info table
 AP_Param param_loader(var_info);
 
@@ -186,6 +191,9 @@ void setup()
         exit(1);
     }
 
+    dataflash.Init(log_structure, sizeof(log_structure)/sizeof(log_structure[0]));
+    dataflash.StartNewLog();
+
     LogReader.wait_type(LOG_GPS_MSG);
     LogReader.wait_type(LOG_IMU_MSG);
     LogReader.wait_type(LOG_GPS_MSG);
@@ -304,6 +312,8 @@ static void read_sensors(uint8_t type)
                 inertial_nav.update(ins.get_delta_time());
             }
             hal.scheduler->stop_clock(hal.scheduler->micros() + (i+1)*update_delta_usec);
+            dataflash.Log_Write_EKF(ahrs);
+            dataflash.Log_Write_AHRS2(ahrs);
             ins.set_gyro(0, ins.get_gyro());
             ins.set_accel(0, ins.get_accel());
         }