From 8160eca47e3e04e8d9b22f7b981b038492f523dd Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 12 Oct 2016 08:20:44 +1100 Subject: [PATCH] AP_NavEKF: update flow sensor interface Adds a position offset which is not used by the backend --- libraries/AP_NavEKF/AP_NavEKF.cpp | 2 +- libraries/AP_NavEKF/AP_NavEKF.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index a6e886f13d..b7663e664d 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -727,7 +727,7 @@ bool NavEKF::use_compass(void) const // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. -void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas) +void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset) { if (core) { core->writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas); diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 0a9cd1b5f3..ac8350369a 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -181,7 +181,7 @@ public: // rawGyroRates are the sensor rotation rates in rad/sec measured by the sensors internal gyro // The sign convention is that a RH physical rotation of the sensor about an axis produces both a positive flow and gyro rate // msecFlowMeas is the scheduler time in msec when the optical flow data was received from the sensor. - void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas); + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset); // return data for debugging optical flow fusion void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;