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 Thomas Schumacher, Jan 2019
17 : Structure based on LIS3MDL driver
18 :
19 : */
20 : #include "AP_Compass_RM3100.h"
21 :
22 : #if AP_COMPASS_RM3100_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 RM3100_POLL_REG 0x00
30 :
31 : #define RM3100_CMM_REG 0x01
32 :
33 : #define RM3100_CCX1_REG 0x04
34 : #define RM3100_CCX0_REG 0x05
35 : #define RM3100_CCY1_REG 0x06
36 : #define RM3100_CCY0_REG 0x07
37 : #define RM3100_CCZ1_REG 0x08
38 : #define RM3100_CCZ0_REG 0x09
39 :
40 : #define RM3100_TMRC_REG 0x0B
41 :
42 : #define RM3100_MX2_REG 0x24
43 : #define RM3100_MX1_REG 0x25
44 : #define RM3100_MX0_REG 0x26
45 : #define RM3100_MY2_REG 0x27
46 : #define RM3100_MY1_REG 0x28
47 : #define RM3100_MY0_REG 0x29
48 : #define RM3100_MZ2_REG 0x2A
49 : #define RM3100_MZ1_REG 0x2B
50 : #define RM3100_MZ0_REG 0x2C
51 :
52 : #define RM3100_BIST_REG 0x33
53 : #define RM3100_STATUS_REG 0x34
54 : #define RM3100_HSHAKE_REG 0x34
55 : #define RM3100_REVID_REG 0x36
56 :
57 : #define CCP0 0xC8 // Cycle Count values
58 : #define CCP1 0x00
59 : #define CCP0_DEFAULT 0xC8 // Default Cycle Count values (used as a whoami check)
60 : #define CCP1_DEFAULT 0x00
61 : #define GAIN_CC50 20.0f // LSB/uT
62 : #define GAIN_CC100 38.0f
63 : #define GAIN_CC200 75.0f
64 :
65 : #define TMRC 0x94 // Update rate 150Hz
66 : #define CMM 0x71 // read 3 axes and set data ready if 3 axes are ready
67 :
68 : extern const AP_HAL::HAL &hal;
69 :
70 0 : AP_Compass_Backend *AP_Compass_RM3100::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
71 : bool force_external,
72 : enum Rotation rotation)
73 : {
74 0 : if (!dev) {
75 0 : return nullptr;
76 : }
77 0 : AP_Compass_RM3100 *sensor = NEW_NOTHROW AP_Compass_RM3100(std::move(dev), force_external, 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_RM3100::AP_Compass_RM3100(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
87 : bool _force_external,
88 0 : enum Rotation _rotation)
89 0 : : dev(std::move(_dev))
90 0 : , force_external(_force_external)
91 0 : , rotation(_rotation)
92 : {
93 0 : }
94 :
95 0 : bool AP_Compass_RM3100::init()
96 : {
97 0 : dev->get_semaphore()->take_blocking();
98 :
99 0 : if (dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
100 : // read has high bit set for SPI
101 0 : dev->set_read_flag(0x80);
102 : }
103 :
104 : // high retries for init
105 0 : dev->set_retries(10);
106 :
107 : // use default cycle count values as a whoami test
108 : uint8_t ccx0;
109 : uint8_t ccx1;
110 : uint8_t ccy0;
111 : uint8_t ccy1;
112 : uint8_t ccz0;
113 : uint8_t ccz1;
114 0 : if (!dev->read_registers(RM3100_CCX1_REG, &ccx1, 1) ||
115 0 : !dev->read_registers(RM3100_CCX0_REG, &ccx0, 1) ||
116 0 : !dev->read_registers(RM3100_CCY1_REG, &ccy1, 1) ||
117 0 : !dev->read_registers(RM3100_CCY0_REG, &ccy0, 1) ||
118 0 : !dev->read_registers(RM3100_CCZ1_REG, &ccz1, 1) ||
119 0 : !dev->read_registers(RM3100_CCZ0_REG, &ccz0, 1) ||
120 0 : ccx1 != CCP1_DEFAULT || ccx0 != CCP0_DEFAULT ||
121 0 : ccy1 != CCP1_DEFAULT || ccy0 != CCP0_DEFAULT ||
122 0 : ccz1 != CCP1_DEFAULT || ccz0 != CCP0_DEFAULT) {
123 : // couldn't read one of the cycle count registers or didn't recognize the default cycle count values
124 0 : dev->get_semaphore()->give();
125 0 : return false;
126 : }
127 :
128 0 : dev->setup_checked_registers(8);
129 :
130 0 : dev->write_register(RM3100_TMRC_REG, TMRC, true); // CMM data rate
131 0 : dev->write_register(RM3100_CMM_REG, CMM, true); // CMM configuration
132 0 : dev->write_register(RM3100_CCX1_REG, CCP1, true); // cycle count x
133 0 : dev->write_register(RM3100_CCX0_REG, CCP0, true); // cycle count x
134 0 : dev->write_register(RM3100_CCY1_REG, CCP1, true); // cycle count y
135 0 : dev->write_register(RM3100_CCY0_REG, CCP0, true); // cycle count y
136 0 : dev->write_register(RM3100_CCZ1_REG, CCP1, true); // cycle count z
137 0 : dev->write_register(RM3100_CCZ0_REG, CCP0, true); // cycle count z
138 :
139 0 : _scaler = (1 / GAIN_CC200) * UTESLA_TO_MGAUSS; // has to be changed if using a different cycle count
140 :
141 : // lower retries for run
142 0 : dev->set_retries(3);
143 :
144 0 : dev->get_semaphore()->give();
145 :
146 : /* register the compass instance in the frontend */
147 0 : dev->set_device_type(DEVTYPE_RM3100);
148 0 : if (!register_compass(dev->get_bus_id())) {
149 0 : return false;
150 : }
151 :
152 0 : DEV_PRINTF("RM3100: Found at address 0x%x as compass %u\n", dev->get_bus_address(), instance);
153 :
154 0 : set_rotation(rotation);
155 :
156 0 : if (force_external) {
157 0 : set_external(true);
158 : }
159 :
160 : // call timer() at 80Hz
161 0 : dev->register_periodic_callback(1000000U/80U,
162 : FUNCTOR_BIND_MEMBER(&AP_Compass_RM3100::timer, void));
163 :
164 0 : return true;
165 : }
166 :
167 0 : void AP_Compass_RM3100::timer()
168 : {
169 : struct PACKED {
170 : uint8_t magx_2;
171 : uint8_t magx_1;
172 : uint8_t magx_0;
173 : uint8_t magy_2;
174 : uint8_t magy_1;
175 : uint8_t magy_0;
176 : uint8_t magz_2;
177 : uint8_t magz_1;
178 : uint8_t magz_0;
179 : } data;
180 :
181 0 : int32_t magx = 0;
182 0 : int32_t magy = 0;
183 0 : int32_t magz = 0;
184 :
185 : // check data ready on 3 axis
186 : uint8_t status;
187 0 : if (!dev->read_registers(RM3100_STATUS_REG, (uint8_t *)&status, 1)) {
188 0 : goto check_registers;
189 : }
190 :
191 0 : if (!(status & 0x80)) {
192 : // data not available yet
193 0 : goto check_registers;
194 : }
195 :
196 0 : if (!dev->read_registers(RM3100_MX2_REG, (uint8_t *)&data, sizeof(data))) {
197 0 : goto check_registers;
198 : }
199 :
200 : // the 24 bits of data for each axis are in 2s complement representation
201 : // each byte is shifted to its position in a 24-bit unsigned integer and from 8 more bits to be left-aligned in a 32-bit integer
202 0 : magx = ((uint32_t)data.magx_2 << 24) | ((uint32_t)data.magx_1 << 16) | ((uint32_t)data.magx_0 << 8);
203 0 : magy = ((uint32_t)data.magy_2 << 24) | ((uint32_t)data.magy_1 << 16) | ((uint32_t)data.magy_0 << 8);
204 0 : magz = ((uint32_t)data.magz_2 << 24) | ((uint32_t)data.magz_1 << 16) | ((uint32_t)data.magz_0 << 8);
205 :
206 : // right-shift signed integer back to get correct measurement value
207 0 : magx >>= 8;
208 0 : magy >>= 8;
209 0 : magz >>= 8;
210 :
211 : #ifdef AP_RM3100_REVERSAL_MASK
212 : // some RM3100 builds get the polarity wrong on one or more of the
213 : // elements. By setting AP_RM3100_REVERSAL_MASK in hwdef.dat you
214 : // can fix it without modifying the hardware
215 : if (AP_RM3100_REVERSAL_MASK & 1U) {
216 : magx = -magx;
217 : }
218 : if (AP_RM3100_REVERSAL_MASK & 2U) {
219 : magy = -magy;
220 : }
221 : if (AP_RM3100_REVERSAL_MASK & 4U) {
222 : magz = -magz;
223 : }
224 : #endif
225 :
226 : {
227 : // apply scaler and store in field vector
228 : Vector3f field{
229 0 : magx * _scaler,
230 0 : magy * _scaler,
231 0 : magz * _scaler
232 0 : };
233 :
234 0 : accumulate_sample(field);
235 : }
236 :
237 0 : check_registers:
238 0 : dev->check_next_register();
239 0 : }
240 :
241 : #endif // AP_COMPASS_RM3100_ENABLED
|
