AP_DAL: support wheel odomotry

libraries/AP_DAL/AP_DAL.cpp

@@ -277,6 +277,7 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
     WRITE_REPLAY_BLOCK_IFCHANGD(ROFH, _ROFH, old);
 }
 
+// log external navigation data
 void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
 {
     end_frame();
@@ -292,6 +293,7 @@ void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
     WRITE_REPLAY_BLOCK_IFCHANGD(REPH, _REPH, old);
 }
 
+// log external velocity data
 void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
 {
     end_frame();
@@ -305,6 +307,20 @@ void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeSta
 
 }
 
+// log wheel odomotry data
+void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
+{
+    end_frame();
+
+    const log_RWOH old = _RWOH;
+    _RWOH.delAng = delAng;
+    _RWOH.delTime = delTime;
+    _RWOH.timeStamp_ms = timeStamp_ms;
+    _RWOH.posOffset = posOffset;
+    _RWOH.radius = radius;
+    WRITE_REPLAY_BLOCK_IFCHANGD(RWOH, _RWOH, old);
+}
+
 #if APM_BUILD_TYPE(APM_BUILD_Replay)
 /*
   handle frame message. This message triggers the EKF2/EKF3 updates and logging
@@ -369,7 +385,7 @@ void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
 }
 
 /*
-  handle external position data
+  handle external velocity data
  */
 void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
 {
@@ -377,6 +393,16 @@ void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
     ekf2.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms);
     ekf3.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms);
 }
+
+/*
+  handle wheel odomotry data
+ */
+void AP_DAL::handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
+{
+    _RWOH = msg;
+    // note that EKF2 does not support wheel odomotry
+    ekf3.writeWheelOdom(msg.delAng, msg.delTime, msg.timeStamp_ms, msg.posOffset, msg.radius);
+}
 #endif // APM_BUILD_Replay
 
 #include <stdio.h>

libraries/AP_DAL/AP_DAL.h

@@ -217,6 +217,9 @@ public:
     void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
     void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
 
+    // log wheel odomotry data
+    void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius);
+
     // Replay support:
 #if APM_BUILD_TYPE(APM_BUILD_Replay)
     void handle_message(const log_RFRH &msg) {
@@ -291,6 +294,7 @@ public:
     void handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
     void handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
     void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
+    void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
 #endif
 
     // map core number for replay
@@ -313,6 +317,7 @@ private:
     struct log_ROFH _ROFH;
     struct log_REPH _REPH;
     struct log_REVH _REVH;
+    struct log_RWOH _RWOH;
 
     // cached variables for speed:
     uint32_t _micros;

libraries/AP_DAL/LogStructure.h

@@ -35,7 +35,8 @@
     LOG_RMGI_MSG, \
     LOG_ROFH_MSG, \
     LOG_REPH_MSG, \
-    LOG_REVH_MSG
+    LOG_REVH_MSG, \
+    LOG_RWOH_MSG
 
 // Replay Data Structures
 struct log_RFRH {
@@ -342,6 +343,17 @@ struct log_REVH {
     uint8_t _end;
 };
 
+// @LoggerMessage: RWOH
+// @Description: Replay wheel odometry data
+struct log_RWOH {
+    float delAng;
+    float delTime;
+    uint32_t timeStamp_ms;
+    Vector3f posOffset;
+    float radius;
+    uint8_t _end;
+};
+
 #define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end)
 
 #define LOG_STRUCTURE_FROM_DAL        \
@@ -402,4 +414,6 @@ struct log_REVH {
     { LOG_REPH_MSG, RLOG_SIZE(REPH),                                   \
       "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
     { LOG_REVH_MSG, RLOG_SIZE(REVH),                                   \
-      "REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" },
+      "REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \
+    { LOG_RWOH_MSG, RLOG_SIZE(RWOH),                                   \
+      "RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" },