From b15cb46d2536e99d55ba59a2e6a5b32b0ec47a87 Mon Sep 17 00:00:00 2001 From: Stephen Dade Date: Mon, 24 Oct 2022 14:00:58 +1100 Subject: [PATCH] AP_DAL: Add support for fixed height optical flow --- libraries/AP_DAL/AP_DAL.cpp | 7 ++++--- libraries/AP_DAL/AP_DAL.h | 2 +- libraries/AP_DAL/LogStructure.h | 3 ++- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/libraries/AP_DAL/AP_DAL.cpp b/libraries/AP_DAL/AP_DAL.cpp index 7cb4545275..d86238ceed 100644 --- a/libraries/AP_DAL/AP_DAL.cpp +++ b/libraries/AP_DAL/AP_DAL.cpp @@ -321,7 +321,7 @@ bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core) } // 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) +void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride) { end_frame(); @@ -331,6 +331,7 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF _ROFH.rawGyroRates = rawGyroRates; _ROFH.msecFlowMeas = msecFlowMeas; _ROFH.posOffset = posOffset; + _ROFH.heightOverride = heightOverride; WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old); } @@ -440,8 +441,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(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset); - ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset); + ekf2.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride); + ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride); } /* diff --git a/libraries/AP_DAL/AP_DAL.h b/libraries/AP_DAL/AP_DAL.h index 53719089f5..dbac21732f 100644 --- a/libraries/AP_DAL/AP_DAL.h +++ b/libraries/AP_DAL/AP_DAL.h @@ -207,7 +207,7 @@ public: } // log optical flow data - void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset); + void writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride); // log external nav data void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms); diff --git a/libraries/AP_DAL/LogStructure.h b/libraries/AP_DAL/LogStructure.h index 527f357c08..e391a6dcf4 100644 --- a/libraries/AP_DAL/LogStructure.h +++ b/libraries/AP_DAL/LogStructure.h @@ -306,6 +306,7 @@ struct log_ROFH { Vector2f rawGyroRates; uint32_t msecFlowMeas; Vector3f posOffset; + float heightOverride; uint8_t rawFlowQuality; uint8_t _end; }; @@ -413,7 +414,7 @@ struct log_RBOH { { LOG_RVOH_MSG, RLOG_SIZE(RVOH), \ "RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \ { LOG_ROFH_MSG, RLOG_SIZE(ROFH), \ - "ROFH", "ffffIfffB", "FX,FY,GX,GY,Tms,PX,PY,PZ,Qual", "---------", "---------" }, \ + "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_REVH_MSG, RLOG_SIZE(REVH), \