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