Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 :
16 : /*
17 : * AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
18 : * Code by Jordi Muñoz and Jose Julio. DIYDrones.com
19 : *
20 : * Sensor is connected to I2C port
21 : * Sensor is initialized in Continuos mode (10Hz)
22 : *
23 : */
24 : #include "AP_Compass_HMC5843.h"
25 :
26 : #if AP_COMPASS_HMC5843_ENABLED
27 :
28 : #include <assert.h>
29 : #include <utility>
30 : #include <stdio.h>
31 :
32 : #include <AP_Math/AP_Math.h>
33 : #include <AP_HAL/utility/sparse-endian.h>
34 : #include <AP_HAL/AP_HAL.h>
35 : #include <AP_InertialSensor/AP_InertialSensor.h>
36 : #include <AP_InertialSensor/AuxiliaryBus.h>
37 :
38 : extern const AP_HAL::HAL& hal;
39 :
40 : /*
41 : * Default address: 0x1E
42 : */
43 :
44 : #define HMC5843_REG_CONFIG_A 0x00
45 : // Valid sample averaging for 5883L
46 : #define HMC5843_SAMPLE_AVERAGING_1 (0x00 << 5)
47 : #define HMC5843_SAMPLE_AVERAGING_2 (0x01 << 5)
48 : #define HMC5843_SAMPLE_AVERAGING_4 (0x02 << 5)
49 : #define HMC5843_SAMPLE_AVERAGING_8 (0x03 << 5)
50 :
51 : #define HMC5843_CONF_TEMP_ENABLE (0x80)
52 :
53 : // Valid data output rates for 5883L
54 : #define HMC5843_OSR_0_75HZ (0x00 << 2)
55 : #define HMC5843_OSR_1_5HZ (0x01 << 2)
56 : #define HMC5843_OSR_3HZ (0x02 << 2)
57 : #define HMC5843_OSR_7_5HZ (0x03 << 2)
58 : #define HMC5843_OSR_15HZ (0x04 << 2)
59 : #define HMC5843_OSR_30HZ (0x05 << 2)
60 : #define HMC5843_OSR_75HZ (0x06 << 2)
61 : // Sensor operation modes
62 : #define HMC5843_OPMODE_NORMAL 0x00
63 : #define HMC5843_OPMODE_POSITIVE_BIAS 0x01
64 : #define HMC5843_OPMODE_NEGATIVE_BIAS 0x02
65 : #define HMC5843_OPMODE_MASK 0x03
66 :
67 : #define HMC5843_REG_CONFIG_B 0x01
68 : #define HMC5883L_GAIN_0_88_GA (0x00 << 5)
69 : #define HMC5883L_GAIN_1_30_GA (0x01 << 5)
70 : #define HMC5883L_GAIN_1_90_GA (0x02 << 5)
71 : #define HMC5883L_GAIN_2_50_GA (0x03 << 5)
72 : #define HMC5883L_GAIN_4_00_GA (0x04 << 5)
73 : #define HMC5883L_GAIN_4_70_GA (0x05 << 5)
74 : #define HMC5883L_GAIN_5_60_GA (0x06 << 5)
75 : #define HMC5883L_GAIN_8_10_GA (0x07 << 5)
76 :
77 : #define HMC5843_GAIN_0_70_GA (0x00 << 5)
78 : #define HMC5843_GAIN_1_00_GA (0x01 << 5)
79 : #define HMC5843_GAIN_1_50_GA (0x02 << 5)
80 : #define HMC5843_GAIN_2_00_GA (0x03 << 5)
81 : #define HMC5843_GAIN_3_20_GA (0x04 << 5)
82 : #define HMC5843_GAIN_3_80_GA (0x05 << 5)
83 : #define HMC5843_GAIN_4_50_GA (0x06 << 5)
84 : #define HMC5843_GAIN_6_50_GA (0x07 << 5)
85 :
86 : #define HMC5843_REG_MODE 0x02
87 : #define HMC5843_MODE_CONTINUOUS 0x00
88 : #define HMC5843_MODE_SINGLE 0x01
89 :
90 : #define HMC5843_REG_DATA_OUTPUT_X_MSB 0x03
91 :
92 : #define HMC5843_REG_ID_A 0x0A
93 :
94 :
95 0 : AP_Compass_HMC5843::AP_Compass_HMC5843(AP_HMC5843_BusDriver *bus,
96 0 : bool force_external, enum Rotation rotation)
97 0 : : _bus(bus)
98 0 : , _rotation(rotation)
99 0 : , _force_external(force_external)
100 : {
101 0 : }
102 :
103 0 : AP_Compass_HMC5843::~AP_Compass_HMC5843()
104 : {
105 0 : delete _bus;
106 0 : }
107 :
108 0 : AP_Compass_Backend *AP_Compass_HMC5843::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
109 : bool force_external,
110 : enum Rotation rotation)
111 : {
112 0 : if (!dev) {
113 0 : return nullptr;
114 : }
115 0 : AP_HMC5843_BusDriver *bus = NEW_NOTHROW AP_HMC5843_BusDriver_HALDevice(std::move(dev));
116 0 : if (!bus) {
117 0 : return nullptr;
118 : }
119 :
120 0 : AP_Compass_HMC5843 *sensor = NEW_NOTHROW AP_Compass_HMC5843(bus, force_external, rotation);
121 0 : if (!sensor || !sensor->init()) {
122 0 : delete sensor;
123 0 : return nullptr;
124 : }
125 :
126 0 : return sensor;
127 : }
128 :
129 : #if AP_INERTIALSENSOR_ENABLED
130 0 : AP_Compass_Backend *AP_Compass_HMC5843::probe_mpu6000(enum Rotation rotation)
131 : {
132 0 : AP_InertialSensor &ins = *AP_InertialSensor::get_singleton();
133 :
134 : AP_HMC5843_BusDriver *bus =
135 : NEW_NOTHROW AP_HMC5843_BusDriver_Auxiliary(ins, HAL_INS_MPU60XX_SPI,
136 0 : HAL_COMPASS_HMC5843_I2C_ADDR);
137 0 : if (!bus) {
138 0 : return nullptr;
139 : }
140 :
141 0 : AP_Compass_HMC5843 *sensor = NEW_NOTHROW AP_Compass_HMC5843(bus, false, rotation);
142 0 : if (!sensor || !sensor->init()) {
143 0 : delete sensor;
144 0 : return nullptr;
145 : }
146 :
147 0 : return sensor;
148 : }
149 : #endif
150 :
151 0 : bool AP_Compass_HMC5843::init()
152 : {
153 0 : AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
154 :
155 0 : if (!bus_sem) {
156 0 : DEV_PRINTF("HMC5843: Unable to get bus semaphore\n");
157 0 : return false;
158 : }
159 0 : bus_sem->take_blocking();
160 :
161 : // high retries for init
162 0 : _bus->set_retries(10);
163 :
164 0 : if (!_bus->configure()) {
165 0 : DEV_PRINTF("HMC5843: Could not configure the bus\n");
166 0 : goto errout;
167 : }
168 :
169 0 : if (!_check_whoami()) {
170 0 : goto errout;
171 : }
172 :
173 0 : if (!_calibrate()) {
174 0 : DEV_PRINTF("HMC5843: Could not calibrate sensor\n");
175 0 : goto errout;
176 : }
177 :
178 0 : if (!_setup_sampling_mode()) {
179 0 : goto errout;
180 : }
181 :
182 0 : if (!_bus->start_measurements()) {
183 0 : DEV_PRINTF("HMC5843: Could not start measurements on bus\n");
184 0 : goto errout;
185 : }
186 :
187 0 : _initialised = true;
188 :
189 : // lower retries for run
190 0 : _bus->set_retries(3);
191 :
192 0 : bus_sem->give();
193 :
194 : // perform an initial read
195 0 : read();
196 :
197 : //register compass instance
198 0 : _bus->set_device_type(DEVTYPE_HMC5883);
199 0 : if (!register_compass(_bus->get_bus_id())) {
200 0 : return false;
201 : }
202 :
203 0 : set_rotation(_rotation);
204 :
205 0 : if (_force_external) {
206 0 : set_external(true);
207 : }
208 :
209 : // read from sensor at 75Hz
210 0 : _bus->register_periodic_callback(13333,
211 : FUNCTOR_BIND_MEMBER(&AP_Compass_HMC5843::_timer, void));
212 :
213 0 : DEV_PRINTF("HMC5843 found on bus 0x%x\n", (unsigned)_bus->get_bus_id());
214 :
215 0 : return true;
216 :
217 0 : errout:
218 0 : bus_sem->give();
219 0 : return false;
220 : }
221 :
222 : /*
223 : * take a reading from the magnetometer
224 : *
225 : * bus semaphore has been taken already by HAL
226 : */
227 0 : void AP_Compass_HMC5843::_timer()
228 : {
229 0 : bool result = _read_sample();
230 :
231 : // always ask for a new sample
232 0 : _take_sample();
233 :
234 0 : if (!result) {
235 0 : return;
236 : }
237 :
238 : // get raw_field - sensor frame, uncorrected
239 0 : Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z);
240 0 : raw_field *= _gain_scale;
241 :
242 : // rotate to the desired orientation
243 0 : if (is_external()) {
244 0 : raw_field.rotate(ROTATION_YAW_90);
245 : }
246 :
247 : // We expect to do reads at 10Hz, and we get new data at most 75Hz, so we
248 : // don't expect to accumulate more than 8 before a read; let's make it
249 : // 14 to give more room for the initialization phase
250 0 : accumulate_sample(raw_field, 14);
251 : }
252 :
253 : /*
254 : * Take accumulated reads from the magnetometer or try to read once if no
255 : * valid data
256 : *
257 : * bus semaphore must not be locked
258 : */
259 0 : void AP_Compass_HMC5843::read()
260 : {
261 0 : if (!_initialised) {
262 : // someone has tried to enable a compass for the first time
263 : // mid-flight .... we can't do that yet (especially as we won't
264 : // have the right orientation!)
265 0 : return;
266 : }
267 :
268 0 : drain_accumulated_samples(&_scaling);
269 : }
270 :
271 0 : bool AP_Compass_HMC5843::_setup_sampling_mode()
272 : {
273 0 : _gain_scale = (1.0f / 1090) * 1000;
274 0 : if (!_bus->register_write(HMC5843_REG_CONFIG_A,
275 : HMC5843_CONF_TEMP_ENABLE |
276 : HMC5843_OSR_75HZ |
277 0 : HMC5843_SAMPLE_AVERAGING_1) ||
278 0 : !_bus->register_write(HMC5843_REG_CONFIG_B,
279 0 : HMC5883L_GAIN_1_30_GA) ||
280 0 : !_bus->register_write(HMC5843_REG_MODE,
281 : HMC5843_MODE_SINGLE)) {
282 0 : return false;
283 : }
284 0 : return true;
285 : }
286 :
287 : /*
288 : * Read Sensor data - bus semaphore must be taken
289 : */
290 0 : bool AP_Compass_HMC5843::_read_sample()
291 : {
292 : struct PACKED {
293 : be16_t rx;
294 : be16_t ry;
295 : be16_t rz;
296 : } val;
297 : int16_t rx, ry, rz;
298 :
299 0 : if (!_bus->block_read(HMC5843_REG_DATA_OUTPUT_X_MSB, (uint8_t *) &val, sizeof(val))){
300 0 : return false;
301 : }
302 :
303 0 : rx = be16toh(val.rx);
304 0 : ry = be16toh(val.rz);
305 0 : rz = be16toh(val.ry);
306 :
307 0 : if (rx == -4096 || ry == -4096 || rz == -4096) {
308 : // no valid data available
309 0 : return false;
310 : }
311 :
312 0 : _mag_x = -rx;
313 0 : _mag_y = ry;
314 0 : _mag_z = -rz;
315 :
316 0 : return true;
317 : }
318 :
319 :
320 : /*
321 : ask for a new oneshot sample
322 : */
323 0 : void AP_Compass_HMC5843::_take_sample()
324 : {
325 0 : _bus->register_write(HMC5843_REG_MODE,
326 : HMC5843_MODE_SINGLE);
327 0 : }
328 :
329 0 : bool AP_Compass_HMC5843::_check_whoami()
330 : {
331 : uint8_t id[3];
332 0 : if (!_bus->block_read(HMC5843_REG_ID_A, id, 3)) {
333 : // can't talk on bus
334 0 : return false;
335 : }
336 0 : if (memcmp(id, "H43", 3) != 0) {
337 : // not a HMC5x83 device
338 0 : return false;
339 : }
340 :
341 0 : return true;
342 : }
343 :
344 0 : bool AP_Compass_HMC5843::_calibrate()
345 : {
346 : uint8_t calibration_gain;
347 0 : int numAttempts = 0, good_count = 0;
348 0 : bool success = false;
349 :
350 0 : calibration_gain = HMC5883L_GAIN_2_50_GA;
351 :
352 : /*
353 : * the expected values are based on observation of real sensors
354 : */
355 0 : float expected[3] = { 1.16*600, 1.08*600, 1.16*600 };
356 :
357 0 : uint8_t base_config = HMC5843_OSR_15HZ;
358 0 : uint8_t num_samples = 0;
359 :
360 0 : while (success == 0 && numAttempts < 25 && good_count < 5) {
361 0 : numAttempts++;
362 :
363 : // force positiveBias (compass should return 715 for all channels)
364 0 : if (!_bus->register_write(HMC5843_REG_CONFIG_A,
365 0 : base_config | HMC5843_OPMODE_POSITIVE_BIAS)) {
366 : // compass not responding on the bus
367 0 : continue;
368 : }
369 :
370 0 : hal.scheduler->delay(50);
371 :
372 : // set gains
373 0 : if (!_bus->register_write(HMC5843_REG_CONFIG_B, calibration_gain) ||
374 0 : !_bus->register_write(HMC5843_REG_MODE, HMC5843_MODE_SINGLE)) {
375 0 : continue;
376 : }
377 :
378 : // read values from the compass
379 0 : hal.scheduler->delay(50);
380 0 : if (!_read_sample()) {
381 : // we didn't read valid values
382 0 : continue;
383 : }
384 :
385 0 : num_samples++;
386 :
387 : float cal[3];
388 :
389 : // hal.console->printf("mag %d %d %d\n", _mag_x, _mag_y, _mag_z);
390 :
391 0 : cal[0] = fabsf(expected[0] / _mag_x);
392 0 : cal[1] = fabsf(expected[1] / _mag_y);
393 0 : cal[2] = fabsf(expected[2] / _mag_z);
394 :
395 : // hal.console->printf("cal=%.2f %.2f %.2f\n", cal[0], cal[1], cal[2]);
396 :
397 : // we throw away the first two samples as the compass may
398 : // still be changing its state from the application of the
399 : // strap excitation. After that we accept values in a
400 : // reasonable range
401 0 : if (numAttempts <= 2) {
402 0 : continue;
403 : }
404 :
405 : #define IS_CALIBRATION_VALUE_VALID(val) (val > 0.7f && val < 1.35f)
406 :
407 0 : if (IS_CALIBRATION_VALUE_VALID(cal[0]) &&
408 0 : IS_CALIBRATION_VALUE_VALID(cal[1]) &&
409 0 : IS_CALIBRATION_VALUE_VALID(cal[2])) {
410 : // hal.console->printf("car=%.2f %.2f %.2f good\n", cal[0], cal[1], cal[2]);
411 0 : good_count++;
412 :
413 0 : _scaling[0] += cal[0];
414 0 : _scaling[1] += cal[1];
415 0 : _scaling[2] += cal[2];
416 : }
417 :
418 : #undef IS_CALIBRATION_VALUE_VALID
419 :
420 : #if 0
421 : /* useful for debugging */
422 : hal.console->printf("MagX: %d MagY: %d MagZ: %d\n", (int)_mag_x, (int)_mag_y, (int)_mag_z);
423 : hal.console->printf("CalX: %.2f CalY: %.2f CalZ: %.2f\n", cal[0], cal[1], cal[2]);
424 : #endif
425 : }
426 :
427 0 : _bus->register_write(HMC5843_REG_CONFIG_A, base_config);
428 :
429 0 : if (good_count >= 5) {
430 0 : _scaling[0] = _scaling[0] / good_count;
431 0 : _scaling[1] = _scaling[1] / good_count;
432 0 : _scaling[2] = _scaling[2] / good_count;
433 0 : success = true;
434 : } else {
435 : /* best guess */
436 0 : _scaling[0] = 1.0;
437 0 : _scaling[1] = 1.0;
438 0 : _scaling[2] = 1.0;
439 0 : if (num_samples > 5) {
440 : // a sensor can be broken for calibration but still
441 : // otherwise workable, accept it if we are reading samples
442 0 : success = true;
443 : }
444 : }
445 :
446 : #if 0
447 : printf("scaling: %.2f %.2f %.2f\n",
448 : _scaling[0], _scaling[1], _scaling[2]);
449 : #endif
450 :
451 0 : return success;
452 : }
453 :
454 : /* AP_HAL::Device implementation of the HMC5843 */
455 0 : AP_HMC5843_BusDriver_HALDevice::AP_HMC5843_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::Device> dev)
456 0 : : _dev(std::move(dev))
457 : {
458 : // set read and auto-increment flags on SPI
459 0 : if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
460 0 : _dev->set_read_flag(0xC0);
461 : }
462 0 : }
463 :
464 0 : bool AP_HMC5843_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
465 : {
466 0 : return _dev->read_registers(reg, buf, size);
467 : }
468 :
469 0 : bool AP_HMC5843_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
470 : {
471 0 : return _dev->read_registers(reg, val, 1);
472 : }
473 :
474 0 : bool AP_HMC5843_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
475 : {
476 0 : return _dev->write_register(reg, val);
477 : }
478 :
479 0 : AP_HAL::Semaphore *AP_HMC5843_BusDriver_HALDevice::get_semaphore()
480 : {
481 0 : return _dev->get_semaphore();
482 : }
483 :
484 0 : AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
485 : {
486 0 : return _dev->register_periodic_callback(period_usec, cb);
487 : }
488 :
489 :
490 : #if AP_INERTIALSENSOR_ENABLED
491 : /* HMC5843 on an auxiliary bus of IMU driver */
492 0 : AP_HMC5843_BusDriver_Auxiliary::AP_HMC5843_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
493 0 : uint8_t addr)
494 : {
495 : /*
496 : * Only initialize members. Fails are handled by configure or while
497 : * getting the semaphore
498 : */
499 0 : _bus = ins.get_auxiliary_bus(backend_id);
500 0 : if (!_bus) {
501 0 : return;
502 : }
503 :
504 0 : _slave = _bus->request_next_slave(addr);
505 : }
506 :
507 0 : AP_HMC5843_BusDriver_Auxiliary::~AP_HMC5843_BusDriver_Auxiliary()
508 : {
509 : /* After started it's owned by AuxiliaryBus */
510 0 : if (!_started) {
511 0 : delete _slave;
512 : }
513 0 : }
514 :
515 0 : bool AP_HMC5843_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
516 : {
517 0 : if (_started) {
518 : /*
519 : * We can only read a block when reading the block of sample values -
520 : * calling with any other value is a mistake
521 : */
522 0 : if (reg != HMC5843_REG_DATA_OUTPUT_X_MSB) {
523 0 : return false;
524 : }
525 :
526 0 : int n = _slave->read(buf);
527 0 : return n == static_cast<int>(size);
528 : }
529 :
530 0 : int r = _slave->passthrough_read(reg, buf, size);
531 :
532 0 : return r > 0 && static_cast<uint32_t>(r) == size;
533 : }
534 :
535 0 : bool AP_HMC5843_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
536 : {
537 0 : return _slave->passthrough_read(reg, val, 1) == 1;
538 : }
539 :
540 0 : bool AP_HMC5843_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
541 : {
542 0 : return _slave->passthrough_write(reg, val) == 1;
543 : }
544 :
545 0 : AP_HAL::Semaphore *AP_HMC5843_BusDriver_Auxiliary::get_semaphore()
546 : {
547 0 : return _bus->get_semaphore();
548 : }
549 :
550 :
551 0 : bool AP_HMC5843_BusDriver_Auxiliary::configure()
552 : {
553 0 : if (!_bus || !_slave) {
554 0 : return false;
555 : }
556 0 : return true;
557 : }
558 :
559 0 : bool AP_HMC5843_BusDriver_Auxiliary::start_measurements()
560 : {
561 0 : if (_bus->register_periodic_read(_slave, HMC5843_REG_DATA_OUTPUT_X_MSB, 6) < 0) {
562 0 : return false;
563 : }
564 :
565 0 : _started = true;
566 :
567 0 : return true;
568 : }
569 :
570 0 : AP_HAL::Device::PeriodicHandle AP_HMC5843_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
571 : {
572 0 : return _bus->register_periodic_callback(period_usec, cb);
573 : }
574 :
575 : // set device type within a device class
576 0 : void AP_HMC5843_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
577 : {
578 0 : _bus->set_device_type(devtype);
579 0 : }
580 :
581 : // return 24 bit bus identifier
582 0 : uint32_t AP_HMC5843_BusDriver_Auxiliary::get_bus_id(void) const
583 : {
584 0 : return _bus->get_bus_id();
585 : }
586 : #endif // AP_INERTIALSENSOR_ENABLED
587 :
588 : #endif // AP_COMPASS_HMC5843_ENABLED
|
