diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 8e2b6fe87b..8942a6e46d 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/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 { diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 8b86a4122d..ddf643bcf6 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/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; diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index e391a6dcf4..14942768f3 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/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), \