597 lines
21 KiB
C
597 lines
21 KiB
C
/*
|
|
$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;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
*/
|