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 : #include "AP_Compass_MMC3416.h"
19 :
20 : #if AP_COMPASS_MMC3416_ENABLED
21 :
22 : #include <AP_HAL/AP_HAL.h>
23 : #include <utility>
24 : #include <AP_Math/AP_Math.h>
25 : #include <stdio.h>
26 : #include <AP_Logger/AP_Logger.h>
27 :
28 : extern const AP_HAL::HAL &hal;
29 :
30 : #define REG_PRODUCT_ID 0x20
31 : #define REG_XOUT_L 0x00
32 : #define REG_STATUS 0x06
33 : #define REG_CONTROL0 0x07
34 : #define REG_CONTROL1 0x08
35 :
36 : // bits in REG_CONTROL0
37 : #define REG_CONTROL0_REFILL 0x80
38 : #define REG_CONTROL0_RESET 0x40
39 : #define REG_CONTROL0_SET 0x20
40 : #define REG_CONTROL0_NB 0x10
41 : #define REG_CONTROL0_TM 0x01
42 :
43 : // datasheet says 50ms min for refill
44 : #define MIN_DELAY_SET_RESET 50
45 :
46 0 : AP_Compass_Backend *AP_Compass_MMC3416::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
47 : bool force_external,
48 : enum Rotation rotation)
49 : {
50 0 : if (!dev) {
51 0 : return nullptr;
52 : }
53 0 : AP_Compass_MMC3416 *sensor = NEW_NOTHROW AP_Compass_MMC3416(std::move(dev), force_external, rotation);
54 0 : if (!sensor || !sensor->init()) {
55 0 : delete sensor;
56 0 : return nullptr;
57 : }
58 :
59 0 : return sensor;
60 : }
61 :
62 0 : AP_Compass_MMC3416::AP_Compass_MMC3416(AP_HAL::OwnPtr<AP_HAL::Device> _dev,
63 : bool _force_external,
64 0 : enum Rotation _rotation)
65 0 : : dev(std::move(_dev))
66 0 : , force_external(_force_external)
67 0 : , rotation(_rotation)
68 : {
69 0 : }
70 :
71 0 : bool AP_Compass_MMC3416::init()
72 : {
73 0 : dev->get_semaphore()->take_blocking();
74 :
75 0 : dev->set_retries(10);
76 :
77 : // reset sensor. This seems to solve some unreliablity on boot reading WHOAMI
78 0 : dev->write_register(REG_CONTROL1, 0x80);
79 0 : hal.scheduler->delay(10);
80 :
81 : uint8_t whoami;
82 0 : if (!dev->read_registers(REG_PRODUCT_ID, &whoami, 1) ||
83 0 : whoami != 0x06) {
84 : // not a MMC3416
85 0 : dev->get_semaphore()->give();
86 0 : return false;
87 : }
88 :
89 : // reset sensor
90 0 : dev->write_register(REG_CONTROL1, 0x80);
91 0 : hal.scheduler->delay(10);
92 :
93 0 : dev->write_register(REG_CONTROL0, 0x00); // single shot
94 0 : dev->write_register(REG_CONTROL1, 0x00); // 16 bit, 7.92ms
95 :
96 0 : dev->get_semaphore()->give();
97 :
98 : /* register the compass instance in the frontend */
99 0 : dev->set_device_type(DEVTYPE_MMC3416);
100 0 : if (!register_compass(dev->get_bus_id())) {
101 0 : return false;
102 : }
103 :
104 0 : printf("Found a MMC3416 on 0x%x as compass %u\n", unsigned(dev->get_bus_id()), instance);
105 :
106 0 : set_rotation(rotation);
107 :
108 0 : if (force_external) {
109 0 : set_external(true);
110 : }
111 :
112 0 : dev->set_retries(1);
113 :
114 : // call timer() at 100Hz
115 0 : dev->register_periodic_callback(10000,
116 : FUNCTOR_BIND_MEMBER(&AP_Compass_MMC3416::timer, void));
117 :
118 : // wait 250ms for the compass to make it's initial readings
119 0 : hal.scheduler->delay(250);
120 :
121 0 : return true;
122 : }
123 :
124 0 : void AP_Compass_MMC3416::timer()
125 : {
126 0 : const uint16_t measure_count_limit = 50;
127 0 : const uint16_t zero_offset = 32768; // 16 bit mode
128 0 : const uint16_t sensitivity = 2048; // counts per Gauss, 16 bit mode
129 0 : const float counts_to_milliGauss = 1.0e3f / sensitivity;
130 :
131 0 : uint32_t now = AP_HAL::millis();
132 0 : if (now - last_sample_ms > 500) {
133 : // seems to be stuck or on first sample, reset state machine
134 0 : state = STATE_REFILL1;
135 0 : last_sample_ms = now;
136 : }
137 :
138 : /*
139 : we use the SET/RESET method to remove bridge offset every
140 : measure_count_limit measurements. This involves a fairly complex
141 : state machine, but means we are much less sensitive to
142 : temperature changes
143 : */
144 0 : switch (state) {
145 0 : case STATE_REFILL1:
146 0 : if (dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
147 0 : state = STATE_REFILL1_WAIT;
148 0 : refill_start_ms = AP_HAL::millis();
149 : }
150 0 : break;
151 :
152 0 : case STATE_REFILL1_WAIT: {
153 : uint8_t status;
154 0 : if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET &&
155 0 : dev->read_registers(REG_STATUS, &status, 1) &&
156 0 : (status & 0x02) == 0) {
157 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_SET) ||
158 0 : !dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
159 0 : state = STATE_REFILL1;
160 : } else {
161 0 : state = STATE_MEASURE_WAIT1;
162 : }
163 : }
164 0 : break;
165 : }
166 :
167 0 : case STATE_MEASURE_WAIT1: {
168 : uint8_t status;
169 0 : if (dev->read_registers(REG_STATUS, &status, 1) && (status & 1)) {
170 0 : if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data0[0], 6)) {
171 0 : state = STATE_REFILL1;
172 0 : break;
173 : }
174 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_REFILL)) {
175 0 : state = STATE_REFILL1;
176 : } else {
177 0 : state = STATE_REFILL2_WAIT;
178 0 : refill_start_ms = AP_HAL::millis();
179 : }
180 : }
181 0 : break;
182 : }
183 :
184 0 : case STATE_REFILL2_WAIT: {
185 : uint8_t status;
186 0 : if (AP_HAL::millis() - refill_start_ms > MIN_DELAY_SET_RESET &&
187 0 : dev->read_registers(REG_STATUS, &status, 1) &&
188 0 : (status & 0x02) == 0) {
189 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_RESET) ||
190 0 : !dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
191 0 : state = STATE_REFILL1;
192 : } else {
193 0 : state = STATE_MEASURE_WAIT2;
194 : }
195 : }
196 0 : break;
197 : }
198 :
199 0 : case STATE_MEASURE_WAIT2: {
200 : uint8_t status;
201 0 : if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
202 0 : break;
203 : }
204 : uint16_t data1[3];
205 0 : if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
206 0 : state = STATE_REFILL1;
207 0 : break;
208 : }
209 0 : Vector3f field;
210 :
211 : /*
212 : calculate field and offset
213 : */
214 0 : Vector3f f1(float(data0[0]) - zero_offset,
215 0 : float(data0[1]) - zero_offset,
216 0 : float(data0[2]) - zero_offset);
217 0 : Vector3f f2(float(data1[0]) - zero_offset,
218 0 : float(data1[1]) - zero_offset,
219 0 : float(data1[2]) - zero_offset);
220 0 : field = (f1 - f2) * (counts_to_milliGauss / 2);
221 0 : Vector3f new_offset = (f1 + f2) * (counts_to_milliGauss / 2);
222 0 : if (!have_initial_offset) {
223 0 : offset = new_offset;
224 0 : have_initial_offset = true;
225 : } else {
226 : // low pass changes to the offset
227 0 : offset = offset * 0.95f + new_offset * 0.05f;
228 : }
229 :
230 : #if 0
231 : // @LoggerMessage: MMO
232 : // @Description: MMC3416 compass data
233 : // @Field: TimeUS: Time since system startup
234 : // @Field: Nx: new measurement X axis
235 : // @Field: Ny: new measurement Y axis
236 : // @Field: Nz: new measurement Z axis
237 : // @Field: Ox: new offset X axis
238 : // @Field: Oy: new offset Y axis
239 : // @Field: Oz: new offset Z axis
240 : AP::logger().Write("MMO", "TimeUS,Nx,Ny,Nz,Ox,Oy,Oz", "Qffffff",
241 : AP_HAL::micros64(),
242 : (double)new_offset.x,
243 : (double)new_offset.y,
244 : (double)new_offset.z,
245 : (double)offset.x,
246 : (double)offset.y,
247 : (double)offset.z);
248 : printf("F(%.1f %.1f %.1f) O(%.1f %.1f %.1f)\n",
249 : field.x, field.y, field.z,
250 : offset.x, offset.y, offset.z);
251 : #endif
252 :
253 0 : last_sample_ms = AP_HAL::millis();
254 :
255 : // sensor is not FRD
256 0 : field.y = -field.y;
257 :
258 0 : accumulate_sample(field);
259 :
260 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) {
261 0 : state = STATE_REFILL1;
262 : } else {
263 0 : state = STATE_MEASURE_WAIT3;
264 : }
265 0 : break;
266 : }
267 :
268 0 : case STATE_MEASURE_WAIT3: {
269 : uint8_t status;
270 0 : if (!dev->read_registers(REG_STATUS, &status, 1) || !(status & 1)) {
271 0 : break;
272 : }
273 : uint16_t data1[3];
274 0 : if (!dev->read_registers(REG_XOUT_L, (uint8_t *)&data1[0], 6)) {
275 0 : state = STATE_REFILL1;
276 0 : break;
277 : }
278 0 : Vector3f field(float(data1[0]) - zero_offset,
279 0 : float(data1[1]) - zero_offset,
280 0 : float(data1[2]) - zero_offset);
281 0 : field *= -counts_to_milliGauss;
282 0 : field += offset;
283 :
284 : // sensor is not FRD
285 0 : field.y = -field.y;
286 :
287 0 : last_sample_ms = AP_HAL::millis();
288 0 : accumulate_sample(field);
289 :
290 : // we stay in STATE_MEASURE_WAIT3 for measure_count_limit cycles
291 0 : if (measure_count++ >= measure_count_limit) {
292 0 : measure_count = 0;
293 0 : state = STATE_REFILL1;
294 : } else {
295 0 : if (!dev->write_register(REG_CONTROL0, REG_CONTROL0_TM)) { // Take Measurement
296 0 : state = STATE_REFILL1;
297 : }
298 : }
299 0 : break;
300 : }
301 : }
302 0 : }
303 :
304 : #endif // AP_COMPASS_MMC3416_ENABLED
|
