// magnetic calibration task void MagCal_task(uint32_t task_init_data) { while(1) { // set the RED LED off and set test pin off ready for magnetic calibration to run LED_RED_SetVal(NULL); TestPin_MagCal_Time_ClrVal(NULL); // wait for the magnetic calibration event // this event will never be enabled for build options which don't require magnetic calibration // FALSE means any bit (of the 1 bit enabled by the mask) unblocks // and NULL means timeout is infinite _lwevent_wait_for(&(mqxglobals.MagCalEventStruct), 1, FALSE, NULL); // set the red LED on and test pin pin on LED_RED_ClrVal(NULL); TestPin_MagCal_Time_SetVal(NULL); // prevent compilation errors when magnetic calibration is not required #if defined COMPUTE_6DOF_GB_BASIC || defined COMPUTE_9DOF_GBY_KALMAN // and run the magnetic calibration MagCal_Run(&thisMagCal, &thisMagBuffer); #endif } // end of infinite loop return; }
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); } }