Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 00:28:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: fixed use of pointers in ringbuffers

Browse Source 
these don't work with AP_DAL
This commit is contained in:
Andrew Tridgell 2020-11-07 19:03:26 +11:00
parent eaa8474563
commit 16f7e51ea5
3 changed files with 7 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -163,7 +163,7 @@ void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
// note correction for different axis and sign conventions used by the px4flow sensor
ofDataNew.flowRadXY = - rawFlowRates; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
// write the flow sensor position in body frame
ofDataNew.body_offset = &posOffset;
ofDataNew.body_offset = posOffset;
// write flow rate measurements corrected for body rates
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;

2
libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp
View File

@ -309,7 +309,7 @@ void NavEKF2_core::FuseOptFlow()
// correct range for flow sensor offset body frame position offset
// the corrected value is the predicted range from the sensor focal point to the
// centre of the image on the ground assuming flat terrain
Vector3f posOffsetBody = (*ofDataDelayed.body_offset) - accelPosOffset;
Vector3f posOffsetBody = ofDataDelayed.body_offset - accelPosOffset;
if (!posOffsetBody.is_zero()) {
Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
range -= posOffsetEarth.z / prevTnb.c.z;

10
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -500,11 +500,11 @@ private:
};
struct of_elements {
Vector2f flowRadXY; // 0..1
Vector2f flowRadXYcomp; // 2..3
uint32_t time_ms; // 4
Vector3f bodyRadXYZ; //8..10
const Vector3f *body_offset;// 5..7
Vector2f flowRadXY;
Vector2f flowRadXYcomp;
uint32_t time_ms;
Vector3f bodyRadXYZ;
Vector3f body_offset;
};
struct ext_nav_elements {