AP_DAL: added optflow support

libraries/AP_DAL/AP_DAL.cpp

@@ -5,6 +5,11 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
 
+#if APM_BUILD_TYPE(APM_BUILD_Replay)
+#include <AP_NavEKF2/AP_NavEKF2.h>
+#include <AP_NavEKF3/AP_NavEKF3.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 enum class FrameItem : uint8_t {
@@ -254,6 +259,72 @@ bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
     return (_RFRF.core_slow & mask) != 0;
 }
 
+// log optical flow data
+void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset)
+{
+    const log_ROFH old = _ROFH;
+    _ROFH.rawFlowQuality = rawFlowQuality;
+    _ROFH.rawFlowRates = rawFlowRates;
+    _ROFH.rawGyroRates = rawGyroRates;
+    _ROFH.msecFlowMeas = msecFlowMeas;
+    _ROFH.posOffset = posOffset;
+    WRITE_REPLAY_BLOCK_IFCHANGD(ROFH, _ROFH, old);
+}
+
+#if APM_BUILD_TYPE(APM_BUILD_Replay)
+/*
+  handle frame message. This message triggers the EKF2/EKF3 updates and logging
+ */
+void AP_DAL::handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
+{
+    _RFRF.core_slow = msg.core_slow;
+
+    /*
+      note that we need to handle the case of LOG_REPLAY=1 with
+      LOG_DISARMED=0. To handle this we need to record the start of the filter
+     */
+    const uint8_t frame_types = msg.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();
+    }
+}
+
+/*
+  handle optical flow message
+ */
+void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
+{
+    _ROFH = msg;
+    ekf2.writeOptFlowMeas(_ROFH.rawFlowQuality, _ROFH.rawFlowRates, _ROFH.rawGyroRates, _ROFH.msecFlowMeas, _ROFH.posOffset);
+    ekf3.writeOptFlowMeas(_ROFH.rawFlowQuality, _ROFH.rawFlowRates, _ROFH.rawGyroRates, _ROFH.msecFlowMeas, _ROFH.posOffset);
+}
+#endif // APM_BUILD_Replay
+
 #include <stdio.h>
 
 namespace AP {

libraries/AP_DAL/AP_DAL.h

@@ -17,6 +17,11 @@
 
 #define DAL_CORE(c) AP::dal().logging_core(c)
 
+#if APM_BUILD_TYPE(APM_BUILD_Replay)
+class NavEKF2;
+class NavEKF3;
+#endif
+
 class AP_DAL {
 public:
 
@@ -205,6 +210,9 @@ public:
         return _RFRH.time_flying_ms;
     }
 
+    // log optical flow data
+    void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset);
+    
     // Replay support:
 #if APM_BUILD_TYPE(APM_BUILD_Replay)
     void handle_message(const log_RFRH &msg) {
@@ -218,10 +226,8 @@ public:
         _home.lng = msg.lng;
         _home.alt = msg.alt;
     }
+    void handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
 
-    void handle_message(const log_RFRF &msg) {
-        _RFRF.core_slow = msg.core_slow;
-    }
     void handle_message(const log_RISH &msg) {
         _ins.handle_message(msg);
     }
@@ -278,6 +284,7 @@ public:
         _visualodom.handle_message(msg);
 #endif
     }
+    void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
 #endif
 
     // map core number for replay
@@ -294,6 +301,7 @@ private:
     struct log_RFRH _RFRH;
     struct log_RFRF _RFRF;
     struct log_RFRN _RFRN;
+    struct log_ROFH _ROFH;
 
     // cached variables for speed:
     uint32_t _micros;
@@ -317,6 +325,9 @@ private:
 
     static bool logging_started;
     static bool force_write;
+
+    bool ekf2_init_done;
+    bool ekf3_init_done;
 };
 
 #define WRITE_REPLAY_BLOCK(sname,v) AP_DAL::WriteLogMessage(LOG_## sname ##_MSG, &v, nullptr, offsetof(log_ ##sname, _end))

libraries/AP_DAL/LogStructure.h

@@ -2,6 +2,7 @@
 
 #include <AP_Logger/LogStructure.h>
 #include <AP_Math/vector3.h>
+#include <AP_Math/vector2.h>
 
 #define LOG_IDS_FROM_DAL \
     LOG_RFRH_MSG, \
@@ -29,7 +30,8 @@
     LOG_RBCI_MSG, \
     LOG_RVOH_MSG, \
     LOG_RMGH_MSG, \
-    LOG_RMGI_MSG
+    LOG_RMGI_MSG, \
+    LOG_ROFH_MSG
 
 // Replay Data Structures
 struct log_RFRH {
@@ -302,6 +304,17 @@ struct log_RVOH {
     uint8_t _end;
 };
 
+// @LoggerMessage: ROFH
+// @Description: Replay optical flow data
+struct log_ROFH {
+    Vector2f rawFlowRates;
+    Vector2f rawGyroRates;
+    uint32_t msecFlowMeas;
+    Vector3f posOffset;
+    uint8_t rawFlowQuality;
+    uint8_t _end;
+};
+
 #define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end)
 
 #define LOG_STRUCTURE_FROM_DAL        \
@@ -356,4 +369,6 @@ struct log_RVOH {
     { LOG_RBCI_MSG, RLOG_SIZE(RBCI),                                   \
       "RBCI", "IffffBB", "LU,PX,PY,PZ,Dist,H,I", "smmmm-#", "?0000--" }, \
     { LOG_RVOH_MSG, RLOG_SIZE(RVOH),                                   \
-      "RVOH", "fffIBBB", "OX,OY,OZ,Del,H,Ena,NPtr", "-------", "-------" },
+      "RVOH", "fffIBBB", "OX,OY,OZ,Del,H,Ena,NPtr", "-------", "-------" }, \
+    { LOG_ROFH_MSG, RLOG_SIZE(ROFH),                                   \
+      "ROFH", "ffffIfffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,Qual", "---------", "---------" },