Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: Add public method reporting horizontal speed limit

Browse Source 
This is required if using optical flow, as depending on height, the speed must be limited to prevent the sensor saturating
This commit is contained in:
priseborough 2014-11-15 17:44:25 +11:00 committed by Andrew Tridgell
parent c8fb376cc4
commit b56b68ce10
2 changed files with 14 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -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
{

3
libraries/AP_NavEKF/AP_NavEKF.h
View File

@ -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;