Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_DAL: Add support for fixed height optical flow

This commit is contained in:
Stephen Dade 2022-10-24 14:00:58 +11:00 committed by Randy Mackay
parent 5d3e636d71
commit b15cb46d25
3 changed files with 7 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_DAL/AP_DAL.cpp
View File

@ -321,7 +321,7 @@ bool AP_DAL::ekf_low_time_remaining(EKFType etype, uint8_t core)
} }
// log optical flow data // 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(); end_frame();
@ -331,6 +331,7 @@ void AP_DAL::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawF
_ROFH.rawGyroRates = rawGyroRates; _ROFH.rawGyroRates = rawGyroRates;
_ROFH.msecFlowMeas = msecFlowMeas; _ROFH.msecFlowMeas = msecFlowMeas;
_ROFH.posOffset = posOffset; _ROFH.posOffset = posOffset;
_ROFH.heightOverride = heightOverride;
WRITE_REPLAY_BLOCK_IFCHANGED(ROFH, _ROFH, old); 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) void AP_DAL::handle_message(const log_ROFH &msg, NavEKF2 &ekf2, NavEKF3 &ekf3)
{ {
_ROFH = msg; _ROFH = msg;
ekf2.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); ekf3.writeOptFlowMeas(msg.rawFlowQuality, msg.rawFlowRates, msg.rawGyroRates, msg.msecFlowMeas, msg.posOffset, msg.heightOverride);
} }
/* /*

2
libraries/AP_DAL/AP_DAL.h
View File

@ -207,7 +207,7 @@ public:
} }
// log optical flow data // 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 // 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); void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);

3
libraries/AP_DAL/LogStructure.h
View File

@ -306,6 +306,7 @@ struct log_ROFH {
Vector2f rawGyroRates; Vector2f rawGyroRates;
uint32_t msecFlowMeas; uint32_t msecFlowMeas;
Vector3f posOffset; Vector3f posOffset;
float heightOverride;
uint8_t rawFlowQuality; uint8_t rawFlowQuality;
uint8_t _end; uint8_t _end;
}; };
@ -413,7 +414,7 @@ struct log_RBOH {
{ LOG_RVOH_MSG, RLOG_SIZE(RVOH), \ { LOG_RVOH_MSG, RLOG_SIZE(RVOH), \
"RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \ "RVOH", "fffIBB", "OX,OY,OZ,Del,H,Ena", "------", "------" }, \
{ LOG_ROFH_MSG, RLOG_SIZE(ROFH), \ { 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), \ { LOG_REPH_MSG, RLOG_SIZE(REPH), \
"REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \ "REPH", "fffffffffIIH", "PX,PY,PZ,Q1,Q2,Q3,Q4,PEr,AEr,TS,RT,D", "------------", "------------" }, \
{ LOG_REVH_MSG, RLOG_SIZE(REVH), \ { LOG_REVH_MSG, RLOG_SIZE(REVH), \