AP_DAL: Add handlers for external lat lng position set

libraries/AP_DAL/AP_DAL.cpp

@@ -352,6 +352,17 @@ void AP_DAL::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float
     WRITE_REPLAY_BLOCK_IFCHANGED(REPH, _REPH, old);
 }
 
+void AP_DAL::log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms)
+{
+    end_frame();
+    const log_RSLL old = _RSLL;
+    _RSLL.lat = loc.lat;
+    _RSLL.lng = loc.lng;
+    _RSLL.posAccSD = posAccuracy;
+    _RSLL.timestamp_ms = timestamp_ms;
+    WRITE_REPLAY_BLOCK_IFCHANGED(RSLL, _RSLL, old);
+}
+
 // log external velocity data
 void AP_DAL::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
 {
@@ -485,6 +496,17 @@ void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
     // note that EKF2 does not support body frame odomotry
     ekf3.writeBodyFrameOdom(msg.quality, msg.delPos, msg.delAng, msg.delTime, msg.timeStamp_ms, msg.delay_ms, msg.posOffset);
 }
+
+/*
+  handle position reset
+ */
+void AP_DAL::handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
+{
+    _RSLL = msg;
+    // note that EKF2 does not support body frame odomotry
+    const Location loc {msg.lat, msg.lng, 0, Location::AltFrame::ABSOLUTE };
+    ekf3.setLatLng(loc, msg.posAccSD, msg.timestamp_ms);
+}
 #endif // APM_BUILD_Replay
 
 namespace AP {

libraries/AP_DAL/AP_DAL.h

@@ -82,6 +82,8 @@ public:
 
     void log_event3(Event event);
     void log_SetOriginLLH3(const Location &loc);
+    void log_SetLatLng(const Location &loc, float posAccuracy, uint32_t timestamp_ms);
+
     void log_writeDefaultAirSpeed3(const float aspeed, const float uncertainty);
     void log_writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type);
 
@@ -317,6 +319,7 @@ public:
     void handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
     void handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
     void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
+    void handle_message(const log_RSLL &msg, NavEKF2 &ekf2, NavEKF3 &ekf3);
 
     // map core number for replay
     uint8_t logging_core(uint8_t c) const;
@@ -340,6 +343,7 @@ private:
     struct log_REVH _REVH;
     struct log_RWOH _RWOH;
     struct log_RBOH _RBOH;
+    struct log_RSLL _RSLL;
 
     // cached variables for speed:
     uint32_t _micros;

libraries/AP_DAL/LogStructure.h

@@ -35,6 +35,7 @@
     LOG_RMGI_MSG, \
     LOG_ROFH_MSG, \
     LOG_REPH_MSG, \
+    LOG_RSLL_MSG, \
     LOG_REVH_MSG, \
     LOG_RWOH_MSG, \
     LOG_RBOH_MSG
@@ -324,6 +325,16 @@ struct log_REPH {
     uint8_t _end;
 };
 
+// @LoggerMessage: RSLL
+// @Description: Replay Set Lat Lng event
+struct log_RSLL {
+    int32_t lat; // WGS-84 latitude in 1E-7 degrees
+    int32_t lng; // WGS-84 longitude in 1E7 degrees
+    float posAccSD; // horizontal position 1 STD uncertainty (m)
+    uint32_t timestamp_ms;
+    uint8_t _end;
+};
+
 // @LoggerMessage: REVH
 // @Description: Replay external position data
 struct log_REVH {
@@ -417,6 +428,8 @@ struct log_RBOH {
       "ROFH", "ffffIffffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,HgtOvr,Qual", "----------", "----------" }, \
     { LOG_REPH_MSG, RLOG_SIZE(REPH),                                   \
       "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
+    { LOG_RSLL_MSG, RLOG_SIZE(RSLL),                         \
+      "RSLL", "IIfI", "Lat,Lng,PosAccSD,TS", "DU--", "GG--" }, \
     { LOG_REVH_MSG, RLOG_SIZE(REVH),                                   \
       "REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \
     { LOG_RWOH_MSG, RLOG_SIZE(RWOH),                                   \