aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: 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 16f7e51ea5
commit f5275fd0ad
4 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

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

2
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

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

2
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

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

6
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

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