diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 69af6bd9f0..e7063aacd5 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -3622,6 +3622,17 @@ 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 +{ + if (useOptFlow()) { + return max((_maxFlowRate - 1.0f), 0.0f) * (flowStates[1] - state.position[2]); + } else { + return 400.0f; //return 80% of max filter speed + } +} + // return weighting of first IMU in blending function void NavEKF::getIMU1Weighting(float &ret) const { diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h index 9fb7c1e4b7..9ff1483af8 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.h +++ b/libraries/AP_NavEKF/AP_NavEKF.h @@ -119,6 +119,9 @@ public: // This command is forgotten by the EKF each time it goes back into static mode (eg the vehicle disarms) uint8_t setInhibitGPS(void); + // return the horizontal speed limit in m/s set by optical flow limitations + float getSpeedLimit(void) const; + // return weighting of first IMU in blending function void getIMU1Weighting(float &ret) const;