From 235b3bebda51f345338b666055db6684d36cb08e Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 10 Sep 2014 19:31:51 +1000 Subject: [PATCH] AP_NavEKF : Add range finder health and enable range finder fusion The 2-state auxiliary filter is also renamed. --- libraries/AP_NavEKF/AP_NavEKF.cpp | 18 ++++++++++++++---- libraries/AP_NavEKF/AP_NavEKF.h | 5 +++-- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 5fc1703617..4e3af8112f 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -921,13 +921,15 @@ void NavEKF::SelectFlowFusion() // indicate that flow fusion has been performed. This is used for load spreading. flowFusePerformed = true; } else if (flow_state.obsIndex == 2) { - // Estimate the focal length scale factor (runs a two state EKF) - OpticalFlowEKF(); + // Estimate the focal length scale factor and terrain offset (runs a two state EKF) + RunAuxiliaryEKF(); // increment the index so that no further flow fusion is performed using this measurement flow_state.obsIndex = 3; // clear the flag indicating that flow fusion has been performed. The 2-state fusion is relatively computationally // cheap and can be perfomred on the same prediction cycle with subsequent fusion steps flowFusePerformed = false; + // reset the flag to indicate that no new range finder data is available for fusion + newDataRng = false; } // Apply corrections to quaternion, position and velocity states across several time steps to reduce 10Hz pulsing in the output if (flowUpdateCount < flowUpdateCountMax) { @@ -2503,7 +2505,7 @@ void NavEKF::FuseMagnetometer() Estimation of optical flow sensor focal length scale factor and terrain offset using a two state EKF This fiter requires optical flow rates that are not motion compensated */ -void NavEKF::OpticalFlowEKF() +void NavEKF::RunAuxiliaryEKF() { // start performance timer perf_begin(_perf_OpticalFlowEKF); @@ -3937,7 +3939,7 @@ void NavEKF::readAirSpdData() // write the raw optical flow measurements // this needs to be called externally. -void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, float &rawSonarRange, uint32_t &msecFlowMeas) +void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange) { // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update // The PX4Flow sensor outputs flow rates with the following axis and sign conventions: @@ -3974,6 +3976,13 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V } else { newDataFlow = false; } + // Use range finder if 3 or more consecutive good samples. This reduces likelihood of using bad data. + if (rangeHealth >= 3) { + rngMea = rawSonarRange; + newDataRng = true; + } else { + newDataRng = false; + } } // calculate the NED earth spin vector in rad/sec @@ -4216,6 +4225,7 @@ void NavEKF::ZeroVariables() gpsPosGlitchOffsetNE.zero(); // optical flow variables newDataFlow = false; + newDataRng = false; flowFusePerformed = false; fuseOptFlowData = false; flowMeaTime_ms = imuSampleTime_ms; diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index ab721c369c..72eb0f3ed5 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -160,7 +160,7 @@ public: // rawGyroRates are the sensor phsyical rotation rates measured by the internal gyro // rawSonarRange is the range in metres measured by the px4flow sensor // 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, float &rawSonarRange, uint32_t &msecFlowMeas); + void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, uint8_t &rangeHealth, float &rawSonarRange); // return data for debugging optical flow fusion void getFlowDebug(float &scaleFactor, float &obsX, float &obsY, float &innovX, float &innovY, float &gndPos, uint8_t &quality) const; @@ -347,7 +347,7 @@ private: void RecallOmega(Vector3f &omegaAvg, uint32_t msecStart, uint32_t msecEnd); // Estimate optical flow focal length scale factor and terrain offset using a 2-state EKF - void OpticalFlowEKF(); + void RunAuxiliaryEKF(); // fuse optical flow measurements into the main filter void FuseOptFlow(); @@ -585,6 +585,7 @@ private: uint8_t flowUpdateCount; // count of the number of minor state corrections using optical flow data uint8_t flowUpdateCountMax; // limit on the number of minor state corrections using optical flow data float flowUpdateCountMaxInv; // floating point inverse of flowUpdateCountMax + bool newDataRng; // true when new valid range finder data has arrived. // states held by optical flow fusion across time steps // optical flow X,Y motion compensated rate measurements are fused across two time steps