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 * 21 * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $ 22 * 23 *****************************************************************************/ 24 25/** 26 * @defgroup ML_SUPERVISOR 27 * @brief Basic sensor fusion supervisor functionalities. 28 * 29 * @{ 30 * @file mlsupervisor.c 31 * @brief Basic sensor fusion supervisor functionalities. 32 */ 33 34#include "ml.h" 35#include "mldl.h" 36#include "mldl_cfg.h" 37#include "mltypes.h" 38#include "mlinclude.h" 39#include "compass.h" 40#include "pressure.h" 41#include "dmpKey.h" 42#include "dmpDefault.h" 43#include "mlstates.h" 44#include "mlFIFO.h" 45#include "mlFIFOHW.h" 46#include "mlMathFunc.h" 47#include "mlsupervisor.h" 48#include "mlmath.h" 49 50#include "mlsl.h" 51#include "mlos.h" 52 53#include <log.h> 54#undef MPL_LOG_TAG 55#define MPL_LOG_TAG "MPL-sup" 56 57static unsigned long lastCompassTime = 0; 58static unsigned long polltime = 0; 59static unsigned long coiltime = 0; 60static int accCount = 0; 61static int compassCalStableCount = 0; 62static int compassCalCount = 0; 63static int coiltimerstart = 0; 64static unsigned long disturbtime = 0; 65static int disturbtimerstart = 0; 66 67static yas_filter_if_s f; 68static yas_filter_handle_t handle; 69 70#define SUPERVISOR_DEBUG 0 71 72struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 }; 73 74/** 75 * @brief This initializes all variables that should be reset on 76 */ 77void inv_init_sensor_fusion_supervisor(void) 78{ 79 lastCompassTime = 0; 80 polltime = 0; 81 inv_obj.acc_state = SF_STARTUP_SETTLE; 82 accCount = 0; 83 compassCalStableCount = 0; 84 compassCalCount = 0; 85 86 yas_filter_init(&f); 87 f.init(&handle); 88 89#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 90 defined CONFIG_MPU_SENSORS_MPU6050B1 91 if (inv_compass_present()) { 92 struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 93 if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) { 94 (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT); 95 } 96 } 97#endif 98 99 if (ml_supervisor_cb.supervisor_reset_func != NULL) { 100 ml_supervisor_cb.supervisor_reset_func(); 101 } 102} 103 104static int MLUpdateCompassCalibration3DOF(int command, long *data, 105 unsigned long deltaTime) 106{ 107 INVENSENSE_FUNC_START; 108 int retValue = INV_SUCCESS; 109 static float m[10][10] = { {0} }; 110 float mInv[10][10] = { {0} }; 111 float mTmp[10][10] = { {0} }; 112 static float xTransY[4] = { 0 }; 113 float magSqr = 0; 114 float inpData[3] = { 0 }; 115 int i, j; 116 int a, b; 117 float d; 118 119 switch (command) { 120 case CAL_ADD_DATA: 121 inpData[0] = (float)data[0]; 122 inpData[1] = (float)data[1]; 123 inpData[2] = (float)data[2]; 124 m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]); 125 m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]); 126 m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]); 127 m[0][3] += (-2 * inpData[0]); 128 m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]); 129 m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]); 130 m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]); 131 m[1][3] += (-2 * inpData[1]); 132 m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]); 133 m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]); 134 m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]); 135 m[2][3] += (-2 * inpData[2]); 136 m[3][0] += (-2 * inpData[0]); 137 m[3][1] += (-2 * inpData[1]); 138 m[3][2] += (-2 * inpData[2]); 139 m[3][3] += 1.0f; 140 magSqr = 141 inpData[0] * inpData[0] + inpData[1] * inpData[1] + 142 inpData[2] * inpData[2]; 143 xTransY[0] += (-2 * inpData[0]) * magSqr; 144 xTransY[1] += (-2 * inpData[1]) * magSqr; 145 xTransY[2] += (-2 * inpData[2]) * magSqr; 146 xTransY[3] += magSqr; 147 break; 148 case CAL_RUN: 149 a = 4; 150 b = a; 151 for (i = 0; i < b; i++) { 152 for (j = 0; j < b; j++) { 153 a = b; 154 inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j); 155 mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a); 156 } 157 } 158 a = b; 159 d = inv_matrix_det(&m[0][0], &a); 160 if (d == 0) { 161 return INV_ERROR; 162 } 163 for (i = 0; i < 3; i++) { 164 float tmp = 0; 165 for (j = 0; j < 4; j++) { 166 tmp += mInv[j][i] / d * xTransY[j]; 167 } 168 inv_obj.compass_test_bias[i] = 169 -(long)(tmp * inv_obj.compass_sens / 16384.0f); 170 } 171 break; 172 case CAL_RESET: 173 for (i = 0; i < 4; i++) { 174 for (j = 0; j < 4; j++) { 175 m[i][j] = 0; 176 } 177 xTransY[i] = 0; 178 } 179 default: 180 break; 181 } 182 return retValue; 183} 184 185/** 186 * Entry point for Sensor Fusion operations 187 * @internal 188 * @param magFB magnetormeter FB 189 * @param accSF accelerometer SF 190 */ 191static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF, 192 unsigned long deltaTime) 193{ 194 static long prevCompassBias[3] = { 0 }; 195 static long magMax[3] = { 196 -1073741824L, 197 -1073741824L, 198 -1073741824L 199 }; 200 static long magMin[3] = { 201 1073741824L, 202 1073741824L, 203 1073741824L 204 }; 205 int gyroMag; 206 long accMag; 207 inv_error_t result; 208 int i; 209 210 if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) { 211 ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime); 212 } 213 214 gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT; 215 accMag = inv_accel_sum_of_sqr(); 216 217 // Scaling below assumes certain scaling. 218#if ACC_MAG_SQR_SHIFT != 16 219#error 220#endif 221 222 if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) { 223 result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime); 224 if (result) { 225 LOG_RESULT_LOCATION(result); 226 return result; 227 } 228 } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) { 229 //Most basic compass calibration, used only with lite MPL 230 if (inv_obj.resetting_compass == 1) { 231 for (i = 0; i < 3; i++) { 232 magMax[i] = -1073741824L; 233 magMin[i] = 1073741824L; 234 prevCompassBias[i] = 0; 235 } 236 compassCalStableCount = 0; 237 compassCalCount = 0; 238 inv_obj.resetting_compass = 0; 239 } 240 if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) { 241 if (compassCalStableCount < 1000) { 242 for (i = 0; i < 3; i++) { 243 if (inv_obj.compass_sensor_data[i] > magMax[i]) { 244 magMax[i] = inv_obj.compass_sensor_data[i]; 245 } 246 if (inv_obj.compass_sensor_data[i] < magMin[i]) { 247 magMin[i] = inv_obj.compass_sensor_data[i]; 248 } 249 } 250 MLUpdateCompassCalibration3DOF(CAL_ADD_DATA, 251 inv_obj.compass_sensor_data, 252 deltaTime); 253 compassCalStableCount += deltaTime; 254 for (i = 0; i < 3; i++) { 255 if (magMax[i] - magMin[i] < 256 (long long)40 * 1073741824 / inv_obj.compass_sens) { 257 compassCalStableCount = 0; 258 } 259 } 260 } else { 261 if (compassCalStableCount >= 1000) { 262 if (MLUpdateCompassCalibration3DOF 263 (CAL_RUN, inv_obj.compass_sensor_data, 264 deltaTime) == INV_SUCCESS) { 265 inv_obj.got_compass_bias = 1; 266 inv_obj.compass_accuracy = 3; 267 for(i=0; i<3; i++) { 268 inv_obj.compass_bias_error[i] = 35; 269 } 270 if (inv_obj.compass_state == SF_UNCALIBRATED) 271 inv_obj.compass_state = SF_STARTUP_SETTLE; 272 inv_set_compass_bias(inv_obj.compass_test_bias); 273 } 274 compassCalCount = 0; 275 compassCalStableCount = 0; 276 for (i = 0; i < 3; i++) { 277 magMax[i] = -1073741824L; 278 magMin[i] = 1073741824L; 279 prevCompassBias[i] = 0; 280 } 281 MLUpdateCompassCalibration3DOF(CAL_RESET, 282 inv_obj.compass_sensor_data, 283 deltaTime); 284 } 285 } 286 } 287 compassCalCount += deltaTime; 288 if (compassCalCount > 20000) { 289 compassCalCount = 0; 290 compassCalStableCount = 0; 291 for (i = 0; i < 3; i++) { 292 magMax[i] = -1073741824L; 293 magMin[i] = 1073741824L; 294 prevCompassBias[i] = 0; 295 } 296 MLUpdateCompassCalibration3DOF(CAL_RESET, 297 inv_obj.compass_sensor_data, 298 deltaTime); 299 } 300 } 301 302 if (inv_obj.acc_state != SF_STARTUP_SETTLE) { 303 if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) { 304 inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing 305 accCount = 0; 306 } else { 307 if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214) 308 && ((inv_obj.acc_state == SF_DISTURBANCE) 309 || (inv_obj.acc_state == SF_SLOW_SETTLE))) { 310 accCount += deltaTime; 311 if (accCount > 0) { 312 inv_obj.acc_state = SF_FAST_SETTLE; 313 accCount = 0; 314 } 315 } else { 316 if (inv_obj.acc_state == SF_DISTURBANCE) { 317 accCount += deltaTime; 318 if (accCount > 500) { 319 inv_obj.acc_state = SF_SLOW_SETTLE; 320 accCount = 0; 321 } 322 } else if (inv_obj.acc_state == SF_SLOW_SETTLE) { 323 accCount += deltaTime; 324 if (accCount > 1000) { 325 inv_obj.acc_state = SF_NORMAL; 326 accCount = 0; 327 } 328 } else if (inv_obj.acc_state == SF_FAST_SETTLE) { 329 accCount += deltaTime; 330 if (accCount > 250) { 331 inv_obj.acc_state = SF_NORMAL; 332 accCount = 0; 333 } 334 } 335 } 336 } 337 } 338 if (inv_obj.acc_state == SF_STARTUP_SETTLE) { 339 accCount += deltaTime; 340 if (accCount > 50) { 341 *accSF = 1073741824; //Startup settling 342 inv_obj.acc_state = SF_NORMAL; 343 accCount = 0; 344 } 345 } else if ((inv_obj.acc_state == SF_NORMAL) 346 || (inv_obj.acc_state == SF_SLOW_SETTLE)) { 347 *accSF = inv_obj.accel_sens * 64; //Normal 348 } else if ((inv_obj.acc_state == SF_DISTURBANCE)) { 349 *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0) 350 } else if (inv_obj.acc_state == SF_FAST_SETTLE) { 351 *accSF = inv_obj.accel_sens * 512; //Amplify accel 352 } 353 if (!inv_get_gyro_present()) { 354 *accSF = inv_obj.accel_sens * 128; 355 } 356 return INV_SUCCESS; 357} 358 359/** 360 * @brief Entry point for software sensor fusion operations. 361 * Manages hardware interaction, calls sensor fusion supervisor for 362 * bias calculation. 363 * @return error code. INV_SUCCESS if no error occurred. 364 */ 365inv_error_t inv_accel_compass_supervisor(void) 366{ 367 inv_error_t result; 368 int adjustSensorFusion = 0; 369 long accSF = 1073741824; 370 static double magFB = 0; 371 long magSensorData[3]; 372 float fcin[3]; 373 float fcout[3]; 374 375 376 if (inv_compass_present()) { /* check for compass data */ 377 int i, j; 378 long long tmp[3] = { 0 }; 379 long long tmp64 = 0; 380 unsigned long ctime = inv_get_tick_count(); 381 if (((inv_get_compass_id() == COMPASS_ID_AK8975) && 382 ((ctime - polltime) > 20)) || 383 (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max 384 if (SUPERVISOR_DEBUG) { 385 MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n"); 386 MPL_LOGV("delta time = %ld\n", ctime - polltime); 387 } 388 polltime = ctime; 389 result = inv_get_compass_data(magSensorData); 390 /* external slave wants the data even if there is an error */ 391 if (result && !inv_obj.external_slave_callback) { 392 if (SUPERVISOR_DEBUG) { 393 MPL_LOGW("inv_get_compass_data returned %d\n", result); 394 } 395 } else { 396 unsigned long compassTime = inv_get_tick_count(); 397 unsigned long deltaTime = 1; 398 399 if (result == INV_SUCCESS) { 400 if (lastCompassTime != 0) { 401 deltaTime = compassTime - lastCompassTime; 402 } 403 lastCompassTime = compassTime; 404 adjustSensorFusion = 1; 405 if (inv_obj.got_init_compass_bias == 0) { 406 inv_obj.got_init_compass_bias = 1; 407 for (i = 0; i < 3; i++) { 408 inv_obj.init_compass_bias[i] = magSensorData[i]; 409 } 410 } 411 for (i = 0; i < 3; i++) { 412 inv_obj.compass_sensor_data[i] = (long)magSensorData[i]; 413 inv_obj.compass_sensor_data[i] -= 414 inv_obj.init_compass_bias[i]; 415 tmp[i] = ((long long)inv_obj.compass_sensor_data[i]) 416 * inv_obj.compass_sens / 16384; 417 tmp[i] -= inv_obj.compass_bias[i]; 418 tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L; 419 } 420 for (i = 0; i < 3; i++) { 421 tmp64 = 0; 422 for (j = 0; j < 3; j++) { 423 tmp64 += (long long) tmp[j] * 424 inv_obj.compass_cal[i * 3 + j]; 425 } 426 inv_obj.compass_calibrated_data[i] = 427 (long) (tmp64 / inv_obj.compass_sens); 428 } 429 //Additions: 430 if ((inv_obj.compass_state == 1) && 431 (inv_obj.compass_overunder == 0)) { 432 if (disturbtimerstart == 0) { 433 disturbtimerstart = 1; 434 disturbtime = ctime; 435 } 436 } else { 437 disturbtimerstart = 0; 438 } 439 440 if (inv_obj.compass_overunder) { 441 if (coiltimerstart == 0) { 442 coiltimerstart = 1; 443 coiltime = ctime; 444 } 445 } 446 if (coiltimerstart == 1) { 447 if (ctime - coiltime > 3000) { 448 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; 449 inv_set_compass_offset(); 450 inv_reset_compass_calibration(); 451 coiltimerstart = 0; 452 } 453 } 454 455 if (disturbtimerstart == 1) { 456 if (ctime - disturbtime > 10000) { 457 inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; 458 inv_set_compass_offset(); 459 inv_reset_compass_calibration(); 460 MPL_LOGI("Compass reset! inv_reset_compass_calibration \n"); 461 disturbtimerstart = 0; 462 } 463 } 464 } 465 if (inv_obj.external_slave_callback) { 466 result = inv_obj.external_slave_callback(&inv_obj); 467 if (result) { 468 LOG_RESULT_LOCATION(result); 469 return result; 470 } 471 } 472 473#ifdef APPLY_COMPASS_FILTER 474 if (inv_get_compass_id() == COMPASS_ID_YAS530) 475 { 476 fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); 477 fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); 478 fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); 479 480 f.update(&handle, fcin, fcout); 481 482 inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); 483 inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); 484 inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); 485 } 486#endif 487 488 if (SUPERVISOR_DEBUG) { 489 MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n", 490 (float)inv_obj.compass_calibrated_data[0] / 491 65536.f, 492 (float)inv_obj.compass_calibrated_data[1] / 493 65536.f, 494 (float)inv_obj.compass_calibrated_data[2] / 495 65536.f); 496 } 497 magFB = 1.0; 498 adjustSensorFusion = 1; 499 result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); 500 if (result) { 501 LOG_RESULT_LOCATION(result); 502 return result; 503 } 504 } 505 } 506 } else { 507 //No compass, but still modify accel 508 unsigned long ctime = inv_get_tick_count(); 509 if (polltime == 0 || ((ctime - polltime) > 80)) { // at the beginning AND every 1/8 second 510 unsigned long deltaTime = 1; 511 adjustSensorFusion = 1; 512 magFB = 0; 513 if (polltime != 0) { 514 deltaTime = ctime - polltime; 515 } 516 MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); 517 polltime = ctime; 518 } 519 } 520 if (adjustSensorFusion == 1) { 521 unsigned char regs[4]; 522 static int prevAccSF = 1; 523 524 if (accSF != prevAccSF) { 525 regs[0] = (unsigned char)((accSF >> 24) & 0xff); 526 regs[1] = (unsigned char)((accSF >> 16) & 0xff); 527 regs[2] = (unsigned char)((accSF >> 8) & 0xff); 528 regs[3] = (unsigned char)(accSF & 0xff); 529 result = inv_set_mpu_memory(KEY_D_0_96, 4, regs); 530 if (result) { 531 LOG_RESULT_LOCATION(result); 532 return result; 533 } 534 prevAccSF = accSF; 535 } 536 } 537 538 if (ml_supervisor_cb.accel_compass_fusion_func != NULL) 539 ml_supervisor_cb.accel_compass_fusion_func(magFB); 540 541 return INV_SUCCESS; 542} 543 544/** 545 * @brief Entry point for software sensor fusion operations. 546 * Manages hardware interaction, calls sensor fusion supervisor for 547 * bias calculation. 548 * @return INV_SUCCESS or non-zero error code on error. 549 */ 550inv_error_t inv_pressure_supervisor(void) 551{ 552 long pressureSensorData[1]; 553 static unsigned long pressurePolltime = 0; 554 if (inv_pressure_present()) { /* check for pressure data */ 555 unsigned long ctime = inv_get_tick_count(); 556 if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second 557 if (SUPERVISOR_DEBUG) { 558 MPL_LOGV("Fetch pressure data\n"); 559 MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime); 560 } 561 pressurePolltime = ctime; 562 if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) { 563 inv_obj.pressure = pressureSensorData[0]; 564 } 565 } 566 } 567 return INV_SUCCESS; 568} 569 570/** 571 * @brief Resets the magnetometer calibration algorithm. 572 * @return INV_SUCCESS if successful, or non-zero error code otherwise. 573 */ 574inv_error_t inv_reset_compass_calibration(void) 575{ 576 if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) { 577 if (ml_supervisor_cb.reset_advanced_compass_func != NULL) 578 ml_supervisor_cb.reset_advanced_compass_func(); 579 } 580 MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1); 581 582 inv_obj.compass_bias_error[0] = P_INIT; 583 inv_obj.compass_bias_error[1] = P_INIT; 584 inv_obj.compass_bias_error[2] = P_INIT; 585 inv_obj.compass_accuracy = 0; 586 587 inv_obj.got_compass_bias = 0; 588 inv_obj.got_init_compass_bias = 0; 589 inv_obj.compass_state = SF_UNCALIBRATED; 590 inv_obj.resetting_compass = 1; 591 592 return INV_SUCCESS; 593} 594 595/** 596 * @} 597 */ 598