CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
Path: blob/master/libraries/AP_Compass/AP_Compass_BMM150.cpp
Views: 1798
1
/*
2
* Copyright (C) 2016 Intel Corporation. All rights reserved.
3
*
4
* This file is free software: you can redistribute it and/or modify it
5
* under the terms of the GNU General Public License as published by the
6
* Free Software Foundation, either version 3 of the License, or
7
* (at your option) any later version.
8
*
9
* This file is distributed in the hope that it will be useful, but
10
* WITHOUT ANY WARRANTY; without even the implied warranty of
11
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12
* See the GNU General Public License for more details.
13
*
14
* You should have received a copy of the GNU General Public License along
15
* with this program. If not, see <http://www.gnu.org/licenses/>.
16
*/
17
#include "AP_Compass_BMM150.h"
18
19
#if AP_COMPASS_BMM150_ENABLED
20
21
#include <AP_HAL/AP_HAL.h>
22
23
#include <utility>
24
25
#include <AP_HAL/utility/sparse-endian.h>
26
#include <AP_Math/AP_Math.h>
27
#include <stdio.h>
28
29
#define CHIP_ID_REG 0x40
30
#define CHIP_ID_VAL 0x32
31
32
#define POWER_AND_OPERATIONS_REG 0x4B
33
#define POWER_CONTROL_VAL (1 << 0)
34
#define SOFT_RESET (1 << 7 | 1 << 1)
35
36
#define OP_MODE_SELF_TEST_ODR_REG 0x4C
37
#define NORMAL_MODE (0 << 1)
38
#define ODR_30HZ (1 << 3 | 1 << 4 | 1 << 5)
39
#define ODR_20HZ (1 << 3 | 0 << 4 | 1 << 5)
40
41
#define DATA_X_LSB_REG 0x42
42
43
#define REPETITIONS_XY_REG 0x51
44
#define REPETITIONS_Z_REG 0X52
45
46
/* Trim registers */
47
#define DIG_X1_REG 0x5D
48
#define DIG_Y1_REG 0x5E
49
#define DIG_Z4_LSB_REG 0x62
50
#define DIG_Z4_MSB_REG 0x63
51
#define DIG_X2_REG 0x64
52
#define DIG_Y2_REG 0x65
53
#define DIG_Z2_LSB_REG 0x68
54
#define DIG_Z2_MSB_REG 0x69
55
#define DIG_Z1_LSB_REG 0x6A
56
#define DIG_Z1_MSB_REG 0x6B
57
#define DIG_XYZ1_LSB_REG 0x6C
58
#define DIG_XYZ1_MSB_REG 0x6D
59
#define DIG_Z3_LSB_REG 0x6E
60
#define DIG_Z3_MSB_REG 0x6F
61
#define DIG_XY2_REG 0x70
62
#define DIG_XY1_REG 0x71
63
64
#define MEASURE_TIME_USEC 16667
65
66
extern const AP_HAL::HAL &hal;
67
68
AP_Compass_Backend *AP_Compass_BMM150::probe(AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev, bool force_external, enum Rotation rotation)
69
{
70
if (!dev) {
71
return nullptr;
72
}
73
AP_Compass_BMM150 *sensor = NEW_NOTHROW AP_Compass_BMM150(std::move(dev), force_external, rotation);
74
if (!sensor || !sensor->init()) {
75
delete sensor;
76
return nullptr;
77
}
78
79
return sensor;
80
}
81
82
AP_Compass_BMM150::AP_Compass_BMM150(AP_HAL::OwnPtr<AP_HAL::Device> dev, bool force_external, enum Rotation rotation)
83
: _dev(std::move(dev)), _rotation(rotation), _force_external(force_external)
84
{
85
}
86
87
bool AP_Compass_BMM150::_load_trim_values()
88
{
89
struct {
90
int8_t dig_x1;
91
int8_t dig_y1;
92
uint8_t rsv[3];
93
le16_t dig_z4;
94
int8_t dig_x2;
95
int8_t dig_y2;
96
uint8_t rsv2[2];
97
le16_t dig_z2;
98
le16_t dig_z1;
99
le16_t dig_xyz1;
100
le16_t dig_z3;
101
int8_t dig_xy2;
102
uint8_t dig_xy1;
103
} PACKED trim_registers, trim_registers2;
104
105
// read the registers twice to confirm we have the right
106
// values. There is no CRC in the registers and these values are
107
// vital to correct operation
108
int8_t tries = 4;
109
while (tries--) {
110
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
111
sizeof(trim_registers))) {
112
continue;
113
}
114
if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers2,
115
sizeof(trim_registers))) {
116
continue;
117
}
118
if (memcmp(&trim_registers, &trim_registers2, sizeof(trim_registers)) == 0) {
119
break;
120
}
121
}
122
if (-1 == tries) {
123
DEV_PRINTF("BMM150: Failed to load trim registers\n");
124
return false;
125
}
126
127
_dig.x1 = trim_registers.dig_x1;
128
_dig.x2 = trim_registers.dig_x2;
129
_dig.xy1 = trim_registers.dig_xy1;
130
_dig.xy2 = trim_registers.dig_xy2;
131
_dig.xyz1 = le16toh(trim_registers.dig_xyz1);
132
_dig.y1 = trim_registers.dig_y1;
133
_dig.y2 = trim_registers.dig_y2;
134
_dig.z1 = le16toh(trim_registers.dig_z1);
135
_dig.z2 = le16toh(trim_registers.dig_z2);
136
_dig.z3 = le16toh(trim_registers.dig_z3);
137
_dig.z4 = le16toh(trim_registers.dig_z4);
138
139
return true;
140
}
141
142
bool AP_Compass_BMM150::init()
143
{
144
uint8_t val = 0;
145
bool ret;
146
147
_dev->get_semaphore()->take_blocking();
148
149
// 10 retries for init
150
_dev->set_retries(10);
151
152
// use checked registers to cope with bus errors
153
_dev->setup_checked_registers(4);
154
155
int8_t boot_tries = 4;
156
while (boot_tries--) {
157
/* Do a soft reset */
158
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
159
hal.scheduler->delay(2);
160
if (!ret) {
161
continue;
162
}
163
164
/* Change power state from suspend mode to sleep mode */
165
ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
166
hal.scheduler->delay(2);
167
if (!ret) {
168
continue;
169
}
170
171
ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
172
if (!ret) {
173
continue;
174
}
175
if (val == CHIP_ID_VAL) {
176
// found it
177
break;
178
}
179
if (boot_tries == 0) {
180
DEV_PRINTF("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL);
181
}
182
}
183
if (-1 == boot_tries) {
184
goto bus_error;
185
}
186
187
ret = _load_trim_values();
188
if (!ret) {
189
goto bus_error;
190
}
191
192
/*
193
* Recommended preset for high accuracy:
194
* - Rep X/Y = 47
195
* - Rep Z = 83
196
* - ODR = 20
197
* But we are going to use 30Hz of ODR
198
*/
199
ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2, true);
200
if (!ret) {
201
goto bus_error;
202
}
203
ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1, true);
204
if (!ret) {
205
goto bus_error;
206
}
207
/* Change operation mode from sleep to normal and set ODR */
208
ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ, true);
209
if (!ret) {
210
goto bus_error;
211
}
212
213
_dev->get_semaphore()->give();
214
215
/* register the compass instance in the frontend */
216
_dev->set_device_type(DEVTYPE_BMM150);
217
if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
218
return false;
219
}
220
set_dev_id(_compass_instance, _dev->get_bus_id());
221
222
set_rotation(_compass_instance, _rotation);
223
224
if (_force_external) {
225
set_external(_compass_instance, true);
226
}
227
228
// 2 retries for run
229
_dev->set_retries(2);
230
231
_dev->register_periodic_callback(MEASURE_TIME_USEC,
232
FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
233
234
_last_read_ms = AP_HAL::millis();
235
236
return true;
237
238
bus_error:
239
_dev->get_semaphore()->give();
240
return false;
241
}
242
243
/*
244
* Compensation algorithm got from https://github.com/BoschSensortec/BMM050_driver
245
* this is not explained in datasheet.
246
*/
247
int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2) const
248
{
249
int32_t inter = ((int32_t)_dig.xyz1) << 14;
250
inter /= rhall;
251
inter -= 0x4000;
252
253
int32_t val = _dig.xy2 * ((inter * inter) >> 7);
254
val += (inter * (((uint32_t)_dig.xy1) << 7));
255
val >>= 9;
256
val += 0x100000;
257
val *= (txy2 + 0xA0);
258
val >>= 12;
259
val *= xy;
260
val >>= 13;
261
val += (txy1 << 3);
262
263
return val;
264
}
265
266
int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall) const
267
{
268
int32_t dividend = int32_t(z - _dig.z4) << 15;
269
int32_t dividend2 = dividend - ((_dig.z3 * (int32_t(rhall) - int32_t(_dig.xyz1))) >> 2);
270
271
int32_t divisor = int32_t(_dig.z1) * (rhall << 1);
272
divisor += 0x8000;
273
divisor >>= 16;
274
divisor += _dig.z2;
275
276
int16_t ret = constrain_int32(dividend2 / divisor, -0x8000, 0x8000);
277
#if 0
278
static uint8_t counter;
279
if (counter++ == 0) {
280
printf("ret=%d z=%d rhall=%u z1=%d z2=%d z3=%d z4=%d xyz1=%d dividend=%d dividend2=%d divisor=%d\n",
281
ret, z, rhall, _dig.z1, _dig.z2, _dig.z3, _dig.z4, _dig.xyz1, dividend, dividend2, divisor);
282
}
283
#endif
284
return ret;
285
}
286
287
void AP_Compass_BMM150::_update()
288
{
289
le16_t data[4];
290
bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
291
292
/* Checking data ready status */
293
if (!ret || !(data[3] & 0x1)) {
294
_dev->check_next_register();
295
uint32_t now = AP_HAL::millis();
296
if (now - _last_read_ms > 250) {
297
// cope with power cycle to sensor
298
_last_read_ms = now;
299
_dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
300
_dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
301
}
302
return;
303
}
304
305
const uint16_t rhall = le16toh(data[3]) >> 2;
306
307
Vector3f raw_field = Vector3f{
308
(float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,
309
rhall, _dig.x1, _dig.x2),
310
(float) _compensate_xy(((int16_t)le16toh(data[1])) >> 3,
311
rhall, _dig.y1, _dig.y2),
312
(float) _compensate_z(((int16_t)le16toh(data[2])) >> 1, rhall)};
313
314
/* apply sensitivity scale 16 LSB/uT */
315
raw_field /= 16;
316
/* convert uT to milligauss */
317
raw_field *= 10;
318
319
_last_read_ms = AP_HAL::millis();
320
321
accumulate_sample(raw_field, _compass_instance);
322
_dev->check_next_register();
323
}
324
325
void AP_Compass_BMM150::read()
326
{
327
drain_accumulated_samples(_compass_instance);
328
}
329
330
331
#endif // AP_COMPASS_BMM150_ENABLED
332
333