diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index be3a1ddfb7..33bf112511 100644 --- a/Tools/Replay/LogReader.cpp +++ b/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; } diff --git a/Tools/Replay/LogReader.h b/Tools/Replay/LogReader.h index 15e2391191..7359e240c4 100644 --- a/Tools/Replay/LogReader.h +++ b/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; diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index adbaf99b09..d4c89fda32 100644 --- a/Tools/Replay/Replay.pde +++ b/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()); }