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 : Driver by Andrew Tridgell, Nov 2016
17 : */
18 : #include "AP_Compass_AK09916.h"
19 :
20 : #if AP_COMPASS_AK09916_ENABLED
21 :
22 : #include <assert.h>
23 : #include <AP_HAL/AP_HAL.h>
24 : #include <utility>
25 : #include <AP_Math/AP_Math.h>
26 : #include <stdio.h>
27 : #include <AP_InertialSensor/AP_InertialSensor_Invensensev2.h>
28 : #include <GCS_MAVLink/GCS.h>
29 : #include <AP_BoardConfig/AP_BoardConfig.h>
30 :
31 : extern const AP_HAL::HAL &hal;
32 :
33 : #define REG_COMPANY_ID 0x00
34 : #define REG_DEVICE_ID 0x01
35 : #define REG_ST1 0x10
36 : #define REG_HXL 0x11
37 : #define REG_HXH 0x12
38 : #define REG_HYL 0x13
39 : #define REG_HYH 0x14
40 : #define REG_HZL 0x15
41 : #define REG_HZH 0x16
42 : #define REG_TMPS 0x17
43 : #define REG_ST2 0x18
44 : #define REG_CNTL1 0x30
45 : #define REG_CNTL2 0x31
46 : #define REG_CNTL3 0x32
47 :
48 : #define REG_ICM_WHOAMI 0x00
49 : #define REG_ICM_PWR_MGMT_1 0x06
50 : #define REG_ICM_INT_PIN_CFG 0x0f
51 :
52 : #define ICM_WHOAMI_VAL 0xEA
53 :
54 : #define AK09915_Device_ID 0x10
55 : #define AK09916_Device_ID 0x09
56 : #define AK09918_Device_ID 0x0c
57 : #define AK09916_MILLIGAUSS_SCALE 10.0f
58 :
59 : extern const AP_HAL::HAL &hal;
60 :
61 0 : AP_Compass_AK09916::AP_Compass_AK09916(AP_AK09916_BusDriver *bus,
62 : bool force_external,
63 0 : enum Rotation rotation)
64 0 : : _bus(bus)
65 0 : , _force_external(force_external)
66 0 : , _rotation(rotation)
67 : {
68 0 : }
69 :
70 0 : AP_Compass_AK09916::~AP_Compass_AK09916()
71 : {
72 0 : delete _bus;
73 0 : }
74 :
75 0 : AP_Compass_Backend *AP_Compass_AK09916::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
76 : bool force_external,
77 : enum Rotation rotation)
78 : {
79 0 : if (!dev) {
80 0 : return nullptr;
81 : }
82 0 : AP_AK09916_BusDriver *bus = NEW_NOTHROW AP_AK09916_BusDriver_HALDevice(std::move(dev));
83 0 : if (!bus) {
84 0 : return nullptr;
85 : }
86 :
87 0 : AP_Compass_AK09916 *sensor = NEW_NOTHROW AP_Compass_AK09916(bus, force_external, rotation);
88 0 : if (!sensor || !sensor->init()) {
89 0 : delete sensor;
90 0 : return nullptr;
91 : }
92 :
93 0 : return sensor;
94 : }
95 :
96 : #if AP_COMPASS_ICM20948_ENABLED
97 0 : AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(AP_HAL::OwnPtr<AP_HAL::Device> dev,
98 : AP_HAL::OwnPtr<AP_HAL::Device> dev_icm,
99 : bool force_external,
100 : enum Rotation rotation)
101 : {
102 0 : if (!dev || !dev_icm) {
103 0 : return nullptr;
104 : }
105 :
106 0 : dev->get_semaphore()->take_blocking();
107 :
108 : /* Allow ICM20x48 to shortcut auxiliary bus and host bus */
109 : uint8_t rval;
110 : uint16_t whoami;
111 0 : uint8_t retries = 5;
112 0 : if (!dev_icm->read_registers(REG_ICM_WHOAMI, &rval, 1) ||
113 0 : rval != ICM_WHOAMI_VAL) {
114 : // not an ICM_WHOAMI
115 0 : goto fail;
116 : }
117 : do {
118 : // reset then bring sensor out of sleep mode
119 0 : if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x80)) {
120 0 : goto fail;
121 : }
122 0 : hal.scheduler->delay(10);
123 :
124 0 : if (!dev_icm->write_register(REG_ICM_PWR_MGMT_1, 0x00)) {
125 0 : goto fail;
126 : }
127 0 : hal.scheduler->delay(10);
128 :
129 : // see if ICM20948 is sleeping
130 0 : if (!dev_icm->read_registers(REG_ICM_PWR_MGMT_1, &rval, 1)) {
131 0 : goto fail;
132 : }
133 0 : if ((rval & 0x40) == 0) {
134 0 : break;
135 : }
136 0 : } while (retries--);
137 :
138 0 : if (rval & 0x40) {
139 : // it didn't come out of sleep
140 0 : goto fail;
141 : }
142 :
143 : // initially force i2c bypass off
144 0 : dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x00);
145 0 : hal.scheduler->delay(1);
146 :
147 : // now check if a AK09916 shows up on the bus. If it does then
148 : // we have both a real AK09916 and a ICM20948 with an embedded
149 : // AK09916. In that case we will fail the driver load and use
150 : // the main AK09916 driver
151 0 : if (dev->read_registers(REG_COMPANY_ID, (uint8_t *)&whoami, 2)) {
152 : // a device is replying on the AK09916 I2C address, don't
153 : // load the ICM20948
154 0 : DEV_PRINTF("ICM20948: AK09916 bus conflict\n");
155 0 : goto fail;
156 : }
157 :
158 : // now force bypass on
159 0 : dev_icm->write_register(REG_ICM_INT_PIN_CFG, 0x02);
160 0 : hal.scheduler->delay(1);
161 0 : dev->get_semaphore()->give();
162 0 : return probe(std::move(dev), force_external, rotation);
163 0 : fail:
164 0 : dev->get_semaphore()->give();
165 0 : return nullptr;
166 : }
167 :
168 : // un-named, assume SPI for compat
169 0 : AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948(uint8_t inv2_instance,
170 : enum Rotation rotation)
171 : {
172 0 : return probe_ICM20948_SPI(inv2_instance,rotation);
173 : }
174 :
175 0 : AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948_SPI(uint8_t inv2_instance,
176 : enum Rotation rotation)
177 : {
178 0 : AP_InertialSensor &ins = AP::ins();
179 :
180 : AP_AK09916_BusDriver *bus =
181 0 : NEW_NOTHROW AP_AK09916_BusDriver_Auxiliary(ins, HAL_INS_INV2_SPI, inv2_instance, HAL_COMPASS_AK09916_I2C_ADDR);
182 0 : if (!bus) {
183 0 : return nullptr;
184 : }
185 :
186 0 : AP_Compass_AK09916 *sensor = NEW_NOTHROW AP_Compass_AK09916(bus, false, rotation);
187 0 : if (!sensor || !sensor->init()) {
188 0 : delete sensor;
189 0 : return nullptr;
190 : }
191 :
192 0 : return sensor;
193 : }
194 :
195 0 : AP_Compass_Backend *AP_Compass_AK09916::probe_ICM20948_I2C(uint8_t inv2_instance,
196 : enum Rotation rotation)
197 : {
198 0 : AP_InertialSensor &ins = AP::ins();
199 :
200 : AP_AK09916_BusDriver *bus =
201 0 : NEW_NOTHROW AP_AK09916_BusDriver_Auxiliary(ins, HAL_INS_INV2_I2C, inv2_instance, HAL_COMPASS_AK09916_I2C_ADDR);
202 0 : if (!bus) {
203 0 : return nullptr;
204 : }
205 :
206 0 : AP_Compass_AK09916 *sensor = NEW_NOTHROW AP_Compass_AK09916(bus, false, rotation);
207 0 : if (!sensor || !sensor->init()) {
208 0 : delete sensor;
209 0 : return nullptr;
210 : }
211 :
212 0 : return sensor;
213 : }
214 : #endif // AP_COMPASS_ICM20948_ENABLED
215 :
216 0 : bool AP_Compass_AK09916::init()
217 : {
218 0 : AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
219 :
220 0 : if (!bus_sem) {
221 0 : return false;
222 : }
223 0 : _bus->get_semaphore()->take_blocking();
224 :
225 0 : if (!_bus->configure()) {
226 0 : DEV_PRINTF("AK09916: Could not configure the bus\n");
227 0 : goto fail;
228 : }
229 :
230 0 : if (!_reset()) {
231 0 : goto fail;
232 : }
233 :
234 0 : if (!_check_id()) {
235 0 : goto fail;
236 : }
237 :
238 : // one checked register for mode
239 0 : _bus->setup_checked_registers(1);
240 :
241 0 : if (!_setup_mode()) {
242 0 : DEV_PRINTF("AK09916: Could not setup mode\n");
243 0 : goto fail;
244 : }
245 :
246 0 : if (!_bus->start_measurements()) {
247 0 : DEV_PRINTF("AK09916: Could not start measurements\n");
248 0 : goto fail;
249 : }
250 :
251 0 : _initialized = true;
252 :
253 : /* register the compass instance in the frontend */
254 0 : _bus->set_device_type(_devtype);
255 0 : if (!register_compass(_bus->get_bus_id())) {
256 0 : goto fail;
257 : }
258 :
259 0 : if (_force_external) {
260 0 : set_external(true);
261 : }
262 :
263 0 : set_rotation(_rotation);
264 :
265 0 : bus_sem->give();
266 :
267 0 : _bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK09916::_update, void));
268 :
269 0 : return true;
270 :
271 0 : fail:
272 0 : bus_sem->give();
273 0 : return false;
274 : }
275 :
276 0 : void AP_Compass_AK09916::read()
277 : {
278 0 : if (!_initialized) {
279 0 : return;
280 : }
281 :
282 0 : drain_accumulated_samples();
283 : }
284 :
285 0 : void AP_Compass_AK09916::_make_adc_sensitivity_adjustment(Vector3f& field) const
286 : {
287 : static const float ADC_16BIT_RESOLUTION = 0.15f;
288 :
289 0 : field *= ADC_16BIT_RESOLUTION;
290 0 : }
291 :
292 0 : void AP_Compass_AK09916::_update()
293 : {
294 0 : struct sample_regs regs = {0};
295 0 : Vector3f raw_field;
296 :
297 0 : if (!_bus->block_read(REG_ST1, (uint8_t *) ®s, sizeof(regs))) {
298 0 : goto check_registers;
299 : }
300 :
301 0 : if (!(regs.st1 & 0x01)) {
302 0 : no_data++;
303 0 : if (no_data == 5) {
304 0 : _reset();
305 0 : _setup_mode();
306 0 : no_data = 0;
307 : }
308 0 : goto check_registers;
309 : }
310 0 : no_data = 0;
311 :
312 : /* Check for overflow. See AK09916's datasheet*/
313 0 : if ((regs.st2 & 0x08)) {
314 0 : goto check_registers;
315 : }
316 :
317 0 : raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
318 :
319 0 : if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
320 0 : goto check_registers;
321 : }
322 :
323 0 : _make_adc_sensitivity_adjustment(raw_field);
324 0 : raw_field *= AK09916_MILLIGAUSS_SCALE;
325 :
326 0 : accumulate_sample(raw_field, 10);
327 :
328 0 : check_registers:
329 0 : _bus->check_next_register();
330 0 : }
331 :
332 0 : bool AP_Compass_AK09916::_check_id()
333 : {
334 0 : for (int i = 0; i < 5; i++) {
335 0 : uint8_t deviceid = 0;
336 :
337 : /* Read AK09916's id */
338 0 : if (_bus->register_read(REG_DEVICE_ID, &deviceid)) {
339 0 : switch (deviceid) {
340 0 : case AK09915_Device_ID:
341 0 : _devtype = DEVTYPE_AK09915;
342 0 : return true;
343 0 : case AK09916_Device_ID:
344 0 : _devtype = DEVTYPE_AK09916;
345 0 : return true;
346 0 : case AK09918_Device_ID:
347 0 : _devtype = DEVTYPE_AK09918;
348 0 : return true;
349 : }
350 : }
351 : }
352 :
353 0 : return false;
354 : }
355 :
356 0 : bool AP_Compass_AK09916::_setup_mode() {
357 0 : return _bus->register_write(REG_CNTL2, 0x08, true); //Continuous Mode 2
358 : }
359 :
360 0 : bool AP_Compass_AK09916::_reset()
361 : {
362 0 : return _bus->register_write(REG_CNTL3, 0x01); //Soft Reset
363 : }
364 :
365 : /* AP_HAL::Device implementation of the AK09916 */
366 0 : AP_AK09916_BusDriver_HALDevice::AP_AK09916_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::Device> dev)
367 0 : : _dev(std::move(dev))
368 : {
369 0 : }
370 :
371 0 : bool AP_AK09916_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
372 : {
373 0 : return _dev->read_registers(reg, buf, size);
374 : }
375 :
376 0 : bool AP_AK09916_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
377 : {
378 0 : return _dev->read_registers(reg, val, 1);
379 : }
380 :
381 0 : bool AP_AK09916_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val, bool checked)
382 : {
383 0 : return _dev->write_register(reg, val, checked);
384 : }
385 :
386 0 : AP_HAL::Semaphore *AP_AK09916_BusDriver_HALDevice::get_semaphore()
387 : {
388 0 : return _dev->get_semaphore();
389 : }
390 :
391 0 : AP_HAL::Device::PeriodicHandle AP_AK09916_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
392 : {
393 0 : return _dev->register_periodic_callback(period_usec, cb);
394 : }
395 :
396 : /* AK09916 on an auxiliary bus of IMU driver */
397 0 : AP_AK09916_BusDriver_Auxiliary::AP_AK09916_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
398 0 : uint8_t backend_instance, uint8_t addr)
399 : {
400 : /*
401 : * Only initialize members. Fails are handled by configure or while
402 : * getting the semaphore
403 : */
404 : #if AP_INERTIALSENSOR_ENABLED
405 0 : _bus = ins.get_auxiliary_bus(backend_id, backend_instance);
406 0 : if (!_bus) {
407 0 : return;
408 : }
409 :
410 0 : _slave = _bus->request_next_slave(addr);
411 : #endif
412 : }
413 :
414 0 : AP_AK09916_BusDriver_Auxiliary::~AP_AK09916_BusDriver_Auxiliary()
415 : {
416 : /* After started it's owned by AuxiliaryBus */
417 0 : if (!_started) {
418 0 : delete _slave;
419 : }
420 0 : }
421 :
422 0 : bool AP_AK09916_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
423 : {
424 0 : if (_started) {
425 : /*
426 : * We can only read a block when reading the block of sample values -
427 : * calling with any other value is a mistake
428 : */
429 0 : if (reg != REG_ST1) {
430 0 : return false;
431 : }
432 :
433 0 : int n = _slave->read(buf);
434 0 : return n == static_cast<int>(size);
435 : }
436 :
437 0 : int r = _slave->passthrough_read(reg, buf, size);
438 :
439 0 : return r > 0 && static_cast<uint32_t>(r) == size;
440 : }
441 :
442 0 : bool AP_AK09916_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
443 : {
444 0 : return _slave->passthrough_read(reg, val, 1) == 1;
445 : }
446 :
447 0 : bool AP_AK09916_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val, bool checked)
448 : {
449 : (void)checked;
450 0 : return _slave->passthrough_write(reg, val) == 1;
451 : }
452 :
453 0 : AP_HAL::Semaphore *AP_AK09916_BusDriver_Auxiliary::get_semaphore()
454 : {
455 0 : return _bus ? _bus->get_semaphore() : nullptr;
456 : }
457 :
458 0 : bool AP_AK09916_BusDriver_Auxiliary::configure()
459 : {
460 0 : if (!_bus || !_slave) {
461 0 : return false;
462 : }
463 0 : return true;
464 : }
465 :
466 0 : bool AP_AK09916_BusDriver_Auxiliary::start_measurements()
467 : {
468 0 : if (_bus->register_periodic_read(_slave, REG_ST1, sizeof(AP_Compass_AK09916::sample_regs)) < 0) {
469 0 : return false;
470 : }
471 :
472 0 : _started = true;
473 :
474 0 : return true;
475 : }
476 :
477 0 : AP_HAL::Device::PeriodicHandle AP_AK09916_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
478 : {
479 0 : return _bus->register_periodic_callback(period_usec, cb);
480 : }
481 :
482 : // set device type within a device class
483 0 : void AP_AK09916_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
484 : {
485 0 : _bus->set_device_type(devtype);
486 0 : }
487 :
488 : // return 24 bit bus identifier
489 0 : uint32_t AP_AK09916_BusDriver_Auxiliary::get_bus_id(void) const
490 : {
491 0 : return _bus->get_bus_id();
492 : }
493 :
494 : #endif // AP_COMPASS_AK09916_ENABLED
|
