Browse Source

AP_NavEKF: make posOffset const ref for optflow

mission-4.1.18
Andrew Tridgell 8 years ago
parent
commit
a05d216ce4
  1. 2
      libraries/AP_NavEKF/AP_NavEKF.cpp
  2. 2
      libraries/AP_NavEKF/AP_NavEKF.h

2
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -728,7 +728,7 @@ bool NavEKF::use_compass(void) const @@ -728,7 +728,7 @@ bool NavEKF::use_compass(void) const
// 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.
// NOTE: AP_NavEKF does not use the posOffset data
void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, Vector3f &posOffset)
void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
{
if (core) {
core->writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas);

2
libraries/AP_NavEKF/AP_NavEKF.h

@ -181,7 +181,7 @@ public: @@ -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, Vector3f &posOffset);
void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const 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;

Loading…
Cancel
Save