fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 04:53:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib Functions: improve documentation and limit checking

Browse Source
This commit is contained in:
Matthias Grob
2018-02-02 00:38:01 -05:00
committed by Daniel Agar
parent 8c201c88ca
commit 8e68f5ed79
1 changed files with 33 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mathlib/math/Functions.hpp
+33 -18
View File
@@ -60,11 +60,12 @@ int sign(T val)
* 1 - pure cubic function
* @return result of function output
*/
template<typename _Tp>
inline const _Tp expo(const _Tp &value, const _Tp &e)
template<typename T>
inline const T expo(const T &value, const T &e)
{
_Tp x = constrain(value, (_Tp) - 1, (_Tp)1);
return (1 - e) * x + e * x * x * x;
T x = constrain(value, (T) -1, (T) 1);
T ec = constrain(e, (T) 0, (T) 1);
return (1 - ec) * x + ec * x * x * x;
}
/*
@@ -76,29 +77,43 @@ inline const _Tp expo(const _Tp &value, const _Tp &e)
* @param g [0,1) function parameter to set SuperExpo shape
* 0 - pure expo function
* 0.99 - very strong bent curve, stays zero until maximum stick input
* 1 - DO NOT USE, division by zero on maxima
* @return result of function output
*/
template<typename _Tp>
inline const _Tp superexpo(const _Tp &value, const _Tp &e, const _Tp &g)
template<typename T>
inline const T superexpo(const T &value, const T &e, const T &g)
{
_Tp x = constrain(value, (_Tp) - 1, (_Tp)1);
return expo(x, e) * (1 - g) / (1 - fabsf(x) * g);
T x = constrain(value, (T) -1, (T) 1);
T gc = constrain(g, (T) 0, (T) 0.99);
return expo(x, e) * (1 - gc) / (1 - fabsf(x) * gc);
}
template<typename _Tp>
inline const _Tp deadzone(const _Tp &value, const _Tp &dz)
/*
* Deadzone function being linear and continuous outside of the deadzone
* 1 ------
* /
* --
* /
* -1 ------
* -1 -dz +dz 1
* @param value [-1,1] input value to function
* @param dz [0,1) ratio between deazone and complete span
* 0 - no deadzone, linear -1 to 1
* 0.5 - deadzone is half of the span [-0.5,0.5]
* 0.99 - almost entire span is deadzone
*/
template<typename T>
inline const T deadzone(const T &value, const T &dz)
{
_Tp x = constrain(value, (_Tp) - 1, (_Tp)1);
_Tp dzc = constrain(dz, (_Tp) - 1, (_Tp)1);
T x = constrain(value, (T) -1, (T) 1);
T dzc = constrain(dz, (T) 0, (T) 0.99);
// Rescale the input such that we get a piecewise linear function that will be continuous with applied deadzone
_Tp out = (x - sign(x) * dzc) / (1 - dzc);
T out = (x - sign(x) * dzc) / (1 - dzc);
// apply the deadzone (values zero around the middle)
return out * (fabsf(x) > dzc);
}
template<typename _Tp>
inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz)
template<typename T>
inline const T expo_deadzone(const T &value, const T &e, const T &dz)
{
return expo(deadzone(value, dz), e);
}
@@ -113,8 +128,8 @@ inline const _Tp expo_deadzone(const _Tp &value, const _Tp &e, const _Tp &dz)
* y_low -------
* x_low x_high
*/
template<typename _Tp>
inline const _Tp gradual(const _Tp &value, const _Tp &x_low, const _Tp &x_high, const _Tp &y_low, const _Tp &y_high)
template<typename T>
inline const T gradual(const T &value, const T &x_low, const T &x_high, const T &y_low, const T &y_high)
{
if (value < x_low) {
return y_low;