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

AP_NavEKF: Expand EKF speed limit public method to handle control limits

This commit is contained in:
priseborough 2014-11-16 12:41:27 +11:00 committed by Andrew Tridgell
parent 9844dacf40
commit d994da0886
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -3622,14 +3622,18 @@ uint8_t NavEKF::setInhibitGPS(void)
}
}
// return the horizontal speed limit in m/s set by optical flow limitations
// allow 1.0 rad/sec margin for angular motion
float NavEKF::getSpeedLimit(void) const
// return the horizontal speed limit in m/s set by optical flow sensor limits
// 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) {
return max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]);
// allow 1.0 rad/sec margin for angular motion
ekfGndSpdLimit = max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]);
// use standard gains up to 5.0 metres height and reduce above that
ekfNavVelGainScaler = 5.0f / max((flowStates[1] - state.position[2]),5.0f);
} else {
return 400.0f; //return 80% of max filter speed
ekfGndSpdLimit = 400.0f; //return 80% of max filter speed
ekfNavVelGainScaler = 1.0f;
}
}

5
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -122,8 +122,9 @@ public:
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t setInhibitGPS(void);
// return the horizontal speed limit in m/s set by optical flow limitations
float getSpeedLimit(void) const;
// return the horizontal speed limit in m/s set by optical flow sensor limits
// 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 getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const;
// return weighting of first IMU in blending function
void getIMU1Weighting(float &ret) const;