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 : #include "AP_Compass_AK8963.h"
17 :
18 : #if AP_COMPASS_AK8963_ENABLED
19 :
20 : #include <assert.h>
21 : #include <utility>
22 :
23 : #include <AP_Math/AP_Math.h>
24 : #include <AP_HAL/AP_HAL.h>
25 :
26 : #include <AP_InertialSensor/AP_InertialSensor_Invensense.h>
27 :
28 : #define AK8963_I2C_ADDR 0x0c
29 :
30 : #define AK8963_WIA 0x00
31 : # define AK8963_Device_ID 0x48
32 :
33 : #define AK8963_HXL 0x03
34 :
35 : /* bit definitions for AK8963 CNTL1 */
36 : #define AK8963_CNTL1 0x0A
37 : # define AK8963_CONTINUOUS_MODE1 0x02
38 : # define AK8963_CONTINUOUS_MODE2 0x06
39 : # define AK8963_SELFTEST_MODE 0x08
40 : # define AK8963_POWERDOWN_MODE 0x00
41 : # define AK8963_FUSE_MODE 0x0f
42 : # define AK8963_16BIT_ADC 0x10
43 : # define AK8963_14BIT_ADC 0x00
44 :
45 : #define AK8963_CNTL2 0x0B
46 : # define AK8963_RESET 0x01
47 :
48 : #define AK8963_ASAX 0x10
49 :
50 : #define AK8963_MILLIGAUSS_SCALE 10.0f
51 :
52 : extern const AP_HAL::HAL &hal;
53 :
54 0 : AP_Compass_AK8963::AP_Compass_AK8963(AP_AK8963_BusDriver *bus,
55 0 : enum Rotation rotation)
56 0 : : _bus(bus)
57 0 : , _rotation(rotation)
58 : {
59 0 : }
60 :
61 0 : AP_Compass_AK8963::~AP_Compass_AK8963()
62 : {
63 0 : delete _bus;
64 0 : }
65 :
66 0 : AP_Compass_Backend *AP_Compass_AK8963::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
67 : enum Rotation rotation)
68 : {
69 0 : if (!dev) {
70 0 : return nullptr;
71 : }
72 0 : AP_AK8963_BusDriver *bus = NEW_NOTHROW AP_AK8963_BusDriver_HALDevice(std::move(dev));
73 0 : if (!bus) {
74 0 : return nullptr;
75 : }
76 :
77 0 : AP_Compass_AK8963 *sensor = NEW_NOTHROW AP_Compass_AK8963(bus, rotation);
78 0 : if (!sensor || !sensor->init()) {
79 0 : delete sensor;
80 0 : return nullptr;
81 : }
82 :
83 0 : return sensor;
84 : }
85 :
86 0 : AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(AP_HAL::OwnPtr<AP_HAL::Device> dev,
87 : enum Rotation rotation)
88 : {
89 0 : if (!dev) {
90 0 : return nullptr;
91 : }
92 : #if AP_INERTIALSENSOR_ENABLED
93 0 : AP_InertialSensor &ins = *AP_InertialSensor::get_singleton();
94 :
95 : /* Allow MPU9250 to shortcut auxiliary bus and host bus */
96 0 : ins.detect_backends();
97 : #endif
98 :
99 0 : return probe(std::move(dev), rotation);
100 : }
101 :
102 0 : AP_Compass_Backend *AP_Compass_AK8963::probe_mpu9250(uint8_t mpu9250_instance,
103 : enum Rotation rotation)
104 : {
105 : #if AP_INERTIALSENSOR_ENABLED
106 0 : AP_InertialSensor &ins = *AP_InertialSensor::get_singleton();
107 :
108 : AP_AK8963_BusDriver *bus =
109 0 : NEW_NOTHROW AP_AK8963_BusDriver_Auxiliary(ins, HAL_INS_MPU9250_SPI, mpu9250_instance, AK8963_I2C_ADDR);
110 0 : if (!bus) {
111 0 : return nullptr;
112 : }
113 :
114 0 : AP_Compass_AK8963 *sensor = NEW_NOTHROW AP_Compass_AK8963(bus, rotation);
115 0 : if (!sensor || !sensor->init()) {
116 0 : delete sensor;
117 0 : return nullptr;
118 : }
119 :
120 0 : return sensor;
121 : #else
122 : return nullptr;
123 : #endif
124 :
125 : }
126 :
127 0 : bool AP_Compass_AK8963::init()
128 : {
129 0 : AP_HAL::Semaphore *bus_sem = _bus->get_semaphore();
130 :
131 0 : if (!bus_sem) {
132 0 : return false;
133 : }
134 0 : _bus->get_semaphore()->take_blocking();
135 :
136 0 : if (!_bus->configure()) {
137 0 : DEV_PRINTF("AK8963: Could not configure the bus\n");
138 0 : goto fail;
139 : }
140 :
141 0 : if (!_check_id()) {
142 0 : DEV_PRINTF("AK8963: Wrong id\n");
143 0 : goto fail;
144 : }
145 :
146 0 : if (!_calibrate()) {
147 0 : DEV_PRINTF("AK8963: Could not read calibration data\n");
148 0 : goto fail;
149 : }
150 :
151 0 : if (!_setup_mode()) {
152 0 : DEV_PRINTF("AK8963: Could not setup mode\n");
153 0 : goto fail;
154 : }
155 :
156 0 : if (!_bus->start_measurements()) {
157 0 : DEV_PRINTF("AK8963: Could not start measurements\n");
158 0 : goto fail;
159 : }
160 :
161 0 : _initialized = true;
162 :
163 : /* register the compass instance in the frontend */
164 0 : _bus->set_device_type(DEVTYPE_AK8963);
165 0 : if (!register_compass(_bus->get_bus_id())) {
166 0 : goto fail;
167 : }
168 :
169 0 : set_rotation(_rotation);
170 0 : bus_sem->give();
171 :
172 0 : _bus->register_periodic_callback(10000, FUNCTOR_BIND_MEMBER(&AP_Compass_AK8963::_update, void));
173 :
174 0 : return true;
175 :
176 0 : fail:
177 0 : bus_sem->give();
178 0 : return false;
179 : }
180 :
181 0 : void AP_Compass_AK8963::read()
182 : {
183 0 : if (!_initialized) {
184 0 : return;
185 : }
186 :
187 0 : drain_accumulated_samples();
188 : }
189 :
190 0 : void AP_Compass_AK8963::_make_adc_sensitivity_adjustment(Vector3f& field) const
191 : {
192 : static const float ADC_16BIT_RESOLUTION = 0.15f;
193 :
194 0 : field *= ADC_16BIT_RESOLUTION;
195 0 : }
196 :
197 0 : void AP_Compass_AK8963::_make_factory_sensitivity_adjustment(Vector3f& field) const
198 : {
199 0 : field.x *= _magnetometer_ASA[0];
200 0 : field.y *= _magnetometer_ASA[1];
201 0 : field.z *= _magnetometer_ASA[2];
202 0 : }
203 :
204 0 : void AP_Compass_AK8963::_update()
205 : {
206 : struct sample_regs regs;
207 0 : Vector3f raw_field;
208 :
209 0 : if (!_bus->block_read(AK8963_HXL, (uint8_t *) ®s, sizeof(regs))) {
210 0 : return;
211 : }
212 :
213 : /* Check for overflow. See AK8963's datasheet, section
214 : * 6.4.3.6 - Magnetic Sensor Overflow. */
215 0 : if ((regs.st2 & 0x08)) {
216 0 : return;
217 : }
218 :
219 0 : raw_field = Vector3f(regs.val[0], regs.val[1], regs.val[2]);
220 :
221 0 : if (is_zero(raw_field.x) && is_zero(raw_field.y) && is_zero(raw_field.z)) {
222 0 : return;
223 : }
224 :
225 0 : _make_factory_sensitivity_adjustment(raw_field);
226 0 : _make_adc_sensitivity_adjustment(raw_field);
227 0 : raw_field *= AK8963_MILLIGAUSS_SCALE;
228 :
229 0 : accumulate_sample(raw_field, 10);
230 : }
231 :
232 0 : bool AP_Compass_AK8963::_check_id()
233 : {
234 0 : for (int i = 0; i < 5; i++) {
235 0 : uint8_t deviceid = 0;
236 :
237 : /* Read AK8963's id */
238 0 : if (_bus->register_read(AK8963_WIA, &deviceid) &&
239 0 : deviceid == AK8963_Device_ID) {
240 0 : return true;
241 : }
242 : }
243 :
244 0 : return false;
245 : }
246 :
247 0 : bool AP_Compass_AK8963::_setup_mode() {
248 0 : return _bus->register_write(AK8963_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC);
249 : }
250 :
251 0 : bool AP_Compass_AK8963::_reset()
252 : {
253 0 : return _bus->register_write(AK8963_CNTL2, AK8963_RESET);
254 : }
255 :
256 :
257 0 : bool AP_Compass_AK8963::_calibrate()
258 : {
259 : /* Enable FUSE-mode in order to be able to read calibration data */
260 0 : if (!_bus->register_write(AK8963_CNTL1, AK8963_FUSE_MODE | AK8963_16BIT_ADC)) {
261 : // without fuse-ROM access mode the sensitivity adjustment
262 : // registers do not hold the calibration data
263 0 : return false;
264 : }
265 :
266 : uint8_t response[3];
267 :
268 0 : if (!_bus->block_read(AK8963_ASAX, response, 3)) {
269 : // the sensitivity adjustment values were not read; response is
270 : // undefined, so do not calibrate from it
271 0 : return false;
272 : }
273 :
274 0 : for (int i = 0; i < 3; i++) {
275 0 : float data = response[i];
276 0 : _magnetometer_ASA[i] = ((data - 128) / 256 + 1);
277 : }
278 :
279 0 : return true;
280 : }
281 :
282 : /* AP_HAL::Device implementation of the AK8963 */
283 0 : AP_AK8963_BusDriver_HALDevice::AP_AK8963_BusDriver_HALDevice(AP_HAL::OwnPtr<AP_HAL::Device> dev)
284 0 : : _dev(std::move(dev))
285 : {
286 0 : }
287 :
288 0 : bool AP_AK8963_BusDriver_HALDevice::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
289 : {
290 0 : return _dev->read_registers(reg, buf, size);
291 : }
292 :
293 0 : bool AP_AK8963_BusDriver_HALDevice::register_read(uint8_t reg, uint8_t *val)
294 : {
295 0 : return _dev->read_registers(reg, val, 1);
296 : }
297 :
298 0 : bool AP_AK8963_BusDriver_HALDevice::register_write(uint8_t reg, uint8_t val)
299 : {
300 0 : return _dev->write_register(reg, val);
301 : }
302 :
303 0 : AP_HAL::Semaphore *AP_AK8963_BusDriver_HALDevice::get_semaphore()
304 : {
305 0 : return _dev->get_semaphore();
306 : }
307 :
308 0 : AP_HAL::Device::PeriodicHandle AP_AK8963_BusDriver_HALDevice::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
309 : {
310 0 : return _dev->register_periodic_callback(period_usec, cb);
311 : }
312 :
313 : /* AK8963 on an auxiliary bus of IMU driver */
314 0 : AP_AK8963_BusDriver_Auxiliary::AP_AK8963_BusDriver_Auxiliary(AP_InertialSensor &ins, uint8_t backend_id,
315 0 : uint8_t backend_instance, uint8_t addr)
316 : {
317 : /*
318 : * Only initialize members. Fails are handled by configure or while
319 : * getting the semaphore
320 : */
321 : #if AP_INERTIALSENSOR_ENABLED
322 0 : _bus = ins.get_auxiliary_bus(backend_id, backend_instance);
323 0 : if (!_bus) {
324 0 : return;
325 : }
326 :
327 0 : _slave = _bus->request_next_slave(addr);
328 : #endif
329 : }
330 :
331 0 : AP_AK8963_BusDriver_Auxiliary::~AP_AK8963_BusDriver_Auxiliary()
332 : {
333 : /* After started it's owned by AuxiliaryBus */
334 0 : if (!_started) {
335 0 : delete _slave;
336 : }
337 0 : }
338 :
339 0 : bool AP_AK8963_BusDriver_Auxiliary::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
340 : {
341 0 : if (_started) {
342 : /*
343 : * We can only read a block when reading the block of sample values -
344 : * calling with any other value is a mistake
345 : */
346 0 : if (reg != AK8963_HXL) {
347 0 : return false;
348 : }
349 :
350 0 : int n = _slave->read(buf);
351 0 : return n == static_cast<int>(size);
352 : }
353 :
354 0 : int r = _slave->passthrough_read(reg, buf, size);
355 :
356 0 : return r > 0 && static_cast<uint32_t>(r) == size;
357 : }
358 :
359 0 : bool AP_AK8963_BusDriver_Auxiliary::register_read(uint8_t reg, uint8_t *val)
360 : {
361 0 : return _slave->passthrough_read(reg, val, 1) == 1;
362 : }
363 :
364 0 : bool AP_AK8963_BusDriver_Auxiliary::register_write(uint8_t reg, uint8_t val)
365 : {
366 0 : return _slave->passthrough_write(reg, val) == 1;
367 : }
368 :
369 0 : AP_HAL::Semaphore *AP_AK8963_BusDriver_Auxiliary::get_semaphore()
370 : {
371 0 : return _bus ? _bus->get_semaphore() : nullptr;
372 : }
373 :
374 0 : bool AP_AK8963_BusDriver_Auxiliary::configure()
375 : {
376 0 : if (!_bus || !_slave) {
377 0 : return false;
378 : }
379 0 : return true;
380 : }
381 :
382 0 : bool AP_AK8963_BusDriver_Auxiliary::start_measurements()
383 : {
384 0 : if (_bus->register_periodic_read(_slave, AK8963_HXL, sizeof(AP_Compass_AK8963::sample_regs)) < 0) {
385 0 : return false;
386 : }
387 :
388 0 : _started = true;
389 :
390 0 : return true;
391 : }
392 :
393 0 : AP_HAL::Device::PeriodicHandle AP_AK8963_BusDriver_Auxiliary::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
394 : {
395 0 : return _bus->register_periodic_callback(period_usec, cb);
396 : }
397 :
398 : // set device type within a device class
399 0 : void AP_AK8963_BusDriver_Auxiliary::set_device_type(uint8_t devtype)
400 : {
401 0 : _bus->set_device_type(devtype);
402 0 : }
403 :
404 : // return 24 bit bus identifier
405 0 : uint32_t AP_AK8963_BusDriver_Auxiliary::get_bus_id(void) const
406 : {
407 0 : return _bus->get_bus_id();
408 : }
409 :
410 : #endif // AP_COMPASS_AK8963_ENABLED
|
