//*****************************************************************************
//
// MPU9150 Sensor callback function.  Called at the end of MPU9150 sensor
// driver transactions. This is called from I2C interrupt context.
//
//*****************************************************************************
void MotionCallback(void* pvCallbackData, uint_fast8_t ui8Status)
{
    //
    // If the transaction succeeded set the data flag to indicate to
    // application that this transaction is complete and data may be ready.
    //
    if(ui8Status == I2CM_STATUS_SUCCESS)
    {
        //
        // Set the motion event flag to show that we have completed the
        // i2c transfer
        //
        HWREGBITW(&g_ui32Events, MOTION_EVENT) = 1;

        if(g_ui8MotionState == MOTION_STATE_RUN);
        {
            //
            // Get local copies of the raw motion sensor data.
            //
            MPU9150DataAccelGetFloat(&g_sMPU9150Inst, g_pfAccel, g_pfAccel + 1,
                                     g_pfAccel + 2);

            MPU9150DataGyroGetFloat(&g_sMPU9150Inst, g_pfGyro, g_pfGyro + 1,
                                    g_pfGyro + 2);

            MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, g_pfMag, g_pfMag + 1,
                                       g_pfMag + 2);

            //
            // Update the DCM. Do this in the ISR so that timing between the
            // calls is consistent and accurate.
            //
            CompDCMMagnetoUpdate(&g_sCompDCMInst, g_pfMag[0], g_pfMag[1],
                                 g_pfMag[2]);
            CompDCMAccelUpdate(&g_sCompDCMInst, g_pfAccel[0], g_pfAccel[1],
                               g_pfAccel[2]);
            CompDCMGyroUpdate(&g_sCompDCMInst, -g_pfGyro[0], -g_pfGyro[1],
                              -g_pfGyro[2]);
            CompDCMUpdate(&g_sCompDCMInst);
        }
    }
    else
    {
        //
        // An Error occurred in the I2C transaction.
        //
        HWREGBITW(&g_ui32Events, MOTION_ERROR_EVENT) = 1;
        g_ui8MotionState = MOTION_STATE_ERROR;
    }

    //
    // Store the most recent status in case it was an error condition
    //
    g_vui8ErrorFlag = ui8Status;
}
예제 #2
0
int sensor(void)
	{
	ui32CompDCMStarted = 0;


		// Go to sleep mode while waiting for data ready.
		//
		while(!g_vui8I2CDoneFlag)
		{
			ROM_SysCtlSleep();
		}

		//
		// Clear the flag
		//
		g_vui8I2CDoneFlag = 0;

		//
		// Get floating point version of the Accel Data in m/s^2.
		//
		MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
				pfAccel + 2);

		//
		// Get floating point version of angular velocities in rad/sec
		//
		MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
				pfGyro + 2);

		//
		// Get floating point version of magnetic fields strength in tesla
		//
		MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
				pfMag + 2);

		//
		// Check if this is our first data ever.
		//




		if(ui32CompDCMStarted == 0)
		{
			//
			// Set flag indicating that DCM is started.
			// Perform the seeding of the DCM with the first data set.
			//
			ui32CompDCMStarted = 1;
			CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
					pfMag[2]);
			CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
					pfAccel[2]);
			CompDCMGyroUpdate(&g_sCompDCMInst, pfGyro[0], pfGyro[1],
					pfGyro[2]);
			CompDCMStart(&g_sCompDCMInst);
		}
		else
		{
			//
			// DCM Is already started.  Perform the incremental update.
			//
			CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
					pfMag[2]);
			CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
					pfAccel[2]);
			CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
					-pfGyro[2]);
			CompDCMUpdate(&g_sCompDCMInst);
		}

		//
		// Increment the skip counter.  Skip counter is used so we do not
		// overflow the UART with data.
		//
		g_ui32PrintSkipCounter++;
		if(g_ui32PrintSkipCounter >= PRINT_SKIP_COUNT)
		{
			//
			// Reset skip counter.
			//
			g_ui32PrintSkipCounter = 0;

			//
			// Get Euler data. (Roll Pitch Yaw)
			//
			CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
					pfEulers + 2);

			//
			// Get Quaternions.
			//
			CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);

			//
			// convert mag data to micro-tesla for better human interpretation.
			//
			pfMag[0] *= 1e6;
			pfMag[1] *= 1e6;
			pfMag[2] *= 1e6;

			//
			// Convert Eulers to degrees. 180/PI = 57.29...
			// Convert Yaw to 0 to 360 to approximate compass headings.
			//
			pfEulers[0] *= 57.295779513082320876798154814105f;
			pfEulers[1] *= 57.295779513082320876798154814105f;
			pfEulers[2] *= 57.295779513082320876798154814105f;
			if(pfEulers[2] < 0)
			{
				pfEulers[2] += 360.0f;
			}

			//
			// Now drop back to using the data as a single array for the
			// purpose of decomposing the float into a integer part and a
			// fraction (decimal) part.
			//
			for(ui32Idx = 0; ui32Idx < 16; ui32Idx++)
			{
				//
				// Conver float value to a integer truncating the decimal part.
				//
				i32IPart[ui32Idx] = (int32_t) pfData[ui32Idx];

				//
				// Multiply by 1000 to preserve first three decimal values.
				// Truncates at the 3rd decimal place.
				//
				i32FPart[ui32Idx] = (int32_t) (pfData[ui32Idx] * 1000.0f);

				//
				// Subtract off the integer part from this newly formed decimal
				// part.
				//
				i32FPart[ui32Idx] = i32FPart[ui32Idx] -
						(i32IPart[ui32Idx] * 1000);

				//
				// make the decimal part a positive number for display.
				//
				if(i32FPart[ui32Idx] < 0)
				{
					i32FPart[ui32Idx] *= -1;
				}
			}

			//

			  // Print the acceleration numbers in the table.
	            //
	            UARTprintf("\033[5;17H%3d.%03d", i32IPart[0], i32FPart[0]);
	            UARTprintf("\033[5;40H%3d.%03d", i32IPart[1], i32FPart[1]);
	            UARTprintf("\033[5;63H%3d.%03d", i32IPart[2], i32FPart[2]);


	            //
	            // Print the angular velocities in the table.
	            //
	            UARTprintf("\033[7;17H%3d.%03d", i32IPart[3], i32FPart[3]);
	            UARTprintf("\033[7;40H%3d.%03d", i32IPart[4], i32FPart[4]);
	            UARTprintf("\033[7;63H%3d.%03d", i32IPart[5], i32FPart[5]);

	            //
	            // Print the magnetic data in the table.
	            //
	            UARTprintf("\033[9;17H%3d.%03d", i32IPart[6], i32FPart[6]);
	            UARTprintf("\033[9;40H%3d.%03d", i32IPart[7], i32FPart[7]);
	            UARTprintf("\033[9;63H%3d.%03d", i32IPart[8], i32FPart[8]);

			//
			// Print the Eulers in a table.
			//


			UARTprintf("\033[14;17H%3d.%03d", i32IPart[9], i32FPart[9]);
			UARTprintf("\033[14;40H%3d.%03d", i32IPart[10], i32FPart[10]);
			UARTprintf("\033[14;63H%3d.%03d", i32IPart[11], i32FPart[11]);




			//
	            // Print the quaternions in a table format.
	            //
	            UARTprintf("\033[19;14H%3d.%03d", i32IPart[12], i32FPart[12]);
	            UARTprintf("\033[19;32H%3d.%03d", i32IPart[13], i32FPart[13]);
	            UARTprintf("\033[19;50H%3d.%03d", i32IPart[14], i32FPart[14]);
	            UARTprintf("\033[19;68H%3d.%03d", i32IPart[15], i32FPart[15]);

	           // currentSpeed=i32IPart[9]; how to get velocity from speed



	}
		return  pitch= i32IPart[9];

	}
void MPU9150AppCallback(void *pvCallbackData, unsigned     int ui8Status)
{
		
    uint_fast32_t ui32Idx, ui32CompDCMStarted;
    float pfData[16];
    float *pfAccel, *pfGyro, *pfMag, *pfEulers, *pfQuaternion;
		//unsigned char tempString[30]={0};
		
		if(ui8Status == I2CM_STATUS_SUCCESS&&sensorTurn==4)
		{
	
				//
				// Initialize convenience pointers that clean up and clarify the code
				// meaning. We want all the data in a single contiguous array so that
				// we can make our pretty printing easier later.
				//
				pfAccel = pfData;
				pfGyro = pfData + 3;
				pfMag = pfData + 6;
				pfEulers = pfData + 9;
				pfQuaternion = pfData + 12;
			
				//
				// Get floating point version of the Accel Data in m/s^2.
				//
				MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
																 pfAccel + 2);

				//
				// Get floating point version of angular velocities in rad/sec
				//
				MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
																pfGyro + 2);

				//
				// Get floating point version of magnetic fields strength in tesla
				//
				MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
																	 pfMag + 2);
																	 
				//
				// Check if this is our first data ever.
				//
				if(ui32CompDCMStarted == 0)
				{
						//
						// Set flag indicating that DCM is started.
						// Perform the seeding of the DCM with the first data set.
						//
						ui32CompDCMStarted = 1;
						CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
																 pfMag[2]);
						CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
															 pfAccel[2]);
						CompDCMGyroUpdate(&g_sCompDCMInst, pfGyro[0], pfGyro[1],
															pfGyro[2]);
						CompDCMStart(&g_sCompDCMInst);
				}
				else
				{
						//
						// DCM Is already started.  Perform the incremental update.
						//
						CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
																 pfMag[2]);
						CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
															 pfAccel[2]);
						CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
															-pfGyro[2]);
						CompDCMUpdate(&g_sCompDCMInst);
				}
				
				//
				// Get Euler data. (Roll Pitch Yaw)
				//
				CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
														 pfEulers + 2);

				//
				// Get Quaternions.
				//
				CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);

				//
				// convert mag data to micro-tesla for better human interpretation.
				//
				pfMag[0] *= 1e6;
				pfMag[1] *= 1e6;
				pfMag[2] *= 1e6;

				//
				// Convert Eulers to degrees. 180/PI = 57.29...
				// Convert Yaw to 0 to 360 to approximate compass headings.
				//
				pfEulers[0] *= 57.295779513082320876798154814105f;
				pfEulers[1] *= 57.295779513082320876798154814105f;
				pfEulers[2] *= 57.295779513082320876798154814105f;
				if(pfEulers[2] < 0)
				{
						pfEulers[2] += 360.0f;
				}

				//
				// Now drop back to using the data as a single array for the
				// purpose of decomposing the float into a integer part and a
				// fraction (decimal) part.
				//
				for(ui32Idx = 0; ui32Idx < 16; ui32Idx++)
				{
						//
						// Conver float value to a integer truncating the decimal part.
						//
						i32IPart[ui32Idx] = (int32_t) pfData[ui32Idx];

						//
						// Multiply by 1000 to preserve first three decimal values.
						// Truncates at the 3rd decimal place.
						//
						i32FPart[ui32Idx] = (int32_t) (pfData[ui32Idx] * 1000.0f);

						//
						// Subtract off the integer part from this newly formed decimal
						// part.
						//
						i32FPart[ui32Idx] = i32FPart[ui32Idx] -
																(i32IPart[ui32Idx] * 1000);

						//
						// make the decimal part a positive number for display.
						//
						if(i32FPart[ui32Idx] < 0)
						{
								i32FPart[ui32Idx] *= -1;
						}
				}

//				//
//				// Print the acceleration numbers in the table.
//				//
//				sprintf(tempString,"\033[5;17H%3d.%03d", i32IPart[0], i32FPart[0]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[5;40H%3d.%03d", i32IPart[1], i32FPart[1]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[5;63H%3d.%03d", i32IPart[2], i32FPart[2]);
//				CLI_Write(tempString);

//				//
//				// Print the angular velocities in the table.
//				//
//				sprintf(tempString,"\033[7;17H%3d.%03d", i32IPart[3], i32FPart[3]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[7;40H%3d.%03d", i32IPart[4], i32FPart[4]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[7;63H%3d.%03d", i32IPart[5], i32FPart[5]);
//				CLI_Write(tempString);

//				//
//				// Print the magnetic data in the table.
//				//
//				sprintf(tempString,"\033[9;17H%3d.%03d", i32IPart[6], i32FPart[6]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[9;40H%3d.%03d", i32IPart[7], i32FPart[7]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[9;63H%3d.%03d", i32IPart[8], i32FPart[8]);
//				CLI_Write(tempString);

//				//
//				// Print the Eulers in a table.
//				//
//				sprintf(tempString,"\033[14;17H%3d.%03d", i32IPart[9], i32FPart[9]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[14;40H%3d.%03d", i32IPart[10], i32FPart[10]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[14;63H%3d.%03d", i32IPart[11], i32FPart[11]);
//				CLI_Write(tempString);

//				//
//				// Print the quaternions in a table format.
//				//
//				sprintf(tempString,"\033[19;14H%3d.%03d", i32IPart[12], i32FPart[12]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[19;32H%3d.%03d", i32IPart[13], i32FPart[13]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[19;50H%3d.%03d", i32IPart[14], i32FPart[14]);
//				CLI_Write(tempString);
//				sprintf(tempString,"\033[19;68H%3d.%03d", i32IPart[15], i32FPart[15]);
//				CLI_Write(tempString);
//				CLI_Write("\n\r");
				sensorTurn=(sensorTurn+1)%NumberOfSensor;
				TimerEnable(TIMER1_BASE, TIMER_A);
	 }

        
}
예제 #4
0
//*****************************************************************************
//
// Main application entry point.
//
//*****************************************************************************
int
main(void)
{
	int_fast32_t i32IPart[16], i32FPart[16];
	uint_fast32_t ui32Idx, ui32CompDCMStarted;
	float pfData[16];
	float *pfAccel, *pfGyro, *pfMag, *pfEulers, *pfQuaternion;

	//
	// Initialize convenience pointers that clean up and clarify the code
	// meaning. We want all the data in a single contiguous array so that
	// we can make our pretty printing easier later.
	//
	pfAccel = pfData;
	pfGyro = pfData + 3;
	pfMag = pfData + 6;
	pfEulers = pfData + 9;
	pfQuaternion = pfData + 12;

	//
	// Setup the system clock to run at 40 Mhz from PLL with crystal reference
	//
	SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ |
			SYSCTL_OSC_MAIN);

	//
	// Enable port B used for motion interrupt.
	//
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);

	//
	// Initialize the UART.
	//
	ConfigureUART();

	//
	// Print the welcome message to the terminal.
	//
	UARTprintf("\033[2JMPU9150 Raw Example\n");

	//
	// Set the color to a purple approximation.
	//
	g_pui32Colors[RED] = 0x8000;
	g_pui32Colors[BLUE] = 0x8000;
	g_pui32Colors[GREEN] = 0x0000;

	//
	// Initialize RGB driver.
	//
	RGBInit(0);
	RGBColorSet(g_pui32Colors);
	RGBIntensitySet(0.5f);
	RGBEnable();

	//
	// The I2C3 peripheral must be enabled before use.
	//
	SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C3);
	SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);

	//
	// Configure the pin muxing for I2C3 functions on port D0 and D1.
	//
	GPIOPinConfigure(GPIO_PD0_I2C3SCL);
	GPIOPinConfigure(GPIO_PD1_I2C3SDA);

	//
	// Select the I2C function for these pins.  This function will also
	// configure the GPIO pins pins for I2C operation, setting them to
	// open-drain operation with weak pull-ups.  Consult the data sheet
	// to see which functions are allocated per pin.
	//
	GPIOPinTypeI2CSCL(GPIO_PORTD_BASE, GPIO_PIN_0);
	GPIOPinTypeI2C(GPIO_PORTD_BASE, GPIO_PIN_1);

	//
	// Configure and Enable the GPIO interrupt. Used for INT signal from the
	// MPU9150
	//
	GPIOPinTypeGPIOInput(GPIO_PORTB_BASE, GPIO_PIN_2);
	GPIOIntEnable(GPIO_PORTB_BASE, GPIO_PIN_2);
	GPIOIntTypeSet(GPIO_PORTB_BASE, GPIO_PIN_2, GPIO_FALLING_EDGE);
	IntEnable(INT_GPIOB);

	//
	// Keep only some parts of the systems running while in sleep mode.
	// GPIOB is for the MPU9150 interrupt pin.
	// UART0 is the virtual serial port
	// TIMER0, TIMER1 and WTIMER5 are used by the RGB driver
	// I2C3 is the I2C interface to the ISL29023
	//
	SysCtlPeripheralClockGating(true);
	SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_GPIOB);
	SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_UART0);
	SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER0);
	SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER1);
	SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_I2C3);
	SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_WTIMER5);

	//
	// Enable interrupts to the processor.
	//
	IntMasterEnable();

	//
	// Initialize I2C3 peripheral.
	//
	I2CMInit(&g_sI2CInst, I2C3_BASE, INT_I2C3, 0xff, 0xff,
			SysCtlClockGet());

	//
	// Initialize the MPU9150 Driver.
	//
	MPU9150Init(&g_sMPU9150Inst, &g_sI2CInst, MPU9150_I2C_ADDRESS,
			MPU9150AppCallback, &g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Write application specifice sensor configuration such as filter settings
	// and sensor range settings.
	//
	g_sMPU9150Inst.pui8Data[0] = MPU9150_CONFIG_DLPF_CFG_94_98;
	g_sMPU9150Inst.pui8Data[1] = MPU9150_GYRO_CONFIG_FS_SEL_250;
	g_sMPU9150Inst.pui8Data[2] = (MPU9150_ACCEL_CONFIG_ACCEL_HPF_5HZ |
			MPU9150_ACCEL_CONFIG_AFS_SEL_2G);
	MPU9150Write(&g_sMPU9150Inst, MPU9150_O_CONFIG, g_sMPU9150Inst.pui8Data, 3,
			MPU9150AppCallback, &g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Configure the data ready interrupt pin output of the MPU9150.
	//
	g_sMPU9150Inst.pui8Data[0] = MPU9150_INT_PIN_CFG_INT_LEVEL |
			MPU9150_INT_PIN_CFG_INT_RD_CLEAR |
			MPU9150_INT_PIN_CFG_LATCH_INT_EN;
	g_sMPU9150Inst.pui8Data[1] = MPU9150_INT_ENABLE_DATA_RDY_EN;
	MPU9150Write(&g_sMPU9150Inst, MPU9150_O_INT_PIN_CFG,
			g_sMPU9150Inst.pui8Data, 2, MPU9150AppCallback,
			&g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Initialize the DCM system. 50 hz sample rate.
	// accel weight = .2, gyro weight = .8, mag weight = .2
	//
	CompDCMInit(&g_sCompDCMInst, 1.0f / 50.0f, 0.2f, 0.6f, 0.2f);

	UARTprintf("\033[2J\033[H");
	UARTprintf("MPU9150 9-Axis Simple Data Application Example\n\n");
	UARTprintf("\033[20GX\033[31G|\033[43GY\033[54G|\033[66GZ\n\n");
	UARTprintf("Accel\033[8G|\033[31G|\033[54G|\n\n");
	UARTprintf("Gyro\033[8G|\033[31G|\033[54G|\n\n");
	UARTprintf("Mag\033[8G|\033[31G|\033[54G|\n\n");
	UARTprintf("\n\033[20GRoll\033[31G|\033[43GPitch\033[54G|\033[66GYaw\n\n");
	UARTprintf("Eulers\033[8G|\033[31G|\033[54G|\n\n");

	UARTprintf("\n\033[17GQ1\033[26G|\033[35GQ2\033[44G|\033[53GQ3\033[62G|"
			"\033[71GQ4\n\n");
	UARTprintf("Q\033[8G|\033[26G|\033[44G|\033[62G|\n\n");

	//
	// Enable blinking indicates config finished successfully
	//
	RGBBlinkRateSet(1.0f);

	ui32CompDCMStarted = 0;

	while(1)
	{
		//
		// Go to sleep mode while waiting for data ready.
		//
		while(!g_vui8I2CDoneFlag)
		{
			SysCtlSleep();
		}

		//
		// Clear the flag
		//
		g_vui8I2CDoneFlag = 0;

		//
		// Get floating point version of the Accel Data in m/s^2.
		//
		MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
				pfAccel + 2);

		//
		// Get floating point version of angular velocities in rad/sec
		//
		MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
				pfGyro + 2);

		//
		// Get floating point version of magnetic fields strength in tesla
		//
		MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
				pfMag + 2);

		//
		// Check if this is our first data ever.
		//
		if(ui32CompDCMStarted == 0)
		{
			//
			// Set flag indicating that DCM is started.
			// Perform the seeding of the DCM with the first data set.
			//
			ui32CompDCMStarted = 1;
			CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
					pfMag[2]);
			CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
					pfAccel[2]);
			CompDCMGyroUpdate(&g_sCompDCMInst, pfGyro[0], pfGyro[1],
					pfGyro[2]);
			CompDCMStart(&g_sCompDCMInst);
		}
		else
		{
			//
			// DCM Is already started.  Perform the incremental update.
			//
			CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
					pfMag[2]);
			CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
					pfAccel[2]);
			CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
					-pfGyro[2]);
			CompDCMUpdate(&g_sCompDCMInst);
		}

		//
		// Increment the skip counter.  Skip counter is used so we do not
		// overflow the UART with data.
		//
		g_ui32PrintSkipCounter++;
		if(g_ui32PrintSkipCounter >= PRINT_SKIP_COUNT)
		{
			//
			// Reset skip counter.
			//
			g_ui32PrintSkipCounter = 0;

			//
			// Get Euler data. (Roll Pitch Yaw)
			//
			CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
					pfEulers + 2);

			//
			// Get Quaternions.
			//
			CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);

			//
			// convert mag data to micro-tesla for better human interpretation.
			//
			pfMag[0] *= 1e6;
			pfMag[1] *= 1e6;
			pfMag[2] *= 1e6;

			//
			// Convert Eulers to degrees. 180/PI = 57.29...
			// Convert Yaw to 0 to 360 to approximate compass headings.
			//
			pfEulers[0] *= 57.295779513082320876798154814105f;
			pfEulers[1] *= 57.295779513082320876798154814105f;
			pfEulers[2] *= 57.295779513082320876798154814105f;
			if(pfEulers[2] < 0)
			{
				pfEulers[2] += 360.0f;
			}

			//
			// Now drop back to using the data as a single array for the
			// purpose of decomposing the float into a integer part and a
			// fraction (decimal) part.
			//
			for(ui32Idx = 0; ui32Idx < 16; ui32Idx++)
			{
				//
				// Conver float value to a integer truncating the decimal part.
				//
				i32IPart[ui32Idx] = (int32_t) pfData[ui32Idx];

				//
				// Multiply by 1000 to preserve first three decimal values.
				// Truncates at the 3rd decimal place.
				//
				i32FPart[ui32Idx] = (int32_t) (pfData[ui32Idx] * 1000.0f);

				//
				// Subtract off the integer part from this newly formed decimal
				// part.
				//
				i32FPart[ui32Idx] = i32FPart[ui32Idx] -
						(i32IPart[ui32Idx] * 1000);

				//
				// make the decimal part a positive number for display.
				//
				if(i32FPart[ui32Idx] < 0)
				{
					i32FPart[ui32Idx] *= -1;
				}
			}

			//
			// Print the acceleration numbers in the table.
			//
			UARTprintf("\033[5;17H%3d.%03d", i32IPart[0], i32FPart[0]);
			UARTprintf("\033[5;40H%3d.%03d", i32IPart[1], i32FPart[1]);
			UARTprintf("\033[5;63H%3d.%03d", i32IPart[2], i32FPart[2]);

			//
			// Print the angular velocities in the table.
			//
			UARTprintf("\033[7;17H%3d.%03d", i32IPart[3], i32FPart[3]);
			UARTprintf("\033[7;40H%3d.%03d", i32IPart[4], i32FPart[4]);
			UARTprintf("\033[7;63H%3d.%03d", i32IPart[5], i32FPart[5]);

			//
			// Print the magnetic data in the table.
			//
			UARTprintf("\033[9;17H%3d.%03d", i32IPart[6], i32FPart[6]);
			UARTprintf("\033[9;40H%3d.%03d", i32IPart[7], i32FPart[7]);
			UARTprintf("\033[9;63H%3d.%03d", i32IPart[8], i32FPart[8]);

			//
			// Print the Eulers in a table.
			//
			UARTprintf("\033[14;17H%3d.%03d", i32IPart[9], i32FPart[9]);
			UARTprintf("\033[14;40H%3d.%03d", i32IPart[10], i32FPart[10]);
			UARTprintf("\033[14;63H%3d.%03d", i32IPart[11], i32FPart[11]);

			//
			// Print the quaternions in a table format.
			//
			UARTprintf("\033[19;14H%3d.%03d", i32IPart[12], i32FPart[12]);
			UARTprintf("\033[19;32H%3d.%03d", i32IPart[13], i32FPart[13]);
			UARTprintf("\033[19;50H%3d.%03d", i32IPart[14], i32FPart[14]);
			UARTprintf("\033[19;68H%3d.%03d", i32IPart[15], i32FPart[15]);

		}
	}
}
예제 #5
0
파일: motion.c 프로젝트: PhamVanNhi/ECE5770
//*****************************************************************************
//
// Main function to handler motion events that are triggered by the MPU9150
// data ready interrupt.
//
//*****************************************************************************
void
MotionMain(void)
{
    switch(g_ui8MotionState)
    {
        //
        // This is our initial data set from the MPU9150, start the DCM.
        //
        case MOTION_STATE_INIT:
        {
            //
            // Check the read data buffer of the MPU9150 to see if the
            // Magnetometer data is ready and present. This may not be the case
            // for the first few data captures.
            //
            if(g_sMPU9150Inst.pui8Data[14] & AK8975_ST1_DRDY)
            {
                //
                // Get local copy of Accel and Mag data to feed to the DCM
                // start.
                //
                MPU9150DataAccelGetFloat(&g_sMPU9150Inst, g_pfAccel,
                                         g_pfAccel + 1, g_pfAccel + 2);
                MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, g_pfMag,
                                          g_pfMag + 1, g_pfMag + 2);
                MPU9150DataGyroGetFloat(&g_sMPU9150Inst, g_pfGyro,
                                        g_pfGyro + 1, g_pfGyro + 2);

                //
                // Feed the initial measurements to the DCM and start it.
                // Due to the structure of our MotionMagCallback function,
                // the floating point magneto data is already in the local
                // data buffer.
                //
                CompDCMMagnetoUpdate(&g_sCompDCMInst, g_pfMag[0], g_pfMag[1],
                                     g_pfMag[2]);
                CompDCMAccelUpdate(&g_sCompDCMInst, g_pfAccel[0], g_pfAccel[1],
                                   g_pfAccel[2]);
                CompDCMStart(&g_sCompDCMInst);

                //
                // Proceed to the run state.
                //
                g_ui8MotionState = MOTION_STATE_RUN;

            }

            //
            // Turn off the LED to show we are done processing motion data.
            //
            g_pui32RGBColors[RED] = 0;
            RGBColorSet(g_pui32RGBColors);

            //
            // Finished
            //
            break;
        }

        //
        // DCM has been started and we are ready for normal operations.
        //
        case MOTION_STATE_RUN:
        {
            //
            // Get the latest Euler data from the DCM. DCMUpdate is done
            // inside the interrupt routine to insure it is not skipped and
            // that the timing is consistent.
            //
            CompDCMComputeEulers(&g_sCompDCMInst, g_pfEulers,
                                 g_pfEulers + 1, g_pfEulers + 2);

            //
            // Pass the latest sensor data back to the Gesture system for
            // classification.  What state do i think i am in?
            //
            GestureEmitClassify(&g_sGestureInst, g_pfEulers, g_pfAccel, g_pfGyro);

            //
            // Update best guess state based on past history and current
            // estimate.
            //
            GestureUpdate(&g_sGestureInst, g_sGestureInst.ui16Emit);

            //
            // Turn off the LED to show we are done processing motion data.
            //
            g_pui32RGBColors[RED] = 0;
            RGBColorSet(g_pui32RGBColors);

            //
            // Finished
            //
            break;
        }

        //
        // An I2C error has occurred at some point. Usually these are due to
        // asynchronous resets of the main MCU and the I2C peripherals. This
        // can cause the slave to hold the bus and the MCU to think it cannot
        // send.  In practice there are ways to clear this condition.  They are
        // not implemented here.  To clear power cycle the board.
        //
        case MOTION_STATE_ERROR:
        {
            //
            // Our tick counter and blink mechanism may not be safe across
            // rollovers of the g_ui32SysTickCount variable.  This rollover
            // only occurs after 1.3+ years of continuous operation.
            //
            if(g_ui32SysTickCount > (g_ui32RGBMotionBlinkCounter + 20))
            {
                //
                // 20 ticks have expired since we last toggled so turn off the
                // LED and reset the counter.
                //
                g_ui32RGBMotionBlinkCounter = g_ui32SysTickCount;
                g_pui32RGBColors[RED] = 0;
                RGBColorSet(g_pui32RGBColors);
            }
            else if(g_ui32SysTickCount == (g_ui32RGBMotionBlinkCounter + 10))
            {
                //
                // 10 ticks have expired since the last counter reset.  turn
                // on the RED LED.
                //
                g_pui32RGBColors[RED] = 0xFFFF;
                RGBColorSet(g_pui32RGBColors);
            }
            break;
        }
    }
}
예제 #6
0
파일: motion.c 프로젝트: peterliu2/tivaWare
//*****************************************************************************
//
// Main function to handler motion events that are triggered by the MPU9150
// data ready interrupt.
//
//*****************************************************************************
void
MotionMain(void)
{
    switch(g_ui8MotionState) {
        //
        // This is our initial data set from the MPU9150, start the DCM.
        //
        case MOTION_STATE_INIT: {
            //
            // Check the read data buffer of the MPU9150 to see if the
            // Magnetometer data is ready and present. This may not be the case
            // for the first few data captures.
            //
            if(g_sMPU9150Inst.pui8Data[14] & AK8975_ST1_DRDY) {
                //
                // Get local copy of Accel and Mag data to feed to the DCM
                // start.
                //
                MPU9150DataAccelGetFloat(&g_sMPU9150Inst, g_pfAccel,
                                         g_pfAccel + 1, g_pfAccel + 2);
                MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, g_pfMag,
                                           g_pfMag + 1, g_pfMag + 2);
                MPU9150DataGyroGetFloat(&g_sMPU9150Inst, g_pfGyro,
                                        g_pfGyro + 1, g_pfGyro + 2);

                //
                // Feed the initial measurements to the DCM and start it.
                // Due to the structure of our MotionMagCallback function,
                // the floating point magneto data is already in the local
                // data buffer.
                //
                CompDCMMagnetoUpdate(&g_sCompDCMInst, g_pfMag[0], g_pfMag[1],
                                     g_pfMag[2]);
                CompDCMAccelUpdate(&g_sCompDCMInst, g_pfAccel[0], g_pfAccel[1],
                                   g_pfAccel[2]);
                CompDCMStart(&g_sCompDCMInst);

                //
                // Proceed to the run state.
                //
                g_ui8MotionState = MOTION_STATE_RUN;
            }

            //
            // Finished
            //
            break;
        }

        //
        // DCM has been started and we are ready for normal operations.
        //
        case MOTION_STATE_RUN: {
            //
            // Get the latest Euler data from the DCM. DCMUpdate is done
            // inside the interrupt routine to insure it is not skipped and
            // that the timing is consistent.
            //
            CompDCMComputeEulers(&g_sCompDCMInst, g_pfEulers,
                                 g_pfEulers + 1, g_pfEulers + 2);

            //
            // Pass the latest sensor data back to the Gesture system for
            // classification.  What state do i think i am in?
            //
            GestureEmitClassify(&g_sGestureInst, g_pfEulers, g_pfAccel,
                                g_pfGyro);

            //
            // Update best guess state based on past history and current
            // estimate.
            //
            GestureUpdate(&g_sGestureInst, g_sGestureInst.ui16Emit);

            //
            // This tick counter and blink mechanism may not be safe across
            // rollovers of the g_ui32SysTickCount variable.  This rollover
            // only occurs after 1.3+ years of continuous operation.
            //
            if(g_ui32SysTickCount > (g_ui32MotionBlinkCounter + 10)) {
                //
                // 10 ticks have expired since we last toggled so toggle the
                // blue LED and reset the counter.
                //
                g_ui32MotionBlinkCounter = g_ui32SysTickCount;
                MAP_GPIOPinWrite(GPIO_PORTQ_BASE, GPIO_PIN_4,
                                 ((GPIOPinRead(GPIO_PORTQ_BASE, GPIO_PIN_4)) ^
                                  GPIO_PIN_4));
            }

            //
            // Finished
            //
            break;
        }

        //
        // An I2C error has occurred at some point. Usually these are due to
        // asynchronous resets of the main MCU and the I2C peripherals. This
        // can cause the slave to hold the bus and the MCU to think it cannot
        // send.  In practice there are ways to clear this condition.  They are
        // not implemented here.  To clear power cycle the board.
        //
        case MOTION_STATE_ERROR: {
            //
            // Display this error and how to clear.
            //
            DpyRectFill(g_sContext.psDisplay, &g_sUserInfoRect, ClrBlack);
            GrContextForegroundSet(&g_sContext, ClrGray);
            GrContextFontSet(&g_sContext, g_psFontCmss16b);
            GrStringDraw(&g_sContext, "I2C Error has occurred. Power cycle",
                         -1, 10, 140, 1);
            GrStringDraw(&g_sContext, "board to clear this error.", -1, 10,
                         160, 1);
            GrContextForegroundSet(&g_sContext, ClrWhite);
            GrContextFontSet(&g_sContext, g_psFontCm18b);

            break;
        }
    }
}
//*****************************************************************************
//
// Main application entry point.
//
//*****************************************************************************
int main(void) {
	int_fast32_t i32IPart[17], i32FPart[17];
	uint_fast32_t ui32Idx, ui32CompDCMStarted;
	float pfData[17];
	float *pfAccel, *pfGyro, *pfMag, *pfEulers, *pfQuaternion;
	float *direction;

	//
	// Initialize convenience pointers that clean up and clarify the code
	// meaning. We want all the data in a single contiguous array so that
	// we can make our pretty printing easier later.
	//
	pfAccel = pfData;
	pfGyro = pfData + 3;
	pfMag = pfData + 6;
	pfEulers = pfData + 9;
	pfQuaternion = pfData + 12;
	direction = pfData + 16;

	//
	// Setup the system clock to run at 40 Mhz from PLL with crystal reference
	//
	ROM_SysCtlClockSet(
			SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ
					| SYSCTL_OSC_MAIN);

	//
	// Enable port E used for motion interrupt.
	//
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

	//
	// Enable port F used for calibration.
	//
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);

	//
	// Initialize the UART.
	//
	ConfigureUART();

	/* EEPROM SETTINGS */
	SysCtlPeripheralEnable(SYSCTL_PERIPH_EEPROM0); // EEPROM activate
	EEPROMInit(); // EEPROM start

	//
	// Print the welcome message to the terminal.
	//
	UARTprintf("\033[2JMPU9150 Raw Example\n");

	//
	// Set the color to a purple approximation.
	//
	g_pui32Colors[RED] = 0x8000;
	g_pui32Colors[BLUE] = 0x8000;
	g_pui32Colors[GREEN] = 0x8000;

	//
	// Initialize RGB driver.
	//
	RGBInit(0);
	RGBColorSet(g_pui32Colors);
	RGBIntensitySet(0.5f);
	RGBEnable();

	// Initialize BGLib
	bglib_output = output;
	ConfigureBLE();

	//
	// The I2C3 peripheral must be enabled before use.
	//
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C3);
	ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);

	//
	// Configure the pin muxing for I2C3 functions on port D0 and D1.
	//
	ROM_GPIOPinConfigure(GPIO_PD0_I2C3SCL);
	ROM_GPIOPinConfigure(GPIO_PD1_I2C3SDA);

	//
	// Select the I2C function for these pins.  This function will also
	// configure the GPIO pins pins for I2C operation, setting them to
	// open-drain operation with weak pull-ups.  Consult the data sheet
	// to see which functions are allocated per pin.
	//
	GPIOPinTypeI2CSCL(GPIO_PORTD_BASE, GPIO_PIN_0);
	ROM_GPIOPinTypeI2C(GPIO_PORTD_BASE, GPIO_PIN_1);

	//
	// Configure and Enable the GPIO interrupt. Used for INT signal from the
	// MPU9150
	//
	ROM_GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_2);
	GPIOIntEnable(GPIO_PORTE_BASE, GPIO_PIN_2);
	ROM_GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_2, GPIO_FALLING_EDGE);
	ROM_IntEnable(INT_GPIOE);

	//
	// Keep only some parts of the systems running while in sleep mode.
	// GPIOE is for the MPU9150 interrupt pin.
	// UART0 is the virtual serial port
	// TIMER0, TIMER1 and WTIMER5 are used by the RGB driver
	// I2C3 is the I2C interface to the ISL29023
	//
	ROM_SysCtlPeripheralClockGating(true);
	ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_GPIOE);
	ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_UART0);
	ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER0);
	ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_TIMER1);
	ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_I2C3);
	ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_WTIMER5);

	//
	// Enable interrupts to the processor.
	//
	ROM_IntMasterEnable();

	//
	// Initialize I2C3 peripheral.
	//
	I2CMInit(&g_sI2CInst, I2C3_BASE, INT_I2C3, 0xff, 0xff,
			ROM_SysCtlClockGet());

	//
	// Initialize the MPU9150 Driver.
	//
	MPU9150Init(&g_sMPU9150Inst, &g_sI2CInst, MPU9150_I2C_ADDRESS,
			MPU9150AppCallback, &g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Configure the sampling rate to 1000 Hz / (1+24).
	//
	g_sMPU9150Inst.pui8Data[0] = 24;
	MPU9150Write(&g_sMPU9150Inst, MPU9150_O_SMPLRT_DIV, g_sMPU9150Inst.pui8Data,
			1, MPU9150AppCallback, &g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Write application specifice sensor configuration such as filter settings
	// and sensor range settings.
	//
	g_sMPU9150Inst.pui8Data[0] = MPU9150_CONFIG_DLPF_CFG_94_98;
	g_sMPU9150Inst.pui8Data[1] = MPU9150_GYRO_CONFIG_FS_SEL_250;
	g_sMPU9150Inst.pui8Data[2] = (MPU9150_ACCEL_CONFIG_ACCEL_HPF_5HZ
			| MPU9150_ACCEL_CONFIG_AFS_SEL_2G);
//	g_sMPU9150Inst.pui8Data[2] = MPU9150_ACCEL_CONFIG_AFS_SEL_2G;
	MPU9150Write(&g_sMPU9150Inst, MPU9150_O_CONFIG, g_sMPU9150Inst.pui8Data, 3,
			MPU9150AppCallback, &g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Configure the data ready interrupt pin output of the MPU9150.
	//
	g_sMPU9150Inst.pui8Data[0] = MPU9150_INT_PIN_CFG_INT_LEVEL
			| MPU9150_INT_PIN_CFG_INT_RD_CLEAR
			| MPU9150_INT_PIN_CFG_LATCH_INT_EN;
	g_sMPU9150Inst.pui8Data[1] = MPU9150_INT_ENABLE_DATA_RDY_EN;
	MPU9150Write(&g_sMPU9150Inst, MPU9150_O_INT_PIN_CFG,
			g_sMPU9150Inst.pui8Data, 2, MPU9150AppCallback, &g_sMPU9150Inst);

	//
	// Wait for transaction to complete
	//
	MPU9150AppI2CWait(__FILE__, __LINE__);

	//
	// Initialize the DCM system. 40 hz sample rate.
	// accel weight = .2, gyro weight = .8, mag weight = .2
	//
	CompDCMInit(&g_sCompDCMInst, 1.0f / 40.0f, 0.2f, 0.6f, 0.2f);

	//
	// Enable blinking indicates config finished successfully
	//
	RGBBlinkRateSet(1.0f);

	//
	// Configure and Enable the GPIO interrupt. Used for calibration
	//
	HWREG(GPIO_PORTF_BASE + GPIO_O_LOCK) = GPIO_LOCK_KEY;
	HWREG(GPIO_PORTF_BASE + GPIO_O_CR) |= 0x01;
	ROM_GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_4);
	GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_STRENGTH_2MA,
			GPIO_PIN_TYPE_STD_WPU);
	ROM_IntEnable(INT_GPIOF);
	ROM_GPIOIntTypeSet(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_FALLING_EDGE);
	GPIOIntEnable(GPIO_PORTF_BASE, GPIO_PIN_4);
	g_calibrationState = 0;

	ui32CompDCMStarted = 0;
	// Configure the white noise, read the error from EEPROM
	EEPROMRead((uint32_t *) zeroErrorAccel,
			EEPROM_ZERO_ERROR_ACCELERATION_ADDRESS, 12);
	EEPROMRead((uint32_t *) linearErrorAccel,
			EEPROM_LINEAR_ERROR_ACCELERATION_ADDRESS, 12);
	EEPROMRead((uint32_t *) zeroErrorGyro, EEPROM_ZERO_ERROR_GYROSCOPE_ADDRESS,
			12);

	while (1) {
		//
		// Go to sleep mode while waiting for data ready.
		//
		while (!g_vui8I2CDoneFlag) {
			//ROM_SysCtlSleep();
		}

		//
		// Clear the flag
		//
		g_vui8I2CDoneFlag = 0;

		//
		// Get floating point version of the Accel Data in m/s^2.
		//
		MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
				pfAccel + 2);

		//
		// Get floating point version of angular velocities in rad/sec
		//
		MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
				pfGyro + 2);

		//
		// Get floating point version of magnetic fields strength in tesla
		//
		MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
				pfMag + 2);

		if (g_calibrationState == 2) {
			zeroErrorAccel[0] = (pfAccel[0]
					+ zeroErrorAccel[0] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			zeroErrorAccel[1] = (pfAccel[1]
					+ zeroErrorAccel[1] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			accelAtGravity[2] = (pfAccel[2]
					+ accelAtGravity[2] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			zeroErrorGyro[0] = (pfGyro[0]
					+ zeroErrorGyro[0] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			zeroErrorGyro[1] = (pfGyro[1]
					+ zeroErrorGyro[1] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			zeroErrorGyro[2] = (pfGyro[2]
					+ zeroErrorGyro[2] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			g_calibrationCount++;
			if (g_calibrationCount > 500) {
				Calibration();
			}
			continue;
		} else if (g_calibrationState == 4) {
			zeroErrorAccel[2] = (pfAccel[2]
					+ zeroErrorAccel[2] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			accelAtGravity[1] = (pfAccel[1]
					+ accelAtGravity[1] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			g_calibrationCount++;
			if (g_calibrationCount > 500) {
				Calibration();
			}
			continue;
		} else if (g_calibrationState == 6) {
			accelAtGravity[0] = (pfAccel[0]
					+ accelAtGravity[0] * g_calibrationCount)
					/ (g_calibrationCount + 1);
			g_calibrationCount++;
			if (g_calibrationCount > 500) {
				Calibration();
			}
			continue;
		}

		// Cancel out white noise
//		pfAccel[0] = pfAccel[0] - zeroErrorAccel[0];
//		pfAccel[1] = pfAccel[1] - zeroErrorAccel[1];
//		pfAccel[2] = pfAccel[2] - zeroErrorAccel[2];
//		pfGyro[0] = pfGyro[0] - zeroErrorGyro[0];
//		pfGyro[1] = pfGyro[1] - zeroErrorGyro[1];
//		pfGyro[2] = pfGyro[2] - zeroErrorGyro[2];
//		// Straighten out linear noise
//		pfAccel[0] = pfAccel[0] * (1 + linearErrorAccel[0]);
//		pfAccel[1] = pfAccel[1] * (1 + linearErrorAccel[1]);
//		pfAccel[2] = pfAccel[2] * (1 + linearErrorAccel[2]);

		//
		// Check if this is our first data ever.
		//
		if (ui32CompDCMStarted == 0) {
			//
			// Set flag indicating that DCM is started.
			// Perform the seeding of the DCM with the first data set.
			//
			ui32CompDCMStarted = 1;
			CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1], pfMag[2]);
			CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
					pfAccel[2]);
			CompDCMGyroUpdate(&g_sCompDCMInst, pfGyro[0], pfGyro[1], pfGyro[2]);
			CompDCMStart(&g_sCompDCMInst);
		} else {
			//
			// DCM Is already started.  Perform the incremental update.
			//
			CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1], pfMag[2]);
			CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
					pfAccel[2]);
			CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
					-pfGyro[2]);
			CompDCMUpdate(&g_sCompDCMInst);
		}

		//
		// Increment the skip counter.  Skip counter is used so we do not
		// overflow the UART with data.
		//
		g_ui32PrintSkipCounter++;
		if (g_ui32PrintSkipCounter >= PRINT_SKIP_COUNT) {
			//
			// Reset skip counter.
			//
			g_ui32PrintSkipCounter = 0;

			//
			// Get Euler data. (Roll Pitch Yaw)
			//
			CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
					pfEulers + 2);

			//
			// Get Quaternions.
			//
			CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);

			//
			// convert mag data to micro-tesla for better human interpretation.
			//
			pfMag[0] *= 1e6;
			pfMag[1] *= 1e6;
			pfMag[2] *= 1e6;

			//
			// Convert Eulers to degrees. 180/PI = 57.29...
			// Convert Yaw to 0 to 360 to approximate compass headings.
			//
			pfEulers[0] *= 57.295779513082320876798154814105f;
			pfEulers[1] *= 57.295779513082320876798154814105f;
			pfEulers[2] *= 57.295779513082320876798154814105f;
			if (pfEulers[2] < 0) {
				pfEulers[2] += 360.0f;
			}

			// Use pfMag to display degrees of the Magnetomer's x-axis
			// (y-axis of accelerometer and gyroscope) to the east of
			// magnetic north pole
//			direction[0] = 0;
//			if (pfMag[1] == 0) {
//				if (pfMag[0] > 0) {
//					direction[0] = 0;
//				} else {
//					direction[0] = 180;
//				}
//			} else if (pfMag[1] > 0) {
//				direction[0] = 90 - atan2f(pfMag[0], pfMag[1]) * 180 / 3.14159265359;
//			} else if (pfMag[1] < 0) {
//				direction[0] = 270 - atan2f(pfMag[0], pfMag[1]) * 180 / 3.14159265359;
//			}

			//
			// Now drop back to using the data as a single array for the
			// purpose of decomposing the float into a integer part and a
			// fraction (decimal) part.
			//
			for (ui32Idx = 0; ui32Idx < 17; ui32Idx++) {
				//
				// Conver float value to a integer truncating the decimal part.
				//
				i32IPart[ui32Idx] = (int32_t) pfData[ui32Idx];

				//
				// Multiply by 1000 to preserve first three decimal values.
				// Truncates at the 3rd decimal place.
				//
				i32FPart[ui32Idx] = (int32_t) (pfData[ui32Idx] * 1000.0f);

				//
				// Subtract off the integer part from this newly formed decimal
				// part.
				//
				i32FPart[ui32Idx] = i32FPart[ui32Idx]
						- (i32IPart[ui32Idx] * 1000);

				//
				// make the decimal part a positive number for display.
				//
				if (i32FPart[ui32Idx] < 0) {
					i32FPart[ui32Idx] *= -1;
				}
			}

			if (g_bleUserFlag == 1) {
				g_bleFlag = 0;
				ble_cmd_attributes_write(58, 0, 12, (uint8_t*)pfEuler);
				while (g_bleFlag == 0) {
				}
			} else if (g_bleDisconnectFlag == 1) {
				ConfigureBLE();
			}

			//
			// Print the acceleration numbers in the table.
			//
//			UARTprintf("%3d.%03d, ", i32IPart[0], i32FPart[0]);
//			UARTprintf("%3d.%03d, ", i32IPart[1], i32FPart[1]);
//			UARTprintf("%3d.%03d\n", i32IPart[2], i32FPart[2]);
//
//			//
//			// Print the angular velocities in the table.
//			//
//			UARTprintf("%3d.%03d, ", i32IPart[3], i32FPart[3]);
//			UARTprintf("%3d.%03d, ", i32IPart[4], i32FPart[4]);
//			UARTprintf("%3d.%03d\n", i32IPart[5], i32FPart[5]);
//
//			//
//			// Print the magnetic data in the table.
//			//
//			UARTprintf("%3d.%03d, ", i32IPart[6], i32FPart[6]);
//			UARTprintf("%3d.%03d, ", i32IPart[7], i32FPart[7]);
//			UARTprintf("%3d.%03d\n", i32IPart[8], i32FPart[8]);
//
//			//
//			// Print the direction in the table.
//			//
//			UARTprintf("%3d.%03d\n", i32IPart[16], i32FPart[16]);
//			//
//			// Print the Eulers in a table.
//			//
//			UARTprintf("%3d.%03d, ", i32IPart[9], i32FPart[9]);
//			UARTprintf("%3d.%03d, ", i32IPart[10], i32FPart[10]);
//			UARTprintf("%3d.%03d\n", i32IPart[11], i32FPart[11]);
//
//			//
//			// Print the quaternions in a table format.
//			//
//			UARTprintf("\033[19;14H%3d.%03d", i32IPart[12], i32FPart[12]);
//			UARTprintf("\033[19;32H%3d.%03d", i32IPart[13], i32FPart[13]);
//			UARTprintf("\033[19;50H%3d.%03d", i32IPart[14], i32FPart[14]);
//			UARTprintf("\033[19;68H%3d.%03d", i32IPart[15], i32FPart[15]);

		}
	}
}
예제 #8
0
//*****************************************************************************
//
// Takes in data array [16], compute dcm data and update the array
//
// Created by Bill Yiqiu Wang on 26July15
//
//*****************************************************************************
void
CompDCM(float data[]) {


	float *pfAccel, *pfGyro, *pfMag, *pfEulers, *pfQuaternion;

    //
    // Initialize convenience pointers that clean up and clarify the code
    // meaning. We want all the data in a single contiguous array so that
    // we can make our pretty printing easier later.
    //
    pfAccel = data;
    pfGyro = data + 3;
    pfMag = data + 6;
    pfEulers = data + 9;
    pfQuaternion = data + 12;

    //
    // Go to sleep mode while waiting for data ready.
    //
    while(!g_vui8I2CDoneFlag)
    {
        ROM_SysCtlSleep();
    }

    //
    // Clear the flag
    //
    g_vui8I2CDoneFlag = 0;

    //
    // Get floating point version of the Accel Data in m/s^2.
    //
    MPU9150DataAccelGetFloat(&g_sMPU9150Inst, pfAccel, pfAccel + 1,
                             pfAccel + 2);

    //
    // Get floating point version of angular velocities in rad/sec
    //
    MPU9150DataGyroGetFloat(&g_sMPU9150Inst, pfGyro, pfGyro + 1,
                            pfGyro + 2);

    //
    // Get floating point version of magnetic fields strength in tesla
    //
    MPU9150DataMagnetoGetFloat(&g_sMPU9150Inst, pfMag, pfMag + 1,
                               pfMag + 2);

    //
    // Perform the incremental update.
    //
    CompDCMMagnetoUpdate(&g_sCompDCMInst, pfMag[0], pfMag[1],
    		pfMag[2]);
    CompDCMAccelUpdate(&g_sCompDCMInst, pfAccel[0], pfAccel[1],
    		pfAccel[2]);
    CompDCMGyroUpdate(&g_sCompDCMInst, -pfGyro[0], -pfGyro[1],
    		-pfGyro[2]);
    CompDCMUpdate(&g_sCompDCMInst);

    //
    // Get Euler data. (Roll Pitch Yaw)
    //
    CompDCMComputeEulers(&g_sCompDCMInst, pfEulers, pfEulers + 1,
                             pfEulers + 2);

    //
    // Get Quaternions.
    //
    CompDCMComputeQuaternion(&g_sCompDCMInst, pfQuaternion);

    //
    // convert mag data to micro-tesla for better human interpretation.
    //
    pfMag[0] *= 1e6;
    pfMag[1] *= 1e6;
    pfMag[2] *= 1e6;

    //
    // Convert Eulers to degrees. 180/PI = 57.29...
    // Convert Yaw to 0 to 360 to approximate compass headings.
    //
    pfEulers[0] *= 57.295779513082320876798154814105f;
    pfEulers[1] *= 57.295779513082320876798154814105f;
    pfEulers[2] *= 57.295779513082320876798154814105f;
    if(pfEulers[2] < 0)
    {
    	pfEulers[2] += 360.0f;
    }
}