aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: remove unnecessary function

Browse Source
This commit is contained in:
priseborough 2014-12-20 21:30:06 +11:00 committed by Randy Mackay
parent a16253796f
commit 3891dada78
2 changed files with 12 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -925,8 +925,10 @@ void NavEKF::SelectMagFusion()
// select fusion of optical flow measurements
void NavEKF::SelectFlowFusion()
{
// if we don't have flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in static mode
if (!useOptFlow() && !staticMode && gpsInhibitMode == 2) {
// Check if the optical flow data is still valid
flowDataValid = (imuSampleTime_ms - flowValidMeaTime_ms < 200);
// if we don't have valid flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in static mode
if (!flowDataValid && !staticMode && gpsInhibitMode == 2) {
velHoldMode = true;
} else {
velHoldMode = false;
@ -943,7 +945,7 @@ void NavEKF::SelectFlowFusion()
// check if fusion should be delayed to spread load. Setting fuseMeNow to true disables this load spreading feature.
bool delayFusion = ((covPredStep || magFusePerformed) && !(timeout || inhibitLoadLeveling));
// if the filter is initialised, we have data to fuse and the vehicle is not excessively tilted, then perform optical flow fusion
if (useOptFlow() && newDataFlow && tiltOK && !delayFusion && !staticMode)
if (flowDataValid && newDataFlow && tiltOK && !delayFusion && !staticMode)
{
// reset state updates and counter used to spread fusion updates across several frames to reduce 10Hz pulsing
memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta));
@ -962,14 +964,14 @@ void NavEKF::SelectFlowFusion()
// update the time stamp
prevFlowFusionTime_ms = imuSampleTime_ms;
} else if (useOptFlow() && flow_state.obsIndex == 1 && !delayFusion && !staticMode){
} else if (flowDataValid && flow_state.obsIndex == 1 && !delayFusion && !staticMode){
// Fuse the optical flow Y axis data into the main filter
FuseOptFlow();
// increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle
flow_state.obsIndex = 2;
// indicate that flow fusion has been performed. This is used for load spreading.
flowFusePerformed = true;
} else if (((useOptFlow() && flow_state.obsIndex == 2) || newDataRng) && !staticMode) {
} else if (((flowDataValid && flow_state.obsIndex == 2) || newDataRng) && !staticMode) {
// enable fusion of range data if available and permitted
if(newDataRng && useRngFinder()) {
fuseRngData = true;
@ -3637,7 +3639,7 @@ uint8_t NavEKF::setInhibitGPS(void)
// return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
void NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
{
if (useOptFlow() || gpsInhibitMode == 2) {
if (flowDataValid || gpsInhibitMode == 2) {
// allow 1.0 rad/sec margin for angular motion
ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * max((flowStates[1] - state.position[2]), 0.1f);
// use standard gains up to 5.0 metres height and reduce above that
@ -4379,6 +4381,7 @@ void NavEKF::ZeroVariables()
memset(&flowIncrStateDelta[0], 0, sizeof(flowIncrStateDelta));
// optical flow variables
newDataFlow = false;
flowDataValid = false;
newDataRng = false;
flowFusePerformed = false;
fuseOptFlowData = false;
@ -4412,17 +4415,6 @@ bool NavEKF::useRngFinder(void) const
return true;
}
// return true if the optical flow sensor is producing valid data and can be used
bool NavEKF::useOptFlow(void) const
{
// only use sensor if data is fresh
if (imuSampleTime_ms - flowValidMeaTime_ms < 200) {
return true;
} else {
return false;
}
}
// return true if optical flow data is available
bool NavEKF::optFlowDataPresent(void) const
{
@ -4540,7 +4532,7 @@ void NavEKF::getFilterStatus(uint8_t &status) const
status = (!state.quat.is_nan()<<0 | // we can implement a better way to detect invalid attitude, but it is tricky to do reliably
!(velTimeout && tasTimeout)<<1 |
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
((gpsInhibitMode == 2 && useOptFlow()) || (!tasTimeout && assume_zero_sideslip()))<<3 |
((gpsInhibitMode == 2 && flowDataValid) || (!tasTimeout && assume_zero_sideslip()))<<3 |
!posTimeout<<4 |
!hgtTimeout<<5 |
!inhibitGndState<<6);

6
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -369,9 +369,6 @@ private:
// Calculate weighting that is applied to IMU1 accel data to blend data from IMU's 1 and 2
void calcIMU_Weighting(float K1, float K2);
// return true if the optical flow sensor is producing valid data and can be used
bool useOptFlow(void) const;
// return true if optical flow data is available
bool optFlowDataPresent(void) const;
@ -592,7 +589,8 @@ private:
// variables added for optical flow fusion
float dtIMUinv; // inverse of IMU time step
bool newDataFlow; // true when new optical flow data has arrived
bool flowFusePerformed; // true when optical flow fusion has been perfomred in that time step
bool flowFusePerformed; // true when optical flow fusion has been performed in that time step
bool flowDataValid; // true while optical flow data is still fresh
state_elements statesAtFlowTime;// States at the middle of the optical flow sample period
bool fuseOptFlowData; // this boolean causes the last optical flow measurement to be fused
float auxFlowObsInnov[2]; // optical flow observation innovations from 2-state focal length scale factor and terrain offset estimator