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

AP_NavEKF3: getOptFlowSample returns latest correct flow data for use in calibration

Browse Source
This commit is contained in:
Randy Mackay 2022-01-20 12:17:40 +09:00
parent ee46b876cb
commit e16a64383f
4 changed files with 44 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_NavEKF3/AP_NavEKF3.cpp
View File

@ -1518,6 +1518,16 @@ void NavEKF3::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &raw
} }
} }
// retrieve latest corrected optical flow samples (used for calibration)
bool NavEKF3::getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const
{
// return optical flow samples from primary core
if (core) {
return core[primary].getOptFlowSample(timeStamp_ms, flowRate, bodyRate, losPred);
}
return false;
}
// write yaw angle sensor measurements // write yaw angle sensor measurements
void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type) void NavEKF3::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type)
{ {

3
libraries/AP_NavEKF3/AP_NavEKF3.h
View File

@ -198,6 +198,9 @@ public:
// posOffset is the XYZ flow sensor position in the body frame in m // posOffset is the XYZ flow sensor position in the body frame in m
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);
// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
/* /*
* Write body frame linear and angular displacement measurements from a visual odometry sensor * Write body frame linear and angular displacement measurements from a visual odometry sensor
* *

22
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
View File

@ -737,6 +737,28 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
} }
} }
} }
// store optical flow rates for use in external calibration
flowCalSample.timestamp_ms = imuSampleTime_ms;
flowCalSample.flowRate.x = ofDataDelayed.flowRadXY.x;
flowCalSample.flowRate.y = ofDataDelayed.flowRadXY.y;
flowCalSample.bodyRate.x = ofDataDelayed.bodyRadXYZ.x;
flowCalSample.bodyRate.y = ofDataDelayed.bodyRadXYZ.y;
flowCalSample.losPred.x = losPred[0];
flowCalSample.losPred.y = losPred[1];
}
// retrieve latest corrected optical flow samples (used for calibration)
bool NavEKF3_core::getOptFlowSample(uint32_t& timestamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const
{
if (flowCalSample.timestamp_ms != 0) {
timestamp_ms = flowCalSample.timestamp_ms;
flowRate = flowCalSample.flowRate;
bodyRate = flowCalSample.bodyRate;
losPred = flowCalSample.losPred;
return true;
}
return false;
} }
/******************************************************** /********************************************************

9
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -248,6 +248,9 @@ public:
// posOffset is the XYZ flow sensor position in the body frame in m // posOffset is the XYZ flow sensor position in the body frame in m
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);
// retrieve latest corrected optical flow samples (used for calibration)
bool getOptFlowSample(uint32_t& timeStamp_ms, Vector2f& flowRate, Vector2f& bodyRate, Vector2f& losPred) const;
/* /*
* Write body frame linear and angular displacement measurements from a visual odometry sensor * Write body frame linear and angular displacement measurements from a visual odometry sensor
* *
@ -1186,6 +1189,12 @@ private:
ftype prevPosE; // east position at last measurement ftype prevPosE; // east position at last measurement
ftype varInnovRng; // range finder observation innovation variance (m^2) ftype varInnovRng; // range finder observation innovation variance (m^2)
ftype innovRng; // range finder observation innovation (m) ftype innovRng; // range finder observation innovation (m)
struct {
uint32_t timestamp_ms; // system timestamp of last correct optical flow sample (used for calibration)
Vector2f flowRate; // latest corrected optical flow flow rate (used for calibration)
Vector2f bodyRate; // latest corrected optical flow body rate (used for calibration)
Vector2f losPred; // EKF estimated component of flowRate that comes from vehicle movement (not rotation)
} flowCalSample;
ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m) ftype hgtMea; // height measurement derived from either baro, gps or range finder data (m)
bool inhibitGndState; // true when the terrain position state is to remain constant bool inhibitGndState; // true when the terrain position state is to remain constant