From afb11cf29bfc395550499faebbaa413f92a4ecf3 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Nov 2020 11:33:40 +1100 Subject: [PATCH] Replay: support wheel odomotry --- Tools/Replay/LR_MsgHandler.cpp | 5 +++++ Tools/Replay/LR_MsgHandler.h | 6 ++++++ Tools/Replay/LogReader.cpp | 2 ++ 3 files changed, 13 insertions(+) diff --git a/Tools/Replay/LR_MsgHandler.cpp b/Tools/Replay/LR_MsgHandler.cpp index bf6a2c813b..a61ebc1cf2 100644 --- a/Tools/Replay/LR_MsgHandler.cpp +++ b/Tools/Replay/LR_MsgHandler.cpp @@ -242,6 +242,11 @@ void LR_MsgHandler_ROFH::process_message(uint8_t *msg) AP::dal().handle_message(MSG_CAST(ROFH,msg), ekf2, ekf3); } +void LR_MsgHandler_RWOH::process_message(uint8_t *msg) +{ + AP::dal().handle_message(MSG_CAST(RWOH,msg), ekf2, ekf3); +} + void LR_MsgHandler_REPH::process_message(uint8_t *msg) { AP::dal().handle_message(MSG_CAST(REPH,msg), ekf2, ekf3); diff --git a/Tools/Replay/LR_MsgHandler.h b/Tools/Replay/LR_MsgHandler.h index 768fa5e9d9..4ef3fcebe0 100644 --- a/Tools/Replay/LR_MsgHandler.h +++ b/Tools/Replay/LR_MsgHandler.h @@ -100,6 +100,12 @@ class LR_MsgHandler_REVH : public LR_MsgHandler_EKF void process_message(uint8_t *msg) override; }; +class LR_MsgHandler_RWOH : public LR_MsgHandler_EKF +{ + using LR_MsgHandler_EKF::LR_MsgHandler_EKF; + void process_message(uint8_t *msg) override; +}; + class LR_MsgHandler_RFRN : public LR_MsgHandler { public: diff --git a/Tools/Replay/LogReader.cpp b/Tools/Replay/LogReader.cpp index 489e10d1eb..3e7e6680c4 100644 --- a/Tools/Replay/LogReader.cpp +++ b/Tools/Replay/LogReader.cpp @@ -226,6 +226,8 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f) msgparser[f.type] = new LR_MsgHandler_REPH(formats[f.type], ekf2, ekf3); } else if (streq(name, "REVH")) { msgparser[f.type] = new LR_MsgHandler_REVH(formats[f.type], ekf2, ekf3); + } else if (streq(name, "RWOH")) { + msgparser[f.type] = new LR_MsgHandler_RWOH(formats[f.type], ekf2, ekf3); } else { // debug(" No parser for (%s)\n", name); }