Replay: support external position and velocity data

Tools/Replay/LR_MsgHandler.cpp

@@ -242,6 +242,16 @@ void LR_MsgHandler_ROFH::process_message(uint8_t *msg)
     AP::dal().handle_message(MSG_CAST(ROFH,msg), ekf2, ekf3);
 }
 
+void LR_MsgHandler_REPH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(REPH,msg), ekf2, ekf3);
+}
+
+void LR_MsgHandler_REVH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(REVH,msg), ekf2, ekf3);
+}
+
 #include <AP_AHRS/AP_AHRS.h>
 #include "VehicleType.h"
 

Tools/Replay/LR_MsgHandler.h

@@ -38,34 +38,68 @@ public:
     void process_message(uint8_t *msg) override;
 };
 
-class LR_MsgHandler_RFRF : public LR_MsgHandler
+class LR_MsgHandler_EKF : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_RFRF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
+    LR_MsgHandler_EKF(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
         LR_MsgHandler(_f),
         ekf2(_ekf2),
         ekf3(_ekf3) {}
     using LR_MsgHandler::LR_MsgHandler;
-    void process_message(uint8_t *msg) override;
+    virtual void process_message(uint8_t *msg) override = 0;
-private:
+protected:
     NavEKF2 &ekf2;
     NavEKF3 &ekf3;
 };
 
-class LR_MsgHandler_ROFH : public LR_MsgHandler
+class LR_MsgHandler_EKF2 : public LR_MsgHandler
 {
 public:
-    LR_MsgHandler_ROFH(struct log_Format &_f, NavEKF2 &_ekf2, NavEKF3 &_ekf3) :
+    LR_MsgHandler_EKF2(struct log_Format &_f, NavEKF2 &_ekf2) :
+        LR_MsgHandler(_f),
+        ekf2(_ekf2) {}
+    using LR_MsgHandler::LR_MsgHandler;
+    virtual void process_message(uint8_t *msg) override = 0;
+protected:
+    NavEKF2 &ekf2;
+};
+
+class LR_MsgHandler_EKF3 : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_EKF3(struct log_Format &_f, NavEKF3 &_ekf3) :
         LR_MsgHandler(_f),
-        ekf2(_ekf2),
         ekf3(_ekf3) {}
     using LR_MsgHandler::LR_MsgHandler;
-    void process_message(uint8_t *msg) override;
+    virtual void process_message(uint8_t *msg) override = 0;
-private:
+protected:
-    NavEKF2 &ekf2;
     NavEKF3 &ekf3;
 };
 
+class LR_MsgHandler_RFRF : public LR_MsgHandler_EKF
+{
+    using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_ROFH : public LR_MsgHandler_EKF
+{
+    using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_REPH : public LR_MsgHandler_EKF
+{
+    using LR_MsgHandler_EKF::LR_MsgHandler_EKF;
+    void process_message(uint8_t *msg) override;
+};
+
+class LR_MsgHandler_REVH : 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:
@@ -73,89 +107,53 @@ public:
     void process_message(uint8_t *msg) override;
 };
 
-class LR_MsgHandler_REV2 : public LR_MsgHandler
+class LR_MsgHandler_REV2 : public LR_MsgHandler_EKF2
 {
 public:
-    LR_MsgHandler_REV2(struct log_Format &_f, NavEKF2 &_ekf2) :
+    using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2;
-        LR_MsgHandler(_f),
-        ekf2(_ekf2) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF2 &ekf2;
 };
 
-class LR_MsgHandler_RSO2 : public LR_MsgHandler
+class LR_MsgHandler_RSO2 : public LR_MsgHandler_EKF2
 {
-public:
+    using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2;
-    LR_MsgHandler_RSO2(struct log_Format &_f, NavEKF2 &_ekf2) :
-        LR_MsgHandler(_f),
-        ekf2(_ekf2) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF2 &ekf2;
 };
 
-class LR_MsgHandler_RWA2 : public LR_MsgHandler
+class LR_MsgHandler_RWA2 : public LR_MsgHandler_EKF2
 {
 public:
-    LR_MsgHandler_RWA2(struct log_Format &_f, NavEKF2 &_ekf2) :
+    using LR_MsgHandler_EKF2::LR_MsgHandler_EKF2;
-        LR_MsgHandler(_f),
-        ekf2(_ekf2) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF2 &ekf2;
 };
 
 
-class LR_MsgHandler_REV3 : public LR_MsgHandler
+class LR_MsgHandler_REV3 : public LR_MsgHandler_EKF3
 {
 public:
-    LR_MsgHandler_REV3(struct log_Format &_f, NavEKF3 &_ekf3) :
+    using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
-        LR_MsgHandler(_f),
-        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF3 &ekf3;
 };
 
-class LR_MsgHandler_RSO3 : public LR_MsgHandler
+class LR_MsgHandler_RSO3 : public LR_MsgHandler_EKF3
 {
 public:
-    LR_MsgHandler_RSO3(struct log_Format &_f, NavEKF3 &_ekf3) :
+    using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
-        LR_MsgHandler(_f),
-        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF3 &ekf3;
 };
 
-class LR_MsgHandler_RWA3 : public LR_MsgHandler
+class LR_MsgHandler_RWA3 : public LR_MsgHandler_EKF3
 {
 public:
-    LR_MsgHandler_RWA3(struct log_Format &_f, NavEKF3 &_ekf3) :
+    using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
-        LR_MsgHandler(_f),
-        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF3 &ekf3;
 };
 
-class LR_MsgHandler_REY3 : public LR_MsgHandler
+class LR_MsgHandler_REY3 : public LR_MsgHandler_EKF3
 {
 public:
-    LR_MsgHandler_REY3(struct log_Format &_f, NavEKF3 &_ekf3) :
+    using LR_MsgHandler_EKF3::LR_MsgHandler_EKF3;
-        LR_MsgHandler(_f),
-        ekf3(_ekf3) {}
     void process_message(uint8_t *msg) override;
-
-private:
-    NavEKF3 &ekf3;
 };
 
 class LR_MsgHandler_RISH : public LR_MsgHandler

Tools/Replay/LogReader.cpp

@@ -222,6 +222,10 @@ bool LogReader::handle_log_format_msg(const struct log_Format &f)
         msgparser[f.type] = new LR_MsgHandler_RVOH(formats[f.type]);
     } else if (streq(name, "ROFH")) {
         msgparser[f.type] = new LR_MsgHandler_ROFH(formats[f.type], ekf2, ekf3);
+    } else if (streq(name, "REPH")) {
+        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 {
         // debug("  No parser for (%s)\n", name);
     }