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 |