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