From a356ad1c799e90e5e61968c045ede64616cf8f8e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 15 May 2015 13:19:55 +1000 Subject: [PATCH] Replay: added MAG2 message --- Tools/Replay/LogReader.cpp | 4 ++++ Tools/Replay/MsgHandler_ARSP.cpp | 3 +-- Tools/Replay/MsgHandler_BARO.cpp | 2 +- Tools/Replay/MsgHandler_GPS_Base.cpp | 2 +- Tools/Replay/MsgHandler_IMU_Base.cpp | 2 +- Tools/Replay/MsgHandler_MAG2.cpp | 6 ++++++ Tools/Replay/MsgHandler_MAG2.h | 11 +++++++++++ Tools/Replay/MsgHandler_MAG_Base.cpp | 2 +- Tools/Replay/MsgHandler_MAG_Base.h | 5 +++++ 9 files changed, 31 insertions(+), 6 deletions(-) create mode 100644 Tools/Replay/MsgHandler_MAG2.cpp create mode 100644 Tools/Replay/MsgHandler_MAG2.h diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 86d046e35e..2727cacac1 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -31,6 +31,7 @@ #include "MsgHandler_AHR2.h" #include "MsgHandler_ATT.h" #include "MsgHandler_MAG.h" +#include "MsgHandler_MAG2.h" #include "MsgHandler_NTUN_Copter.h" #include "MsgHandler_ARSP.h" @@ -182,6 +183,9 @@ bool LogReader::update(char type[5]) } else if (streq(name, "MAG")) { msgparser[f.type] = new MsgHandler_MAG(formats[f.type], dataflash, last_timestamp_usec, compass); + } else if (streq(name, "MAG2")) { + msgparser[f.type] = new MsgHandler_MAG2(formats[f.type], dataflash, + last_timestamp_usec, compass); } else if (streq(name, "NTUN")) { // the label "NTUN" is used by rover, copter and plane - // and they all look different! creation of a parser is diff --git a/Tools/Replay/MsgHandler_ARSP.cpp b/Tools/Replay/MsgHandler_ARSP.cpp index e8e87d1b9d..800ccddd12 100644 --- a/Tools/Replay/MsgHandler_ARSP.cpp +++ b/Tools/Replay/MsgHandler_ARSP.cpp @@ -7,6 +7,5 @@ void MsgHandler_ARSP::process_message(uint8_t *msg) airspeed.setHIL(require_field_float(msg, "Airspeed"), require_field_float(msg, "DiffPress"), require_field_float(msg, "Temp")); - - dataflash.Log_Write_Airspeed(airspeed); + dataflash.WriteBlock(msg, f.length); } diff --git a/Tools/Replay/MsgHandler_BARO.cpp b/Tools/Replay/MsgHandler_BARO.cpp index 9fe55553a9..73b8a0f4a5 100644 --- a/Tools/Replay/MsgHandler_BARO.cpp +++ b/Tools/Replay/MsgHandler_BARO.cpp @@ -6,5 +6,5 @@ void MsgHandler_BARO::process_message(uint8_t *msg) baro.setHIL(0, require_field_float(msg, "Press"), require_field_int16_t(msg, "Temp") * 0.01f); - dataflash.Log_Write_Baro(baro); + dataflash.WriteBlock(msg, f.length); } diff --git a/Tools/Replay/MsgHandler_GPS_Base.cpp b/Tools/Replay/MsgHandler_GPS_Base.cpp index 8e60444441..6a70ec3cfa 100644 --- a/Tools/Replay/MsgHandler_GPS_Base.cpp +++ b/Tools/Replay/MsgHandler_GPS_Base.cpp @@ -29,6 +29,6 @@ void MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, rel_altitude = 0.01f * require_field_int32_t(msg, "RelAlt"); } - dataflash.Log_Write_GPS(gps, gps_offset, rel_altitude); + dataflash.WriteBlock(msg, f.length); } diff --git a/Tools/Replay/MsgHandler_IMU_Base.cpp b/Tools/Replay/MsgHandler_IMU_Base.cpp index a97055fa9f..e3ef65698b 100644 --- a/Tools/Replay/MsgHandler_IMU_Base.cpp +++ b/Tools/Replay/MsgHandler_IMU_Base.cpp @@ -17,5 +17,5 @@ void MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg) ins.set_accel(imu_offset, accel2); } - dataflash.Log_Write_IMU(ins); + dataflash.WriteBlock(msg, f.length); } diff --git a/Tools/Replay/MsgHandler_MAG2.cpp b/Tools/Replay/MsgHandler_MAG2.cpp new file mode 100644 index 0000000000..e4455c27ed --- /dev/null +++ b/Tools/Replay/MsgHandler_MAG2.cpp @@ -0,0 +1,6 @@ +#include "MsgHandler_MAG2.h" + +void MsgHandler_MAG2::process_message(uint8_t *msg) +{ + update_from_msg_compass(1, msg); +} diff --git a/Tools/Replay/MsgHandler_MAG2.h b/Tools/Replay/MsgHandler_MAG2.h new file mode 100644 index 0000000000..94e6cb63fd --- /dev/null +++ b/Tools/Replay/MsgHandler_MAG2.h @@ -0,0 +1,11 @@ +#include "MsgHandler_MAG_Base.h" + +class MsgHandler_MAG2 : public MsgHandler_MAG_Base +{ +public: + MsgHandler_MAG2(log_Format &_f, DataFlash_Class &_dataflash, + uint64_t &_last_timestamp_usec, Compass &_compass) + : MsgHandler_MAG_Base(_f, _dataflash, _last_timestamp_usec,_compass) {}; + + virtual void process_message(uint8_t *msg); +}; diff --git a/Tools/Replay/MsgHandler_MAG_Base.cpp b/Tools/Replay/MsgHandler_MAG_Base.cpp index ea3bc97f3b..4d1b364431 100644 --- a/Tools/Replay/MsgHandler_MAG_Base.cpp +++ b/Tools/Replay/MsgHandler_MAG_Base.cpp @@ -14,6 +14,6 @@ void MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_ // mag_offset is a vector indicating the compass' calibration... compass.set_offsets(compass_offset, mag_offset); - dataflash.Log_Write_Compass(compass); + dataflash.WriteBlock(msg, f.length); } diff --git a/Tools/Replay/MsgHandler_MAG_Base.h b/Tools/Replay/MsgHandler_MAG_Base.h index 306f548964..41e30a1834 100644 --- a/Tools/Replay/MsgHandler_MAG_Base.h +++ b/Tools/Replay/MsgHandler_MAG_Base.h @@ -1,3 +1,6 @@ +#ifndef MSGHANDLER_MAG_BASE_H +#define MSGHANDLER_MAG_BASE_H + #include "MsgHandler.h" class MsgHandler_MAG_Base : public MsgHandler @@ -13,3 +16,5 @@ protected: private: Compass &compass; }; + +#endif // MSGHANDLER_MAG_BASE