From f5275fd0ad0fcdd35662ecb1cddc779a3f2d5bde Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 7 Nov 2020 19:03:26 +1100 Subject: [PATCH] AP_NavEKF3: fixed use of pointers in ringbuffers these don't work with AP_DAL --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 6 +++--- libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 6 +++--- 4 files changed, 8 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 53314431bd..752b2a903e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -127,7 +127,7 @@ void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, con // subtract delay from timestamp timeStamp_ms -= delay_ms; - bodyOdmDataNew.body_offset = &posOffset; + bodyOdmDataNew.body_offset = posOffset; bodyOdmDataNew.vel = delPos * (1.0f/delTime); bodyOdmDataNew.time_ms = timeStamp_ms; bodyOdmDataNew.angRate = delAng * (1.0f/delTime); @@ -155,7 +155,7 @@ void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStam } wheel_odm_elements wheelOdmDataNew = {}; - wheelOdmDataNew.hub_offset = &posOffset; + wheelOdmDataNew.hub_offset = posOffset; wheelOdmDataNew.delAng = delAng; wheelOdmDataNew.radius = radius; wheelOdmDataNew.delTime = delTime; @@ -218,7 +218,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp index 49f74bbeae..d39572a4f4 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp @@ -309,7 +309,7 @@ void NavEKF3_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_NavEKF3/AP_NavEKF3_PosVelFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp index bf1432fc29..c955a7201e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp @@ -1145,7 +1145,7 @@ void NavEKF3_core::FuseBodyVel() bodyVelPred = (prevTnb * stateStruct.velocity); // correct sensor offset body frame position offset relative to IMU - Vector3f posOffsetBody = (*bodyOdmDataDelayed.body_offset) - accelPosOffset; + Vector3f posOffsetBody = bodyOdmDataDelayed.body_offset - accelPosOffset; // correct prediction for relative motion due to rotation // note - % operator overloaded for cross product diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index b3e7883aa6..2f4f6cf5dd 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -593,13 +593,13 @@ private: Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec) uint32_t time_ms; // measurement timestamp (msec) Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec) - const Vector3f *body_offset;// pointer to XYZ position of the optical flow sensor in body frame (m) + Vector3f body_offset; // XYZ position of the optical flow sensor in body frame (m) }; struct vel_odm_elements { Vector3f vel; // XYZ velocity measured in body frame (m/s) float velErr; // velocity measurement error 1-std (m/s) - const Vector3f *body_offset;// pointer to XYZ position of the velocity sensor in body frame (m) + Vector3f body_offset;// XYZ position of the velocity sensor in body frame (m) Vector3f angRate; // angular rate estimated from odometry (rad/sec) uint32_t time_ms; // measurement timestamp (msec) }; @@ -607,7 +607,7 @@ private: struct wheel_odm_elements { float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s) float radius; // wheel radius (m) - const Vector3f *hub_offset; // pointer to XYZ position of the wheel hub in body frame (m) + Vector3f hub_offset; // XYZ position of the wheel hub in body frame (m) float delTime; // time interval that the measurement was accumulated over (sec) uint32_t time_ms; // measurement timestamp (msec) };