Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
2189 - 1
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
2
#ifndef Compass_h
3
#define Compass_h
4
 
5
#include <inttypes.h>
6
#include "AP_Math.h"
7
 
8
// compass product id
9
#define AP_COMPASS_TYPE_UNKNOWN  0x00
10
#define AP_COMPASS_TYPE_HIL      0x01
11
#define AP_COMPASS_TYPE_HMC5843  0x02
12
#define AP_COMPASS_TYPE_HMC5883L 0x03
13
 
14
class Compass
15
{
16
public:
17
        int16_t                 product_id;     /// product id
18
        int16_t         mag_x;          ///< magnetic field strength along the X axis
19
        int16_t         mag_y;          ///< magnetic field strength along the Y axis
20
        int16_t         mag_z;          ///< magnetic field strength along the Z axis
21
        float           heading;        ///< compass heading in radians
22
        float           heading_x;      ///< compass vector X magnitude
23
        float           heading_y;      ///< compass vector Y magnitude
24
        uint32_t        last_update;    ///< micros() time of last update
25
        bool                    healthy;        ///< true if last read OK
26
 
27
        /// Constructor
28
        ///
29
        Compass();
30
 
31
        /// Initialize the compass device.
32
        ///
33
        /// @returns    True if the compass was initialized OK, false if it was not
34
        ///             found or is not functioning.
35
        ///
36
        virtual bool init();
37
 
38
        /// Read the compass and update the mag_ variables.
39
        ///
40
        virtual bool read(void) /* = 0 */;
41
 
42
        /// Calculate the tilt-compensated heading_ variables.
43
        ///
44
        /// @param  roll                The current airframe roll angle.
45
        /// @param  pitch               The current airframe pitch angle.
46
        ///
47
        // virtual void calculate(float roll, float pitch);
48
 
49
        /// Calculate the tilt-compensated heading_ variables.
50
        ///
51
        /// @param dcm_matrix                   The current orientation rotation matrix
52
        ///
53
        virtual void calculate(const Matrix3f &dcm_matrix);
54
 
55
        /// Set the compass orientation matrix, used to correct for
56
        /// various compass mounting positions.
57
        ///
58
        /// @param  rotation_matrix     Rotation matrix to transform magnetometer readings
59
        ///                             to the body frame.
60
        ///
61
        //virtual void set_orientation(enum Rotation rotation);
62
 
63
        /// Sets the compass offset x/y/z values.
64
        ///
65
        /// @param  offsets             Offsets to the raw mag_ values.
66
        ///
67
        // virtual void set_offsets(const Vector3f &offsets);
68
 
69
        /// Saves the current compass offset x/y/z values.
70
        ///
71
        /// This should be invoked periodically to save the offset values maintained by
72
        /// ::null_offsets.
73
        ///
74
        // virtual void save_offsets();
75
 
76
        /// Returns the current offset values
77
        ///
78
        /// @returns                    The current compass offsets.
79
        ///
80
        // virtual Vector3f &get_offsets();
81
 
82
        /// Sets the initial location used to get declination
83
        ///
84
        /// @param  latitude             GPS Latitude.
85
        /// @param  longitude            GPS Longitude.
86
        ///
87
        // void set_initial_location(long latitude, long longitude);
88
 
89
        /// Program new offset values.
90
        ///
91
        /// @param  x                   Offset to the raw mag_x value.
92
        /// @param  y                   Offset to the raw mag_y value.
93
        /// @param  z                   Offset to the raw mag_z value.
94
        ///
95
        // void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
96
 
97
        /// Perform automatic offset updates
98
        ///
99
        // void null_offsets(void);
100
 
101
        /// Enable/Start automatic offset updates 
102
        ///
103
        void null_offsets_enable(void);
104
 
105
 
106
        /// Disable/Stop automatic offset updates
107
        ///
108
        void null_offsets_disable(void);
109
 
110
    /// return true if the compass should be used for yaw calculations
111
    bool use_for_yaw(void) { return healthy && _use_for_yaw; }
112
 
113
        /// Sets the local magnetic field declination.
114
        ///
115
        /// @param  radians             Local field declination.
116
        ///
117
        // virtual void set_declination(float radians);
118
        float get_declination();
119
 
120
    // static const struct AP_Param::GroupInfo var_info[];
121
 
122
protected:
123
        float                   _declination;
124
    int8_t              _learn;                 ///<enable calibration learning
125
    int8_t              _use_for_yaw;           ///<enable use for yaw calculation
126
        bool                _null_enable;               ///< enabled flag for offset nulling
127
        bool                _null_init_done;        ///< first-time-around flag used by offset nulling
128
    int8_t              _auto_declination;      ///<enable automatic declination code
129
    enum Rotation               _orientation;
130
 
131
    Vector3f            _offset;
132
    ///< used by offset correction
133
    static const uint8_t _mag_history_size = 20;
134
    uint8_t                             _mag_history_index;
135
        Vector3i            _mag_history[_mag_history_size];
136
};
137
#endif