From 16f7e51ea53a3026d8a41a35bedd6c9876643933 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Nov 2020 19:03:26 +1100 Subject: [PATCH] AP_NavEKF2: fixed use of pointers in ringbuffers these don't work with AP_DAL --- libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 74ad035f70..2056c345c5 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp index 5127ad0759..f026a0a79c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_OptFlowFusion.cpp @@ -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; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 63c94c0784..b11084b019 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -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 {