/* 功能初始化 */
static void function_init(void)
{
/* imu mpu9250 */
mpu9250_init();
debug_log("mpu9250 初始化完成.\r\n");
/* 参数设置 */
data_config();
debug_log("参数设置完成.\r\n");
/* 融合算法初始化 */
debug_log("融合算法初始化完成.\r\n");
fusion_init();
/* step2: 启动心跳中触发imu模块读取 */
debug_log("imu i2c中断读启动.\r\n");
imu_start(); /* TODO:中断读还是有BUG 性能问题? */
/* 计算一个融合周期(默认10ms) 融合算法需要时间 */
/* 测试融合性能达标 */
debug_log("融合算法测试启动启动.\r\n");
fusion_test_a_fusion_period();
/* step3: 启动任务 */
}
void raw_data_reset(void)
{
rawcount = OVERSAMPLE_RATIO;
fusion_init();
memset(&magcal, 0, sizeof(magcal));
magcal.V[2] = 80.0f; // initial guess
magcal.invW[0][0] = 1.0f;
magcal.invW[1][1] = 1.0f;
magcal.invW[2][2] = 1.0f;
magcal.FitError = 100.0f;
magcal.FitErrorAge = 100.0f;
magcal.B = 50.0f;
}
void raw_data(const int16_t *data)
{
static int force_orientation_counter=0;
float x, y, z, ratio, magdiff;
Point_t point;
add_magcal_data(data);
x = magcal.V[0];
y = magcal.V[1];
z = magcal.V[2];
if (MagCal_Run()) {
x -= magcal.V[0];
y -= magcal.V[1];
z -= magcal.V[2];
magdiff = sqrtf(x * x + y * y + z * z);
//printf("magdiff = %.2f\n", magdiff);
if (magdiff > 0.8f) {
fusion_init();
rawcount = OVERSAMPLE_RATIO;
force_orientation_counter = 240;
}
}
if (force_orientation_counter > 0) {
if (--force_orientation_counter == 0) {
//printf("delayed forcible orientation reset\n");
fusion_init();
rawcount = OVERSAMPLE_RATIO;
}
}
if (rawcount >= OVERSAMPLE_RATIO) {
memset(&accel, 0, sizeof(accel));
memset(&mag, 0, sizeof(mag));
memset(&gyro, 0, sizeof(gyro));
rawcount = 0;
}
x = (float)data[0] * G_PER_COUNT;
y = (float)data[1] * G_PER_COUNT;
z = (float)data[2] * G_PER_COUNT;
accel.GpFast[0] = x;
accel.GpFast[1] = y;
accel.GpFast[2] = y;
accel.Gp[0] += x;
accel.Gp[1] += y;
accel.Gp[2] += y;
x = (float)data[3] * DEG_PER_SEC_PER_COUNT;
y = (float)data[4] * DEG_PER_SEC_PER_COUNT;
z = (float)data[5] * DEG_PER_SEC_PER_COUNT;
gyro.Yp[0] += x;
gyro.Yp[1] += y;
gyro.Yp[2] += z;
gyro.YpFast[rawcount][0] = x;
gyro.YpFast[rawcount][1] = y;
gyro.YpFast[rawcount][2] = z;
apply_calibration(data[6], data[7], data[8], &point);
mag.BcFast[0] = point.x;
mag.BcFast[1] = point.y;
mag.BcFast[2] = point.z;
mag.Bc[0] += point.x;
mag.Bc[1] += point.y;
mag.Bc[2] += point.z;
rawcount++;
if (rawcount >= OVERSAMPLE_RATIO) {
ratio = 1.0f / (float)OVERSAMPLE_RATIO;
accel.Gp[0] *= ratio;
accel.Gp[1] *= ratio;
accel.Gp[2] *= ratio;
gyro.Yp[0] *= ratio;
gyro.Yp[1] *= ratio;
gyro.Yp[2] *= ratio;
mag.Bc[0] *= ratio;
mag.Bc[1] *= ratio;
mag.Bc[2] *= ratio;
fusion_update(&accel, &mag, &gyro, &magcal);
fusion_read(¤t_orientation);
}
}
