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
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);
}
/*

2
libraries/AP_DAL/AP_DAL.h
View File

@ -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);

3
libraries/AP_DAL/LogStructure.h
View File

@ -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), \