diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index c8f7bc2c23..c655798966 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -26,6 +26,7 @@ #include "MsgHandler_IMU3.h" #include "MsgHandler_SIM.h" #include "MsgHandler_BARO.h" +#include "MsgHandler_ARM.h" #include "MsgHandler_AHR2.h" #include "MsgHandler_ATT.h" #include "MsgHandler_MAG.h" @@ -120,6 +121,8 @@ bool LogReader::update(char type[5]) memcpy(name, f.name, 4); ::printf("Defining log format for type (%d) (%s)\n", f.type, name); + dataflash.WriteBlock(&f, sizeof(f)); + // map from format name to a parser subclass: if (streq(name, "PARM")) { parameter_handler = new MsgHandler_PARM(formats[f.type], dataflash, @@ -159,6 +162,9 @@ bool LogReader::update(char type[5]) } else if (streq(name, "BARO")) { msgparser[f.type] = new MsgHandler_BARO(formats[f.type], dataflash, last_timestamp_usec, baro); + } else if (streq(name, "ARM")) { + msgparser[f.type] = new MsgHandler_ARM(formats[f.type], dataflash, + last_timestamp_usec); } else if (streq(name, "AHR2")) { msgparser[f.type] = new MsgHandler_AHR2(formats[f.type], dataflash, last_timestamp_usec, diff --git a/Tools/Replay/MsgHandler_ARM.cpp b/Tools/Replay/MsgHandler_ARM.cpp new file mode 100644 index 0000000000..5522655ab2 --- /dev/null +++ b/Tools/Replay/MsgHandler_ARM.cpp @@ -0,0 +1,14 @@ +#include "MsgHandler_ARM.h" + +extern const AP_HAL::HAL& hal; + +void MsgHandler_ARM::process_message(uint8_t *msg) +{ + wait_timestamp_from_msg(msg); + uint8_t ArmState = require_field_uint8_t(msg, "ArmState"); + hal.util->set_soft_armed(ArmState); + printf("Armed state: %u at %lu\n", + (unsigned)ArmState, + (unsigned long)hal.scheduler->millis()); + dataflash.WriteBlock(msg, f.length); +} diff --git a/Tools/Replay/MsgHandler_ARM.h b/Tools/Replay/MsgHandler_ARM.h new file mode 100644 index 0000000000..f3b429b694 --- /dev/null +++ b/Tools/Replay/MsgHandler_ARM.h @@ -0,0 +1,11 @@ +#include "MsgHandler.h" + +class MsgHandler_ARM : public MsgHandler +{ +public: + MsgHandler_ARM(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec) + : MsgHandler(_f, _dataflash, _last_timestamp_usec) { }; + + virtual void process_message(uint8_t *msg); +};