| /* |
| $License: |
| Copyright 2011 InvenSense, Inc. |
| |
| Licensed under the Apache License, Version 2.0 (the "License"); |
| you may not use this file except in compliance with the License. |
| You may obtain a copy of the License at |
| |
| http://www.apache.org/licenses/LICENSE-2.0 |
| |
| Unless required by applicable law or agreed to in writing, software |
| distributed under the License is distributed on an "AS IS" BASIS, |
| WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| See the License for the specific language governing permissions and |
| limitations under the License. |
| $ |
| */ |
| |
| /****************************************************************************** |
| * |
| * $Id: mlsupervisor.c 5637 2011-06-14 01:13:53Z mcaramello $ |
| * |
| *****************************************************************************/ |
| |
| /** |
| * @defgroup ML_SUPERVISOR |
| * @brief Basic sensor fusion supervisor functionalities. |
| * |
| * @{ |
| * @file mlsupervisor.c |
| * @brief Basic sensor fusion supervisor functionalities. |
| */ |
| |
| #include "ml.h" |
| #include "mldl.h" |
| #include "mldl_cfg.h" |
| #include "mltypes.h" |
| #include "mlinclude.h" |
| #include "compass.h" |
| #include "pressure.h" |
| #include "dmpKey.h" |
| #include "dmpDefault.h" |
| #include "mlstates.h" |
| #include "mlFIFO.h" |
| #include "mlFIFOHW.h" |
| #include "mlMathFunc.h" |
| #include "mlsupervisor.h" |
| #include "mlmath.h" |
| |
| #include "mlsl.h" |
| #include "mlos.h" |
| |
| #include <log.h> |
| #undef MPL_LOG_TAG |
| #define MPL_LOG_TAG "MPL-sup" |
| |
| static unsigned long lastCompassTime = 0; |
| static unsigned long polltime = 0; |
| static unsigned long coiltime = 0; |
| static int accCount = 0; |
| static int compassCalStableCount = 0; |
| static int compassCalCount = 0; |
| static int coiltimerstart = 0; |
| static unsigned long disturbtime = 0; |
| static int disturbtimerstart = 0; |
| |
| static yas_filter_if_s f; |
| static yas_filter_handle_t handle; |
| |
| #define SUPERVISOR_DEBUG 0 |
| |
| struct inv_supervisor_cb_obj ml_supervisor_cb = { 0 }; |
| |
| /** |
| * @brief This initializes all variables that should be reset on |
| */ |
| void inv_init_sensor_fusion_supervisor(void) |
| { |
| lastCompassTime = 0; |
| polltime = 0; |
| inv_obj.acc_state = SF_STARTUP_SETTLE; |
| accCount = 0; |
| compassCalStableCount = 0; |
| compassCalCount = 0; |
| |
| yas_filter_init(&f); |
| f.init(&handle); |
| |
| #if defined CONFIG_MPU_SENSORS_MPU6050A2 || \ |
| defined CONFIG_MPU_SENSORS_MPU6050B1 |
| if (inv_compass_present()) { |
| struct mldl_cfg *mldl_cfg = inv_get_dl_config(); |
| if (mldl_cfg->pdata->compass.bus == EXT_SLAVE_BUS_SECONDARY) { |
| (void)inv_send_external_sensor_data(INV_ELEMENT_1 | INV_ELEMENT_2 | INV_ELEMENT_3, INV_16_BIT); |
| } |
| } |
| #endif |
| |
| if (ml_supervisor_cb.supervisor_reset_func != NULL) { |
| ml_supervisor_cb.supervisor_reset_func(); |
| } |
| } |
| |
| static int MLUpdateCompassCalibration3DOF(int command, long *data, |
| unsigned long deltaTime) |
| { |
| INVENSENSE_FUNC_START; |
| int retValue = INV_SUCCESS; |
| static float m[10][10] = { {0} }; |
| float mInv[10][10] = { {0} }; |
| float mTmp[10][10] = { {0} }; |
| static float xTransY[4] = { 0 }; |
| float magSqr = 0; |
| float inpData[3] = { 0 }; |
| int i, j; |
| int a, b; |
| float d; |
| |
| switch (command) { |
| case CAL_ADD_DATA: |
| inpData[0] = (float)data[0]; |
| inpData[1] = (float)data[1]; |
| inpData[2] = (float)data[2]; |
| m[0][0] += (-2 * inpData[0]) * (-2 * inpData[0]); |
| m[0][1] += (-2 * inpData[0]) * (-2 * inpData[1]); |
| m[0][2] += (-2 * inpData[0]) * (-2 * inpData[2]); |
| m[0][3] += (-2 * inpData[0]); |
| m[1][0] += (-2 * inpData[1]) * (-2 * inpData[0]); |
| m[1][1] += (-2 * inpData[1]) * (-2 * inpData[1]); |
| m[1][2] += (-2 * inpData[1]) * (-2 * inpData[2]); |
| m[1][3] += (-2 * inpData[1]); |
| m[2][0] += (-2 * inpData[2]) * (-2 * inpData[0]); |
| m[2][1] += (-2 * inpData[2]) * (-2 * inpData[1]); |
| m[2][2] += (-2 * inpData[2]) * (-2 * inpData[2]); |
| m[2][3] += (-2 * inpData[2]); |
| m[3][0] += (-2 * inpData[0]); |
| m[3][1] += (-2 * inpData[1]); |
| m[3][2] += (-2 * inpData[2]); |
| m[3][3] += 1.0f; |
| magSqr = |
| inpData[0] * inpData[0] + inpData[1] * inpData[1] + |
| inpData[2] * inpData[2]; |
| xTransY[0] += (-2 * inpData[0]) * magSqr; |
| xTransY[1] += (-2 * inpData[1]) * magSqr; |
| xTransY[2] += (-2 * inpData[2]) * magSqr; |
| xTransY[3] += magSqr; |
| break; |
| case CAL_RUN: |
| a = 4; |
| b = a; |
| for (i = 0; i < b; i++) { |
| for (j = 0; j < b; j++) { |
| a = b; |
| inv_matrix_det_inc(&m[0][0], &mTmp[0][0], &a, i, j); |
| mInv[j][i] = SIGNM(i + j) * inv_matrix_det(&mTmp[0][0], &a); |
| } |
| } |
| a = b; |
| d = inv_matrix_det(&m[0][0], &a); |
| if (d == 0) { |
| return INV_ERROR; |
| } |
| for (i = 0; i < 3; i++) { |
| float tmp = 0; |
| for (j = 0; j < 4; j++) { |
| tmp += mInv[j][i] / d * xTransY[j]; |
| } |
| inv_obj.compass_test_bias[i] = |
| -(long)(tmp * inv_obj.compass_sens / 16384.0f); |
| } |
| break; |
| case CAL_RESET: |
| for (i = 0; i < 4; i++) { |
| for (j = 0; j < 4; j++) { |
| m[i][j] = 0; |
| } |
| xTransY[i] = 0; |
| } |
| default: |
| break; |
| } |
| return retValue; |
| } |
| |
| /** |
| * Entry point for Sensor Fusion operations |
| * @internal |
| * @param magFB magnetormeter FB |
| * @param accSF accelerometer SF |
| */ |
| static inv_error_t MLSensorFusionSupervisor(double *magFB, long *accSF, |
| unsigned long deltaTime) |
| { |
| static long prevCompassBias[3] = { 0 }; |
| static long magMax[3] = { |
| -1073741824L, |
| -1073741824L, |
| -1073741824L |
| }; |
| static long magMin[3] = { |
| 1073741824L, |
| 1073741824L, |
| 1073741824L |
| }; |
| int gyroMag; |
| long accMag; |
| inv_error_t result; |
| int i; |
| |
| if (ml_supervisor_cb.progressive_no_motion_supervisor_func != NULL) { |
| ml_supervisor_cb.progressive_no_motion_supervisor_func(deltaTime); |
| } |
| |
| gyroMag = inv_get_gyro_sum_of_sqr() >> GYRO_MAG_SQR_SHIFT; |
| accMag = inv_accel_sum_of_sqr(); |
| |
| // Scaling below assumes certain scaling. |
| #if ACC_MAG_SQR_SHIFT != 16 |
| #error |
| #endif |
| |
| if (ml_supervisor_cb.sensor_fusion_advanced_func != NULL) { |
| result = ml_supervisor_cb.sensor_fusion_advanced_func(magFB, deltaTime); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| } else if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_MOTION) { |
| //Most basic compass calibration, used only with lite MPL |
| if (inv_obj.resetting_compass == 1) { |
| for (i = 0; i < 3; i++) { |
| magMax[i] = -1073741824L; |
| magMin[i] = 1073741824L; |
| prevCompassBias[i] = 0; |
| } |
| compassCalStableCount = 0; |
| compassCalCount = 0; |
| inv_obj.resetting_compass = 0; |
| } |
| if ((gyroMag > 400) || (inv_get_gyro_present() == 0)) { |
| if (compassCalStableCount < 1000) { |
| for (i = 0; i < 3; i++) { |
| if (inv_obj.compass_sensor_data[i] > magMax[i]) { |
| magMax[i] = inv_obj.compass_sensor_data[i]; |
| } |
| if (inv_obj.compass_sensor_data[i] < magMin[i]) { |
| magMin[i] = inv_obj.compass_sensor_data[i]; |
| } |
| } |
| MLUpdateCompassCalibration3DOF(CAL_ADD_DATA, |
| inv_obj.compass_sensor_data, |
| deltaTime); |
| compassCalStableCount += deltaTime; |
| for (i = 0; i < 3; i++) { |
| if (magMax[i] - magMin[i] < |
| (long long)40 * 1073741824 / inv_obj.compass_sens) { |
| compassCalStableCount = 0; |
| } |
| } |
| } else { |
| if (compassCalStableCount >= 1000) { |
| if (MLUpdateCompassCalibration3DOF |
| (CAL_RUN, inv_obj.compass_sensor_data, |
| deltaTime) == INV_SUCCESS) { |
| inv_obj.got_compass_bias = 1; |
| inv_obj.compass_accuracy = 3; |
| for(i=0; i<3; i++) { |
| inv_obj.compass_bias_error[i] = 35; |
| } |
| if (inv_obj.compass_state == SF_UNCALIBRATED) |
| inv_obj.compass_state = SF_STARTUP_SETTLE; |
| inv_set_compass_bias(inv_obj.compass_test_bias); |
| } |
| compassCalCount = 0; |
| compassCalStableCount = 0; |
| for (i = 0; i < 3; i++) { |
| magMax[i] = -1073741824L; |
| magMin[i] = 1073741824L; |
| prevCompassBias[i] = 0; |
| } |
| MLUpdateCompassCalibration3DOF(CAL_RESET, |
| inv_obj.compass_sensor_data, |
| deltaTime); |
| } |
| } |
| } |
| compassCalCount += deltaTime; |
| if (compassCalCount > 20000) { |
| compassCalCount = 0; |
| compassCalStableCount = 0; |
| for (i = 0; i < 3; i++) { |
| magMax[i] = -1073741824L; |
| magMin[i] = 1073741824L; |
| prevCompassBias[i] = 0; |
| } |
| MLUpdateCompassCalibration3DOF(CAL_RESET, |
| inv_obj.compass_sensor_data, |
| deltaTime); |
| } |
| } |
| |
| if (inv_obj.acc_state != SF_STARTUP_SETTLE) { |
| if (((accMag > 260000L) || (accMag < 6000)) || (gyroMag > 2250000L)) { |
| inv_obj.acc_state = SF_DISTURBANCE; //No accels, fast swing |
| accCount = 0; |
| } else { |
| if ((gyroMag < 400) && (accMag < 200000L) && (accMag > 26214) |
| && ((inv_obj.acc_state == SF_DISTURBANCE) |
| || (inv_obj.acc_state == SF_SLOW_SETTLE))) { |
| accCount += deltaTime; |
| if (accCount > 0) { |
| inv_obj.acc_state = SF_FAST_SETTLE; |
| accCount = 0; |
| } |
| } else { |
| if (inv_obj.acc_state == SF_DISTURBANCE) { |
| accCount += deltaTime; |
| if (accCount > 500) { |
| inv_obj.acc_state = SF_SLOW_SETTLE; |
| accCount = 0; |
| } |
| } else if (inv_obj.acc_state == SF_SLOW_SETTLE) { |
| accCount += deltaTime; |
| if (accCount > 1000) { |
| inv_obj.acc_state = SF_NORMAL; |
| accCount = 0; |
| } |
| } else if (inv_obj.acc_state == SF_FAST_SETTLE) { |
| accCount += deltaTime; |
| if (accCount > 250) { |
| inv_obj.acc_state = SF_NORMAL; |
| accCount = 0; |
| } |
| } |
| } |
| } |
| } |
| if (inv_obj.acc_state == SF_STARTUP_SETTLE) { |
| accCount += deltaTime; |
| if (accCount > 50) { |
| *accSF = 1073741824; //Startup settling |
| inv_obj.acc_state = SF_NORMAL; |
| accCount = 0; |
| } |
| } else if ((inv_obj.acc_state == SF_NORMAL) |
| || (inv_obj.acc_state == SF_SLOW_SETTLE)) { |
| *accSF = inv_obj.accel_sens * 64; //Normal |
| } else if ((inv_obj.acc_state == SF_DISTURBANCE)) { |
| *accSF = inv_obj.accel_sens * 64; //Don't use accel (should be 0) |
| } else if (inv_obj.acc_state == SF_FAST_SETTLE) { |
| *accSF = inv_obj.accel_sens * 512; //Amplify accel |
| } |
| if (!inv_get_gyro_present()) { |
| *accSF = inv_obj.accel_sens * 128; |
| } |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief Entry point for software sensor fusion operations. |
| * Manages hardware interaction, calls sensor fusion supervisor for |
| * bias calculation. |
| * @return error code. INV_SUCCESS if no error occurred. |
| */ |
| inv_error_t inv_accel_compass_supervisor(void) |
| { |
| inv_error_t result; |
| int adjustSensorFusion = 0; |
| long accSF = 1073741824; |
| static double magFB = 0; |
| long magSensorData[3]; |
| float fcin[3]; |
| float fcout[3]; |
| |
| |
| if (inv_compass_present()) { /* check for compass data */ |
| int i, j; |
| long long tmp[3] = { 0 }; |
| long long tmp64 = 0; |
| unsigned long ctime = inv_get_tick_count(); |
| if (((inv_get_compass_id() == COMPASS_ID_AK8975) && |
| ((ctime - polltime) > 20)) || |
| (polltime == 0 || ((ctime - polltime) > 20))) { // 50Hz max |
| if (SUPERVISOR_DEBUG) { |
| MPL_LOGV("Fetch compass data from inv_process_fifo_packet\n"); |
| MPL_LOGV("delta time = %ld\n", ctime - polltime); |
| } |
| polltime = ctime; |
| result = inv_get_compass_data(magSensorData); |
| /* external slave wants the data even if there is an error */ |
| if (result && !inv_obj.external_slave_callback) { |
| if (SUPERVISOR_DEBUG) { |
| MPL_LOGW("inv_get_compass_data returned %d\n", result); |
| } |
| } else { |
| unsigned long compassTime = inv_get_tick_count(); |
| unsigned long deltaTime = 1; |
| |
| if (result == INV_SUCCESS) { |
| if (lastCompassTime != 0) { |
| deltaTime = compassTime - lastCompassTime; |
| } |
| lastCompassTime = compassTime; |
| adjustSensorFusion = 1; |
| if (inv_obj.got_init_compass_bias == 0) { |
| inv_obj.got_init_compass_bias = 1; |
| for (i = 0; i < 3; i++) { |
| inv_obj.init_compass_bias[i] = magSensorData[i]; |
| } |
| } |
| for (i = 0; i < 3; i++) { |
| inv_obj.compass_sensor_data[i] = (long)magSensorData[i]; |
| inv_obj.compass_sensor_data[i] -= |
| inv_obj.init_compass_bias[i]; |
| tmp[i] = ((long long)inv_obj.compass_sensor_data[i]) |
| * inv_obj.compass_sens / 16384; |
| tmp[i] -= inv_obj.compass_bias[i]; |
| tmp[i] = (tmp[i] * inv_obj.compass_scale[i]) / 65536L; |
| } |
| for (i = 0; i < 3; i++) { |
| tmp64 = 0; |
| for (j = 0; j < 3; j++) { |
| tmp64 += (long long) tmp[j] * |
| inv_obj.compass_cal[i * 3 + j]; |
| } |
| inv_obj.compass_calibrated_data[i] = |
| (long) (tmp64 / inv_obj.compass_sens); |
| } |
| //Additions: |
| if ((inv_obj.compass_state == 1) && |
| (inv_obj.compass_overunder == 0)) { |
| if (disturbtimerstart == 0) { |
| disturbtimerstart = 1; |
| disturbtime = ctime; |
| } |
| } else { |
| disturbtimerstart = 0; |
| } |
| |
| if (inv_obj.compass_overunder) { |
| if (coiltimerstart == 0) { |
| coiltimerstart = 1; |
| coiltime = ctime; |
| } |
| } |
| if (coiltimerstart == 1) { |
| if (ctime - coiltime > 3000) { |
| inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; |
| inv_set_compass_offset(); |
| inv_reset_compass_calibration(); |
| coiltimerstart = 0; |
| } |
| } |
| |
| if (disturbtimerstart == 1) { |
| if (ctime - disturbtime > 10000) { |
| inv_obj.flags[INV_COMPASS_OFFSET_VALID] = 0; |
| inv_set_compass_offset(); |
| inv_reset_compass_calibration(); |
| MPL_LOGI("Compass reset! inv_reset_compass_calibration \n"); |
| disturbtimerstart = 0; |
| } |
| } |
| } |
| if (inv_obj.external_slave_callback) { |
| result = inv_obj.external_slave_callback(&inv_obj); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| } |
| |
| #ifdef APPLY_COMPASS_FILTER |
| if (inv_get_compass_id() == COMPASS_ID_YAS530) |
| { |
| fcin[0] = 1000*((float)inv_obj.compass_calibrated_data[0] /65536.f); |
| fcin[1] = 1000*((float)inv_obj.compass_calibrated_data[1] /65536.f); |
| fcin[2] = 1000*((float)inv_obj.compass_calibrated_data[2] /65536.f); |
| |
| f.update(&handle, fcin, fcout); |
| |
| inv_obj.compass_calibrated_data[0] = (long)(fcout[0]*65536.f/1000.f); |
| inv_obj.compass_calibrated_data[1] = (long)(fcout[1]*65536.f/1000.f); |
| inv_obj.compass_calibrated_data[2] = (long)(fcout[2]*65536.f/1000.f); |
| } |
| #endif |
| |
| if (SUPERVISOR_DEBUG) { |
| MPL_LOGI("RM : %+10.6f %+10.6f %+10.6f\n", |
| (float)inv_obj.compass_calibrated_data[0] / |
| 65536.f, |
| (float)inv_obj.compass_calibrated_data[1] / |
| 65536.f, |
| (float)inv_obj.compass_calibrated_data[2] / |
| 65536.f); |
| } |
| magFB = 1.0; |
| adjustSensorFusion = 1; |
| result = MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| } |
| } |
| } else { |
| //No compass, but still modify accel |
| unsigned long ctime = inv_get_tick_count(); |
| if (polltime == 0 || ((ctime - polltime) > 80)) { // at the beginning AND every 1/8 second |
| unsigned long deltaTime = 1; |
| adjustSensorFusion = 1; |
| magFB = 0; |
| if (polltime != 0) { |
| deltaTime = ctime - polltime; |
| } |
| MLSensorFusionSupervisor(&magFB, &accSF, deltaTime); |
| polltime = ctime; |
| } |
| } |
| if (adjustSensorFusion == 1) { |
| unsigned char regs[4]; |
| static int prevAccSF = 1; |
| |
| if (accSF != prevAccSF) { |
| regs[0] = (unsigned char)((accSF >> 24) & 0xff); |
| regs[1] = (unsigned char)((accSF >> 16) & 0xff); |
| regs[2] = (unsigned char)((accSF >> 8) & 0xff); |
| regs[3] = (unsigned char)(accSF & 0xff); |
| result = inv_set_mpu_memory(KEY_D_0_96, 4, regs); |
| if (result) { |
| LOG_RESULT_LOCATION(result); |
| return result; |
| } |
| prevAccSF = accSF; |
| } |
| } |
| |
| if (ml_supervisor_cb.accel_compass_fusion_func != NULL) |
| ml_supervisor_cb.accel_compass_fusion_func(magFB); |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief Entry point for software sensor fusion operations. |
| * Manages hardware interaction, calls sensor fusion supervisor for |
| * bias calculation. |
| * @return INV_SUCCESS or non-zero error code on error. |
| */ |
| inv_error_t inv_pressure_supervisor(void) |
| { |
| long pressureSensorData[1]; |
| static unsigned long pressurePolltime = 0; |
| if (inv_pressure_present()) { /* check for pressure data */ |
| unsigned long ctime = inv_get_tick_count(); |
| if ((pressurePolltime == 0 || ((ctime - pressurePolltime) > 80))) { //every 1/8 second |
| if (SUPERVISOR_DEBUG) { |
| MPL_LOGV("Fetch pressure data\n"); |
| MPL_LOGV("delta time = %ld\n", ctime - pressurePolltime); |
| } |
| pressurePolltime = ctime; |
| if (inv_get_pressure_data(&pressureSensorData[0]) == INV_SUCCESS) { |
| inv_obj.pressure = pressureSensorData[0]; |
| } |
| } |
| } |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @brief Resets the magnetometer calibration algorithm. |
| * @return INV_SUCCESS if successful, or non-zero error code otherwise. |
| */ |
| inv_error_t inv_reset_compass_calibration(void) |
| { |
| if (inv_params_obj.bias_mode & INV_MAG_BIAS_FROM_GYRO) { |
| if (ml_supervisor_cb.reset_advanced_compass_func != NULL) |
| ml_supervisor_cb.reset_advanced_compass_func(); |
| } |
| MLUpdateCompassCalibration3DOF(CAL_RESET, inv_obj.compass_sensor_data, 1); |
| |
| inv_obj.compass_bias_error[0] = P_INIT; |
| inv_obj.compass_bias_error[1] = P_INIT; |
| inv_obj.compass_bias_error[2] = P_INIT; |
| inv_obj.compass_accuracy = 0; |
| |
| inv_obj.got_compass_bias = 0; |
| inv_obj.got_init_compass_bias = 0; |
| inv_obj.compass_state = SF_UNCALIBRATED; |
| inv_obj.resetting_compass = 1; |
| |
| return INV_SUCCESS; |
| } |
| |
| /** |
| * @} |
| */ |
