Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Apply single definition of using optical flow

This commit is contained in:
priseborough 2014-11-16 07:01:50 +11:00 committed by Andrew Tridgell
parent b56b68ce10
commit 416eaf4633
1 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -933,7 +933,7 @@ void NavEKF::SelectMagFusion()
void NavEKF::SelectFlowFusion() 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 we don't have flow measurements and are not using GPS, dead reckon using current velocity vector unless we are in static mode
if (imuSampleTime_ms - flowMeaTime_ms > 200 && !staticMode && gpsInhibitMode == 2) { if (!useOptFlow() && !staticMode && gpsInhibitMode == 2) {
velHoldMode = true; velHoldMode = true;
} else { } else {
velHoldMode = false; velHoldMode = false;
@ -969,14 +969,14 @@ void NavEKF::SelectFlowFusion()
// update the time stamp // update the time stamp
prevFlowFusionTime_ms = imuSampleTime_ms; prevFlowFusionTime_ms = imuSampleTime_ms;
} else if (flow_state.obsIndex == 1 && !delayFusion && !staticMode){ } else if (useOptFlow() && flow_state.obsIndex == 1 && !delayFusion && !staticMode){
// Fuse the optical flow Y axis data into the main filter // Fuse the optical flow Y axis data into the main filter
FuseOptFlow(); FuseOptFlow();
// increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle // increment the index to fuse the X and Y data using the 2-state EKF on the next prediction cycle
flow_state.obsIndex = 2; flow_state.obsIndex = 2;
// indicate that flow fusion has been performed. This is used for load spreading. // indicate that flow fusion has been performed. This is used for load spreading.
flowFusePerformed = true; flowFusePerformed = true;
} else if ((flow_state.obsIndex == 2 || newDataRng) && !staticMode) { } else if (((useOptFlow() && flow_state.obsIndex == 2) || newDataRng) && !staticMode) {
// enable fusion of range data if available and permitted // enable fusion of range data if available and permitted
if(newDataRng && useRngFinder()) { if(newDataRng && useRngFinder()) {
fuseRngData = true; fuseRngData = true;
@ -3613,7 +3613,7 @@ uint8_t NavEKF::setInhibitGPS(void)
} }
// If we have received flow sensor data in the last second then indicate that we can do relative position // If we have received flow sensor data in the last second then indicate that we can do relative position
// otherwise indicate we can provide attitude and height only // otherwise indicate we can provide attitude and height only
if ((imuSampleTime_ms - flowMeaTime_ms) < 1000) { if (useOptFlow()) {
gpsInhibitMode = 2; gpsInhibitMode = 2;
return 2; return 2;
} else { } else {
@ -3626,7 +3626,7 @@ uint8_t NavEKF::setInhibitGPS(void)
// allow 1.0 rad/sec margin for angular motion // allow 1.0 rad/sec margin for angular motion
float NavEKF::getSpeedLimit(void) const float NavEKF::getSpeedLimit(void) const
{ {
if (useOptFlow()) { if (useOptFlow() || gpsInhibitMode == 2) {
return max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]); return max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]);
} else { } else {
return 400.0f; //return 80% of max filter speed return 400.0f; //return 80% of max filter speed
@ -4388,8 +4388,12 @@ bool NavEKF::useRngFinder(void) const
// return true if we should use the optical flow sensor // return true if we should use the optical flow sensor
bool NavEKF::useOptFlow(void) const bool NavEKF::useOptFlow(void) const
{ {
// TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor // only use sensor if data is fresh
return true; if (imuSampleTime_ms - flowMeaTime_ms < 200) {
return true;
} else {
return false;
}
} }
// return true if the vehicle code has requested use of static mode // return true if the vehicle code has requested use of static mode
@ -4493,7 +4497,7 @@ void NavEKF::getFilterStatus(uint8_t &status) const
status = (!state.quat.is_nan()<<0 | status = (!state.quat.is_nan()<<0 |
!(velTimeout && posTimeout)<<1 | !(velTimeout && posTimeout)<<1 |
!((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 | !((velTimeout && hgtTimeout) || (hgtTimeout && _fusionModeGPS > 0))<<2 |
(gpsInhibitMode == 2 && (imuSampleTime_ms - flowMeaTime_ms) < 1000)<<3 | (gpsInhibitMode == 2 && useOptFlow())<<3 |
!posTimeout<<4 | !posTimeout<<4 |
!hgtTimeout<<5 | !hgtTimeout<<5 |
!inhibitGndState<<6); !inhibitGndState<<6);