1 /*
2 $License:
3 Copyright 2011 InvenSense, Inc.
4
5 Licensed under the Apache License, Version 2.0 (the "License");
6 you may not use this file except in compliance with the License.
7 You may obtain a copy of the License at
8
9 http://www.apache.org/licenses/LICENSE-2.0
10
11 Unless required by applicable law or agreed to in writing, software
12 distributed under the License is distributed on an "AS IS" BASIS,
13 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 See the License for the specific language governing permissions and
15 limitations under the License.
16 $
17 */
18 /*******************************************************************************
19 *
20 * $Id: compass.c 5641 2011-06-14 02:10:02Z mcaramello $
21 *
22 *******************************************************************************/
23
24 /**
25 * @defgroup COMPASSDL
26 * @brief Motion Library - Compass Driver Layer.
27 * Provides the interface to setup and handle an compass
28 * connected to either the primary or the seconday I2C interface
29 * of the gyroscope.
30 *
31 * @{
32 * @file compass.c
33 * @brief Compass setup and handling methods.
34 */
35
36 /* ------------------ */
37 /* - Include Files. - */
38 /* ------------------ */
39
40 #include <string.h>
41 #include <stdlib.h>
42 #include "compass.h"
43
44 #include "ml.h"
45 #include "mlinclude.h"
46 #include "dmpKey.h"
47 #include "mlFIFO.h"
48 #include "mldl.h"
49 #include "mldl_cfg.h"
50 #include "mlMathFunc.h"
51 #include "mlsl.h"
52 #include "mlos.h"
53
54 #include "log.h"
55 #undef MPL_LOG_TAG
56 #define MPL_LOG_TAG "MPL-compass"
57
58 #define COMPASS_DEBUG 0
59
60 /* --------------------- */
61 /* - Global Variables. - */
62 /* --------------------- */
63
64 /* --------------------- */
65 /* - Static Variables. - */
66 /* --------------------- */
67
68 /* --------------- */
69 /* - Prototypes. - */
70 /* --------------- */
71
72 /* -------------- */
73 /* - Externs. - */
74 /* -------------- */
75
76 /* -------------- */
77 /* - Functions. - */
78 /* -------------- */
79
square(float data)80 static float square(float data)
81 {
82 return data * data;
83 }
84
adaptive_filter_init(struct yas_adaptive_filter * adap_filter,int len,float noise)85 static void adaptive_filter_init(struct yas_adaptive_filter *adap_filter, int len, float noise)
86 {
87 int i;
88
89 adap_filter->num = 0;
90 adap_filter->index = 0;
91 adap_filter->noise = noise;
92 adap_filter->len = len;
93
94 for (i = 0; i < adap_filter->len; ++i) {
95 adap_filter->sequence[i] = 0;
96 }
97 }
98
cmpfloat(const void * p1,const void * p2)99 static int cmpfloat(const void *p1, const void *p2)
100 {
101 return *(float*)p1 - *(float*)p2;
102 }
103
104
adaptive_filter_filter(struct yas_adaptive_filter * adap_filter,float in)105 static float adaptive_filter_filter(struct yas_adaptive_filter *adap_filter, float in)
106 {
107 float avg, sum, median, sorted[YAS_DEFAULT_FILTER_LEN];
108 int i;
109
110 if (adap_filter->len <= 1) {
111 return in;
112 }
113 if (adap_filter->num < adap_filter->len) {
114 adap_filter->sequence[adap_filter->index++] = in;
115 adap_filter->num++;
116 return in;
117 }
118 if (adap_filter->len <= adap_filter->index) {
119 adap_filter->index = 0;
120 }
121 adap_filter->sequence[adap_filter->index++] = in;
122
123 avg = 0;
124 for (i = 0; i < adap_filter->len; i++) {
125 avg += adap_filter->sequence[i];
126 }
127 avg /= adap_filter->len;
128
129 memcpy(sorted, adap_filter->sequence, adap_filter->len * sizeof(float));
130 qsort(&sorted, adap_filter->len, sizeof(float), cmpfloat);
131 median = sorted[adap_filter->len/2];
132
133 sum = 0;
134 for (i = 0; i < adap_filter->len; i++) {
135 sum += square(avg - adap_filter->sequence[i]);
136 }
137 sum /= adap_filter->len;
138
139 if (sum <= adap_filter->noise) {
140 return median;
141 }
142
143 return ((in - avg) * (sum - adap_filter->noise) / sum + avg);
144 }
145
thresh_filter_init(struct yas_thresh_filter * thresh_filter,float threshold)146 static void thresh_filter_init(struct yas_thresh_filter *thresh_filter, float threshold)
147 {
148 thresh_filter->threshold = threshold;
149 thresh_filter->last = 0;
150 }
151
thresh_filter_filter(struct yas_thresh_filter * thresh_filter,float in)152 static float thresh_filter_filter(struct yas_thresh_filter *thresh_filter, float in)
153 {
154 if (in < thresh_filter->last - thresh_filter->threshold
155 || thresh_filter->last + thresh_filter->threshold < in) {
156 thresh_filter->last = in;
157 return in;
158 }
159 else {
160 return thresh_filter->last;
161 }
162 }
163
init(yas_filter_handle_t * t)164 static int init(yas_filter_handle_t *t)
165 {
166 float noise[] = {
167 YAS_DEFAULT_FILTER_NOISE,
168 YAS_DEFAULT_FILTER_NOISE,
169 YAS_DEFAULT_FILTER_NOISE,
170 };
171 int i;
172
173 if (t == NULL) {
174 return -1;
175 }
176
177 for (i = 0; i < 3; i++) {
178 adaptive_filter_init(&t->adap_filter[i], YAS_DEFAULT_FILTER_LEN, noise[i]);
179 thresh_filter_init(&t->thresh_filter[i], YAS_DEFAULT_FILTER_THRESH);
180 }
181
182 return 0;
183 }
184
update(yas_filter_handle_t * t,float * input,float * output)185 static int update(yas_filter_handle_t *t, float *input, float *output)
186 {
187 int i;
188
189 if (t == NULL || input == NULL || output == NULL) {
190 return -1;
191 }
192
193 for (i = 0; i < 3; i++) {
194 output[i] = adaptive_filter_filter(&t->adap_filter[i], input[i]);
195 output[i] = thresh_filter_filter(&t->thresh_filter[i], output[i]);
196 }
197
198 return 0;
199 }
200
yas_filter_init(yas_filter_if_s * f)201 int yas_filter_init(yas_filter_if_s *f)
202 {
203 if (f == NULL) {
204 return -1;
205 }
206 f->init = init;
207 f->update = update;
208
209 return 0;
210 }
211
212 /**
213 * @brief Used to determine if a compass is
214 * configured and used by the MPL.
215 * @return INV_SUCCESS if the compass is present.
216 */
inv_compass_present(void)217 unsigned char inv_compass_present(void)
218 {
219 INVENSENSE_FUNC_START;
220 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
221 if (NULL != mldl_cfg->compass &&
222 NULL != mldl_cfg->compass->resume &&
223 mldl_cfg->requested_sensors & INV_THREE_AXIS_COMPASS)
224 return TRUE;
225 else
226 return FALSE;
227 }
228
229 /**
230 * @brief Query the compass slave address.
231 * @return The 7-bit compass slave address.
232 */
inv_get_compass_slave_addr(void)233 unsigned char inv_get_compass_slave_addr(void)
234 {
235 INVENSENSE_FUNC_START;
236 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
237 if (NULL != mldl_cfg->pdata)
238 return mldl_cfg->pdata->compass.address;
239 else
240 return 0;
241 }
242
243 /**
244 * @brief Get the ID of the compass in use.
245 * @return ID of the compass in use.
246 */
inv_get_compass_id(void)247 unsigned short inv_get_compass_id(void)
248 {
249 INVENSENSE_FUNC_START;
250 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
251 if (NULL != mldl_cfg->compass) {
252 return mldl_cfg->compass->id;
253 }
254 return ID_INVALID;
255 }
256
257 /**
258 * @brief Get a sample of compass data from the device.
259 * @param data
260 * the buffer to store the compass raw data for
261 * X, Y, and Z axes.
262 * @return INV_SUCCESS or a non-zero error code.
263 */
inv_get_compass_data(long * data)264 inv_error_t inv_get_compass_data(long *data)
265 {
266 inv_error_t result;
267 int ii;
268 struct mldl_cfg *mldl_cfg = inv_get_dl_config();
269
270 unsigned char *tmp = inv_obj.compass_raw_data;
271
272 if (mldl_cfg->compass->read_len > sizeof(inv_obj.compass_raw_data)) {
273 LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
274 return INV_ERROR_INVALID_CONFIGURATION;
275 }
276
277 if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_PRIMARY ||
278 !(mldl_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
279 /*--- read the compass sensor data.
280 The compass read function may return an INV_ERROR_COMPASS_* errors
281 when the data is not ready (read/refresh frequency mismatch) or
282 the internal data sampling timing of the device was not respected.
283 Returning the error code will make the sensor fusion supervisor
284 ignore this compass data sample. ---*/
285 result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
286 inv_get_serial_handle(),
287 inv_get_serial_handle(),
288 tmp);
289 if (result) {
290 if (COMPASS_DEBUG) {
291 MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
292 }
293 return result;
294 }
295 for (ii = 0; ii < 3; ii++) {
296 if (EXT_SLAVE_BIG_ENDIAN == mldl_cfg->compass->endian)
297 data[ii] =
298 ((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
299 else
300 data[ii] =
301 ((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
302 }
303
304 inv_obj.compass_overunder = (int)tmp[6];
305
306 } else {
307 #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \
308 defined CONFIG_MPU_SENSORS_MPU6050B1
309 result = inv_get_external_sensor_data(data, 3);
310 if (result) {
311 LOG_RESULT_LOCATION(result);
312 return result;
313 }
314 #if defined CONFIG_MPU_SENSORS_MPU6050A2
315 {
316 static unsigned char first = TRUE;
317 // one-off write to AKM
318 if (first) {
319 unsigned char regs[] = {
320 // beginning Mantis register for one-off slave R/W
321 MPUREG_I2C_SLV4_ADDR,
322 // the slave to write to
323 mldl_cfg->pdata->compass.address,
324 // the register to write to
325 /*mldl_cfg->compass->trigger->reg */ 0x0A,
326 // the value to write
327 /*mldl_cfg->compass->trigger->value */ 0x01,
328 // enable the write
329 0xC0
330 };
331 result =
332 inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
333 ARRAY_SIZE(regs), regs);
334 first = FALSE;
335 } else {
336 unsigned char regs[] = {
337 MPUREG_I2C_SLV4_CTRL,
338 0xC0
339 };
340 result =
341 inv_serial_write(inv_get_serial_handle(), mldl_cfg->addr,
342 ARRAY_SIZE(regs), regs);
343 }
344 }
345 #endif
346 #else
347 return INV_ERROR_INVALID_CONFIGURATION;
348 #endif // CONFIG_MPU_SENSORS_xxxx
349 }
350 data[0] = inv_q30_mult(data[0], inv_obj.compass_asa[0]);
351 data[1] = inv_q30_mult(data[1], inv_obj.compass_asa[1]);
352 data[2] = inv_q30_mult(data[2], inv_obj.compass_asa[2]);
353
354 return INV_SUCCESS;
355 }
356
357 /**
358 * @brief Sets the compass bias.
359 * @param bias
360 * Compass bias, length 3. Scale is micro Tesla's * 2^16.
361 * Frame is mount frame which may be different from body frame.
362 * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
363 */
inv_set_compass_bias(long * bias)364 inv_error_t inv_set_compass_bias(long *bias)
365 {
366 inv_error_t result = INV_SUCCESS;
367 long biasC[3];
368 struct mldl_cfg *mldlCfg = inv_get_dl_config();
369
370 inv_obj.compass_bias[0] = bias[0];
371 inv_obj.compass_bias[1] = bias[1];
372 inv_obj.compass_bias[2] = bias[2];
373
374 // Find Bias in units of the raw data scaled by 2^16 in chip mounting frame
375 biasC[0] =
376 (long)(bias[0] * (1LL << 30) / inv_obj.compass_sens) +
377 inv_obj.init_compass_bias[0] * (1L << 16);
378 biasC[1] =
379 (long)(bias[1] * (1LL << 30) / inv_obj.compass_sens) +
380 inv_obj.init_compass_bias[1] * (1L << 16);
381 biasC[2] =
382 (long)(bias[2] * (1LL << 30) / inv_obj.compass_sens) +
383 inv_obj.init_compass_bias[2] * (1L << 16);
384
385 if (inv_dmpkey_supported(KEY_CPASS_BIAS_X)) {
386 unsigned char reg[4];
387 long biasB[3];
388 signed char *orC = mldlCfg->pdata->compass.orientation;
389
390 // Now transform to body frame
391 biasB[0] = biasC[0] * orC[0] + biasC[1] * orC[1] + biasC[2] * orC[2];
392 biasB[1] = biasC[0] * orC[3] + biasC[1] * orC[4] + biasC[2] * orC[5];
393 biasB[2] = biasC[0] * orC[6] + biasC[1] * orC[7] + biasC[2] * orC[8];
394
395 result =
396 inv_set_mpu_memory(KEY_CPASS_BIAS_X, 4,
397 inv_int32_to_big8(biasB[0], reg));
398 result =
399 inv_set_mpu_memory(KEY_CPASS_BIAS_Y, 4,
400 inv_int32_to_big8(biasB[1], reg));
401 result =
402 inv_set_mpu_memory(KEY_CPASS_BIAS_Z, 4,
403 inv_int32_to_big8(biasB[2], reg));
404 }
405 return result;
406 }
407
408 /**
409 * @brief Write a single register on the compass slave device, regardless
410 * of the bus it is connected to and the MPU's configuration.
411 * @param reg
412 * the register to write to on the slave compass device.
413 * @param val
414 * the value to write.
415 * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
416 */
inv_compass_write_reg(unsigned char reg,unsigned char val)417 inv_error_t inv_compass_write_reg(unsigned char reg, unsigned char val)
418 {
419 struct ext_slave_config config;
420 unsigned char data[2];
421 inv_error_t result;
422
423 data[0] = reg;
424 data[1] = val;
425
426 config.key = MPU_SLAVE_WRITE_REGISTERS;
427 config.len = 2;
428 config.apply = TRUE;
429 config.data = data;
430
431 result = inv_mpu_config_compass(inv_get_dl_config(),
432 inv_get_serial_handle(),
433 inv_get_serial_handle(), &config);
434 if (result) {
435 LOG_RESULT_LOCATION(result);
436 return result;
437 }
438 return result;
439 }
440
441 /**
442 * @brief Read values from the compass slave device registers, regardless
443 * of the bus it is connected to and the MPU's configuration.
444 * @param reg
445 * the register to read from on the slave compass device.
446 * @param val
447 * a buffer of 3 elements to store the values read from the
448 * compass device.
449 * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
450 */
inv_compass_read_reg(unsigned char reg,unsigned char * val)451 inv_error_t inv_compass_read_reg(unsigned char reg, unsigned char *val)
452 {
453 struct ext_slave_config config;
454 unsigned char data[2];
455 inv_error_t result;
456
457 data[0] = reg;
458
459 config.key = MPU_SLAVE_READ_REGISTERS;
460 config.len = 2;
461 config.apply = TRUE;
462 config.data = data;
463
464 result = inv_mpu_get_compass_config(inv_get_dl_config(),
465 inv_get_serial_handle(),
466 inv_get_serial_handle(), &config);
467 if (result) {
468 LOG_RESULT_LOCATION(result);
469 return result;
470 }
471 *val = data[1];
472 return result;
473 }
474
475 /**
476 * @brief Read values from the compass slave device scale registers, regardless
477 * of the bus it is connected to and the MPU's configuration.
478 * @param reg
479 * the register to read from on the slave compass device.
480 * @param val
481 * a buffer of 3 elements to store the values read from the
482 * compass device.
483 * @return INV_SUCCESS = 0 if successful. A non-zero error code otherwise.
484 */
inv_compass_read_scale(long * val)485 inv_error_t inv_compass_read_scale(long *val)
486 {
487 struct ext_slave_config config;
488 unsigned char data[3];
489 inv_error_t result;
490
491 config.key = MPU_SLAVE_READ_SCALE;
492 config.len = 3;
493 config.apply = TRUE;
494 config.data = data;
495
496 result = inv_mpu_get_compass_config(inv_get_dl_config(),
497 inv_get_serial_handle(),
498 inv_get_serial_handle(), &config);
499 if (result) {
500 LOG_RESULT_LOCATION(result);
501 return result;
502 }
503 val[0] = ((data[0] - 128) << 22) + (1L << 30);
504 val[1] = ((data[1] - 128) << 22) + (1L << 30);
505 val[2] = ((data[2] - 128) << 22) + (1L << 30);
506 return result;
507 }
508
inv_set_compass_offset(void)509 inv_error_t inv_set_compass_offset(void)
510 {
511 struct ext_slave_config config;
512 unsigned char data[3];
513 inv_error_t result;
514
515 config.key = MPU_SLAVE_OFFSET_VALS;
516 config.len = 3;
517 config.apply = TRUE;
518 config.data = data;
519
520 if(inv_obj.flags[INV_COMPASS_OFFSET_VALID]) {
521 /* push stored values */
522 data[0] = (char)inv_obj.compass_offsets[0];
523 data[1] = (char)inv_obj.compass_offsets[1];
524 data[2] = (char)inv_obj.compass_offsets[2];
525 MPL_LOGI("push compass offsets %hhd, %hhd, %hhd", data[0], data[1], data[2]);
526 result = inv_mpu_config_compass(inv_get_dl_config(),
527 inv_get_serial_handle(),
528 inv_get_serial_handle(), &config);
529 } else {
530 /* compute new values and store them */
531 result = inv_mpu_get_compass_config(inv_get_dl_config(),
532 inv_get_serial_handle(),
533 inv_get_serial_handle(), &config);
534 MPL_LOGI("pulled compass offsets %hhd %hhd %hhd", data[0], data[1], data[2]);
535 if(result == INV_SUCCESS) {
536 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 1;
537 inv_obj.compass_offsets[0] = data[0];
538 inv_obj.compass_offsets[1] = data[1];
539 inv_obj.compass_offsets[2] = data[2];
540 }
541 }
542
543 if (result) {
544 LOG_RESULT_LOCATION(result);
545 return result;
546 }
547
548 return result;
549 }
550
inv_compass_check_range(void)551 inv_error_t inv_compass_check_range(void)
552 {
553 struct ext_slave_config config;
554 unsigned char data[3];
555 inv_error_t result;
556
557 config.key = MPU_SLAVE_RANGE_CHECK;
558 config.len = 3;
559 config.apply = TRUE;
560 config.data = data;
561
562 result = inv_mpu_get_compass_config(inv_get_dl_config(),
563 inv_get_serial_handle(),
564 inv_get_serial_handle(), &config);
565 if (result) {
566 LOG_RESULT_LOCATION(result);
567 return result;
568 }
569
570 if(data[0] || data[1] || data[2]) {
571 /* some value clipped */
572 return INV_ERROR_COMPASS_DATA_ERROR;
573 }
574 return INV_SUCCESS;
575 }
576
577 /**
578 * @}
579 */
580