AP_NavEKF3: ofDataNew made local
This commit is contained in:
parent
11847cfcf5
commit
85ade10e85
@ -196,6 +196,7 @@ void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f
|
||||
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
|
||||
if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
|
||||
// correct flow sensor body rates for bias and write
|
||||
of_elements ofDataNew {};
|
||||
ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
|
||||
ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
|
||||
// the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
|
||||
|
@ -1164,7 +1164,6 @@ private:
|
||||
|
||||
// variables added for optical flow fusion
|
||||
EKF_obs_buffer_t<of_elements> storedOF; // OF data buffer
|
||||
of_elements ofDataNew; // OF data at the current time horizon
|
||||
bool flowDataValid; // true while optical flow data is still fresh
|
||||
Vector2F auxFlowObsInnov; // optical flow rate innovation from 1-state terrain offset estimator
|
||||
uint32_t flowValidMeaTime_ms; // time stamp from latest valid flow measurement (msec)
|
||||
|
Loading…
Reference in New Issue
Block a user