Line data Source code
1 : /*
2 : * Copyright (C) 2016 Emlid Ltd. 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 : * Driver by RadioLink LjWang, Jun 2017
18 : * GPS compass module See<http://www.radiolink.com>
19 : */
20 : #include "AP_Compass_QMC5883L.h"
21 :
22 : #if AP_COMPASS_QMC5883L_ENABLED
23 :
24 : #include <stdio.h>
25 : #include <utility>
26 :
27 : #include <AP_HAL/AP_HAL.h>
28 : #include <AP_HAL/utility/sparse-endian.h>
29 : #include <AP_Math/AP_Math.h>
30 :
31 : #define QMC5883L_REG_CONF1 0x09
32 : #define QMC5883L_REG_CONF2 0x0A
33 :
34 : // data output rates for 5883L
35 : #define QMC5883L_ODR_10HZ (0x00 << 2)
36 : #define QMC5883L_ODR_50HZ (0x01 << 2)
37 : #define QMC5883L_ODR_100HZ (0x02 << 2)
38 : #define QMC5883L_ODR_200HZ (0x03 << 2)
39 :
40 : // Sensor operation modes
41 : #define QMC5883L_MODE_STANDBY 0x00
42 : #define QMC5883L_MODE_CONTINUOUS 0x01
43 :
44 : #define QMC5883L_RNG_2G (0x00 << 4)
45 : #define QMC5883L_RNG_8G (0x01 << 4)
46 :
47 : #define QMC5883L_OSR_512 (0x00 << 6)
48 : #define QMC5883L_OSR_256 (0x01 << 6)
49 : #define QMC5883L_OSR_128 (0x10 << 6)
50 : #define QMC5883L_OSR_64 (0x11 << 6)
51 :
52 : #define QMC5883L_RST 0x80
53 :
54 : #define QMC5883L_REG_DATA_OUTPUT_X 0x00
55 : #define QMC5883L_REG_STATUS 0x06
56 :
57 : #define QMC5883L_REG_ID 0x0D
58 : #define QMC5883_ID_VAL 0xFF
59 :
60 0 : AP_Compass_Backend *AP_Compass_QMC5883L::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
61 : bool force_external,
62 : enum Rotation rotation)
63 : {
64 0 : if (!dev) {
65 0 : return nullptr;
66 : }
67 :
68 0 : AP_Compass_QMC5883L *sensor = NEW_NOTHROW AP_Compass_QMC5883L(std::move(dev),force_external,rotation);
69 0 : if (!sensor || !sensor->init()) {
70 0 : delete sensor;
71 0 : return nullptr;
72 : }
73 :
74 0 : return sensor;
75 : }
76 :
77 0 : AP_Compass_QMC5883L::AP_Compass_QMC5883L(AP_HAL::OwnPtr<AP_HAL::Device> dev,
78 : bool force_external,
79 0 : enum Rotation rotation)
80 0 : : _dev(std::move(dev))
81 0 : , _rotation(rotation)
82 0 : , _force_external(force_external)
83 : {
84 0 : }
85 :
86 0 : bool AP_Compass_QMC5883L::init()
87 : {
88 0 : _dev->get_semaphore()->take_blocking();
89 :
90 0 : _dev->set_retries(10);
91 :
92 : #if 0
93 : _dump_registers();
94 : #endif
95 :
96 0 : if(!_check_whoami()){
97 0 : goto fail;
98 : }
99 :
100 0 : if (!_dev->write_register(0x0B, 0x01)||
101 0 : !_dev->write_register(0x20, 0x40)||
102 0 : !_dev->write_register(0x21, 0x01)||
103 0 : !_dev->write_register(QMC5883L_REG_CONF1,
104 : QMC5883L_MODE_CONTINUOUS|
105 : QMC5883L_ODR_100HZ|
106 : QMC5883L_OSR_512|
107 : QMC5883L_RNG_8G)) {
108 0 : goto fail;
109 : }
110 :
111 : // lower retries for run
112 0 : _dev->set_retries(3);
113 :
114 0 : _dev->get_semaphore()->give();
115 :
116 : //register compass instance
117 0 : _dev->set_device_type(DEVTYPE_QMC5883L);
118 0 : if (!register_compass(_dev->get_bus_id())) {
119 0 : return false;
120 : }
121 :
122 0 : printf("%s found on bus %u id %u address 0x%02x\n", name,
123 0 : _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address());
124 :
125 0 : set_rotation(_rotation);
126 :
127 0 : if (_force_external) {
128 0 : set_external(true);
129 : }
130 :
131 : //Enable 100HZ
132 0 : _dev->register_periodic_callback(10000,
133 : FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883L::timer, void));
134 :
135 0 : return true;
136 :
137 0 : fail:
138 0 : _dev->get_semaphore()->give();
139 0 : return false;
140 : }
141 0 : bool AP_Compass_QMC5883L::_check_whoami()
142 : {
143 : uint8_t whoami;
144 : //Affected by other devices,must read registers 0x00 once or reset,after can read the ID registers reliably
145 0 : _dev->read_registers(0x00,&whoami,1);
146 0 : if (!_dev->read_registers(0x0C, &whoami,1)||
147 0 : whoami != 0x01){
148 0 : return false;
149 : }
150 0 : if (!_dev->read_registers(QMC5883L_REG_ID, &whoami,1)||
151 0 : whoami != QMC5883_ID_VAL){
152 0 : return false;
153 : }
154 0 : return true;
155 : }
156 :
157 0 : void AP_Compass_QMC5883L::timer()
158 : {
159 : struct PACKED {
160 : le16_t rx;
161 : le16_t ry;
162 : le16_t rz;
163 : } buffer;
164 :
165 0 : const float range_scale = 1000.0f / 3000.0f;
166 :
167 : uint8_t status;
168 0 : if(!_dev->read_registers(QMC5883L_REG_STATUS,&status,1)){
169 0 : return;
170 : }
171 : //new data is ready
172 0 : if (!(status & 0x04)) {
173 0 : return;
174 : }
175 :
176 0 : if(!_dev->read_registers(QMC5883L_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))){
177 0 : return ;
178 : }
179 :
180 0 : auto x = -static_cast<int16_t>(le16toh(buffer.rx));
181 0 : auto y = static_cast<int16_t>(le16toh(buffer.ry));
182 0 : auto z = -static_cast<int16_t>(le16toh(buffer.rz));
183 :
184 : #if 0
185 : printf("mag.x:%d\n",x);
186 : printf("mag.y:%d\n",y);
187 : printf("mag.z:%d\n",z);
188 : #endif
189 :
190 0 : Vector3f field = Vector3f{x * range_scale , y * range_scale, z * range_scale };
191 :
192 : // rotate to the desired orientation
193 0 : if (is_external()) {
194 0 : field.rotate(ROTATION_YAW_90);
195 : }
196 :
197 0 : accumulate_sample(field, 20);
198 : }
199 :
200 0 : void AP_Compass_QMC5883L::_dump_registers()
201 : {
202 0 : printf("QMC5883L registers dump\n");
203 0 : for (uint8_t reg = QMC5883L_REG_DATA_OUTPUT_X; reg <= 0x30; reg++) {
204 : uint8_t v;
205 0 : _dev->read_registers(reg,&v,1);
206 0 : printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
207 0 : if ((reg - ( QMC5883L_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
208 0 : printf("\n");
209 : }
210 : }
211 0 : }
212 :
213 : #endif // AP_COMPASS_QMC5883L_ENABLED
|
