Details | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 2189 | - | 1 | #include "AP_Math.h" |
| 2 | #include <math.h> |
||
| 3 | |||
| 4 | // a varient of asin() that checks the input ranges and ensures a |
||
| 5 | // valid angle as output. If nan is given as input then zero is |
||
| 6 | // returned. |
||
| 7 | float safe_asin(float v) |
||
| 8 | { |
||
| 9 | if (isnan(v)) { |
||
| 10 | return 0.0; |
||
| 11 | } |
||
| 12 | if (v >= 1.0) { |
||
| 13 | return M_PI/2; |
||
| 14 | } |
||
| 15 | if (v <= -1.0) { |
||
| 16 | return -M_PI/2; |
||
| 17 | } |
||
| 18 | return asin(v); |
||
| 19 | } |
||
| 20 | |||
| 21 | // a varient of sqrt() that checks the input ranges and ensures a |
||
| 22 | // valid value as output. If a negative number is given then 0 is |
||
| 23 | // returned. The reasoning is that a negative number for sqrt() in our |
||
| 24 | // code is usually caused by small numerical rounding errors, so the |
||
| 25 | // real input should have been zero |
||
| 26 | float safe_sqrt(float v) |
||
| 27 | { |
||
| 28 | float ret = sqrt(v); |
||
| 29 | if (isnan(ret)) { |
||
| 30 | return 0; |
||
| 31 | } |
||
| 32 | return ret; |
||
| 33 | } |
||
| 34 | |||
| 35 | |||
| 36 | // find a rotation that is the combination of two other |
||
| 37 | // rotations. This is used to allow us to add an overall board |
||
| 38 | // rotation to an existing rotation of a sensor such as the compass |
||
| 39 | // Note that this relies the set of rotations being complete. The |
||
| 40 | // optional 'found' parameter is for the test suite to ensure that it is. |
||
| 41 | enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found) |
||
| 42 | { |
||
| 43 | Vector3f tv1, tv2; |
||
| 44 | enum Rotation r; |
||
| 45 | tv1(1,2,3); |
||
| 46 | tv1.rotate(r1); |
||
| 47 | tv1.rotate(r2); |
||
| 48 | |||
| 49 | for (r=ROTATION_NONE; r<ROTATION_MAX; |
||
| 50 | r = (enum Rotation)((uint8_t)r+1)) { |
||
| 51 | Vector3f diff; |
||
| 52 | tv2(1,2,3); |
||
| 53 | tv2.rotate(r); |
||
| 54 | diff = tv1 - tv2; |
||
| 55 | if (diff.length() < 1.0e-6) { |
||
| 56 | // we found a match |
||
| 57 | if (found) { |
||
| 58 | *found = true; |
||
| 59 | } |
||
| 60 | return r; |
||
| 61 | } |
||
| 62 | } |
||
| 63 | |||
| 64 | // we found no matching rotation. Someone has edited the |
||
| 65 | // rotations list and broken its completeness property ... |
||
| 66 | if (found) { |
||
| 67 | *found = false; |
||
| 68 | } |
||
| 69 | return ROTATION_NONE; |
||
| 70 | } |