Line data Source code
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 0 : AP_Compass_Backend *AP_Compass_BMM150::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev, bool force_external, enum Rotation rotation)
69 : {
70 0 : if (!dev) {
71 0 : return nullptr;
72 : }
73 0 : AP_Compass_BMM150 *sensor = NEW_NOTHROW AP_Compass_BMM150(std::move(dev), force_external, rotation);
74 0 : if (!sensor || !sensor->init()) {
75 0 : delete sensor;
76 0 : return nullptr;
77 : }
78 :
79 0 : return sensor;
80 : }
81 :
82 0 : AP_Compass_BMM150::AP_Compass_BMM150(AP_HAL::OwnPtr<AP_HAL::Device> dev, bool force_external, enum Rotation rotation)
83 0 : : _dev(std::move(dev)), _rotation(rotation), _force_external(force_external)
84 : {
85 0 : }
86 :
87 0 : 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 0 : int8_t tries = 4;
109 0 : while (tries--) {
110 0 : if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers,
111 : sizeof(trim_registers))) {
112 0 : continue;
113 : }
114 0 : if (!_dev->read_registers(DIG_X1_REG, (uint8_t *)&trim_registers2,
115 : sizeof(trim_registers))) {
116 0 : continue;
117 : }
118 0 : if (memcmp(&trim_registers, &trim_registers2, sizeof(trim_registers)) == 0) {
119 0 : break;
120 : }
121 : }
122 0 : if (-1 == tries) {
123 0 : DEV_PRINTF("BMM150: Failed to load trim registers\n");
124 0 : return false;
125 : }
126 :
127 0 : _dig.x1 = trim_registers.dig_x1;
128 0 : _dig.x2 = trim_registers.dig_x2;
129 0 : _dig.xy1 = trim_registers.dig_xy1;
130 0 : _dig.xy2 = trim_registers.dig_xy2;
131 0 : _dig.xyz1 = le16toh(trim_registers.dig_xyz1);
132 0 : _dig.y1 = trim_registers.dig_y1;
133 0 : _dig.y2 = trim_registers.dig_y2;
134 0 : _dig.z1 = le16toh(trim_registers.dig_z1);
135 0 : _dig.z2 = le16toh(trim_registers.dig_z2);
136 0 : _dig.z3 = le16toh(trim_registers.dig_z3);
137 0 : _dig.z4 = le16toh(trim_registers.dig_z4);
138 :
139 0 : return true;
140 : }
141 :
142 0 : bool AP_Compass_BMM150::init()
143 : {
144 0 : uint8_t val = 0;
145 : bool ret;
146 :
147 0 : _dev->get_semaphore()->take_blocking();
148 :
149 : // 10 retries for init
150 0 : _dev->set_retries(10);
151 :
152 : // use checked registers to cope with bus errors
153 0 : _dev->setup_checked_registers(4);
154 :
155 0 : int8_t boot_tries = 4;
156 0 : while (boot_tries--) {
157 : /* Do a soft reset */
158 0 : ret = _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
159 0 : hal.scheduler->delay(2);
160 0 : if (!ret) {
161 0 : continue;
162 : }
163 :
164 : /* Change power state from suspend mode to sleep mode */
165 0 : ret = _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
166 0 : hal.scheduler->delay(2);
167 0 : if (!ret) {
168 0 : continue;
169 : }
170 :
171 0 : ret = _dev->read_registers(CHIP_ID_REG, &val, 1);
172 0 : if (!ret) {
173 0 : continue;
174 : }
175 0 : if (val == CHIP_ID_VAL) {
176 : // found it
177 0 : break;
178 : }
179 0 : if (boot_tries == 0) {
180 0 : DEV_PRINTF("BMM150: Wrong chip ID 0x%02x should be 0x%02x\n", val, CHIP_ID_VAL);
181 : }
182 : }
183 0 : if (-1 == boot_tries) {
184 0 : goto bus_error;
185 : }
186 :
187 0 : ret = _load_trim_values();
188 0 : if (!ret) {
189 0 : 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 0 : ret = _dev->write_register(REPETITIONS_XY_REG, (47 - 1) / 2, true);
200 0 : if (!ret) {
201 0 : goto bus_error;
202 : }
203 0 : ret = _dev->write_register(REPETITIONS_Z_REG, 83 - 1, true);
204 0 : if (!ret) {
205 0 : goto bus_error;
206 : }
207 : /* Change operation mode from sleep to normal and set ODR */
208 0 : ret = _dev->write_register(OP_MODE_SELF_TEST_ODR_REG, NORMAL_MODE | ODR_30HZ, true);
209 0 : if (!ret) {
210 0 : goto bus_error;
211 : }
212 :
213 0 : _dev->get_semaphore()->give();
214 :
215 : /* register the compass instance in the frontend */
216 0 : _dev->set_device_type(DEVTYPE_BMM150);
217 0 : if (!register_compass(_dev->get_bus_id())) {
218 0 : return false;
219 : }
220 :
221 0 : set_rotation(_rotation);
222 :
223 0 : if (_force_external) {
224 0 : set_external(true);
225 : }
226 :
227 : // 2 retries for run
228 0 : _dev->set_retries(2);
229 :
230 0 : _dev->register_periodic_callback(MEASURE_TIME_USEC,
231 : FUNCTOR_BIND_MEMBER(&AP_Compass_BMM150::_update, void));
232 :
233 0 : _last_read_ms = AP_HAL::millis();
234 :
235 0 : return true;
236 :
237 0 : bus_error:
238 0 : _dev->get_semaphore()->give();
239 0 : return false;
240 : }
241 :
242 : /*
243 : * Compensation algorithm got from https://github.com/BoschSensortec/BMM050_driver
244 : * this is not explained in datasheet.
245 : */
246 0 : int16_t AP_Compass_BMM150::_compensate_xy(int16_t xy, uint32_t rhall, int32_t txy1, int32_t txy2) const
247 : {
248 0 : int32_t inter = ((int32_t)_dig.xyz1) << 14;
249 0 : inter /= rhall;
250 0 : inter -= 0x4000;
251 :
252 0 : int32_t val = _dig.xy2 * ((inter * inter) >> 7);
253 0 : val += (inter * (((uint32_t)_dig.xy1) << 7));
254 0 : val >>= 9;
255 0 : val += 0x100000;
256 0 : val *= (txy2 + 0xA0);
257 0 : val >>= 12;
258 0 : val *= xy;
259 0 : val >>= 13;
260 0 : val += (txy1 << 3);
261 :
262 0 : return val;
263 : }
264 :
265 0 : int16_t AP_Compass_BMM150::_compensate_z(int16_t z, uint32_t rhall) const
266 : {
267 0 : int32_t dividend = int32_t(z - _dig.z4) << 15;
268 0 : int32_t dividend2 = dividend - ((_dig.z3 * (int32_t(rhall) - int32_t(_dig.xyz1))) >> 2);
269 :
270 0 : int32_t divisor = int32_t(_dig.z1) * (rhall << 1);
271 0 : divisor += 0x8000;
272 0 : divisor >>= 16;
273 0 : divisor += _dig.z2;
274 :
275 0 : int16_t ret = constrain_int32(dividend2 / divisor, -0x8000, 0x8000);
276 : #if 0
277 : static uint8_t counter;
278 : if (counter++ == 0) {
279 : printf("ret=%d z=%d rhall=%u z1=%d z2=%d z3=%d z4=%d xyz1=%d dividend=%d dividend2=%d divisor=%d\n",
280 : ret, z, rhall, _dig.z1, _dig.z2, _dig.z3, _dig.z4, _dig.xyz1, dividend, dividend2, divisor);
281 : }
282 : #endif
283 0 : return ret;
284 : }
285 :
286 0 : void AP_Compass_BMM150::_update()
287 : {
288 : le16_t data[4];
289 0 : bool ret = _dev->read_registers(DATA_X_LSB_REG, (uint8_t *) &data, sizeof(data));
290 :
291 : /* Checking data ready status */
292 0 : if (!ret || !(data[3] & 0x1)) {
293 0 : _dev->check_next_register();
294 0 : uint32_t now = AP_HAL::millis();
295 0 : if (now - _last_read_ms > 250) {
296 : // cope with power cycle to sensor
297 0 : _last_read_ms = now;
298 0 : _dev->write_register(POWER_AND_OPERATIONS_REG, SOFT_RESET);
299 0 : _dev->write_register(POWER_AND_OPERATIONS_REG, POWER_CONTROL_VAL, true);
300 : }
301 0 : return;
302 : }
303 :
304 0 : const uint16_t rhall = le16toh(data[3]) >> 2;
305 :
306 : Vector3f raw_field = Vector3f{
307 0 : (float) _compensate_xy(((int16_t)le16toh(data[0])) >> 3,
308 0 : rhall, _dig.x1, _dig.x2),
309 0 : (float) _compensate_xy(((int16_t)le16toh(data[1])) >> 3,
310 0 : rhall, _dig.y1, _dig.y2),
311 0 : (float) _compensate_z(((int16_t)le16toh(data[2])) >> 1, rhall)};
312 :
313 : /* apply sensitivity scale 16 LSB/uT */
314 0 : raw_field /= 16;
315 : /* convert uT to milligauss */
316 0 : raw_field *= 10;
317 :
318 0 : _last_read_ms = AP_HAL::millis();
319 :
320 0 : accumulate_sample(raw_field);
321 0 : _dev->check_next_register();
322 : }
323 :
324 : #endif // AP_COMPASS_BMM150_ENABLED
|
