Line data Source code
1 : #pragma once
2 :
3 : #include <cmath>
4 : #include <limits>
5 : #include <stdint.h>
6 : #include <type_traits>
7 :
8 : #include <AP_Common/AP_Common.h>
9 : #include <AP_Param/AP_Param.h>
10 :
11 : #include "definitions.h"
12 : #include "crc.h"
13 : #include "matrix3.h"
14 : #include "polygon.h"
15 : #include "quaternion.h"
16 : #include "rotations.h"
17 : #include "vector2.h"
18 : #include "vector3.h"
19 : #include "spline5.h"
20 : #include "location.h"
21 : #include "control.h"
22 :
23 : static const float NaNf = nanf("0x4152");
24 :
25 : #if HAL_WITH_EKF_DOUBLE
26 : typedef Vector2<double> Vector2F;
27 : typedef Vector3<double> Vector3F;
28 : typedef Matrix3<double> Matrix3F;
29 : typedef QuaternionD QuaternionF;
30 : #else
31 : typedef Vector2<float> Vector2F;
32 : typedef Vector3<float> Vector3F;
33 : typedef Matrix3<float> Matrix3F;
34 : typedef Quaternion QuaternionF;
35 : #endif
36 :
37 : // define AP_Param type AP_Vector3f
38 : AP_PARAMDEFV(Vector3f, Vector3f, AP_PARAM_VECTOR3F);
39 :
40 : /*
41 : * Check whether two floats are equal
42 : */
43 : template <typename Arithmetic1, typename Arithmetic2>
44 : typename std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type
45 : is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
46 :
47 : template <typename Arithmetic1, typename Arithmetic2>
48 : typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
49 : is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2);
50 :
51 : /*
52 : * @brief: Check whether a float is zero
53 : */
54 : template <typename T>
55 7629197823 : inline bool is_zero(const T fVal1) {
56 : static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
57 : "Template parameter not of type float");
58 7629197823 : return is_zero(static_cast<float>(fVal1));
59 : }
60 :
61 : /*
62 : * @brief: Check whether a float is greater than zero
63 : */
64 : template <typename T>
65 10668291771 : inline bool is_positive(const T fVal1) {
66 : static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
67 : "Template parameter not of type float");
68 10668291771 : return (static_cast<float>(fVal1) >= FLT_EPSILON);
69 : }
70 :
71 :
72 : /*
73 : * @brief: Check whether a float is less than zero
74 : */
75 : template <typename T>
76 5531594338 : inline bool is_negative(const T fVal1) {
77 : static_assert(std::is_floating_point<T>::value || std::is_base_of<T,AP_Float>::value,
78 : "Template parameter not of type float");
79 5531594338 : return (static_cast<float>(fVal1) <= (-1.0 * FLT_EPSILON));
80 : }
81 :
82 : /*
83 : * @brief: Check whether a double is greater than zero
84 : */
85 486289343 : inline bool is_positive(const double fVal1) {
86 486289343 : return (fVal1 >= static_cast<double>(FLT_EPSILON));
87 : }
88 :
89 : /*
90 : * @brief: Check whether a double is less than zero
91 : */
92 521 : inline bool is_negative(const double fVal1) {
93 521 : return (fVal1 <= static_cast<double>((-1.0 * FLT_EPSILON)));
94 : }
95 :
96 : /*
97 : * A variant of asin() that checks the input ranges and ensures a valid angle
98 : * as output. If nan is given as input then zero is returned.
99 : */
100 : template <typename T>
101 : float safe_asin(const T v);
102 :
103 : /*
104 : * A variant of sqrt() that checks the input ranges and ensures a valid value
105 : * as output. If a negative number is given then 0 is returned. The reasoning
106 : * is that a negative number for sqrt() in our code is usually caused by small
107 : * numerical rounding errors, so the real input should have been zero
108 : */
109 : template <typename T>
110 : float safe_sqrt(const T v);
111 :
112 : // matrix multiplication of two NxN matrices
113 : template <typename T>
114 : void mat_mul(const T *A, const T *B, T *C, uint16_t n);
115 :
116 : // matrix inverse
117 : template <typename T>
118 : bool mat_inverse(const T *x, T *y, uint16_t dim) WARN_IF_UNUSED;
119 :
120 : // matrix identity
121 : template <typename T>
122 : void mat_identity(T *x, uint16_t dim);
123 :
124 : /*
125 : * Constrain an angle to be within the range: -180 to 180 degrees. The second
126 : * parameter changes the units. Default: 1 == degrees, 10 == dezi,
127 : * 100 == centi.
128 : */
129 : template <typename T>
130 : T wrap_180(const T angle);
131 :
132 : /*
133 : * Wrap an angle in centi-degrees. See wrap_180().
134 : */
135 : template <typename T>
136 : T wrap_180_cd(const T angle);
137 :
138 : /*
139 : * Constrain an euler angle to be within the range: 0 to 360 degrees. The
140 : * second parameter changes the units. Default: 1 == degrees, 10 == dezi,
141 : * 100 == centi.
142 : */
143 : float wrap_360(const float angle);
144 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
145 : double wrap_360(const double angle);
146 : #endif
147 : int wrap_360(const int angle);
148 :
149 : int wrap_360_cd(const int angle);
150 : long wrap_360_cd(const long angle);
151 : float wrap_360_cd(const float angle);
152 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
153 : double wrap_360_cd(const double angle);
154 : #endif
155 :
156 :
157 : /*
158 : wrap an angle in radians to 0..2PI
159 : */
160 : float wrap_2PI(const float radian);
161 : double wrap_2PI(const double radian);
162 :
163 : /*
164 : wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
165 : */
166 : template <typename T>
167 : T wrap_PI(const T radian);
168 :
169 : /*
170 : * Constrain a value to be within the range: low and high
171 : */
172 : template <typename T>
173 : T constrain_value(const T amt, const T low, const T high);
174 :
175 : template <typename T>
176 : T constrain_value_line(const T amt, const T low, const T high, uint32_t line);
177 :
178 : #define constrain_float(amt, low, high) constrain_value_line(float(amt), float(low), float(high), uint32_t(__AP_LINE__))
179 : #define constrain_ftype(amt, low, high) constrain_value_line(ftype(amt), ftype(low), ftype(high), uint32_t(__AP_LINE__))
180 :
181 0 : inline int8_t constrain_int8(const int8_t amt, const int8_t low, const int8_t high)
182 : {
183 0 : return constrain_value(amt, low, high);
184 : }
185 :
186 : inline uint8_t constrain_uint8(const uint8_t amt, const uint8_t low, const uint8_t high)
187 : {
188 : return constrain_value(amt, low, high);
189 : }
190 :
191 207334065 : inline int16_t constrain_int16(const int16_t amt, const int16_t low, const int16_t high)
192 : {
193 207334065 : return constrain_value(amt, low, high);
194 : }
195 :
196 1898417 : inline uint16_t constrain_uint16(const uint16_t amt, const uint16_t low, const uint16_t high)
197 : {
198 1898417 : return constrain_value(amt, low, high);
199 : }
200 :
201 939513657 : inline int32_t constrain_int32(const int32_t amt, const int32_t low, const int32_t high)
202 : {
203 939513657 : return constrain_value(amt, low, high);
204 : }
205 :
206 140860547 : inline uint32_t constrain_uint32(const uint32_t amt, const uint32_t low, const uint32_t high)
207 : {
208 140860547 : return constrain_value(amt, low, high);
209 : }
210 :
211 8004 : inline int64_t constrain_int64(const int64_t amt, const int64_t low, const int64_t high)
212 : {
213 8004 : return constrain_value(amt, low, high);
214 : }
215 :
216 : inline uint64_t constrain_uint64(const uint64_t amt, const uint64_t low, const uint64_t high)
217 : {
218 : return constrain_value(amt, low, high);
219 : }
220 :
221 1887 : inline double constrain_double(const double amt, const double low, const double high)
222 : {
223 1887 : return constrain_value(amt, low, high);
224 : }
225 :
226 : // degrees -> radians
227 4938218780 : static inline constexpr double radians(double deg)
228 : {
229 4938218780 : return deg * DEG_TO_RAD;
230 : }
231 :
232 5928356593 : static inline constexpr float radians(float deg)
233 : {
234 5928356593 : return deg * DEG_TO_RAD;
235 : }
236 :
237 276377922 : static inline constexpr float radians(int deg)
238 : {
239 276377922 : return deg * DEG_TO_RAD;
240 : }
241 :
242 : // centidegrees -> radians
243 40559072 : static inline constexpr float cd_to_rad(float cdeg)
244 : {
245 40559072 : return cdeg * CDEG_TO_RAD;
246 : }
247 :
248 : // radians -> centidegrees
249 14784731 : static inline constexpr float rad_to_cd(float rad)
250 : {
251 14784731 : return rad * RAD_TO_CDEG;
252 : }
253 :
254 : // radians -> degrees
255 2578853535 : static inline constexpr float degrees(float rad)
256 : {
257 2578853535 : return rad * RAD_TO_DEG;
258 : }
259 :
260 : template<typename T>
261 5241620877 : ftype sq(const T val)
262 : {
263 5241620863 : ftype v = static_cast<ftype>(val);
264 5241620877 : return v*v;
265 : }
266 17252041191 : static inline constexpr float sq(const float val)
267 : {
268 17252041191 : return val*val;
269 : }
270 :
271 : /*
272 : * Variadic template for calculating the square norm of a vector of any
273 : * dimension.
274 : */
275 : template<typename T, typename... Params>
276 1207603383 : ftype sq(const T first, const Params... parameters)
277 : {
278 1207603383 : return sq(first) + sq(parameters...);
279 : }
280 :
281 : /*
282 : * Variadic template for calculating the norm (pythagoras) of a vector of any
283 : * dimension.
284 : */
285 : template<typename T, typename U, typename... Params>
286 620687363 : ftype norm(const T first, const U second, const Params... parameters)
287 : {
288 620687363 : return sqrtF(sq(first, second, parameters...));
289 : }
290 :
291 780025472 : inline float norm(const float first, const float second) {
292 780025472 : return sqrtf(sq(first) + sq(second));
293 : }
294 2532093869 : inline float norm(const float first, const float second, const float third) {
295 2532093869 : return sqrtf(sq(first) + sq(second) + sq(third));
296 : }
297 :
298 : #undef MIN
299 : template<typename A, typename B>
300 4145563003 : static inline auto MIN(const A &one, const B &two) -> decltype(one < two ? one : two)
301 : {
302 4145563003 : return one < two ? one : two;
303 : }
304 :
305 : #undef MAX
306 : template<typename A, typename B>
307 12254429656 : static inline auto MAX(const A &one, const B &two) -> decltype(one > two ? one : two)
308 : {
309 12254429656 : return one > two ? one : two;
310 : }
311 :
312 2 : inline constexpr uint32_t hz_to_nsec(uint32_t freq)
313 : {
314 2 : return AP_NSEC_PER_SEC / freq;
315 : }
316 :
317 2 : inline constexpr uint32_t nsec_to_hz(uint32_t nsec)
318 : {
319 2 : return AP_NSEC_PER_SEC / nsec;
320 : }
321 :
322 : inline constexpr uint32_t usec_to_nsec(uint32_t usec)
323 : {
324 : return usec * AP_NSEC_PER_USEC;
325 : }
326 :
327 2 : inline constexpr uint32_t nsec_to_usec(uint32_t nsec)
328 : {
329 2 : return nsec / AP_NSEC_PER_USEC;
330 : }
331 :
332 3 : inline constexpr uint32_t hz_to_usec(uint32_t freq)
333 : {
334 3 : return AP_USEC_PER_SEC / freq;
335 : }
336 :
337 2 : inline constexpr uint32_t usec_to_hz(uint32_t usec)
338 : {
339 2 : return AP_USEC_PER_SEC / usec;
340 : }
341 :
342 : /*
343 : linear interpolation based on a variable in a range
344 : return value will be in the range [output_low,output_high]
345 :
346 : Either polarity is supported, so input_low can be higher than input_high
347 :
348 : Read this as, "Take the value 'input_value' as a value between
349 : 'input_low' and 'input_high'. Return a value between 'output_low'
350 : and 'output_high' proportonal to the position of 'input_value'
351 : between 'input_low' and 'input_high'"
352 : */
353 : float linear_interpolate(float output_low, float output_high,
354 : float input_value,
355 : float input_low, float input_high);
356 :
357 : /* cubic "expo" curve generator
358 : * alpha range: [0,1] min to max expo
359 : * input range: [-1,1]
360 : */
361 : float expo_curve(float alpha, float input);
362 :
363 : /* throttle curve generator
364 : * thr_mid: output at mid stick
365 : * alpha: expo coefficient
366 : * thr_in: [0-1]
367 : */
368 : float throttle_curve(float thr_mid, float alpha, float thr_in);
369 :
370 : /* simple 16 bit random number generator */
371 : uint16_t get_random16(void);
372 :
373 : // generate a random float between -1 and 1, for use in SITL
374 : float rand_float(void);
375 :
376 : // generate a random Vector3f with each value between -1.0 and 1.0
377 : Vector3f rand_vec3f(void);
378 :
379 : // return true if two rotations are equal
380 : bool rotation_equal(enum Rotation r1, enum Rotation r2) WARN_IF_UNUSED;
381 :
382 : /*
383 : * return a velocity correction (in m/s in NED) for a sensor's position given it's position offsets
384 : * this correction should be added to the sensor NED measurement
385 : * sensor_offset_bf is in meters in body frame (Foward, Right, Down)
386 : * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
387 : * angular_rate is rad/sec
388 : */
389 : Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf, const Matrix3F &rot_ef_to_bf, const Vector3F &angular_rate);
390 :
391 : /*
392 : calculate a low pass filter alpha value
393 : */
394 : float calc_lowpass_alpha_dt(float dt, float cutoff_freq);
395 :
396 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
397 : // fill an array of float with NaN, used to invalidate memory in SITL
398 : void fill_nanf(float *f, uint16_t count);
399 : void fill_nanf(double *f, uint16_t count);
400 : #endif
401 :
402 : // from https://embeddedartistry.com/blog/2018/07/12/simple-fixed-point-conversion-in-c/
403 : // Convert to/from 16-bit fixed-point and float
404 : float fixed2float(const uint16_t input, const uint8_t fractional_bits = 8);
405 : uint16_t float2fixed(const float input, const uint8_t fractional_bits = 8);
406 :
407 : /*
408 : calculate turn rate in deg/sec given a bank angle and airspeed for a
409 : fixed wing aircraft
410 : */
411 : float fixedwing_turn_rate(float bank_angle_deg, float airspeed);
412 :
413 : // convert degrees fahrenheit to Kelvin
414 : float degF_to_Kelvin(float temp_f);
415 :
416 : /*
417 : constraining conversion functions to prevent undefined behaviour
418 : */
419 : int16_t float_to_int16(const float v);
420 : uint16_t float_to_uint16(const float v);
421 : int32_t float_to_int32(const float v);
422 : uint32_t float_to_uint32(const float v);
423 : uint32_t double_to_uint32(const double v);
424 : int32_t double_to_int32(const double v);
425 :
426 : /*
427 : Convert from float to int32_t without breaking Wstrict-aliasing due to type punning
428 : */
429 : int32_t float_to_int32_le(const float& value) WARN_IF_UNUSED;
430 :
431 : /*
432 : Convert from uint32_t to float without breaking Wstrict-aliasing due to type punning
433 : */
434 : float int32_to_float_le(const uint32_t& value) WARN_IF_UNUSED;
435 :
436 : /*
437 : Convert from uint64_t to double without breaking Wstrict-aliasing due to type punning
438 : */
439 : double uint64_to_double_le(const uint64_t& value) WARN_IF_UNUSED;
440 :
441 : /*
442 : get a twos-complement value from the first 'length' bits of a uint32_t
443 : With thanks to betaflight
444 : */
445 : int32_t get_twos_complement(uint32_t raw, uint8_t length) WARN_IF_UNUSED;
|
