Line data Source code
1 : #include "AP_Math.h"
2 :
3 : #include <float.h>
4 :
5 : #include <AP_InternalError/AP_InternalError.h>
6 :
7 : /*
8 : * is_equal(): Integer implementation, provided for convenience and
9 : * compatibility with old code. Expands to the same as comparing the values
10 : * directly
11 : */
12 : template <typename Arithmetic1, typename Arithmetic2>
13 : typename std::enable_if<std::is_integral<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value ,bool>::type
14 3460715 : is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
15 : {
16 : typedef typename std::common_type<Arithmetic1, Arithmetic2>::type common_type;
17 3460715 : return static_cast<common_type>(v_1) == static_cast<common_type>(v_2);
18 : }
19 :
20 : /*
21 : * is_equal(): double/float implementation - takes into account
22 : * std::numeric_limits<T>::epsilon() to return if 2 values are equal.
23 : */
24 : template <typename Arithmetic1, typename Arithmetic2>
25 : typename std::enable_if<std::is_floating_point<typename std::common_type<Arithmetic1, Arithmetic2>::type>::value, bool>::type
26 13568431591 : is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
27 : {
28 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
29 : typedef typename std::common_type<Arithmetic1, Arithmetic2>::type common_type;
30 : typedef typename std::remove_cv<common_type>::type common_type_nonconst;
31 : if (std::is_same<double, common_type_nonconst>::value) {
32 10421949 : return fabs(v_1 - v_2) < std::numeric_limits<double>::epsilon();
33 : }
34 : #endif
35 : #pragma clang diagnostic push
36 : #pragma clang diagnostic ignored "-Wabsolute-value"
37 : // clang doesn't realise we catch the double case above and warns
38 : // about loss of precision here.
39 13558009642 : return fabsf(v_1 - v_2) < std::numeric_limits<float>::epsilon();
40 : #pragma clang diagnostic pop
41 : }
42 :
43 : template bool is_equal<int>(const int v_1, const int v_2);
44 : template bool is_equal<short>(const short v_1, const short v_2);
45 : template bool is_equal<long>(const long v_1, const long v_2);
46 : template bool is_equal<float>(const float v_1, const float v_2);
47 : template bool is_equal<double>(const double v_1, const double v_2);
48 :
49 : template <typename T>
50 1061683559 : float safe_asin(const T v)
51 : {
52 1061683559 : const float f = static_cast<const float>(v);
53 1061683559 : if (isnan(f)) {
54 2 : return 0.0f;
55 : }
56 1061683499 : if (f >= 1.0f) {
57 487478 : return static_cast<float>(M_PI_2);
58 : }
59 1061196021 : if (f <= -1.0f) {
60 440948 : return static_cast<float>(-M_PI_2);
61 : }
62 1060755073 : return asinf(f);
63 : }
64 :
65 : template float safe_asin<int>(const int v);
66 : template float safe_asin<short>(const short v);
67 : template float safe_asin<float>(const float v);
68 : template float safe_asin<double>(const double v);
69 :
70 : // sqrt which takes any type and returns 0 if the input is NaN or less than zero
71 : template <typename T>
72 201024247 : float safe_sqrt(const T v)
73 : {
74 : // cast before checking so we sqrtf the same value we check
75 201024247 : const float val = static_cast<float>(v);
76 : // use IEEE-754 compliant function which returns false if val is NaN
77 201024247 : if (isgreaterequal(val, 0)) {
78 201024242 : return sqrtf(val);
79 : }
80 6 : return 0;
81 : }
82 :
83 : template float safe_sqrt<int>(const int v);
84 : template float safe_sqrt<short>(const short v);
85 : template float safe_sqrt<float>(const float v);
86 : template float safe_sqrt<double>(const double v);
87 :
88 : /*
89 : replacement for std::swap() needed for STM32
90 : */
91 1885472300 : static void swap_float(float &f1, float &f2)
92 : {
93 1885472300 : float tmp = f1;
94 1885472300 : f1 = f2;
95 1885472300 : f2 = tmp;
96 1885472300 : }
97 :
98 : /*
99 : * linear interpolation based on a variable in a range
100 : */
101 947009071 : float linear_interpolate(float output_low, float output_high,
102 : float input_value,
103 : float input_low, float input_high)
104 : {
105 947009071 : if (input_low > input_high) {
106 : // support either polarity
107 942736150 : swap_float(input_low, input_high);
108 942736150 : swap_float(output_low, output_high);
109 : }
110 947009071 : if (input_value <= input_low) {
111 625609 : return output_low;
112 : }
113 946383462 : if (input_value >= input_high) {
114 694276 : return output_high;
115 : }
116 945689186 : float p = (input_value - input_low) / (input_high - input_low);
117 945689186 : return output_low + p * (output_high - output_low);
118 : }
119 :
120 : /* cubic "expo" curve generator
121 : * alpha range: [0,1] min to max expo
122 : * input range: [-1,1]
123 : */
124 2082805 : float expo_curve(float alpha, float x)
125 : {
126 2082805 : return (1.0f - alpha) * x + alpha * x * x * x;
127 : }
128 :
129 : /* throttle curve generator
130 : * thr_mid: output at mid stick
131 : * alpha: expo coefficient
132 : * thr_in: [0-1]
133 : */
134 26498 : float throttle_curve(float thr_mid, float alpha, float thr_in)
135 : {
136 26498 : float alpha2 = alpha + 1.25 * (1.0f - alpha) * (0.5f - thr_mid) / 0.5f;
137 26498 : alpha2 = constrain_float(alpha2, 0.0f, 1.0f);
138 26498 : float thr_out = 0.0f;
139 26498 : if (thr_in < 0.5f) {
140 26468 : float t = linear_interpolate(-1.0f, 0.0f, thr_in, 0.0f, 0.5f);
141 26468 : thr_out = linear_interpolate(0.0f, thr_mid, expo_curve(alpha, t), -1.0f, 0.0f);
142 : } else {
143 30 : float t = linear_interpolate(0.0f, 1.0f, thr_in, 0.5f, 1.0f);
144 30 : thr_out = linear_interpolate(thr_mid, 1.0f, expo_curve(alpha2, t), 0.0f, 1.0f);
145 : }
146 26498 : return thr_out;
147 : }
148 :
149 : template <typename T>
150 225819907 : T wrap_180(const T angle)
151 : {
152 225819907 : auto res = wrap_360(angle);
153 225819908 : if (res > T(180)) {
154 29137909 : res -= T(360);
155 : }
156 225819908 : return res;
157 : }
158 :
159 : template <typename T>
160 5726922 : T wrap_180_cd(const T angle)
161 : {
162 5726922 : auto res = wrap_360_cd(angle);
163 5726922 : if (res > T(18000)) {
164 2197941 : res -= T(36000);
165 : }
166 5726922 : return res;
167 : }
168 :
169 : template int wrap_180<int>(const int angle);
170 : template short wrap_180<short>(const short angle);
171 : template float wrap_180<float>(const float angle);
172 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
173 : template double wrap_180<double>(const double angle);
174 : #endif
175 :
176 : template int wrap_180_cd<int>(const int angle);
177 : template long wrap_180_cd<long>(const long angle);
178 : template short wrap_180_cd<short>(const short angle);
179 : template float wrap_180_cd<float>(const float angle);
180 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
181 : template double wrap_180_cd<double>(const double angle);
182 : #endif
183 :
184 305348424 : float wrap_360(const float angle)
185 : {
186 305348424 : float res = fmodf(angle, 360.0f);
187 305348424 : if (res < 0) {
188 73794777 : res += 360.0f;
189 : }
190 305348424 : return res;
191 : }
192 :
193 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
194 4077818 : double wrap_360(const double angle)
195 : {
196 4077818 : double res = fmod(angle, 360.0);
197 4077818 : if (res < 0) {
198 2054112 : res += 360.0;
199 : }
200 4077818 : return res;
201 : }
202 : #endif
203 :
204 41 : int wrap_360(const int angle)
205 : {
206 41 : int res = angle % 360;
207 41 : if (res < 0) {
208 2 : res += 360;
209 : }
210 41 : return res;
211 : }
212 :
213 14015792 : float wrap_360_cd(const float angle)
214 : {
215 14015792 : float res = fmodf(angle, 36000.0f);
216 14015792 : if (res < 0) {
217 8377339 : res += 36000.0f;
218 : }
219 14015792 : return res;
220 : }
221 :
222 : #if AP_MATH_ALLOW_DOUBLE_FUNCTIONS
223 1512 : double wrap_360_cd(const double angle)
224 : {
225 1512 : double res = fmod(angle, 36000.0);
226 1512 : if (res < 0) {
227 738 : res += 36000.0;
228 : }
229 1512 : return res;
230 : }
231 : #endif
232 :
233 3234969 : int wrap_360_cd(const int angle)
234 : {
235 3234969 : int res = angle % 36000;
236 3234969 : if (res < 0) {
237 203534 : res += 36000;
238 : }
239 3234969 : return res;
240 : }
241 :
242 2884 : long wrap_360_cd(const long angle)
243 : {
244 2884 : long res = angle % 36000;
245 2884 : if (res < 0) {
246 1436 : res += 36000;
247 : }
248 2884 : return res;
249 : }
250 :
251 480581840 : float wrap_2PI(const float radian)
252 : {
253 480581840 : float res = fmodf(radian, float(M_2PI));
254 480581840 : if (res < 0) {
255 114596358 : res += float(M_2PI);
256 : }
257 480581840 : return res;
258 : }
259 :
260 146411112 : double wrap_2PI(const double radian)
261 : {
262 146411112 : double res = fmod(radian, M_2PI);
263 146411112 : if (res < 0) {
264 43490014 : res += M_2PI;
265 : }
266 146411112 : return res;
267 : }
268 :
269 : template <typename T>
270 625752663 : T wrap_PI(const T radian)
271 : {
272 625752663 : T res = wrap_2PI(radian);
273 625752662 : if (res > T(M_PI)) {
274 175236567 : res -= T(M_2PI);
275 : }
276 625752662 : return res;
277 : }
278 :
279 : template float wrap_PI<float>(const float radian);
280 : template double wrap_PI<double>(const double radian);
281 :
282 : template <typename T>
283 13158388499 : T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
284 : {
285 : // the check for NaN as a float prevents propagation of floating point
286 : // errors through any function that uses constrain_value(). The normal
287 : // float semantics already handle -Inf and +Inf
288 13158388499 : if (isnan(amt)) {
289 : #if AP_INTERNALERROR_ENABLED
290 1 : AP::internalerror().error(AP_InternalError::error_t::constraining_nan, line);
291 : #endif
292 1 : return (low + high) / 2;
293 : }
294 :
295 13158369868 : if (amt < low) {
296 278042494 : return low;
297 : }
298 :
299 12880327374 : if (amt > high) {
300 209727786 : return high;
301 : }
302 :
303 12670599588 : return amt;
304 : }
305 :
306 : template float constrain_value_line<float>(const float amt, const float low, const float high, uint32_t line);
307 : template double constrain_value_line<double>(const double amt, const double low, const double high, uint32_t line);
308 :
309 : template <typename T>
310 1289624770 : T constrain_value(const T amt, const T low, const T high)
311 : {
312 : // the check for NaN as a float prevents propagation of floating point
313 : // errors through any function that uses constrain_value(). The normal
314 : // float semantics already handle -Inf and +Inf
315 : if (std::is_floating_point<T>::value) {
316 16637 : if (isnan(amt)) {
317 2 : INTERNAL_ERROR(AP_InternalError::error_t::constraining_nan);
318 2 : return (low + high) / 2;
319 : }
320 : }
321 :
322 1289624768 : if (amt < low) {
323 538313508 : return low;
324 : }
325 :
326 751311260 : if (amt > high) {
327 2248613 : return high;
328 : }
329 :
330 749062647 : return amt;
331 : }
332 :
333 : template int constrain_value<int>(const int amt, const int low, const int high);
334 : template unsigned int constrain_value<unsigned int>(const unsigned int amt, const unsigned int low, const unsigned int high);
335 : template long constrain_value<long>(const long amt, const long low, const long high);
336 : template unsigned long constrain_value<unsigned long>(const unsigned long amt, const unsigned long low, const unsigned long high);
337 : template long long constrain_value<long long>(const long long amt, const long long low, const long long high);
338 : template unsigned long long constrain_value<unsigned long long>(const unsigned long long amt, const unsigned long long low, const unsigned long long high);
339 : template short constrain_value<short>(const short amt, const short low, const short high);
340 : template unsigned short constrain_value<unsigned short>(const unsigned short amt, const unsigned short low, const unsigned short high);
341 : template float constrain_value<float>(const float amt, const float low, const float high);
342 : template double constrain_value<double>(const double amt, const double low, const double high);
343 :
344 : template int8_t constrain_value<int8_t>(const int8_t amt, const int8_t low, const int8_t high);
345 : template uint8_t constrain_value<uint8_t>(const uint8_t amt, const uint8_t low, const uint8_t high);
346 :
347 : /*
348 : simple 16 bit random number generator
349 : */
350 27532 : uint16_t get_random16(void)
351 : {
352 : static uint32_t m_z = 1234;
353 : static uint32_t m_w = 76542;
354 27532 : m_z = 36969 * (m_z & 0xFFFFu) + (m_z >> 16);
355 27532 : m_w = 18000 * (m_w & 0xFFFFu) + (m_w >> 16);
356 27532 : return ((m_z << 16) + m_w) & 0xFFFF;
357 : }
358 :
359 :
360 : // generate a random float between -1 and 1
361 12864255147 : float rand_float(void)
362 : {
363 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
364 12864254147 : return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
365 : #else
366 1000 : return (get_random16() / 65535.0) * 2 - 1;
367 : #endif
368 : }
369 :
370 : // generate a random Vector3f with each value between -1.0 and 1.0
371 46666720 : Vector3f rand_vec3f(void)
372 : {
373 46666720 : return Vector3f{
374 : rand_float(),
375 : rand_float(),
376 : rand_float()
377 46666720 : };
378 : }
379 :
380 : /*
381 : return true if two rotations are equivalent
382 : This copes with the fact that we have some duplicates, like ROLL_180_YAW_90 and PITCH_180_YAW_270
383 : */
384 6 : bool rotation_equal(enum Rotation r1, enum Rotation r2)
385 : {
386 6 : if (r1 == r2) {
387 2 : return true;
388 : }
389 4 : Vector3f v(1,2,3);
390 4 : Vector3f v1 = v;
391 4 : Vector3f v2 = v;
392 4 : v1.rotate(r1);
393 4 : v2.rotate(r2);
394 4 : return (v1 - v2).length() < 0.001;
395 : }
396 :
397 : /*
398 : * return a velocity correction (in m/s in NED) for a sensor's position given it's position offsets
399 : * this correction should be added to the sensor NED measurement
400 : * sensor_offset_bf is in meters in body frame (Foward, Right, Down)
401 : * rot_ef_to_bf is a rotation matrix to rotate from earth-frame (NED) to body frame
402 : * angular_rate is rad/sec
403 : */
404 4 : Vector3F get_vel_correction_for_sensor_offset(const Vector3F &sensor_offset_bf, const Matrix3F &rot_ef_to_bf, const Vector3F &angular_rate)
405 : {
406 4 : if (sensor_offset_bf.is_zero()) {
407 2 : return Vector3F();
408 : }
409 :
410 : // correct velocity
411 2 : const Vector3F vel_offset_body = angular_rate % sensor_offset_bf;
412 2 : return rot_ef_to_bf.mul_transpose(vel_offset_body) * -1.0;
413 : }
414 :
415 : /*
416 : calculate a low pass filter alpha value
417 : */
418 1940720509 : float calc_lowpass_alpha_dt(float dt, float cutoff_freq)
419 : {
420 1940720509 : if (is_negative(dt) || is_negative(cutoff_freq)) {
421 0 : INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
422 0 : return 1.0;
423 : }
424 1940719197 : if (is_zero(cutoff_freq)) {
425 139552056 : return 1.0;
426 : }
427 1801161358 : if (is_zero(dt)) {
428 1435457 : return 0.0;
429 : }
430 1799727579 : float rc = 1.0f / (M_2PI * cutoff_freq);
431 1799727579 : return dt / (dt + rc);
432 : }
433 :
434 : #ifndef AP_MATH_FILL_NANF_USE_MEMCPY
435 : #define AP_MATH_FILL_NANF_USE_MEMCPY (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
436 : #endif
437 :
438 : #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
439 : // fill an array of float with NaN, used to invalidate memory in SITL
440 1137190328 : void fill_nanf(float *f, uint16_t count)
441 : {
442 : #if AP_MATH_FILL_NANF_USE_MEMCPY
443 : static bool created;
444 : static float many_nanfs[2048];
445 1137190328 : if (!created) {
446 4290606 : for (uint16_t i=0; i<ARRAY_SIZE(many_nanfs); i++) {
447 4288512 : created = true;
448 4288512 : many_nanfs[i] = std::numeric_limits<float>::signaling_NaN();
449 : }
450 : }
451 1137190328 : if (count > ARRAY_SIZE(many_nanfs)) {
452 0 : AP_HAL::panic("Too big an area to fill");
453 : }
454 1137190328 : memcpy(f, many_nanfs, count*sizeof(many_nanfs[0]));
455 : #else
456 : const float n = std::numeric_limits<float>::signaling_NaN();
457 : while (count--) {
458 : *f++ = n;
459 : }
460 : #endif
461 1137190328 : }
462 :
463 173491036 : void fill_nanf(double *f, uint16_t count)
464 : {
465 : #if AP_MATH_FILL_NANF_USE_MEMCPY
466 : static bool created;
467 : static double many_nanfs[2048];
468 173491036 : if (!created) {
469 3190293 : for (uint16_t i=0; i<ARRAY_SIZE(many_nanfs); i++) {
470 3188736 : created = true;
471 3188736 : many_nanfs[i] = std::numeric_limits<double>::signaling_NaN();
472 : }
473 : }
474 173491036 : if (count > ARRAY_SIZE(many_nanfs)) {
475 0 : AP_HAL::panic("Too big an area to fill");
476 : }
477 173491036 : memcpy(f, many_nanfs, count*sizeof(many_nanfs[0]));
478 : #else
479 : while (count--) {
480 : *f++ = std::numeric_limits<double>::signaling_NaN();
481 : }
482 : #endif
483 173491036 : }
484 :
485 : #endif // CONFIG_HAL_BOARD == HAL_BOARD_SITL
486 :
487 : // Convert 16-bit fixed-point to float
488 4 : float fixed2float(const uint16_t input, const uint8_t fractional_bits)
489 : {
490 4 : return ((float)input / (float)(1U << fractional_bits));
491 : }
492 :
493 : // Convert float to 16-bit fixed-point
494 4 : uint16_t float2fixed(const float input, const uint8_t fractional_bits)
495 : {
496 4 : return (uint16_t)(roundf(input * (1U << fractional_bits)));
497 : }
498 :
499 : /*
500 : calculate turn rate in deg/sec given a bank angle and airspeed for a
501 : fixed wing aircraft
502 : */
503 6796 : float fixedwing_turn_rate(float bank_angle_deg, float airspeed)
504 : {
505 6796 : bank_angle_deg = constrain_float(bank_angle_deg, -80, 80);
506 6796 : return degrees(GRAVITY_MSS*tanf(radians(bank_angle_deg))/MAX(airspeed,1));
507 : }
508 :
509 : // convert degrees fahrenheit to Kelvin
510 470 : float degF_to_Kelvin(float temp_f)
511 : {
512 470 : return (temp_f + 459.67) * 0.55556;
513 : }
514 :
515 : /*
516 : conversion functions to prevent undefined behaviour
517 : */
518 3176374 : int16_t float_to_int16(const float v)
519 : {
520 3176374 : return int16_t(constrain_float(v, INT16_MIN, INT16_MAX));
521 : }
522 :
523 688 : int32_t float_to_int32(const float v)
524 : {
525 688 : return int32_t(constrain_float(v, INT32_MIN, INT32_MAX));
526 : }
527 :
528 299 : uint16_t float_to_uint16(const float v)
529 : {
530 299 : return uint16_t(constrain_float(v, 0, UINT16_MAX));
531 : }
532 :
533 688 : uint32_t float_to_uint32(const float v)
534 : {
535 688 : return uint32_t(constrain_float(v, 0, UINT32_MAX));
536 : }
537 :
538 1887 : uint32_t double_to_uint32(const double v)
539 : {
540 1887 : return uint32_t(constrain_double(v, 0, UINT32_MAX));
541 : }
542 :
543 0 : int32_t double_to_int32(const double v)
544 : {
545 0 : return int32_t(constrain_double(v, INT32_MIN, UINT32_MAX));
546 : }
547 :
548 :
549 514 : int32_t float_to_int32_le(const float& value)
550 : {
551 : int32_t out;
552 : static_assert(sizeof(value) == sizeof(out), "mismatched sizes");
553 :
554 : // Use memcpy because it's the most portable.
555 : // It might not be the fastest way on all hardware.
556 : // At least it's defined behavior in both c and c++.
557 514 : memcpy(&out, &value, sizeof(out));
558 514 : return out;
559 : }
560 :
561 313106 : float int32_to_float_le(const uint32_t& value)
562 : {
563 : float out;
564 : static_assert(sizeof(value) == sizeof(out), "mismatched sizes");
565 :
566 : // Use memcpy because it's the most portable.
567 : // It might not be the fastest way on all hardware.
568 : // At least it's defined behavior in both c and c++.
569 313106 : memcpy(&out, &value, sizeof(out));
570 313106 : return out;
571 : }
572 :
573 70239 : double uint64_to_double_le(const uint64_t& value)
574 : {
575 : double out;
576 : static_assert(sizeof(value) == sizeof(out), "mismatched sizes");
577 :
578 : // Use memcpy because it's the most portable.
579 : // It might not be the fastest way on all hardware.
580 : // At least it's defined behavior in both c and c++.
581 70239 : memcpy(&out, &value, sizeof(out));
582 70239 : return out;
583 : }
584 :
585 : /*
586 : get a twos-complement value from the first 'length' bits of a uint32_t
587 : With thanks to betaflight
588 : */
589 0 : int32_t get_twos_complement(uint32_t raw, uint8_t length)
590 : {
591 0 : if (raw & ((int)1 << (length - 1))) {
592 0 : return ((int32_t)raw) - ((int32_t)1 << length);
593 : }
594 0 : return raw;
595 : }
|
