2 * This file is part of Cleanflight and Betaflight.
4 * Cleanflight and Betaflight are free software. You can redistribute
5 * this software and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
10 * Cleanflight and Betaflight are distributed in the hope that they
11 * will be useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
26 #define sq(x) ((x)*(x))
28 #define power3(x) ((x)*(x)*(x))
30 // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations
31 #define FAST_MATH // order 9 approximation
32 #define VERY_FAST_MATH // order 7 approximation
34 // Use floating point M_PI instead explicitly.
35 #define M_PIf 3.14159265358979323846f
36 #define M_EULERf 2.71828182845904523536f
37 #define M_SQRT2f 1.41421356237309504880f
38 #define M_LN2f 0.69314718055994530942f
40 #define RAD (M_PIf / 180.0f)
41 #define DEGREES_TO_DECIDEGREES(angle) ((angle) * 10)
42 #define DECIDEGREES_TO_DEGREES(angle) ((angle) / 10)
43 #define DECIDEGREES_TO_RADIANS(angle) ((angle) / 10.0f * 0.0174532925f)
44 #define DEGREES_TO_RADIANS(angle) ((angle) * 0.0174532925f)
46 #define CM_S_TO_KM_H(centimetersPerSecond) ((centimetersPerSecond) * 36 / 1000)
47 #define CM_S_TO_MPH(centimetersPerSecond) ((centimetersPerSecond) * 10000 / 5080 / 88)
50 __extension__ ({ __typeof__ (a) _a = (a); \
51 __typeof__ (b) _b = (b); \
54 __extension__ ({ __typeof__ (a) _a = (a); \
55 __typeof__ (b) _b = (b); \
58 __extension__ ({ __typeof__ (x) _x = (x); \
63 #define HZ_TO_INTERVAL(x) (1.0f / (x))
64 #define HZ_TO_INTERVAL_US(x) (1000000 / (x))
66 typedef int32_t fix12_t
;
68 typedef struct stdev_s
70 float m_oldM
, m_newM
, m_oldS
, m_newS
;
74 // Floating point 3 vector.
75 typedef struct fp_vector
{
81 typedef union u_fp_vector
{
86 // Floating point Euler angles.
87 // Be carefull, could be either of degrees or radians.
88 typedef struct fp_angles
{
99 typedef struct fp_rotationMatrix_s
{
100 float m
[3][3]; // matrix
101 } fp_rotationMatrix_t
;
103 int gcd(int num
, int denom
);
104 int32_t applyDeadband(int32_t value
, int32_t deadband
);
105 float fapplyDeadband(float value
, float deadband
);
107 void devClear(stdev_t
*dev
);
108 void devPush(stdev_t
*dev
, float x
);
109 float devVariance(stdev_t
*dev
);
110 float devStandardDeviation(stdev_t
*dev
);
111 float degreesToRadians(int16_t degrees
);
113 int scaleRange(int x
, int srcFrom
, int srcTo
, int destFrom
, int destTo
);
114 float scaleRangef(float x
, float srcFrom
, float srcTo
, float destFrom
, float destTo
);
116 void normalizeV(struct fp_vector
*src
, struct fp_vector
*dest
);
118 void rotateV(struct fp_vector
*v
, fp_angles_t
*delta
);
119 void buildRotationMatrix(fp_angles_t
*delta
, fp_rotationMatrix_t
*rotation
);
120 void applyRotation(float *v
, fp_rotationMatrix_t
*rotationMatrix
);
122 int32_t quickMedianFilter3(int32_t * v
);
123 int32_t quickMedianFilter5(int32_t * v
);
124 int32_t quickMedianFilter7(int32_t * v
);
125 int32_t quickMedianFilter9(int32_t * v
);
127 float quickMedianFilter3f(float * v
);
128 float quickMedianFilter5f(float * v
);
129 float quickMedianFilter7f(float * v
);
130 float quickMedianFilter9f(float * v
);
132 #if defined(FAST_MATH) || defined(VERY_FAST_MATH)
133 float sin_approx(float x
);
134 float cos_approx(float x
);
135 float atan2_approx(float y
, float x
);
136 float acos_approx(float x
);
137 #define tan_approx(x) (sin_approx(x) / cos_approx(x))
138 float exp_approx(float val
);
139 float log_approx(float val
);
140 float pow_approx(float a
, float b
);
142 #define sin_approx(x) sinf(x)
143 #define cos_approx(x) cosf(x)
144 #define atan2_approx(y,x) atan2f(y,x)
145 #define acos_approx(x) acosf(x)
146 #define tan_approx(x) tanf(x)
147 #define exp_approx(x) expf(x)
148 #define log_approx(x) logf(x)
149 #define pow_approx(a, b) powf(b, a)
152 void arraySubInt32(int32_t *dest
, int32_t *array1
, int32_t *array2
, int count
);
154 int16_t qPercent(fix12_t q
);
155 int16_t qMultiply(fix12_t q
, int16_t input
);
156 fix12_t
qConstruct(int16_t num
, int16_t den
);
158 static inline int constrain(int amt
, int low
, int high
)
168 static inline float constrainf(float amt
, float low
, float high
)