Subversion Repositories FlightCtrl

Rev

Go to most recent revision | 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
}