From 9635231088c9195e2a0dc826403ce11637274645 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Nov 2020 16:58:32 +1100 Subject: [PATCH] AP_DAL: added body frame odomotry --- libraries/AP_DAL/AP_DAL.cpp | 35 +++++++++++++++++++++++++++------ libraries/AP_DAL/AP_DAL.h | 3 +++ libraries/AP_DAL/LogStructure.h | 20 +++++++++++++++++-- 3 files changed, 50 insertions(+), 8 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 12a767b7e1..32b71d049d 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -321,6 +321,19 @@ void AP_DAL::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, WRITE_REPLAY_BLOCK_IFCHANGD(RWOH, _RWOH, old); } +void AP_DAL::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) +{ + end_frame(); + + const log_RBOH old = _RBOH; + _RBOH.quality = quality; + _RBOH.delPos = delPos; + _RBOH.delAng = delAng; + _RBOH.delTime = delTime; + _RBOH.timeStamp_ms = timeStamp_ms; + WRITE_REPLAY_BLOCK_IFCHANGD(RBOH, _RBOH, old); +} + #if APM_BUILD_TYPE(APM_BUILD_Replay) /* handle frame message. This message triggers the EKF2/EKF3 updates and logging @@ -370,8 +383,8 @@ void AP_DAL::handle_message(const log_RFRF &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) 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); + ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset); + ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset); } /* @@ -380,8 +393,8 @@ void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) { _REPH = msg; - ekf2.writeExtNavData(_REPH.pos, _REPH.quat, _REPH.posErr, _REPH.angErr, _REPH.timeStamp_ms, _REPH.delay_ms, _REPH.resetTime_ms); - ekf3.writeExtNavData(_REPH.pos, _REPH.quat, _REPH.posErr, _REPH.angErr, _REPH.timeStamp_ms, _REPH.delay_ms, _REPH.resetTime_ms); + ekf2.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms); + ekf3.writeExtNavData(msg.pos, msg.quat, msg.posErr, msg.angErr, msg.timeStamp_ms, msg.delay_ms, msg.resetTime_ms); } /* @@ -390,8 +403,8 @@ void AP_DAL::handle_message(const log_REPH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) void AP_DAL::handle_message(const log_REVH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) { _REVH = msg; - ekf2.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms); - ekf3.writeExtNavVelData(_REVH.vel, _REVH.err, _REVH.timeStamp_ms, _REVH.delay_ms); + ekf2.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms); + ekf3.writeExtNavVelData(msg.vel, msg.err, msg.timeStamp_ms, msg.delay_ms); } /* @@ -403,6 +416,16 @@ void AP_DAL::handle_message(const log_RWOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) // note that EKF2 does not support wheel odomotry ekf3.writeWheelOdom(msg.delAng, msg.delTime, msg.timeStamp_ms, msg.posOffset, msg.radius); } + +/* + handle body frame odomotry + */ +void AP_DAL::handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3) +{ + _RBOH = msg; + // 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); +} #endif // APM_BUILD_Replay #include diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 3dba68f1bf..32ff5cf501 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/libraries/AP_DAL/AP_DAL.h @@ -219,6 +219,7 @@ public: // log wheel odomotry data void writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius); + void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset); // Replay support: #if APM_BUILD_TYPE(APM_BUILD_Replay) @@ -295,6 +296,7 @@ public: 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); + void handle_message(const log_RBOH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3); #endif // map core number for replay @@ -318,6 +320,7 @@ private: struct log_REPH _REPH; struct log_REVH _REVH; struct log_RWOH _RWOH; + struct log_RBOH _RBOH; // cached variables for speed: uint32_t _micros; diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 75526bf7f5..328e884024 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -36,7 +36,8 @@ LOG_ROFH_MSG, \ LOG_REPH_MSG, \ LOG_REVH_MSG, \ - LOG_RWOH_MSG + LOG_RWOH_MSG, \ + LOG_RBOH_MSG // Replay Data Structures struct log_RFRH { @@ -354,6 +355,19 @@ struct log_RWOH { uint8_t _end; }; +// @LoggerMessage: RBOH +// @Description: Replay body odometry data +struct log_RBOH { + float quality; + Vector3f delPos; + Vector3f delAng; + float delTime; + uint32_t timeStamp_ms; + Vector3f posOffset; + uint16_t delay_ms; + uint8_t _end; +}; + #define RLOG_SIZE(sname) 3+offsetof(struct log_ ##sname,_end) #define LOG_STRUCTURE_FROM_DAL \ @@ -416,4 +430,6 @@ struct log_RWOH { { LOG_REVH_MSG, RLOG_SIZE(REVH), \ "REVH", "ffffIH", "VX,VY,VZ,Er,TS,D", "------", "------" }, \ { LOG_RWOH_MSG, RLOG_SIZE(RWOH), \ - "RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" }, + "RWOH", "ffIffff", "DA,DT,TS,PX,PY,PZ,R", "-------", "-------" }, \ + { LOG_RBOH_MSG, RLOG_SIZE(RBOH), \ + "RBOH", "ffffffffIfffH", "Q,DPX,DPY,DPZ,DAX,DAY,DAZ,DT,TS,OX,OY,OZ,D", "-------------", "-------------" },