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 : * Driver by Lokesh Ramina, Jan 2022
16 : */
17 : #include "AP_Compass_QMC5883P.h"
18 :
19 : #include <stdio.h>
20 :
21 : #include <AP_HAL/AP_HAL.h>
22 : #include <AP_Math/AP_Math.h>
23 :
24 : #if AP_COMPASS_QMC5883P_ENABLED
25 :
26 : //Register Address
27 : #define QMC5883P_REG_ID 0x00
28 : #define QMC5883P_REG_DATA_OUTPUT_X 0x01
29 : #define QMC5883P_REG_DATA_OUTPUT_Z_MSB 0x06
30 : #define QMC5883P_REG_STATUS 0x09
31 : #define QMC5883P_REG_CONF1 0x0A
32 : #define QMC5883P_REG_CONF2 0x0B
33 :
34 : #define QMC5883P_ID_VAL 0x80
35 :
36 : //Register values
37 : // Sensor operation modes
38 : #define QMC5883P_MODE_SUSPEND 0x00
39 : #define QMC5883P_MODE_NORMAL 0x01
40 : #define QMC5883P_MODE_SINGLE 0x02
41 : #define QMC5883P_MODE_CONTINUOUS 0x03
42 :
43 : // ODR data output rates for 5883L
44 : #define QMC5883P_ODR_10HZ (0x00 << 2)
45 : #define QMC5883P_ODR_50HZ (0x01 << 2)
46 : #define QMC5883P_ODR_100HZ (0x02 << 2)
47 : #define QMC5883P_ODR_200HZ (0x03 << 2)
48 :
49 : // Over sampling Ratio OSR1
50 : #define QMC5883P_OSR1_8 (0x00 << 4)
51 : #define QMC5883P_OSR1_4 (0x01 << 4)
52 : #define QMC5883P_OSR1_2 (0x02 << 4)
53 : #define QMC5883P_OSR1_1 (0x03 << 4)
54 :
55 : // Down sampling Rate OSR2
56 : #define QMC5883P_OSR2_8 0x08
57 :
58 : //RNG
59 : #define QMC5883P_RNG_30G (0x00 << 2)
60 : #define QMC5883P_RNG_12G (0x01 << 2)
61 : #define QMC5883P_RNG_8G (0x10 << 2)
62 : #define QMC5883P_RNG_2G (0x11 << 2)
63 :
64 : #define QMC5883P_SET_XYZ_SIGN 0x29
65 :
66 : //Reset
67 : #define QMC5883P_RST 0x80
68 :
69 : //Status Val
70 : #define QMC5883P_STATUS_DATA_READY 0x01
71 :
72 : #ifndef DEBUG
73 : #define DEBUG 0
74 : #endif
75 :
76 0 : AP_Compass_Backend *AP_Compass_QMC5883P::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
77 : bool force_external,
78 : enum Rotation rotation)
79 : {
80 0 : if (!dev) {
81 0 : return nullptr;
82 : }
83 0 : AP_Compass_QMC5883P *sensor = NEW_NOTHROW AP_Compass_QMC5883P(std::move(dev),force_external,rotation);
84 0 : if (!sensor || !sensor->init()) {
85 0 : delete sensor;
86 0 : return nullptr;
87 : }
88 0 : return sensor;
89 : }
90 :
91 0 : AP_Compass_QMC5883P::AP_Compass_QMC5883P(AP_HAL::OwnPtr<AP_HAL::Device> dev,
92 : bool force_external,
93 0 : enum Rotation rotation)
94 0 : : _dev(std::move(dev))
95 0 : , _rotation(rotation)
96 0 : , _force_external(force_external)
97 : {
98 0 : }
99 :
100 0 : bool AP_Compass_QMC5883P::init()
101 : {
102 0 : _dev->get_semaphore()->take_blocking();
103 :
104 0 : _dev->set_retries(10);
105 : #if DEBUG
106 : _dump_registers();
107 : #endif
108 0 : if (!_check_whoami()) {
109 0 : goto fail;
110 : }
111 : //As mentioned in the Datasheet 7.2 to do continues mode 0x29 will set sign for X,Y,Z
112 0 : if (!_dev->write_register(QMC5883P_REG_DATA_OUTPUT_Z_MSB, QMC5883P_SET_XYZ_SIGN)||
113 0 : !_dev->write_register(QMC5883P_REG_CONF1,
114 : QMC5883P_MODE_CONTINUOUS|
115 : QMC5883P_ODR_100HZ|
116 : QMC5883P_OSR1_8|
117 0 : QMC5883P_OSR2_8)||
118 0 : !_dev->write_register(QMC5883P_REG_CONF2,QMC5883P_OSR2_8)) {
119 0 : goto fail;
120 : }
121 :
122 : // lower retries for run
123 0 : _dev->set_retries(3);
124 :
125 0 : _dev->get_semaphore()->give();
126 :
127 : //register compass instance
128 0 : _dev->set_device_type(DEVTYPE_QMC5883P);
129 0 : if (!register_compass(_dev->get_bus_id())) {
130 0 : return false;
131 : }
132 :
133 0 : printf("%s found on bus %u id %u address 0x%02x\n", name,
134 0 : _dev->bus_num(), unsigned(_dev->get_bus_id()), _dev->get_bus_address());
135 :
136 0 : set_rotation(_rotation);
137 :
138 0 : if (_force_external) {
139 0 : set_external(true);
140 : }
141 :
142 : //Enable 100HZ
143 0 : _dev->register_periodic_callback(10000,
144 : FUNCTOR_BIND_MEMBER(&AP_Compass_QMC5883P::timer, void));
145 :
146 0 : return true;
147 :
148 0 : fail:
149 0 : _dev->get_semaphore()->give();
150 0 : return false;
151 : }
152 0 : bool AP_Compass_QMC5883P::_check_whoami()
153 : {
154 : uint8_t whoami;
155 0 : if (!_dev->read_registers(QMC5883P_REG_ID, &whoami,1)||
156 0 : whoami != QMC5883P_ID_VAL) {
157 0 : return false;
158 : }
159 0 : return true;
160 : }
161 :
162 0 : void AP_Compass_QMC5883P::timer()
163 : {
164 : struct PACKED {
165 : int16_t rx;
166 : int16_t ry;
167 : int16_t rz;
168 : } buffer;
169 :
170 0 : const float range_scale = 1000.0f / 3000.0f;
171 :
172 : uint8_t status;
173 0 : if (!_dev->read_registers(QMC5883P_REG_STATUS,&status,1)) {
174 0 : return;
175 : }
176 : //new data is ready
177 0 : if (!(status & QMC5883P_STATUS_DATA_READY)) {
178 0 : return;
179 : }
180 :
181 0 : if (!_dev->read_registers(QMC5883P_REG_DATA_OUTPUT_X, (uint8_t *) &buffer, sizeof(buffer))) {
182 0 : return ;
183 : }
184 :
185 0 : auto x = buffer.rx;
186 0 : auto y = buffer.ry;
187 0 : auto z = buffer.rz;
188 :
189 : #if DEBUG
190 : printf("mag.x:%d\n",x);
191 : printf("mag.y:%d\n",y);
192 : printf("mag.z:%d\n",z);
193 : #endif
194 :
195 0 : Vector3f field = Vector3f{x * range_scale, y * range_scale, z * range_scale };
196 :
197 0 : accumulate_sample(field, 20);
198 : }
199 :
200 0 : void AP_Compass_QMC5883P::_dump_registers()
201 : {
202 0 : printf("QMC5883P registers dump\n");
203 0 : for (uint8_t reg = QMC5883P_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 - ( QMC5883P_REG_DATA_OUTPUT_X-1)) % 16 == 0) {
208 0 : printf("\n");
209 : }
210 : }
211 0 : }
212 :
213 : #endif //AP_COMPASS_QMC5883P_ENABLED
|
