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 80 static float square(float data) 81 { 82 return data * data; 83 } 84 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 99 static int cmpfloat(const void *p1, const void *p2) 100 { 101 return *(float*)p1 - *(float*)p2; 102 } 103 104 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 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 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 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 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 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 */ 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 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 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
