C++ (Cpp) MagCal_Run Examples

Example #1

File: mqx_tasks.c Project: MonicaTakacs/Open-Source-Sensor-Fusion

// 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;
}

Example #2

File: rawdata.c Project: DatanoiseTV/IMUread

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(&current_orientation);
	}
}