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 : #include "AP_Compass_BMM350.h"
16 :
17 : #if AP_COMPASS_BMM350_ENABLED
18 :
19 : #include <AP_HAL/AP_HAL.h>
20 :
21 : #include <utility>
22 :
23 : #include <AP_HAL/utility/sparse-endian.h>
24 : #include <AP_Math/AP_Math.h>
25 :
26 : #define BMM350_REG_CHIP_ID 0x00
27 : #define BMM350_REG_PMU_CMD_AGGR_SET 0x04
28 : #define BMM350_REG_PMU_CMD_AXIS_EN 0x05
29 : #define BMM350_REG_PMU_CMD 0x06
30 : #define BMM350_REG_PMU_CMD_STATUS_0 0x07
31 : #define BMM350_REG_INT_CTRL 0x2E
32 : #define BMM350_REG_MAG_X_XLSB 0x31
33 : #define BMM350_REG_OTP_CMD 0x50
34 : #define BMM350_REG_OTP_DATA_MSB 0x52
35 : #define BMM350_REG_OTP_DATA_LSB 0x53
36 : #define BMM350_REG_OTP_STATUS 0x55
37 : #define BMM350_REG_CMD 0x7E
38 :
39 : // OTP(one-time programmable memory)
40 : #define BMM350_OTP_CMD_DIR_READ (0x01<<5U)
41 : #define BMM350_OTP_CMD_PWR_OFF_OTP (0x04<<5U)
42 :
43 : #define BMM350_OTP_STATUS_ERROR_MASK 0xE0
44 : #define BMM350_OTP_STATUS_CMD_DONE 0x01
45 :
46 : #define BMM350_TEMP_OFF_SENS 0x0D
47 : #define BMM350_MAG_OFFSET_X 0x0E
48 : #define BMM350_MAG_OFFSET_Y 0x0F
49 : #define BMM350_MAG_OFFSET_Z 0x10
50 : #define BMM350_MAG_SENS_X 0x10
51 : #define BMM350_MAG_SENS_Y 0x11
52 : #define BMM350_MAG_SENS_Z 0x11
53 : #define BMM350_MAG_TCO_X 0x12
54 : #define BMM350_MAG_TCO_Y 0x13
55 : #define BMM350_MAG_TCO_Z 0x14
56 : #define BMM350_MAG_TCS_X 0x12
57 : #define BMM350_MAG_TCS_Y 0x13
58 : #define BMM350_MAG_TCS_Z 0x14
59 : #define BMM350_MAG_DUT_T_0 0x18
60 : #define BMM350_CROSS_X_Y 0x15
61 : #define BMM350_CROSS_Y_X 0x15
62 : #define BMM350_CROSS_Z_X 0x16
63 : #define BMM350_CROSS_Z_Y 0x16
64 : #define BMM350_SENS_CORR_Y 0.01f
65 : #define BMM350_TCS_CORR_Z 0.0001f
66 :
67 : #define BMM350_CMD_SOFTRESET 0xB6
68 : #define BMM350_INT_MODE_PULSED (0<<0U)
69 : #define BMM350_INT_POL_ACTIVE_HIGH (1<<1U)
70 : #define BMM350_INT_OD_PUSHPULL (1<<2U)
71 : #define BMM350_INT_OUTPUT_DISABLE (0<<3U)
72 : #define BMM350_INT_DRDY_EN (1<<7U)
73 :
74 : // Averaging performance
75 : #define BMM350_AVERAGING_4 (0x02 << 4U)
76 : #define BMM350_AVERAGING_8 (0x03 << 4U)
77 :
78 : // Output data rate
79 : #define BMM350_ODR_100HZ 0x04
80 : #define BMM350_ODR_50HZ 0x05
81 :
82 : // Power modes
83 : #define BMM350_PMU_CMD_SUSPEND_MODE 0x00
84 : #define BMM350_PMU_CMD_NORMAL_MODE 0x01
85 : #define BMM350_PMU_CMD_UPD_OAE 0x02
86 : #define BMM350_PMU_CMD_FGR 0x05
87 : #define BMM350_PMU_CMD_BR 0x07
88 :
89 : // OTP data length
90 : #define BMM350_OTP_DATA_LENGTH 32U
91 :
92 : // Chip ID of BMM350
93 : #define BMM350_CHIP_ID 0x33
94 :
95 : #define BMM350_XY_SENSITIVE 14.55f
96 : #define BMM350_Z_SENSITIVE 9.0f
97 : #define BMM350_TEMP_SENSITIVE 0.00204f
98 : #define BMM350_XY_INA_GAIN 19.46f
99 : #define BMM350_Z_INA_GAIN 31.0f
100 : #define BMM350_ADC_GAIN (1.0f / 1.5f)
101 : #define BMM350_LUT_GAIN 0.714607238769531f
102 : #define BMM350_POWER ((float)(1000000.0 / 1048576.0))
103 :
104 : #define BMM350_XY_SCALE (BMM350_POWER / (BMM350_XY_SENSITIVE * BMM350_XY_INA_GAIN * BMM350_ADC_GAIN * BMM350_LUT_GAIN))
105 : #define BMM350_Z_SCALE (BMM350_POWER / (BMM350_Z_SENSITIVE * BMM350_Z_INA_GAIN * BMM350_ADC_GAIN * BMM350_LUT_GAIN))
106 : #define BMM350_TEMP_SCALE (1.0f / (BMM350_TEMP_SENSITIVE * BMM350_ADC_GAIN * BMM350_LUT_GAIN * 1048576))
107 :
108 : extern const AP_HAL::HAL &hal;
109 :
110 0 : AP_Compass_Backend *AP_Compass_BMM350::probe(AP_HAL::OwnPtr<AP_HAL::Device> dev,
111 : bool force_external,
112 : enum Rotation rotation)
113 : {
114 0 : if (!dev) {
115 0 : return nullptr;
116 : }
117 0 : AP_Compass_BMM350 *sensor = NEW_NOTHROW AP_Compass_BMM350(std::move(dev), force_external, rotation);
118 0 : if (!sensor || !sensor->init()) {
119 0 : delete sensor;
120 0 : return nullptr;
121 : }
122 :
123 0 : return sensor;
124 : }
125 :
126 0 : AP_Compass_BMM350::AP_Compass_BMM350(AP_HAL::OwnPtr<AP_HAL::Device> dev,
127 : bool force_external,
128 0 : enum Rotation rotation)
129 0 : : _dev(std::move(dev))
130 0 : , _force_external(force_external)
131 0 : , _rotation(rotation)
132 : {
133 0 : }
134 :
135 : /**
136 : * @brief Read out OTP(one-time programmable memory) data of sensor which is the compensation coefficients
137 : * @see https://github.com/boschsensortec/BMM350-SensorAPI
138 : */
139 0 : bool AP_Compass_BMM350::read_otp_data()
140 : {
141 : uint16_t otp_data[BMM350_OTP_DATA_LENGTH];
142 :
143 0 : for (uint8_t index = 0; index < BMM350_OTP_DATA_LENGTH; index++) {
144 : // Set OTP address
145 0 : if (!_dev->write_register(BMM350_REG_OTP_CMD, (BMM350_OTP_CMD_DIR_READ | index))) {
146 0 : return false;
147 : }
148 :
149 : // Wait for OTP status be ready
150 0 : int8_t tries = 3; // Try polling 3 times
151 0 : while (tries--)
152 : {
153 : uint8_t status;
154 0 : hal.scheduler->delay_microseconds(300);
155 : // Read OTP status
156 0 : if (!read_bytes(BMM350_REG_OTP_STATUS, &status, 1) || ((status & BMM350_OTP_STATUS_ERROR_MASK) != 0)) {
157 0 : return false;
158 : }
159 0 : if (status & BMM350_OTP_STATUS_CMD_DONE) {
160 0 : break;
161 : }
162 : }
163 :
164 0 : if (tries == -1) {
165 0 : return false;
166 : }
167 :
168 : // Read OTP data
169 : be16_t reg_data;
170 0 : if (!read_bytes(BMM350_REG_OTP_DATA_MSB, (uint8_t *)®_data, 2)) {
171 0 : return false;
172 : }
173 :
174 0 : otp_data[index] = be16toh(reg_data);
175 : }
176 :
177 : // Update magnetometer offset and sensitivity data.
178 : // 12-bit unsigned integer to be left-aligned in a 16-bit integer
179 0 : _mag_comp.offset_coef.x = float(int16_t((otp_data[BMM350_MAG_OFFSET_X] & 0x0FFF) << 4) >> 4);
180 0 : _mag_comp.offset_coef.y = float(int16_t((((otp_data[BMM350_MAG_OFFSET_X] & 0xF000) >> 4) +
181 0 : (otp_data[BMM350_MAG_OFFSET_Y] & 0x00FF)) << 4) >> 4);
182 0 : _mag_comp.offset_coef.z = float(int16_t(((otp_data[BMM350_MAG_OFFSET_Y] & 0x0F00) +
183 0 : (otp_data[BMM350_MAG_OFFSET_Z] & 0x00FF)) << 4) >> 4);
184 0 : _mag_comp.offset_coef.temp = float(int8_t(otp_data[BMM350_TEMP_OFF_SENS])) * (1.0f / 5.0f);
185 :
186 0 : _mag_comp.sensit_coef.x = float(int8_t((otp_data[BMM350_MAG_SENS_X] & 0xFF00) >> 8)) * (1.0f / 256.0f);
187 0 : _mag_comp.sensit_coef.y = float(int8_t(otp_data[BMM350_MAG_SENS_Y])) * (1.0f / 256.0f) + BMM350_SENS_CORR_Y;
188 0 : _mag_comp.sensit_coef.z = float(int8_t((otp_data[BMM350_MAG_SENS_Z] & 0xFF00) >> 8)) * (1.0f / 256.0f);
189 0 : _mag_comp.sensit_coef.temp = float(int8_t((otp_data[BMM350_TEMP_OFF_SENS] & 0xFF00) >> 8)) * (1.0f / 512.0f);
190 :
191 0 : _mag_comp.tco.x = float(int8_t(otp_data[BMM350_MAG_TCO_X])) * (1.0f / 32.0f);
192 0 : _mag_comp.tco.y = float(int8_t(otp_data[BMM350_MAG_TCO_Y])) * (1.0f / 32.0f);
193 0 : _mag_comp.tco.z = float(int8_t(otp_data[BMM350_MAG_TCO_Z])) * (1.0f / 32.0f);
194 :
195 0 : _mag_comp.tcs.x = float(int8_t((otp_data[BMM350_MAG_TCS_X] & 0xFF00) >> 8)) * (1.0f / 16384.0f);
196 0 : _mag_comp.tcs.y = float(int8_t((otp_data[BMM350_MAG_TCS_Y] & 0xFF00) >> 8)) * (1.0f / 16384.0f);
197 0 : _mag_comp.tcs.z = float(int8_t((otp_data[BMM350_MAG_TCS_Z] & 0xFF00) >> 8)) * (1.0f / 16384.0f) - BMM350_TCS_CORR_Z;
198 :
199 0 : _mag_comp.t0_reading = float(int16_t(otp_data[BMM350_MAG_DUT_T_0])) * (1.0f / 512.0f) + 23.0f;
200 :
201 0 : _mag_comp.cross_axis.cross_x_y = float(int8_t(otp_data[BMM350_CROSS_X_Y])) * (1.0f / 800.0f);
202 0 : _mag_comp.cross_axis.cross_y_x = float(int8_t((otp_data[BMM350_CROSS_Y_X] & 0xFF00) >> 8)) * (1.0f / 800.0f);
203 0 : _mag_comp.cross_axis.cross_z_x = float(int8_t(otp_data[BMM350_CROSS_Z_X])) * (1.0f / 800.0f);
204 0 : _mag_comp.cross_axis.cross_z_y = float(int8_t((otp_data[BMM350_CROSS_Z_Y] & 0xFF00) >> 8)) * (1.0f / 800.0f);
205 :
206 0 : return true;
207 : }
208 :
209 : /**
210 : * @brief Wait PMU_CMD register operation completed. Need to specify which command just sent
211 : */
212 0 : bool AP_Compass_BMM350::wait_pmu_cmd_ready(const uint8_t cmd, const uint32_t timeout)
213 : {
214 0 : const uint32_t start_tick = AP_HAL::millis();
215 :
216 : do {
217 0 : hal.scheduler->delay(1);
218 : uint8_t status;
219 0 : if (!read_bytes(BMM350_REG_PMU_CMD_STATUS_0, &status, 1)) {
220 0 : return false;
221 : }
222 0 : if (((status & 0x01) == 0x00) && (((status & 0xE0) >> 5) == cmd)) {
223 0 : return true;
224 : }
225 0 : } while ((AP_HAL::millis() - start_tick) < timeout);
226 :
227 0 : return false;
228 : }
229 :
230 : /**
231 : * @brief Reset bit of magnetic register and wait for change to normal mode
232 : */
233 0 : bool AP_Compass_BMM350::mag_reset_and_wait()
234 : {
235 : uint8_t data;
236 :
237 : // Get PMU command status 0 data
238 0 : if (!read_bytes(BMM350_REG_PMU_CMD_STATUS_0, &data, 1)) {
239 0 : return false;
240 : }
241 :
242 : // Check whether the power mode is Normal
243 0 : if (data & 0x08) {
244 : // Set PMU command to suspend mode
245 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_SUSPEND_MODE)) {
246 0 : return false;
247 : }
248 0 : wait_pmu_cmd_ready(BMM350_PMU_CMD_SUSPEND_MODE, 6);
249 : }
250 :
251 : // Set BR(bit reset) to PMU_CMD register
252 : // In offical example, it wait for 14ms. But it may not be enough, so we wait an extra 5ms
253 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_BR) ||
254 0 : !wait_pmu_cmd_ready(BMM350_PMU_CMD_BR, 19)) {
255 0 : return false;
256 : }
257 :
258 : // Set FGR(flux-guide reset) to PMU_CMD register
259 : // 18ms got from offical example, we wait an extra 5ms
260 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_FGR) ||
261 0 : !wait_pmu_cmd_ready(BMM350_PMU_CMD_FGR, 23)) {
262 0 : return false;
263 : }
264 :
265 : // Switch to normal mode
266 0 : if (!set_power_mode(POWER_MODE_NORMAL)) {
267 0 : return false;
268 : }
269 :
270 0 : return true;
271 : }
272 :
273 : /**
274 : * @brief Switch sensor power mode
275 : */
276 0 : bool AP_Compass_BMM350::set_power_mode(const enum power_mode mode)
277 : {
278 : uint8_t pmu_cmd;
279 :
280 : // Get PMU register data as current mode
281 0 : if (!read_bytes(BMM350_REG_PMU_CMD, &pmu_cmd, 1)) {
282 0 : return false;
283 : }
284 :
285 0 : if (pmu_cmd == BMM350_PMU_CMD_NORMAL_MODE || pmu_cmd == BMM350_PMU_CMD_UPD_OAE) {
286 : // Set PMU command to suspend mode
287 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD, POWER_MODE_SUSPEND)) {
288 0 : return false;
289 : }
290 : // Wait for sensor switch to suspend mode
291 : // Wait for maximum 6ms, it got from the official example, not explained in datasheet
292 0 : wait_pmu_cmd_ready(POWER_MODE_SUSPEND, 6);
293 : }
294 :
295 : // Set PMU command to target mode
296 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD, mode)) {
297 0 : return false;
298 : }
299 :
300 : // Wait for mode change. When switch from suspend mode to normal mode, we wait for at most 38ms.
301 : // It got from official example too
302 0 : wait_pmu_cmd_ready(mode, 38);
303 :
304 0 : return true;
305 : }
306 :
307 : /**
308 : * @brief Read bytes from sensor
309 : */
310 0 : bool AP_Compass_BMM350::read_bytes(const uint8_t reg, uint8_t *out, const uint16_t read_len)
311 : {
312 0 : uint8_t data[read_len + 2];
313 :
314 0 : if (!_dev->read_registers(reg, data, read_len + 2)) {
315 0 : return false;
316 : }
317 :
318 0 : memcpy(out, &data[2], read_len);
319 :
320 0 : return true;
321 0 : }
322 :
323 0 : bool AP_Compass_BMM350::init()
324 : {
325 0 : _dev->get_semaphore()->take_blocking();
326 :
327 : // 10 retries for init
328 0 : _dev->set_retries(10);
329 :
330 : // Use checked registers to cope with bus errors
331 0 : _dev->setup_checked_registers(4);
332 :
333 0 : int8_t boot_retries = 5;
334 0 : while (boot_retries--) {
335 : // Soft reset
336 0 : if (!_dev->write_register(BMM350_REG_CMD, BMM350_CMD_SOFTRESET)) {
337 0 : continue;
338 : }
339 0 : hal.scheduler->delay(24); // Wait 24ms for soft reset complete, it got from offical example
340 :
341 : // Read and verify chip ID
342 : uint8_t chip_id;
343 0 : if (!read_bytes(BMM350_REG_CHIP_ID, &chip_id, 1)) {
344 0 : continue;
345 : }
346 0 : if (chip_id == BMM350_CHIP_ID) {
347 0 : break;
348 : }
349 : }
350 :
351 0 : if (boot_retries == -1) {
352 0 : goto err;
353 : }
354 :
355 : // Read out OTP data
356 0 : if (!read_otp_data()) {
357 0 : goto err;
358 : }
359 :
360 : // Power off OTP
361 0 : if (!_dev->write_register(BMM350_REG_OTP_CMD, BMM350_OTP_CMD_PWR_OFF_OTP)) {
362 0 : goto err;
363 : }
364 :
365 : // Magnetic reset
366 0 : if (!mag_reset_and_wait()) {
367 0 : goto err;
368 : }
369 :
370 : // Configure interrupt settings and enable DRDY
371 : // Set INT mode as PULSED | active_high polarity | PUSH_PULL | unmap | DRDY interrupt
372 0 : if (!_dev->write_register(BMM350_REG_INT_CTRL, (BMM350_INT_MODE_PULSED |
373 : BMM350_INT_POL_ACTIVE_HIGH |
374 : BMM350_INT_OD_PUSHPULL |
375 : BMM350_INT_OUTPUT_DISABLE |
376 : BMM350_INT_DRDY_EN))) {
377 0 : goto err;
378 : }
379 :
380 : // Setup ODR and performance. 100Hz ODR and 4 average for lownoise
381 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD_AGGR_SET, (BMM350_AVERAGING_4 | BMM350_ODR_100HZ))) {
382 0 : goto err;
383 : }
384 :
385 : // Update ODR and avg parameter
386 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_UPD_OAE)) {
387 0 : goto err;
388 : }
389 : // Wait at most 20ms for update ODR and avg paramter
390 0 : wait_pmu_cmd_ready(BMM350_PMU_CMD_UPD_OAE, 20);
391 :
392 : // Enable measurement of 3 axis
393 0 : if (!_dev->write_register(BMM350_REG_PMU_CMD_AXIS_EN, 0x07)) {
394 0 : goto err;
395 : }
396 :
397 : // Switch power mode to normal mode
398 0 : if (!set_power_mode(POWER_MODE_NORMAL)) {
399 0 : goto err;
400 : }
401 :
402 : // Lower retries for run
403 0 : _dev->set_retries(3);
404 :
405 0 : _dev->get_semaphore()->give();
406 :
407 : /* Register the compass instance in the frontend */
408 0 : _dev->set_device_type(DEVTYPE_BMM350);
409 0 : if (!register_compass(_dev->get_bus_id())) {
410 0 : return false;
411 : }
412 :
413 : // printf("BMM350: Found at address 0x%x as compass %u\n", _dev->get_bus_address(), _compass_instance);
414 :
415 0 : set_rotation(_rotation);
416 :
417 0 : if (_force_external) {
418 0 : set_external(true);
419 : }
420 :
421 : // Call timer() at 100Hz
422 0 : _dev->register_periodic_callback(1000000U/100U, FUNCTOR_BIND_MEMBER(&AP_Compass_BMM350::timer, void));
423 :
424 0 : return true;
425 :
426 0 : err:
427 0 : _dev->get_semaphore()->give();
428 0 : return false;
429 : }
430 :
431 0 : void AP_Compass_BMM350::timer()
432 : {
433 : struct PACKED {
434 : uint8_t magx[3];
435 : uint8_t magy[3];
436 : uint8_t magz[3];
437 : uint8_t temp[3];
438 : } data;
439 :
440 : // Read out measurement data
441 0 : if (!read_bytes(BMM350_REG_MAG_X_XLSB, (uint8_t *)&data, sizeof(data))) {
442 0 : return;
443 : }
444 :
445 : // Converts 24-bit raw data to signed integer value
446 0 : const int32_t magx_raw = int32_t(((uint32_t)data.magx[0] << 8) + ((uint32_t)data.magx[1] << 16) + ((uint32_t)data.magx[2] << 24)) >> 8;
447 0 : const int32_t magy_raw = int32_t(((uint32_t)data.magy[0] << 8) + ((uint32_t)data.magy[1] << 16) + ((uint32_t)data.magy[2] << 24)) >> 8;
448 0 : const int32_t magz_raw = int32_t(((uint32_t)data.magz[0] << 8) + ((uint32_t)data.magz[1] << 16) + ((uint32_t)data.magz[2] << 24)) >> 8;
449 0 : const int32_t temp_raw = int32_t(((uint32_t)data.temp[0] << 8) + ((uint32_t)data.temp[1] << 16) + ((uint32_t)data.temp[2] << 24)) >> 8;
450 :
451 : // Convert mag lsb to uT and temp lsb to degC
452 0 : float magx = (float)magx_raw * BMM350_XY_SCALE;
453 0 : float magy = (float)magy_raw * BMM350_XY_SCALE;
454 0 : float magz = (float)magz_raw * BMM350_Z_SCALE;
455 0 : float temp = ((float)temp_raw * BMM350_TEMP_SCALE) - 25.49f;
456 :
457 : // Apply compensation
458 0 : temp = ((1 + _mag_comp.sensit_coef.temp) * temp) + _mag_comp.offset_coef.temp;
459 : // Compensate raw magnetic data
460 0 : magx = ((1 + _mag_comp.sensit_coef.x) * magx) + _mag_comp.offset_coef.x + (_mag_comp.tco.x * (temp - _mag_comp.t0_reading));
461 0 : magx /= 1 + _mag_comp.tcs.x * (temp - _mag_comp.t0_reading);
462 :
463 0 : magy = ((1 + _mag_comp.sensit_coef.y) * magy) + _mag_comp.offset_coef.y + (_mag_comp.tco.y * (temp - _mag_comp.t0_reading));
464 0 : magy /= 1 + _mag_comp.tcs.y * (temp - _mag_comp.t0_reading);
465 :
466 0 : magz = ((1 + _mag_comp.sensit_coef.z) * magz) + _mag_comp.offset_coef.z + (_mag_comp.tco.z * (temp - _mag_comp.t0_reading));
467 0 : magz /= 1 + _mag_comp.tcs.z * (temp - _mag_comp.t0_reading);
468 :
469 0 : const float cr_ax_comp_x = (magx - _mag_comp.cross_axis.cross_x_y * magy) / (1 - _mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_x_y);
470 0 : const float cr_ax_comp_y = (magy - _mag_comp.cross_axis.cross_y_x * magx) / (1 - _mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_x_y);
471 0 : const float cr_ax_comp_z = (magz + (magx * (_mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_z_y - _mag_comp.cross_axis.cross_z_x) -
472 0 : magy * (_mag_comp.cross_axis.cross_z_y - _mag_comp.cross_axis.cross_x_y * _mag_comp.cross_axis.cross_z_x)) /
473 0 : (1 - _mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_x_y));
474 :
475 : // Store in field vector and convert uT to milligauss
476 0 : Vector3f field { cr_ax_comp_x * 10.0f, cr_ax_comp_y * 10.0f, cr_ax_comp_z * 10.0f };
477 0 : accumulate_sample(field);
478 : }
479 :
480 : #endif // AP_COMPASS_BMM350_ENABLED
|
