/**
*
* Initializes the RGB and sets it to shine solid green.
*
* \note This function is an extension of the rgb driver of the EK-TM4C123GXL
* firmware package provided by Texas Instruments.
*
* \note The RGB library does use Timer 0B and Timer 1A and Timer 1B
*
**/
void twe_RGBInitSetGreen(void) {
uint32_t RGBcolor[3];
twe_RGBInitSolid(0); //initialize the RGB for a solid output
RGBIntensitySet(0.3f); // Set the intensity level (0.0f to 1.0f)
RGBcolor[RED] = 0x0000;
RGBcolor[GREEN] = 0xFFFF; // set the color to green
RGBcolor[BLUE] = 0x0000;
RGBColorSet(RGBcolor);
RGBEnable();
}
//*****************************************************************************
//
// Initializes the switch task.
//
//*****************************************************************************
unsigned long
AccelerometerTaskInit(void)
{
// volatile unsigned char foo = 28;
RGBInit(1);
RGBIntensitySet(0.3f);
//
// Turn on the Green LED
//
g_ucColorsIndx = 0;
g_ulColors[g_ucColorsIndx] = 0x8000;
RGBColorSet(g_ulColors);
RGBDisable();
//////////////
g_pAccelerometerQueue = xQueueCreate(ACCELEROMETER_QUEUE_SIZE, ACCELEROMETER_ITEM_SIZE);
I2CSetup(I2C0BASEADDR, 40000);
if (((I2CRegRead(I2C0BASEADDR, SLAVEID, DEVID)) & 0xE5) == 0)
{
while (1);
}
I2CRegWrite(I2C0BASEADDR, SLAVEID, THRESH_FF, 0x06);
simple_delay();
I2CRegWrite(I2C0BASEADDR, SLAVEID, TIME_FF, 0x15);
simple_delay();
I2CRegWrite(I2C0BASEADDR, SLAVEID, INT_MAP, 0x00);
simple_delay();
I2CRegWrite(I2C0BASEADDR, SLAVEID, POWER_CTL, 0x08);
simple_delay();
I2CRegWrite(I2C0BASEADDR, SLAVEID, INT_ENABLE, 0x04);
simple_delay();
if(xTaskCreate(AccelerometerTask, (signed portCHAR *)"Acclerometer",
ACCELEROMETERTASKSTACKSIZE, NULL, tskIDLE_PRIORITY +
PRIORITY_ACCELEROMETER_TASK, NULL) != pdTRUE)
{
return(1);
}
//
// Success.
//
return(0);
}
/**
Sets RGB LED color to the selected values.
@pre halRgbLedPwmInit() has been called to initialize the PWM engine.
@post RGB LED displays the selected colors.
@note you must include hal_ek-lm4f120XL_rgb.h in this file and ensure that the .c file is included in the build path
*/
void halRgbSetLeds(uint8_t red, uint8_t blue, uint8_t green)
{
unsigned long ulColor[3];
#ifdef DEBUG_HAL_RGB_SET_LEDS
printf("halRgbSetLeds: R=%02x, B=%02x, G=%02x\r\n", red, blue, green);
#endif
ulColor[RED] = (unsigned long) ( (red << 8) * (RED_WHITE_BALANCE));
ulColor[BLUE] = (unsigned long) ( (blue << 8) * (BLUE_WHITE_BALANCE));
ulColor[GREEN] = (unsigned long) ( (green << 8) * (GREEN_WHITE_BALANCE));
RGBColorSet(ulColor);
}
//*****************************************************************************
//
// Command: rgb
//
// Takes a single argument that is a string between 000000 and FFFFFF.
// This is the HTML color code that should be used to set the RGB LED color.
//
// http://www.w3schools.com/html/html_colors.asp
//
//*****************************************************************************
int
CMD_rgb (int argc, char **argv)
{
unsigned long ulHTMLColor;
if(argc == 2)
{
ulHTMLColor = ustrtoul(argv[1], 0, 16);
g_sAppState.ulColors[RED] = (ulHTMLColor & 0xFF0000) >> 8;
g_sAppState.ulColors[GREEN] = (ulHTMLColor & 0x00FF00);
g_sAppState.ulColors[BLUE] = (ulHTMLColor & 0x0000FF) << 8;
g_sAppState.ulMode = APP_MODE_REMOTE;
g_sAppState.ulModeTimer = 0;
RGBColorSet(g_sAppState.ulColors);
}
//*****************************************************************************
//
// MPU9150 Application error handler. Show the user if we have encountered an
// I2C error.
//
//*****************************************************************************
void
MPU9150AppErrorHandler(char *pcFilename, uint_fast32_t ui32Line)
{
//
// Set terminal color to red and print error status and locations
//
UARTprintf("\033[31;1m");
UARTprintf("Error: %d, File: %s, Line: %d\n"
"See I2C status definitions in sensorlib\\i2cm_drv.h\n",
g_vui8ErrorFlag, pcFilename, ui32Line);
//
// Return terminal color to normal
//
UARTprintf("\033[0m");
//
// Set RGB Color to RED
//
g_pui32Colors[0] = 0xFFFF;
g_pui32Colors[1] = 0;
g_pui32Colors[2] = 0;
RGBColorSet(g_pui32Colors);
//
// Increase blink rate to get attention
//
RGBBlinkRateSet(10.0f);
//
// Go to sleep wait for interventions. A more robust application could
// attempt corrective actions here.
//
while(1)
{
//
// Do Nothing
//
}
}
void initsensorhub(void)
{
//
// Enable port B used for motion interrupt.
//
ROM_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.
//
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_PORTB_BASE, GPIO_PIN_2);
GPIOIntEnable(GPIO_PORTB_BASE, GPIO_PIN_2);
ROM_GPIOIntTypeSet(GPIO_PORTB_BASE, GPIO_PIN_2, GPIO_FALLING_EDGE);
ROM_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
//
ROM_SysCtlPeripheralClockGating(true);
ROM_SysCtlPeripheralSleepEnable(SYSCTL_PERIPH_GPIOB);
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__);
//
// 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);
//
// 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;
}
//*****************************************************************************
//
// Main 'C' Language entry point.
//
//*****************************************************************************
int
main(void)
{
float fTemperature, fPressure, fAltitude;
int32_t i32IntegerPart;
int32_t i32FractionPart;
//
// 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);
//
// Initialize the UART.
//
ConfigureUART();
//
// Print the welcome message to the terminal.
//
UARTprintf("\033[2JBMP180 Example\n");
//
// Set the color to a white approximation.
//
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x8000;
//
// Initialize RGB driver. Use a default intensity and blink rate.
//
RGBInit(0);
RGBColorSet(g_pui32Colors);
RGBIntensitySet(0.5f);
RGBEnable();
//
// 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.
// This step is not necessary if your part does not support pin muxing.
//
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);
//
// Initialize the GPIO for the LED.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
ROM_GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1);
ROM_GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1, 0x00);
//
// Enable interrupts to the processor.
//
ROM_IntMasterEnable();
//
// Initialize the I2C3 peripheral.
//
I2CMInit(&g_sI2CInst, I2C3_BASE, INT_I2C3, 0xff, 0xff,
ROM_SysCtlClockGet());
//
// Initialize the BMP180.
//
BMP180Init(&g_sBMP180Inst, &g_sI2CInst, BMP180_I2C_ADDRESS,
BMP180AppCallback, &g_sBMP180Inst);
//
// Wait for initialization callback to indicate reset request is complete.
//
while(g_vui8DataFlag == 0)
{
//
// Wait for I2C Transactions to complete.
//
}
//
// Reset the data ready flag
//
g_vui8DataFlag = 0;
//
// Enable the system ticks at 10 Hz.
//
ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / (10 * 3));
ROM_SysTickIntEnable();
ROM_SysTickEnable();
//
// After all the init and config we start blink the LED
//
RGBBlinkRateSet(1.0f);
//
// Begin the data collection and printing. Loop Forever.
//
while(1)
{
//
// Read the data from the BMP180 over I2C. This command starts a
// temperature measurement. Then polls until temperature is ready.
// Then automatically starts a pressure measurement and polls for that
// to complete. When both measurement are complete and in the local
// buffer then the application callback is called from the I2C
// interrupt context. Polling is done on I2C interrupts allowing
// processor to continue doing other tasks as needed.
//
BMP180DataRead(&g_sBMP180Inst, BMP180AppCallback, &g_sBMP180Inst);
while(g_vui8DataFlag == 0)
{
//
// Wait for the new data set to be available.
//
}
//
// Reset the data ready flag.
//
g_vui8DataFlag = 0;
//
// Get a local copy of the latest temperature data in float format.
//
BMP180DataTemperatureGetFloat(&g_sBMP180Inst, &fTemperature);
//
// Convert the floats to an integer part and fraction part for easy
// print.
//
i32IntegerPart = (int32_t) fTemperature;
i32FractionPart =(int32_t) (fTemperature * 1000.0f);
i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);
if(i32FractionPart < 0)
{
i32FractionPart *= -1;
}
//
// Print temperature with three digits of decimal precision.
//
UARTprintf("Temperature %3d.%03d\t\t", i32IntegerPart, i32FractionPart);
//
// Get a local copy of the latest air pressure data in float format.
//
BMP180DataPressureGetFloat(&g_sBMP180Inst, &fPressure);
//
// Convert the floats to an integer part and fraction part for easy
// print.
//
i32IntegerPart = (int32_t) fPressure;
i32FractionPart =(int32_t) (fPressure * 1000.0f);
i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);
if(i32FractionPart < 0)
{
i32FractionPart *= -1;
}
//
// Print Pressure with three digits of decimal precision.
//
UARTprintf("Pressure %3d.%03d\t\t", i32IntegerPart, i32FractionPart);
//
// Calculate the altitude.
//
fAltitude = 44330.0f * (1.0f - powf(fPressure / 101325.0f,
1.0f / 5.255f));
//
// Convert the floats to an integer part and fraction part for easy
// print.
//
i32IntegerPart = (int32_t) fAltitude;
i32FractionPart =(int32_t) (fAltitude * 1000.0f);
i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);
if(i32FractionPart < 0)
{
i32FractionPart *= -1;
}
//
// Print altitude with three digits of decimal precision.
//
UARTprintf("Altitude %3d.%03d", i32IntegerPart, i32FractionPart);
//
// Print new line.
//
UARTprintf("\n");
//
// Delay to keep printing speed reasonable. About 100 milliseconds.
//
ROM_SysCtlDelay(ROM_SysCtlClockGet() / (10 * 3));
}//while end
}
//*****************************************************************************
//
//! Set the output color and intensity.
//!
//! \param pui32RGBColor points to a three element array representing the
//! relative intensity of each color. Red is element 0, Green is element 1,
//! Blue is element 2. 0x0000 is off. 0xFFFF is fully on.
//!
//! \param fIntensity is used to scale the intensity of all three colors by
//! the same amount. fIntensity should be between 0.0 and 1.0. This scale
//! factor is applied to all three colors.
//!
//! This function should be called by the application to set the color and
//! intensity of the RGB LED.
//!
//! \return None.
//
//*****************************************************************************
void
RGBSet(volatile uint32_t * pui32RGBColor, float fIntensity)
{
RGBColorSet(pui32RGBColor);
RGBIntensitySet(fIntensity);
}
//*****************************************************************************
//
// 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]);
}
}
}
//*****************************************************************************
//
// 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;
}
}
}
//*****************************************************************************
//
// 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;
//
// Turn on the LED to show we are ready to process motion date
//
g_pui32RGBColors[RED] = 0xFFFF;
RGBColorSet(g_pui32RGBColors);
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;
g_ui32RGBMotionBlinkCounter = g_ui32SysTickCount;
}
//
// Store the most recent status in case it was an error condition
//
g_vui8ErrorFlag = ui8Status;
}
static portTASK_FUNCTION(ConsumoTask,pvParameters)
{
double consumo=0.0374*exp(0.02*((velocidad*100)/240)); //actualizamos el consumo
TickType_t tiempo_ant =xTaskGetTickCount( ); //obtenemos los tick transcurridos
unsigned char frame[MAX_FRAME_SIZE];
int num_datos;
double combustible;
int16_t ejes[3];
double altitud;
while(1)
{
if(xQueueReceive(velocidadQueue,&velocidad, configTICK_RATE_HZ)){
//Si recibimos un nuevo valor de velocidad cambiamos brillo del led azul
color[BLUE]=0xFFFF;
RGBSet(color,((float)velocidad)/241);
}
if((xTaskGetTickCount( )-tiempo_ant)>=configTICK_RATE_HZ*(60/tiempoSim) && combustible!=0){
//Cada minuto real (1 hora simulada)
combustible=getCombustible();
//Modificamos el combustible segun el consumo
combustible -= 0.5*exp(0.02*(velocidad*100/240)) ;
if(combustible<=20){ //Encendemos el Led Verde si el combustible es menor que 20
color[GREEN]=0xFFFF;
RGBColorSet(color);
}
if(combustible<=0){ //Si el combustible es cero desactivamos el ADC
combustible=0;
velocidad=0;
color[BLUE]=0x0;
xEventGroupClearBits( xEventGroup, PilotoAutomaticoBit );
ADCSequenceDisable(ADC0_BASE,0);
}
//Enviamos el combustible
setCombustible(combustible);
num_datos=create_frame(frame, COMANDO_FUEL, &combustible, sizeof(combustible), MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
tiempo_ant =xTaskGetTickCount( );
}
getEjes(ejes);
altitud=getAltitud();
if(ejes[PITCH]>-45 && combustible==0 && altitud>0){ //si el combustible es cero ponemos PITCH =45 poco a poco
ejes[PITCH]--;
setEjes(ejes[PITCH],ejes[ROLL],ejes[YAW]);
num_datos=create_frame(frame, COMANDO_EJES, ejes, sizeof(ejes), MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
}
}
}
//*****************************************************************************
//
//! Set the output color and intensity.
//!
//! \param pulRGBColor points to a three element array representing the
//! relative intensity of each color. Red is element 0, Green is element 1,
//! Blue is element 2. 0x0000 is off. 0xFFFF is fully on.
//!
//! \param fIntensity is used to scale the intensity of all three colors by
//! the same amount. fIntensity should be between 0.0 and 1.0. This scale
//! factor is applied to all three colors.
//!
//! This function should be called by the application to set the color and
//! intensity of the RGB LED.
//!
//! \return None.
//
//*****************************************************************************
void
RGBSet(volatile unsigned long * pulRGBColor, float fIntensity)
{
RGBColorSet(pulRGBColor);
RGBIntensitySet(fIntensity);
}
//*****************************************************************************
//
// Uses the fColorWheelPos variable to update the color mix shown on the RGB
//
// ui32ForceUpdate when set forces a color update even if a color change
// has not been detected. Used primarily at startup to init the color after
// a hibernate.
//
// This function is called by the SysTickIntHandler to update the colors on
// the RGB LED whenever a button or timeout event has changed the color wheel
// position. Color is determined by a series of sine functions and conditions
//
//*****************************************************************************
void
AppRainbow(uint32_t ui32ForceUpdate)
{
static float fPrevPos;
float fCurPos;
float fTemp;
volatile uint32_t * pui32Colors;
pui32Colors = g_sAppState.ui32Colors;
fCurPos = g_sAppState.fColorWheelPos;
if((fCurPos != fPrevPos) || ui32ForceUpdate)
{
//
// Preserve the new color wheel position
//
fPrevPos = fCurPos;
//
// Adjust the BLUE value based on the control state
//
fTemp = 65535.0f * sinf(fCurPos);
if(fTemp < 0)
{
pui32Colors[GREEN] = 0;
}
else
{
pui32Colors[GREEN] = (uint32_t) fTemp;
}
//
// Adjust the RED value based on the control state
//
fTemp = 65535.0f * sinf(fCurPos - APP_PI / 2.0f);
if(fTemp < 0)
{
pui32Colors[BLUE] = 0;
}
else
{
pui32Colors[BLUE] = (uint32_t) fTemp;
}
//
// Adjust the GREEN value based on the control state
//
if(fCurPos < APP_PI)
{
fTemp = 65535.0f * sinf(fCurPos + APP_PI * 0.5f);
}
else
{
fTemp = 65535.0f * sinf(fCurPos + APP_PI);
}
if(fTemp < 0)
{
pui32Colors[RED] = 0;
}
else
{
pui32Colors[RED] = (uint32_t) fTemp;
}
//
// Update the actual LED state
//
RGBColorSet(pui32Colors);
}
}
//*****************************************************************************
//
// Main 'C' Language entry point.
//
//*****************************************************************************
int
main(void)
{
float fAmbient;
int32_t i32IntegerPart, i32FractionPart;
uint8_t ui8Mask;
//
// 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 the peripherals used by this example.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
//
// Initialize the UART and its pins.
//
ConfigureUART();
//
// Print the welcome message to the terminal.
//
UARTprintf("\033[2JISL29023 Example\n");
//
// Set the color to a white approximation.
//
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x8000;
//
// Initialize RGB driver. Use a default intensity and blink rate.
//
RGBInit(0);
RGBColorSet(g_pui32Colors);
RGBIntensitySet(0.5f);
RGBEnable();
//
// 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.
// This step is not necessary if your part does not support pin muxing.
//
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
// ISL29023
//
ROM_GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_5);
GPIOIntEnable(GPIO_PORTE_BASE, GPIO_PIN_5);
ROM_GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_5, GPIO_FALLING_EDGE);
ROM_IntEnable(INT_GPIOE);
//
// Keep only some parts of the systems running while in sleep mode.
// GPIOE is for the ISL29023 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);
//
// Configure desired interrupt priorities. Setting the I2C interrupt to be
// of more priority than SysTick and the GPIO interrupt means those
// interrupt routines can use the I2CM_DRV Application context does not use
// I2CM_DRV API and GPIO and SysTick are at the same priority level. This
// prevents re-entrancy problems with I2CM_DRV but keeps the MCU in sleep
// state as much as possible. UART is at least priority so it can operate
// in the background.
//
ROM_IntPrioritySet(INT_I2C3, 0x00);
ROM_IntPrioritySet(FAULT_SYSTICK, 0x40);
ROM_IntPrioritySet(INT_GPIOE, 0x80);
ROM_IntPrioritySet(INT_UART0, 0x80);
//
// Enable interrupts to the processor.
//
ROM_IntMasterEnable();
//
// Initialize I2C3 peripheral.
//
I2CMInit(&g_sI2CInst, I2C3_BASE, INT_I2C3, 0xff, 0xff,
ROM_SysCtlClockGet());
//
// Initialize the ISL29023 Driver.
//
ISL29023Init(&g_sISL29023Inst, &g_sI2CInst, ISL29023_I2C_ADDRESS,
ISL29023AppCallback, &g_sISL29023Inst);
//
// Wait for transaction to complete
//
ISL29023AppI2CWait(__FILE__, __LINE__);
//
// Configure the ISL29023 to measure ambient light continuously. Set a 8
// sample persistence before the INT pin is asserted. Clears the INT flag.
// Persistence setting of 8 is sufficient to ignore camera flashes.
//
ui8Mask = (ISL29023_CMD_I_OP_MODE_M | ISL29023_CMD_I_INT_PERSIST_M |
ISL29023_CMD_I_INT_FLAG_M);
ISL29023ReadModifyWrite(&g_sISL29023Inst, ISL29023_O_CMD_I, ~ui8Mask,
(ISL29023_CMD_I_OP_MODE_ALS_CONT |
ISL29023_CMD_I_INT_PERSIST_8),
ISL29023AppCallback, &g_sISL29023Inst);
//
// Wait for transaction to complete
//
ISL29023AppI2CWait(__FILE__, __LINE__);
//
// Configure the upper threshold to 80% of maximum value
//
g_sISL29023Inst.pui8Data[1] = 0xCC;
g_sISL29023Inst.pui8Data[2] = 0xCC;
ISL29023Write(&g_sISL29023Inst, ISL29023_O_INT_HT_LSB,
g_sISL29023Inst.pui8Data, 2, ISL29023AppCallback,
&g_sISL29023Inst);
//
// Wait for transaction to complete
//
ISL29023AppI2CWait(__FILE__, __LINE__);
//
// Configure the lower threshold to 20% of maximum value
//
g_sISL29023Inst.pui8Data[1] = 0x33;
g_sISL29023Inst.pui8Data[2] = 0x33;
ISL29023Write(&g_sISL29023Inst, ISL29023_O_INT_LT_LSB,
g_sISL29023Inst.pui8Data, 2, ISL29023AppCallback,
&g_sISL29023Inst);
//
// Wait for transaction to complete
//
ISL29023AppI2CWait(__FILE__, __LINE__);
//
//Configure and enable SysTick Timer
//
ROM_SysTickPeriodSet(ROM_SysCtlClockGet() / SYSTICKS_PER_SECOND);
ROM_SysTickIntEnable();
ROM_SysTickEnable();
//
// After all the init and config we start blink the LED
//
RGBBlinkRateSet(1.0f);
//
// Loop Forever
//
while(1)
{
ROM_SysCtlSleep();
if(g_vui8DataFlag)
{
g_vui8DataFlag = 0;
//
// Get a local floating point copy of the latest light data
//
ISL29023DataLightVisibleGetFloat(&g_sISL29023Inst, &fAmbient);
//
// Perform the conversion from float to a printable set of integers
//
i32IntegerPart = (int32_t)fAmbient;
i32FractionPart = (int32_t)(fAmbient * 1000.0f);
i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);
if(i32FractionPart < 0)
{
i32FractionPart *= -1;
}
//
// Print the temperature as integer and fraction parts.
//
UARTprintf("Visible Lux: %3d.%03d\n", i32IntegerPart,
i32FractionPart);
//
// Check if the intensity of light has crossed a threshold. If so
// then adjust range of sensor readings to track intensity.
//
if(g_vui8IntensityFlag)
{
//
// Disable the low priority interrupts leaving only the I2C
// interrupt enabled.
//
ROM_IntPriorityMaskSet(0x40);
//
// Reset the intensity trigger flag.
//
g_vui8IntensityFlag = 0;
//
// Adjust the lux range.
//
ISL29023AppAdjustRange(&g_sISL29023Inst);
//
// Now we must manually clear the flag in the ISL29023
// register.
//
ISL29023Read(&g_sISL29023Inst, ISL29023_O_CMD_I,
g_sISL29023Inst.pui8Data, 1, ISL29023AppCallback,
&g_sISL29023Inst);
//
// Wait for transaction to complete
//
ISL29023AppI2CWait(__FILE__, __LINE__);
//
// Disable priority masking so all interrupts are enabled.
//
ROM_IntPriorityMaskSet(0);
}
}
}
}
//*****************************************************************************
//
// Main 'C' Language entry point.
//
//*****************************************************************************
int
main(void)
{
float fAmbient, fObject;
int_fast32_t i32IntegerPart;
int_fast32_t i32FractionPart;
//
// 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 the peripherals used by this example.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
//
// Initialize the UART.
//
ConfigureUART();
//
// Print the welcome message to the terminal.
//
UARTprintf("\033[2J\033[1;1HTMP006 Example\n");
//
// Setup the color of the RGB LED.
//
g_pui32Colors[RED] = 0;
g_pui32Colors[BLUE] = 0xFFFF;
g_pui32Colors[GREEN] = 0;
//
// Initialize the RGB Driver and start RGB blink operation.
//
RGBInit(0);
RGBColorSet(g_pui32Colors);
RGBIntensitySet(0.5f);
RGBEnable();
//
// 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.
// This step is not necessary if your part does not support pin muxing.
//
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 DRDY from the TMP006
//
ROM_GPIOPinTypeGPIOInput(GPIO_PORTE_BASE, GPIO_PIN_0);
GPIOIntEnable(GPIO_PORTE_BASE, GPIO_PIN_0);
ROM_GPIOIntTypeSet(GPIO_PORTE_BASE, GPIO_PIN_0, GPIO_FALLING_EDGE);
ROM_IntEnable(INT_GPIOE);
//
// Keep only some parts of the systems running while in sleep mode.
// GPIOE is for the TMP006 data ready interrupt.
// UART0 is the virtual serial port
// TIMER0, TIMER1 and WTIMER5 are used by the RGB driver
// I2C3 is the I2C interface to the TMP006
//
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,
SysCtlClockGet());
//
// Initialize the TMP006
//
TMP006Init(&g_sTMP006Inst, &g_sI2CInst, TMP006_I2C_ADDRESS,
TMP006AppCallback, &g_sTMP006Inst);
//
// Put the processor to sleep while we wait for the I2C driver to
// indicate that the transaction is complete.
//
while((g_vui8DataFlag == 0) && (g_vui8ErrorFlag == 0))
{
ROM_SysCtlSleep();
}
//
// If an error occurred call the error handler immediately.
//
if(g_vui8ErrorFlag)
{
TMP006AppErrorHandler(__FILE__, __LINE__);
}
//
// clear the data flag for next use.
//
g_vui8DataFlag = 0;
//
// Delay for 10 milliseconds for TMP006 reset to complete.
// Not explicitly required. Datasheet does not say how long a reset takes.
//
ROM_SysCtlDelay(ROM_SysCtlClockGet() / (100 * 3));
//
// Enable the DRDY pin indication that a conversion is in progress.
//
TMP006ReadModifyWrite(&g_sTMP006Inst, TMP006_O_CONFIG,
~TMP006_CONFIG_EN_DRDY_PIN_M,
TMP006_CONFIG_EN_DRDY_PIN, TMP006AppCallback,
&g_sTMP006Inst);
//
// Wait for the DRDY enable I2C transaction to complete.
//
while((g_vui8DataFlag == 0) && (g_vui8ErrorFlag == 0))
{
ROM_SysCtlSleep();
}
//
// If an error occurred call the error handler immediately.
//
if(g_vui8ErrorFlag)
{
TMP006AppErrorHandler(__FILE__, __LINE__);
}
//
// clear the data flag for next use.
//
g_vui8DataFlag = 0;
//
// Last thing before the loop start blinking to show we got this far and
// the tmp006 is setup and ready for auto measure
//
RGBBlinkRateSet(1.0f);
//
// Loop Forever
//
while(1)
{
//
// Put the processor to sleep while we wait for the TMP006 to
// signal that data is ready. Also continue to sleep while I2C
// transactions get the raw data from the TMP006
//
while((g_vui8DataFlag == 0) && (g_vui8ErrorFlag == 0))
{
ROM_SysCtlSleep();
}
//
// If an error occurred call the error handler immediately.
//
if(g_vui8ErrorFlag)
{
TMP006AppErrorHandler(__FILE__, __LINE__);
}
//
// Reset the flag
//
g_vui8DataFlag = 0;
//
// Get a local copy of the latest data in float format.
//
TMP006DataTemperatureGetFloat(&g_sTMP006Inst, &fAmbient, &fObject);
//
// Convert the floating point ambient temperature to an integer part
// and fraction part for easy printing.
//
i32IntegerPart = (int32_t)fAmbient;
i32FractionPart = (int32_t)(fAmbient * 1000.0f);
i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);
if(i32FractionPart < 0)
{
i32FractionPart *= -1;
}
UARTprintf("Ambient %3d.%03d\t", i32IntegerPart, i32FractionPart);
//
// Convert the floating point ambient temperature to an integer part
// and fraction part for easy printing.
//
i32IntegerPart = (int32_t)fObject;
i32FractionPart = (int32_t)(fObject * 1000.0f);
i32FractionPart = i32FractionPart - (i32IntegerPart * 1000);
if(i32FractionPart < 0)
{
i32FractionPart *= -1;
}
UARTprintf("Object %3d.%03d\n", i32IntegerPart, i32FractionPart);
}
}
//*****************************************************************************
//
// 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]);
}
}
}
//*****************************************************************************
//
// Calibration state machine
//
//*****************************************************************************
void Calibration() {
switch (g_calibrationState) {
case 0: {
g_calibrationState++;
//
// Reset errors to default
//
zeroErrorAccel[0] = 0.3530;
zeroErrorAccel[1] = 0.1563;
zeroErrorAccel[2] = -0.3140;
EEPROMProgram((uint32_t *) zeroErrorAccel,
EEPROM_ZERO_ERROR_ACCELERATION_ADDRESS, 12);
zeroErrorGyro[0] = 0.0526;
zeroErrorGyro[1] = 0.0156;
zeroErrorGyro[2] = 0.0157;
EEPROMProgram((uint32_t *) zeroErrorGyro,
EEPROM_ZERO_ERROR_GYROSCOPE_ADDRESS, 12);
//
// Set the color to RED.
//
RGBBlinkRateSet(0.0f);
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x0000;
g_pui32Colors[GREEN] = 0x0000;
RGBColorSet(g_pui32Colors);
RGBEnable();
break;
}
case 1: {
g_calibrationState++;
g_calibrationCount = 0;
zeroErrorAccel[0] = 0;
zeroErrorAccel[1] = 0;
zeroErrorAccel[2] = 0;
zeroErrorGyro[0] = 0;
zeroErrorGyro[1] = 0;
zeroErrorGyro[2] = 0;
RGBBlinkRateSet(1.0f);
break;
}
case 2: {
g_calibrationState++;
RGBBlinkRateSet(0.0f);
//
// Set the color to BLUE.
//
g_pui32Colors[RED] = 0x0000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x0000;
RGBColorSet(g_pui32Colors);
// Write the calibration result to EEPROM
EEPROMProgram((uint32_t *) zeroErrorAccel,
EEPROM_ZERO_ERROR_ACCELERATION_ADDRESS, 8);
EEPROMProgram((uint32_t *) zeroErrorGyro,
EEPROM_ZERO_ERROR_GYROSCOPE_ADDRESS, 12);
break;
}
case 3: {
g_calibrationState++;
g_calibrationCount = 0;
RGBBlinkRateSet(1.0f);
break;
}
case 4: {
g_calibrationState++;
RGBBlinkRateSet(0.0f);
//
// Set the color to GREEN.
//
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x00000;
RGBColorSet(g_pui32Colors);
// Write the calibration result to EEPROM
linearErrorAccel[1] = (accelAtGravity[1] - zeroErrorAccel[1]) / 9.81
- 1;
linearErrorAccel[2] = (accelAtGravity[2] - zeroErrorAccel[2]) / 9.81
- 1;
EEPROMProgram((uint32_t *)zeroErrorAccel+2, EEPROM_ZERO_ERROR_ACCELERATION_ADDRESS+8, 4);
EEPROMProgram((uint32_t *)linearErrorAccel+1, EEPROM_LINEAR_ERROR_ACCELERATION_ADDRESS+4, 8);
break;
}
case 5: {
g_calibrationState++;
g_calibrationCount = 0;
RGBBlinkRateSet(1.0f);
break;
}
case 6: { // finish calibration
g_calibrationState = 0;
//
// Set the color to tri-colour.
//
g_pui32Colors[RED] = 0x8000;
g_pui32Colors[BLUE] = 0x8000;
g_pui32Colors[GREEN] = 0x8000;
RGBColorSet(g_pui32Colors);
// Write the calibration result to EEPROM
linearErrorAccel[0] = (accelAtGravity[0] - zeroErrorAccel[0]) / 9.81
- 1;
EEPROMProgram((uint32_t *)linearErrorAccel, EEPROM_LINEAR_ERROR_ACCELERATION_ADDRESS, 4);
break;
}
default:
break;
}
}
//*****************************************************************************
//
//! Set the current output intensity.
//!
//! \param fIntensity is used to scale the intensity of all three colors by
//! the same amount. fIntensity should be between 0.0 and 1.0. This scale
//! factor is applied individually to all three colors.
//!
//! This function should be called by the application to set the intensity
//! of the RGB LED.
//!
//! \return None.
//
//*****************************************************************************
void
RGBIntensitySet(float fIntensity)
{
g_fIntensity = fIntensity;
RGBColorSet(g_ulColors);
}
static portTASK_FUNCTION(HighTask,pvParameters)
{
unsigned char frame[MAX_FRAME_SIZE];
int num_datos;
double altitud;
int16_t ejes[3];
double combustible;
float velocidad;
int tiempoSim;
while(1)
{
vTaskDelay(configTICK_RATE_HZ); //Cada seg
//Obtenemos las variables
altitud=getAltitud();
velocidad=getVelocidad();
tiempoSim=getTiempoSim();
if(altitud>0){ //Si la altitud es mayor que cero cambiamos la altitud
getEjes(ejes);
altitud += sin((ejes[PITCH]*3.14f)/180) *(velocidad*(1000/(60/tiempoSim)));
if(altitud>99999){ //99999 es el maximo del panel QT
altitud=99999;
}
//Enviamos la trama con la altitud
setAltitud(altitud);
num_datos=create_frame(frame, COMANDO_HIGH, &altitud, sizeof(altitud), MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
//Obtenemos el combustible
combustible=getCombustible();
if(combustible==0){//Si ya estamos sin combustible hacemos parpadear los led con la altitud
color[BLUE]=0xFFFF;
if(altitud<=800){
color[RED]=0xFFFF;
color[GREEN]=0x0;
}
RGBColorSet(color);
if(altitud<2000){
RGBBlinkRateSet((float)(1000/(altitud+1)));
}
tiempoSim=getTiempoSim();
//Aumentamos la velocidad a 9,8m/s^2
velocidad+=9.8*(60*tiempoSim)/1000;
setVelocidad(velocidad);
//Enviamos la nueva velocidad
num_datos=create_frame(frame, COMANDO_SPEED, &velocidad, sizeof(velocidad), MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
}
}else{ //Si la altitud es menor o igual a 0
//Variables a cero
velocidad=0;
altitud=0;
setVelocidad(velocidad);
setAltitud(altitud);
//Enviamos comando colision
if(combustible!=0) ADCSequenceDisable(ADC0_BASE,0);
num_datos=create_frame(frame, COMANDO_COLISION,NULL, 0, MAX_FRAME_SIZE);
if (num_datos>=0){
send_frame(frame, num_datos);
}else{
logError(num_datos);
}
//BLOQUEAMOS LA TIVA
vTaskDelete(sensorTaskHandle);
vTaskDelete( consumoTaskHandle );
vTaskDelete(altitudTaskHandle);
vTaskDelete( turbulenciasTaskHandle );
vTaskEndScheduler( );
while(1);
}
}
}