1895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/* 2895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $License: 3895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Copyright 2011 InvenSense, Inc. 4895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 5895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Licensed under the Apache License, Version 2.0 (the "License"); 6895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall you may not use this file except in compliance with the License. 7895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall You may obtain a copy of the License at 8895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 9895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall http://www.apache.org/licenses/LICENSE-2.0 10895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 11895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall Unless required by applicable law or agreed to in writing, software 12895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall distributed under the License is distributed on an "AS IS" BASIS, 13895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall See the License for the specific language governing permissions and 15895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall limitations under the License. 16895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall $ 17895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 18895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 19895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/****************************************************************************** 20895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 21895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $ 22895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 23895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *****************************************************************************/ 24895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 25895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 26895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @defgroup ML_SUPERVISOR 27895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Basic sensor fusion supervisor functionalities. 28895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * 29895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @{ 30895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @file mlsupervisor.c 31895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Basic sensor fusion supervisor functionalities. 32895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 33895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 34895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "ml.h" 35895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl.h" 36895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mldl_cfg.h" 37895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mltypes.h" 38895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlinclude.h" 39895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "compass.h" 40895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "pressure.h" 41895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpKey.h" 42895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "dmpDefault.h" 43895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlstates.h" 44895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFO.h" 45895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlFIFOHW.h" 46895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlMathFunc.h" 47895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsupervisor.h" 48895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlmath.h" 49895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 50895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlsl.h" 51895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include "mlos.h" 52895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 53895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#include <log.h> 54895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#undef MPL_LOG_TAG 55895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define MPL_LOG_TAG "MPL-sup" 56895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 57895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned long lastCompassTime = 0; 58895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned long polltime = 0; 59895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned long coiltime = 0; 60895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int accCount = 0; 61895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int compassCalStableCount = 0; 62895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int compassCalCount = 0; 63895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int coiltimerstart = 0; 64895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic unsigned long disturbtime = 0; 65895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int disturbtimerstart = 0; 66895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 67895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic yas_filter_if_s f; 68895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic yas_filter_handle_t handle; 69895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 70895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#define SUPERVISOR_DEBUG 0 71895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 72895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstruct inv_supervisor_cb_obj ml_supervisor_cb = { 0 }; 73895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 74895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 75895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief This initializes all variables that should be reset on 76895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 77895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallvoid inv_init_sensor_fusion_supervisor(void) 78895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 79895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lastCompassTime = 0; 80895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall polltime = 0; 81895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_STARTUP_SETTLE; 82895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 83895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalStableCount = 0; 84895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalCount = 0; 85895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 86895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall yas_filter_init(&f); 87895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall f.init(&handle); 88895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 89895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ 90895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall defined CONFIG_MPU_SENSORS_MPU6050B1 91895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_compass_present()) { 92895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall struct mldl_cfg *mldl_cfg = inv_get_dl_config(); 93895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) { 94895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT); 95895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 96895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 97895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 98895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 99895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ml_supervisor_cb.supervisor_reset_func != NULL) { 100895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ml_supervisor_cb.supervisor_reset_func(); 101895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 102895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 103895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 104895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic int MLUpdateCompassCalibration3DOF(int command, long *data, 105895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long deltaTime) 106895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 107895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall INVENSENSE_FUNC_START; 108895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int retValue = INV_SUCCESS; 109895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static float m[10][10] = { {0} }; 110895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float mInv[10][10] = { {0} }; 111895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float mTmp[10][10] = { {0} }; 112895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static float xTransY[4] = { 0 }; 113895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float magSqr = 0; 114895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float inpData[3] = { 0 }; 115895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j; 116895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int a, b; 117895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float d; 118895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 119895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall switch (command) { 120895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case CAL_ADD_DATA: 121895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inpData[0] = (float)data[0]; 122895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inpData[1] = (float)data[1]; 123895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inpData[2] = (float)data[2]; 124895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]); 125895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]); 126895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]); 127895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[0][3] += (-2 * inpData[0]); 128895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]); 129895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]); 130895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]); 131895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[1][3] += (-2 * inpData[1]); 132895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]); 133895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]); 134895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]); 135895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[2][3] += (-2 * inpData[2]); 136895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[3][0] += (-2 * inpData[0]); 137895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[3][1] += (-2 * inpData[1]); 138895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[3][2] += (-2 * inpData[2]); 139895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[3][3] += 1.0f; 140895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magSqr = 141895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inpData[0] * inpData[0] + inpData[1] * inpData[1] + 142895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inpData[2] * inpData[2]; 143895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall xTransY[0] += (-2 * inpData[0]) * magSqr; 144895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall xTransY[1] += (-2 * inpData[1]) * magSqr; 145895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall xTransY[2] += (-2 * inpData[2]) * magSqr; 146895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall xTransY[3] += magSqr; 147895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 148895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case CAL_RUN: 149895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall a = 4; 150895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall b = a; 151895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < b; i++) { 152895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < b; j++) { 153895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall a = b; 154895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j); 155895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a); 156895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 157895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 158895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall a = b; 159895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall d = inv_matrix_det(&m[0][0], &a); 160895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (d == 0) { 161895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_ERROR; 162895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 163895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 164895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float tmp = 0; 165895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 4; j++) { 166895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp += mInv[j][i] / d * xTransY[j]; 167895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 168895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_test_bias[i] = 169895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall -(long)(tmp * inv_obj.compass_sens / 16384.0f); 170895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 171895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 172895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall case CAL_RESET: 173895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 4; i++) { 174895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 4; j++) { 175895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall m[i][j] = 0; 176895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 177895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall xTransY[i] = 0; 178895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 179895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall default: 180895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall break; 181895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 182895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return retValue; 183895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 184895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 185895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 186895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Entry point for Sensor Fusion operations 187895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @internal 188895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param magFB magnetormeter FB 189895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @param accSF accelerometer SF 190895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 191895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallstatic inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF, 192895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long deltaTime) 193895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 194895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static long prevCompassBias[3] = { 0 }; 195895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static long magMax[3] = { 196895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall -1073741824L, 197895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall -1073741824L, 198895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall -1073741824L 199895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 200895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static long magMin[3] = { 201895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1073741824L, 202895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1073741824L, 203895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 1073741824L 204895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall }; 205895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int gyroMag; 206895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accMag; 207895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 208895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i; 209895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 210895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) { 211895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime); 212895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 213895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 214895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT; 215895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accMag = inv_accel_sum_of_sqr(); 216895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 217895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall // Scaling below assumes certain scaling. 218895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#if ACC_MAG_SQR_SHIFT != 16 219895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#error 220895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 221895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 222895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) { 223895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime); 224895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 225895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 226895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 227895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 228895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) { 229895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Most basic compass calibration, used only with lite MPL 230895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.resetting_compass == 1) { 231895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 232895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMax[i] = -1073741824L; 233895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMin[i] = 1073741824L; 234895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall prevCompassBias[i] = 0; 235895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 236895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalStableCount = 0; 237895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalCount = 0; 238895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.resetting_compass = 0; 239895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 240895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) { 241895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (compassCalStableCount < 1000) { 242895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 243895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.compass_sensor_data[i] > magMax[i]) { 244895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMax[i] = inv_obj.compass_sensor_data[i]; 245895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 246895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.compass_sensor_data[i] < magMin[i]) { 247895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMin[i] = inv_obj.compass_sensor_data[i]; 248895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 249895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 250895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MLUpdateCompassCalibration3DOF(CAL_ADD_DATA, 251895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sensor_data, 252895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall deltaTime); 253895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalStableCount += deltaTime; 254895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 255895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (magMax[i] - magMin[i] < 256895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long long)40 * 1073741824 / inv_obj.compass_sens) { 257895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalStableCount = 0; 258895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 259895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 260895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 261895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (compassCalStableCount >= 1000) { 262895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (MLUpdateCompassCalibration3DOF 263895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (CAL_RUN, inv_obj.compass_sensor_data, 264895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall deltaTime) == INV_SUCCESS) { 265895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_compass_bias = 1; 266895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_accuracy = 3; 267895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for(i=0; i<3; i++) { 268895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[i] = 35; 269895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 270895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.compass_state == SF_UNCALIBRATED) 271895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_state = SF_STARTUP_SETTLE; 272895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_compass_bias(inv_obj.compass_test_bias); 273895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 274895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalCount = 0; 275895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalStableCount = 0; 276895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 277895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMax[i] = -1073741824L; 278895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMin[i] = 1073741824L; 279895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall prevCompassBias[i] = 0; 280895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 281895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MLUpdateCompassCalibration3DOF(CAL_RESET, 282895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sensor_data, 283895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall deltaTime); 284895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 285895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 286895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 287895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalCount += deltaTime; 288895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (compassCalCount > 20000) { 289895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalCount = 0; 290895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall compassCalStableCount = 0; 291895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 292895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMax[i] = -1073741824L; 293895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magMin[i] = 1073741824L; 294895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall prevCompassBias[i] = 0; 295895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 296895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MLUpdateCompassCalibration3DOF(CAL_RESET, 297895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sensor_data, 298895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall deltaTime); 299895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 300895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 301895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 302895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.acc_state != SF_STARTUP_SETTLE) { 303895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) { 304895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing 305895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 306895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 307895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214) 308895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall && ((inv_obj.acc_state == SF_DISTURBANCE) 309895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (inv_obj.acc_state == SF_SLOW_SETTLE))) { 310895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount += deltaTime; 311895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accCount > 0) { 312895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_FAST_SETTLE; 313895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 314895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 315895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 316895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.acc_state == SF_DISTURBANCE) { 317895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount += deltaTime; 318895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accCount > 500) { 319895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_SLOW_SETTLE; 320895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 321895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 322895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (inv_obj.acc_state == SF_SLOW_SETTLE) { 323895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount += deltaTime; 324895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accCount > 1000) { 325895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_NORMAL; 326895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 327895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 328895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (inv_obj.acc_state == SF_FAST_SETTLE) { 329895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount += deltaTime; 330895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accCount > 250) { 331895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_NORMAL; 332895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 333895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 334895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 335895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 336895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 337895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 338895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.acc_state == SF_STARTUP_SETTLE) { 339895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount += deltaTime; 340895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accCount > 50) { 341895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *accSF = 1073741824; //Startup settling 342895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.acc_state = SF_NORMAL; 343895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall accCount = 0; 344895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 345895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if ((inv_obj.acc_state == SF_NORMAL) 346895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall || (inv_obj.acc_state == SF_SLOW_SETTLE)) { 347895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *accSF = inv_obj.accel_sens * 64; //Normal 348895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if ((inv_obj.acc_state == SF_DISTURBANCE)) { 349895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0) 350895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else if (inv_obj.acc_state == SF_FAST_SETTLE) { 351895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *accSF = inv_obj.accel_sens * 512; //Amplify accel 352895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 353895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (!inv_get_gyro_present()) { 354895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall *accSF = inv_obj.accel_sens * 128; 355895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 356895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 357895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 358895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 359895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 360895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Entry point for software sensor fusion operations. 361895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Manages hardware interaction, calls sensor fusion supervisor for 362895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bias calculation. 363895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return error code. INV_SUCCESS if no error occurred. 364895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 365895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_accel_compass_supervisor(void) 366895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 367895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_error_t result; 368895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int adjustSensorFusion = 0; 369895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long accSF = 1073741824; 370895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static double magFB = 0; 371895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long magSensorData[3]; 372895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float fcin[3]; 373895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall float fcout[3]; 374895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 375895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 376895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_compass_present()) { /* check for compass data */ 377895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall int i, j; 378895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp[3] = { 0 }; 379895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long long tmp64 = 0; 380895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long ctime = inv_get_tick_count(); 381895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (((inv_get_compass_id() == COMPASS_ID_AK8975) && 382895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ((ctime - polltime) > 20)) || 383895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max 384895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (SUPERVISOR_DEBUG) { 385895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n"); 386895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("delta time = %ld\n", ctime - polltime); 387895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 388895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall polltime = ctime; 389895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_get_compass_data(magSensorData); 390895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall /* external slave wants the data even if there is an error */ 391895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result && !inv_obj.external_slave_callback) { 392895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (SUPERVISOR_DEBUG) { 393895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGW("inv_get_compass_data returned %d\n", result); 394895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 395895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 396895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long compassTime = inv_get_tick_count(); 397895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long deltaTime = 1; 398895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 399895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result == INV_SUCCESS) { 400895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (lastCompassTime != 0) { 401895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall deltaTime = compassTime - lastCompassTime; 402895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 403895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall lastCompassTime = compassTime; 404895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adjustSensorFusion = 1; 405895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.got_init_compass_bias == 0) { 406895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_init_compass_bias = 1; 407895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 408895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i] = magSensorData[i]; 409895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 410895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 411895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 412895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sensor_data[i] = (long)magSensorData[i]; 413895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_sensor_data[i] -= 414895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.init_compass_bias[i]; 415895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp[i] = ((long long)inv_obj.compass_sensor_data[i]) 416895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * inv_obj.compass_sens / 16384; 417895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp[i] -= inv_obj.compass_bias[i]; 418895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L; 419895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 420895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (i = 0; i < 3; i++) { 421895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp64 = 0; 422895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall for (j = 0; j < 3; j++) { 423895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall tmp64 += (long long) tmp[j] * 424895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_cal[i * 3 + j]; 425895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 426895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_calibrated_data[i] = 427895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (long) (tmp64 / inv_obj.compass_sens); 428895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 429895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //Additions: 430895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((inv_obj.compass_state == 1) && 431895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (inv_obj.compass_overunder == 0)) { 432895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (disturbtimerstart == 0) { 433895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall disturbtimerstart = 1; 434895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall disturbtime = ctime; 435895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 436895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 437895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall disturbtimerstart = 0; 438895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 439895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 440895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.compass_overunder) { 441895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (coiltimerstart == 0) { 442895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall coiltimerstart = 1; 443895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall coiltime = ctime; 444895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 445895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 446895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (coiltimerstart == 1) { 447895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ctime - coiltime > 3000) { 448895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; 449895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_compass_offset(); 450895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_compass_calibration(); 451895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall coiltimerstart = 0; 452895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 453895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 454895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 455895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (disturbtimerstart == 1) { 456895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ctime - disturbtime > 10000) { 457895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; 458895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_set_compass_offset(); 459895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_reset_compass_calibration(); 460895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("Compass reset! inv_reset_compass_calibration \n"); 461895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall disturbtimerstart = 0; 462895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 463895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 464895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 465895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_obj.external_slave_callback) { 466895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_obj.external_slave_callback(&inv_obj); 467895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 468895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 469895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 470895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 471895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 472895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 473895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#ifdef APPLY_COMPASS_FILTER 474895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_compass_id() == COMPASS_ID_YAS530) 475895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall { 476895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); 477895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); 478895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); 479895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 480895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall f.update(&handle, fcin, fcout); 481895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 482895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); 483895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); 484895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); 485895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 486895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall#endif 487895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 488895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (SUPERVISOR_DEBUG) { 489895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n", 490895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (float)inv_obj.compass_calibrated_data[0] / 491895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 65536.f, 492895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (float)inv_obj.compass_calibrated_data[1] / 493895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 65536.f, 494895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall (float)inv_obj.compass_calibrated_data[2] / 495895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 65536.f); 496895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 497895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magFB = 1.0; 498895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adjustSensorFusion = 1; 499895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); 500895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 501895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 502895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 503895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 504895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 505895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 506895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } else { 507895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall //No compass, but still modify accel 508895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long ctime = inv_get_tick_count(); 509895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (polltime == 0 || ((ctime - polltime) > 80)) { // at the beginning AND every 1/8 second 510895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long deltaTime = 1; 511895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall adjustSensorFusion = 1; 512895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall magFB = 0; 513895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (polltime != 0) { 514895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall deltaTime = ctime - polltime; 515895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 516895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); 517895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall polltime = ctime; 518895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 519895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 520895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (adjustSensorFusion == 1) { 521895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned char regs[4]; 522895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static int prevAccSF = 1; 523895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 524895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (accSF != prevAccSF) { 525895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[0] = (unsigned char)((accSF >> 24) & 0xff); 526895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[1] = (unsigned char)((accSF >> 16) & 0xff); 527895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[2] = (unsigned char)((accSF >> 8) & 0xff); 528895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall regs[3] = (unsigned char)(accSF & 0xff); 529895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall result = inv_set_mpu_memory(KEY_D_0_96, 4, regs); 530895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (result) { 531895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall LOG_RESULT_LOCATION(result); 532895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return result; 533895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 534895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall prevAccSF = accSF; 535895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 536895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 537895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 538895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ml_supervisor_cb.accel_compass_fusion_func != NULL) 539895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ml_supervisor_cb.accel_compass_fusion_func(magFB); 540895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 541895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 542895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 543895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 544895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 545895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Entry point for software sensor fusion operations. 546895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * Manages hardware interaction, calls sensor fusion supervisor for 547895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * bias calculation. 548895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS or non-zero error code on error. 549895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 550895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_pressure_supervisor(void) 551895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 552895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall long pressureSensorData[1]; 553895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall static unsigned long pressurePolltime = 0; 554895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_pressure_present()) { /* check for pressure data */ 555895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall unsigned long ctime = inv_get_tick_count(); 556895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second 557895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (SUPERVISOR_DEBUG) { 558895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("Fetch pressure data\n"); 559895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime); 560895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 561895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall pressurePolltime = ctime; 562895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) { 563895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.pressure = pressureSensorData[0]; 564895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 565895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 566895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 567895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 568895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 569895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 570895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 571895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @brief Resets the magnetometer calibration algorithm. 572895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @return INV_SUCCESS if successful, or non-zero error code otherwise. 573895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 574895401859313187f15a800e62d43e6bcbf48fadaJP Abgrallinv_error_t inv_reset_compass_calibration(void) 575895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall{ 576895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) { 577895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall if (ml_supervisor_cb.reset_advanced_compass_func != NULL) 578895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall ml_supervisor_cb.reset_advanced_compass_func(); 579895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall } 580895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1); 581895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 582895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[0] = P_INIT; 583895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[1] = P_INIT; 584895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_bias_error[2] = P_INIT; 585895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_accuracy = 0; 586895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 587895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_compass_bias = 0; 588895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.got_init_compass_bias = 0; 589895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.compass_state = SF_UNCALIBRATED; 590895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall inv_obj.resetting_compass = 1; 591895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 592895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall return INV_SUCCESS; 593895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall} 594895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall 595895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall/** 596895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall * @} 597895401859313187f15a800e62d43e6bcbf48fadaJP Abgrall */ 598