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 : thanks to Robert Dickenson and the PX4 team for register definitions
19 : */
20 : #include "AP_Compass_LIS3MDL.h"
21 :
22 : #if AP_COMPASS_LIS3MDL_ENABLED
23 :
24 : #include <AP_HAL/AP_HAL.h>
25 : #include <utility>
26 : #include <AP_Math/AP_Math.h>
27 : #include <stdio.h>
28 :
29 : #define ADDR_CTRL_REG1 0x20
30 : #define ADDR_CTRL_REG2 0x21
31 : #define ADDR_CTRL_REG3 0x22
32 : #define ADDR_CTRL_REG4 0x23
33 : #define ADDR_CTRL_REG5 0x24
34 :
35 : #define ADDR_STATUS_REG 0x27
36 : #define ADDR_OUT_X_L 0x28
37 : #define ADDR_OUT_X_H 0x29
38 : #define ADDR_OUT_Y_L 0x2a
39 : #define ADDR_OUT_Y_H 0x2b
40 : #define ADDR_OUT_Z_L 0x2c
41 : #define ADDR_OUT_Z_H 0x2d
42 : #define ADDR_OUT_T_L 0x2e
43 : #define ADDR_OUT_T_H 0x2f
44 :
45 : #define MODE_REG_CONTINOUS_MODE (0 << 0)
46 : #define MODE_REG_SINGLE_MODE (1 << 0)
47 :
48 : #define ADDR_WHO_AM_I 0x0f
49 : #define ID_WHO_AM_I 0x3d
50 :
51 0 : AP_Compass_Backend *AP_Compass_LIS3MDL::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
52 : bool force_external,
53 : enum Rotation rotation)
54 : {
55 0 : if (!dev) {
56 0 : return nullptr;
57 : }
58 0 : AP_Compass_LIS3MDL *sensor = NEW_NOTHROW AP_Compass_LIS3MDL(std::move(dev), force_external, rotation);
59 0 : if (!sensor || !sensor->init()) {
60 0 : delete sensor;
61 0 : return nullptr;
62 : }
63 :
64 0 : return sensor;
65 : }
66 :
67 0 : AP_Compass_LIS3MDL::AP_Compass_LIS3MDL(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
68 : bool _force_external,
69 0 : enum Rotation _rotation)
70 0 : : dev(std::move(_dev))
71 0 : , force_external(_force_external)
72 0 : , rotation(_rotation)
73 : {
74 0 : }
75 :
76 0 : bool AP_Compass_LIS3MDL::init()
77 : {
78 0 : dev->get_semaphore()->take_blocking();
79 :
80 0 : if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
81 0 : dev->set_read_flag(0xC0);
82 : }
83 :
84 : // high retries for init
85 0 : dev->set_retries(10);
86 :
87 : uint8_t whoami;
88 0 : if (!dev->read_registers(ADDR_WHO_AM_I, &whoami, 1) ||
89 0 : whoami != ID_WHO_AM_I) {
90 : // not a 3MDL
91 0 : goto fail;
92 : }
93 :
94 0 : dev->setup_checked_registers(5);
95 :
96 0 : dev->write_register(ADDR_CTRL_REG1, 0xFC, true); // 80Hz, UHP
97 0 : dev->write_register(ADDR_CTRL_REG2, 0, true); // 4Ga range
98 0 : dev->write_register(ADDR_CTRL_REG3, 0, true); // continuous
99 0 : dev->write_register(ADDR_CTRL_REG4, 0x0C, true); // z-axis ultra high perf
100 0 : dev->write_register(ADDR_CTRL_REG5, 0x40, true); // block-data-update
101 :
102 : // lower retries for run
103 0 : dev->set_retries(3);
104 :
105 0 : dev->get_semaphore()->give();
106 :
107 : /* register the compass instance in the frontend */
108 0 : dev->set_device_type(DEVTYPE_LIS3MDL);
109 0 : if (!register_compass(dev->get_bus_id())) {
110 0 : return false;
111 : }
112 :
113 0 : printf("Found a LIS3MDL on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), instance);
114 :
115 0 : set_rotation(rotation);
116 :
117 0 : if (force_external) {
118 0 : set_external(true);
119 : }
120 :
121 : // call timer() at 80Hz
122 0 : dev->register_periodic_callback(1000000U/80U,
123 : FUNCTOR_BIND_MEMBER(&AP_Compass_LIS3MDL::timer, void));
124 :
125 0 : return true;
126 :
127 0 : fail:
128 0 : dev->get_semaphore()->give();
129 0 : return false;
130 : }
131 :
132 0 : void AP_Compass_LIS3MDL::timer()
133 : {
134 : struct PACKED {
135 : int16_t magx;
136 : int16_t magy;
137 : int16_t magz;
138 : } data;
139 0 : const float range_scale = 1000.0f / 6842.0f;
140 :
141 : // check data ready
142 : uint8_t status;
143 0 : if (!dev->read_registers(ADDR_STATUS_REG, (uint8_t *)&status, 1)) {
144 0 : goto check_registers;
145 : }
146 0 : if (!(status & 0x08)) {
147 : // data not available yet
148 0 : goto check_registers;
149 : }
150 :
151 0 : if (!dev->read_registers(ADDR_OUT_X_L, (uint8_t *)&data, sizeof(data))) {
152 0 : goto check_registers;
153 : }
154 :
155 : {
156 : Vector3f field{
157 0 : data.magx * range_scale,
158 0 : data.magy * range_scale,
159 0 : data.magz * range_scale,
160 0 : };
161 :
162 0 : accumulate_sample(field);
163 : }
164 :
165 0 : check_registers:
166 0 : dev->check_next_register();
167 0 : }
168 :
169 : #endif // AP_COMPASS_LIS3MDL_ENABLED
|
