Replay: handle optical flow

and move frame processing to AP_DAL

Tools/Replay/LR_MsgHandler.cpp

@@ -22,37 +22,7 @@ void LR_MsgHandler_RFRH::process_message(uint8_t *msg)
 
 void LR_MsgHandler_RFRF::process_message(uint8_t *msg)
 {
-    AP::dal().handle_message(MSG_CAST(RFRF,msg));
-    const log_RFRF &RFRF = MSG_CAST(RFRF,msg);
-    uint8_t frame_types = RFRF.frame_types;
-    if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF2)) {
-        ekf2_init_done = ekf2.InitialiseFilter();
-    }
-    if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF2)) {
-        if (!ekf2_init_done) {
-            ekf2_init_done = ekf2.InitialiseFilter();
-        }
-        if (ekf2_init_done) {
-            ekf2.UpdateFilter();
-        }
-    }
-    if (frame_types & uint8_t(AP_DAL::FrameType::InitialiseFilterEKF3)) {
-        ekf3_init_done = ekf3.InitialiseFilter();
-    }
-    if (frame_types & uint8_t(AP_DAL::FrameType::UpdateFilterEKF3)) {
-        if (!ekf3_init_done) {
-            ekf3_init_done = ekf3.InitialiseFilter();
-        }
-        if (ekf3_init_done) {
-            ekf3.UpdateFilter();
-        }
-    }
-    if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF2)) {
-        ekf2.Log_Write();
-    }
-    if (frame_types & uint8_t(AP_DAL::FrameType::LogWriteEKF3)) {
-        ekf3.Log_Write();
-    }
+    AP::dal().handle_message(MSG_CAST(RFRF,msg), ekf2, ekf3);
 }
 
 void LR_MsgHandler_RFRN::process_message(uint8_t *msg)
@@ -267,6 +237,11 @@ void LR_MsgHandler_RVOH::process_message(uint8_t *msg)
     AP::dal().handle_message(MSG_CAST(RVOH,msg));
 }
 
+void LR_MsgHandler_ROFH::process_message(uint8_t *msg)
+{
+    AP::dal().handle_message(MSG_CAST(ROFH,msg), ekf2, ekf3);
+}
+
 #include <AP_AHRS/AP_AHRS.h>
 #include "VehicleType.h"
 

Tools/Replay/LR_MsgHandler.h

@@ -50,8 +50,20 @@ public:
 private:
     NavEKF2 &ekf2;
     NavEKF3 &ekf3;
-    bool ekf2_init_done;
-    bool ekf3_init_done;
+};
+
+class LR_MsgHandler_ROFH : public LR_MsgHandler
+{
+public:
+    LR_MsgHandler_ROFH(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;
+private:
+    NavEKF2 &ekf2;
+    NavEKF3 &ekf3;
 };
 
 class LR_MsgHandler_RFRN : public LR_MsgHandler

Tools/Replay/LogReader.cpp

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