Line data Source code
1 : /*
2 : * This file is free software: you can redistribute it and/or modify it
3 : * under the terms of the GNU General Public License as published by the
4 : * Free Software Foundation, either version 3 of the License, or
5 : * (at your option) any later version.
6 : *
7 : * This file is distributed in the hope that it will be useful, but
8 : * WITHOUT ANY WARRANTY; without even the implied warranty of
9 : * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
10 : * See the GNU General Public License for more details.
11 : *
12 : * You should have received a copy of the GNU General Public License along
13 : * with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : #include "AP_Compass_MMC5xx3.h"
17 :
18 : #if AP_COMPASS_MMC5XX3_ENABLED
19 :
20 : #include <AP_HAL/AP_HAL.h>
21 : #include <stdio.h>
22 :
23 : extern const AP_HAL::HAL &hal;
24 :
25 : #define REG_PRODUCT_ID 0x2F
26 : #define REG_XOUT_L 0x00
27 : #define REG_STATUS 0x08
28 : #define REG_CONTROL0 0x09
29 : #define REG_CONTROL1 0x0A
30 : #define REG_CONTROL2 0x0B
31 :
32 : // bits in REG_CONTROL0
33 : #define REG_CONTROL0_RESET 0x10 // Set coil for measuring offset
34 : #define REG_CONTROL0_SET 0x08 // Reset coil for measuring offset
35 : #define REG_CONTROL0_TMM 0x01 // Take Measurement for Magnetic field
36 : #define REG_CONTROL0_TMT 0x02 // Take Measurement for Temperature
37 :
38 : // bits in REG_CONTROL1
39 : #define REG_CONTROL1_SW_RST 0x80 // Software reset
40 : #define REG_CONTROL1_BW0 0x01
41 : #define REG_CONTROL1_BW1 0x02
42 :
43 : #define MMC5983_ID 0x30
44 :
45 0 : AP_Compass_Backend *AP_Compass_MMC5XX3::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
46 : bool force_external,
47 : enum Rotation rotation)
48 : {
49 0 : if (!dev) {
50 0 : return nullptr;
51 : }
52 0 : AP_Compass_MMC5XX3 *sensor = NEW_NOTHROW AP_Compass_MMC5XX3(std::move(dev), force_external, rotation);
53 0 : if (!sensor || !sensor->init()) {
54 0 : delete sensor;
55 0 : return nullptr;
56 : }
57 :
58 0 : return sensor;
59 : }
60 :
61 0 : AP_Compass_MMC5XX3::AP_Compass_MMC5XX3(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
62 : bool _force_external,
63 0 : enum Rotation _rotation)
64 0 : : dev(std::move(_dev))
65 0 : , force_external(_force_external)
66 0 : , have_initial_offset(false)
67 0 : , rotation(_rotation)
68 : {
69 0 : }
70 :
71 0 : bool AP_Compass_MMC5XX3::init()
72 : {
73 : // take i2c bus semaphore
74 0 : WITH_SEMAPHORE(dev->get_semaphore());
75 :
76 0 : dev->set_retries(10);
77 :
78 : // setup to allow reads on SPI
79 0 : if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
80 0 : dev->set_read_flag(0x80);
81 : }
82 :
83 : // Reading REG_PRODUCT_ID fails sometimes on SPI, so we retry up to 10 times
84 0 : uint8_t whoami = 0;
85 0 : uint8_t tries = 10;
86 0 : while (whoami == 0 && tries > 0) {
87 0 : tries--;
88 0 : dev->read_registers(REG_PRODUCT_ID, &whoami, 1);
89 0 : hal.scheduler->delay(5);
90 : }
91 :
92 0 : if (whoami != MMC5983_ID) {
93 : // printf("MMC5983 got unexpected product id: %d, expected: %d\n", whoami, MMC5983_ID);
94 : // not a MMC5983
95 0 : return false;
96 : }
97 :
98 : // reset sensor
99 0 : dev->write_register(REG_CONTROL1, REG_CONTROL1_SW_RST);
100 :
101 : // 10ms minimum startup time
102 0 : hal.scheduler->delay(15);
103 :
104 : // setup for 100Hz output
105 0 : if (!dev->write_register(REG_CONTROL1, 0)) {
106 0 : return false;
107 : }
108 :
109 :
110 : /* register the compass instance in the frontend */
111 0 : dev->set_device_type(DEVTYPE_MMC5983);
112 0 : if (!register_compass(dev->get_bus_id())) {
113 0 : return false;
114 : }
115 :
116 0 : printf("Found a MMC5983 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), instance);
117 :
118 0 : set_rotation(rotation);
119 :
120 0 : if (force_external) {
121 0 : set_external(true);
122 : }
123 :
124 0 : dev->set_retries(1);
125 :
126 : // call timer() at 100Hz
127 0 : dev->register_periodic_callback(10000U,
128 : FUNCTOR_BIND_MEMBER(&AP_Compass_MMC5XX3::timer, void));
129 :
130 0 : return true;
131 0 : }
132 :
133 0 : void AP_Compass_MMC5XX3::timer()
134 : {
135 : // recalculate the offset with set/reset operation every measure_count_limit measurements
136 : // sensor is read at about 100Hz, so about every 10 seconds
137 0 : const uint16_t measure_count_limit = 1000U;
138 0 : const uint16_t zero_offset = 32768U; // 16 bit mode
139 0 : const uint16_t sensitivity = 4096U; // counts per Gauss, 16 bit mode
140 0 : constexpr float counts_to_milliGauss = 1.0e3f / sensitivity;
141 :
142 : /*
143 : we use the SET/RESET method to remove bridge offset every
144 : measure_count_limit measurements. This involves a fairly complex
145 : state machine, but means we are much less sensitive to
146 : temperature changes
147 : */
148 0 : switch (state) {
149 :
150 : // perform a set operation
151 0 : case MMCState::STATE_SET: {
152 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET)) {
153 0 : break;
154 : }
155 : // minimum time to wait after set/reset before take measurement request is 1ms
156 0 : state = MMCState::STATE_SET_MEASURE;
157 0 : break;
158 : }
159 :
160 : // request a measurement for field and offset calculation after set operation
161 0 : case MMCState::STATE_SET_MEASURE: {
162 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
163 0 : break;
164 : }
165 0 : state = MMCState::STATE_SET_WAIT;
166 0 : break;
167 : }
168 :
169 : // wait for measurement to be ready after set operation, then read the
170 : // measurement data and request a reset operation
171 0 : case MMCState::STATE_SET_WAIT: {
172 : uint8_t status;
173 0 : if (!dev->read_registers(REG_STATUS, &status, 1)) {
174 0 : state = MMCState::STATE_SET;
175 0 : break;
176 : }
177 :
178 : // check if measurement is ready
179 0 : if (!(status & 1)) {
180 0 : break;
181 : }
182 :
183 : // read measurement
184 0 : if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data0[0], 6)) {
185 0 : state = MMCState::STATE_SET;
186 0 : break;
187 : }
188 :
189 : // request set operation
190 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET)) {
191 0 : break;
192 : }
193 : // minimum time to wait after set/reset before take measurement request is 1ms
194 0 : state = MMCState::STATE_RESET_MEASURE;
195 0 : break;
196 : }
197 :
198 : // request a measurement for field and offset calculation after reset operation
199 0 : case MMCState::STATE_RESET_MEASURE: {
200 : // take measurement request
201 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
202 0 : state = MMCState::STATE_SET;
203 0 : break;
204 : }
205 :
206 0 : state = MMCState::STATE_RESET_WAIT;
207 0 : break;
208 : }
209 :
210 : // wait for measurement to be ready after reset operation,
211 : // then read the measurement data, calculate the field and offset,
212 : // and begin requesting field measurements
213 0 : case MMCState::STATE_RESET_WAIT: {
214 : uint8_t status;
215 0 : if (!dev->read_registers(REG_STATUS, &status, 1)) {
216 0 : state = MMCState::STATE_SET;
217 0 : break;
218 : }
219 : // check if measurement is ready
220 0 : if (!(status & 1)) {
221 0 : break;
222 : }
223 :
224 : uint8_t data1[6];
225 0 : if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
226 0 : state = MMCState::STATE_SET;
227 0 : break;
228 : }
229 :
230 : /*
231 : calculate field and offset
232 : */
233 0 : Vector3f f1 {float((data0[0] << 8) + data0[1]) - zero_offset,
234 0 : float((data0[2] << 8) + data0[3]) - zero_offset,
235 0 : float((data0[4] << 8) + data0[5]) - zero_offset};
236 0 : Vector3f f2 {float((data1[0] << 8) + data1[1]) - zero_offset,
237 0 : float((data1[2] << 8) + data1[3]) - zero_offset,
238 0 : float((data1[4] << 8) + data1[5]) - zero_offset};
239 :
240 0 : Vector3f field {(f2 - f1) * counts_to_milliGauss * 0.5f};
241 0 : Vector3f new_offset {(f1 + f2) * counts_to_milliGauss * 0.5f};
242 :
243 0 : if (!have_initial_offset) {
244 0 : offset = new_offset;
245 0 : have_initial_offset = true;
246 : } else {
247 : // low pass changes to the offset
248 0 : offset = offset * 0.5f + new_offset * 0.5f;
249 : }
250 :
251 0 : accumulate_sample(field);
252 :
253 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) {
254 0 : printf("failed to initiate measurement\n");
255 0 : state = MMCState::STATE_SET;
256 : } else {
257 0 : state = MMCState::STATE_MEASURE;
258 : }
259 :
260 0 : break;
261 : }
262 :
263 : // take repeated field measurements, set/reset is performed again after
264 : // measure_count_limit measurements
265 0 : case MMCState::STATE_MEASURE: {
266 : uint8_t status;
267 0 : if (!dev->read_registers(REG_STATUS, &status, 1)) {
268 0 : state = MMCState::STATE_SET;
269 0 : break;
270 : }
271 :
272 : // check if measurement is ready
273 0 : if (!(status & 1)) {
274 0 : break;
275 : }
276 :
277 : uint8_t data1[6];
278 0 : if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
279 0 : printf("cant read data\n");
280 0 : state = MMCState::STATE_SET;
281 0 : break;
282 : }
283 :
284 0 : Vector3f field {float((data1[0] << 8) + data1[1]) - zero_offset,
285 0 : float((data1[2] << 8) + data1[3]) - zero_offset,
286 0 : float((data1[4] << 8) + data1[5]) - zero_offset};
287 0 : field *= counts_to_milliGauss;
288 0 : field -= offset;
289 0 : accumulate_sample(field);
290 :
291 : // we stay in STATE_MEASURE for measure_count_limit cycles
292 0 : if (measure_count++ >= measure_count_limit) {
293 0 : measure_count = 0;
294 0 : state = MMCState::STATE_SET;
295 : } else {
296 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TMM)) { // Take Measurement
297 0 : state = MMCState::STATE_SET;
298 : }
299 : }
300 0 : break;
301 : }
302 : }
303 0 : }
304 :
305 : #endif // AP_COMPASS_MMC5XX3_ENABLED
306 :
|
