diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 3c2d1ac6d1..8a1d3335cb 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -200,6 +200,9 @@ bool LogReader::update(char type[5]) msgparser[f.type] = new MsgHandler_ARSP(formats[f.type], dataflash, last_timestamp_usec, airspeed); + } else if (streq(name, "FRAM")) { + msgparser[f.type] = new MsgHandler_FRAM(formats[f.type], dataflash, + last_timestamp_usec); } else { ::printf(" No parser for (%s)\n", name); } diff --git a/Tools/Replay/MsgHandler.cpp b/Tools/Replay/MsgHandler.cpp index f292ce9631..00ee388f88 100644 --- a/Tools/Replay/MsgHandler.cpp +++ b/Tools/Replay/MsgHandler.cpp @@ -345,6 +345,12 @@ void MsgHandler_ARSP::process_message(uint8_t *msg) dataflash.WriteBlock(msg, f.length); } +void MsgHandler_FRAM::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + dataflash.WriteBlock(msg, f.length); +} + void MsgHandler_ATT::process_message(uint8_t *msg) { diff --git a/Tools/Replay/MsgHandler.h b/Tools/Replay/MsgHandler.h index 0061143b92..e8e9b55bc8 100644 --- a/Tools/Replay/MsgHandler.h +++ b/Tools/Replay/MsgHandler.h @@ -206,6 +206,16 @@ private: AP_Airspeed &airspeed; }; +class MsgHandler_FRAM : public MsgHandler +{ +public: + MsgHandler_FRAM(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec) : + MsgHandler(_f, _dataflash, _last_timestamp_usec) { }; + + virtual void process_message(uint8_t *msg); +}; + class MsgHandler_ATT : public MsgHandler { diff --git a/Tools/Replay/Replay.pde b/Tools/Replay/Replay.pde index c73844773e..87b65c46dd 100644 --- a/Tools/Replay/Replay.pde +++ b/Tools/Replay/Replay.pde @@ -103,6 +103,7 @@ static uint16_t update_rate = 50; static uint32_t arm_time_ms; static bool ahrs_healthy; static bool have_imu2; +static bool have_fram; static uint8_t num_user_parameters; static struct { @@ -316,10 +317,23 @@ static void read_sensors(const char *type) ::printf("Barometer initialised\n"); barometer.update_calibration(); } + } + + bool run_ahrs = false; + if (streq(type,"FRAM")) { + if (!have_fram) { + have_fram = true; + printf("Have FRAM framing\n"); + } + run_ahrs = true; } // special handling of IMU messages as these trigger an ahrs.update() - if ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2)) { + if (!have_fram && + ((streq(type,"IMU") && !have_imu2) || (streq(type, "IMU2") && have_imu2))) { + run_ahrs = true; + } + if (run_ahrs) { ahrs.update(); if (ahrs.get_home().lat != 0) { inertial_nav.update(ins.get_delta_time());