void SysTick_Handler(void)
{
uint8_t Delay_Ticks = 1;
uint8_t* Buf;
uint32_t idx;
if (Freq == 20)
{
Delay_Ticks = 100;
}
if (Freq == 2000)
{
Delay_Ticks = 1;
}
DataReady ++;
if (DataReady >= Delay_Ticks)
{
STM_EVAL_LEDOn(LED3);
Demo_GyroReadAngRate((float*)Buffer);
Demo_CompassReadMag((float*)MagBuffer);
Demo_CompassReadAcc((float*)AccBuffer);
Buf = (uint8_t*)Buffer;
for(idx = 0; idx < 12; idx++)
Send_Buffer[idx] = Buf[idx];
Buf = (uint8_t*)MagBuffer;
for(idx = 12; idx < 24; idx++)
Send_Buffer[idx] = Buf[idx-12];
Buf = (uint8_t*)AccBuffer;
for(idx = 24; idx < 36; idx++)
Send_Buffer[idx] = Buf[idx-24];
Buf = (uint8_t*)ADC_Result;
for(idx = 36; idx < 54; idx++)
Send_Buffer[idx] = Buf[idx-36];
CDC_Send_DATA ((unsigned char*)Send_Buffer,54);
DataReady = 0;
STM_EVAL_LEDOff(LED3);
}
}
/**
* @brief Main program.
* @param None
* @retval None
*/
int main(void)
{
uint8_t i = 0;
/* SysTick end of count event each 10ms */
RCC_GetClocksFreq(&RCC_Clocks);
SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);
/* Initialize LEDs and User Button available on STM32F3-Discovery board */
STM_EVAL_LEDInit(LED3);
STM_EVAL_LEDInit(LED4);
STM_EVAL_LEDInit(LED5);
STM_EVAL_LEDInit(LED6);
STM_EVAL_LEDInit(LED7);
STM_EVAL_LEDInit(LED8);
STM_EVAL_LEDInit(LED9);
STM_EVAL_LEDInit(LED10);
STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_EXTI);
/* Configure the USB */
Demo_USB();
/* Reset UserButton_Pressed variable */
UserButtonPressed = 0x00;
/* Infinite loop */
while (1)
{
/* LEDs Off */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
/* Waiting User Button is pressed */
while (UserButtonPressed == 0x00)
{
/* Toggle LD3 */
STM_EVAL_LEDToggle(LED3);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD5 */
STM_EVAL_LEDToggle(LED5);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD7 */
STM_EVAL_LEDToggle(LED7);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD9 */
STM_EVAL_LEDToggle(LED9);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD10 */
STM_EVAL_LEDToggle(LED10);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD8 */
STM_EVAL_LEDToggle(LED8);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD6 */
STM_EVAL_LEDToggle(LED6);
/* Insert 50 ms delay */
Delay(5);
/* Toggle LD4 */
STM_EVAL_LEDToggle(LED4);
/* Insert 50 ms delay */
Delay(5);
}
DataReady = 0x00;
/* All LEDs Off */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
/* Demo Gyroscope */
Demo_GyroConfig();
/* Waiting User Button is pressed */
while (UserButtonPressed == 0x01)
{
/* Wait for data ready */
while(DataReady != 0x05)
{}
DataReady = 0x00;
/* LEDs Off */
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
/* Read Gyro Angular data */
Demo_GyroReadAngRate(Buffer);
/* Update autoreload and capture compare registers value*/
Xval = ABS((int8_t)(Buffer[0]));
Yval = ABS((int8_t)(Buffer[1]));
if ( Xval>Yval)
{
if ((int8_t)Buffer[0] > 5.0f)
{
/* LD10 On */
STM_EVAL_LEDOn(LED10);
}
if ((int8_t)Buffer[0] < -5.0f)
{
/* LD3 On */
STM_EVAL_LEDOn(LED3);
}
}
else
{
if ((int8_t)Buffer[1] < -5.0f)
{
/* LD6 on */
STM_EVAL_LEDOn(LED6);
}
if ((int8_t)Buffer[1] > 5.0f)
{
/* LD7 On */
STM_EVAL_LEDOn(LED7);
}
}
}
DataReady = 0x00;
/* LEDs Off */
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
/* Demo Compass */
Demo_CompassConfig();
/* Waiting User Button is pressed */
while (UserButtonPressed == 0x02)
{
/* Wait for data ready */
while(DataReady !=0x05)
{}
DataReady = 0x00;
/* Read Compass data */
Demo_CompassReadMag(MagBuffer);
Demo_CompassReadAcc(AccBuffer);
for(i=0;i<3;i++)
AccBuffer[i] /= 100.0f;
fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
fSinRoll = -AccBuffer[1]/fNormAcc;
fCosRoll = (sqrt(1.0-(fSinRoll * fSinRoll)));
fSinPitch = AccBuffer[0]/fNormAcc;
fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));
if ( fSinRoll >0)
{
if (fCosRoll>0)
{
RollAng = acos(fCosRoll)*180/PI;
}
else
{
RollAng = acos(fCosRoll)*180/PI + 180;
}
}
else
{
if (fCosRoll>0)
{
RollAng = acos(fCosRoll)*180/PI + 360;
}
else
{
RollAng = acos(fCosRoll)*180/PI + 180;
}
}
if ( fSinPitch >0)
{
if (fCosPitch>0)
{
PitchAng = acos(fCosPitch)*180/PI;
}
else
{
PitchAng = acos(fCosPitch)*180/PI + 180;
}
}
else
{
if (fCosPitch>0)
{
PitchAng = acos(fCosPitch)*180/PI + 360;
}
else
{
PitchAng = acos(fCosPitch)*180/PI + 180;
}
}
if (RollAng >=360)
{
RollAng = RollAng - 360;
}
if (PitchAng >=360)
{
PitchAng = PitchAng - 360;
}
fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;
if (HeadingValue < 0)
{
HeadingValue = HeadingValue + 360;
}
if ((RollAng <= 40.0f) && (PitchAng <= 40.0f))
{
if (((HeadingValue < 25.0f)&&(HeadingValue >= 0.0f))||((HeadingValue >=340.0f)&&(HeadingValue <= 360.0f)))
{
STM_EVAL_LEDOn(LED10);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
}
else if ((HeadingValue <70.0f)&&(HeadingValue >= 25.0f))
{
STM_EVAL_LEDOn(LED9);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED7);
}
else if ((HeadingValue < 115.0f)&&(HeadingValue >= 70.0f))
{
STM_EVAL_LEDOn(LED7);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED5);
}
else if ((HeadingValue <160.0f)&&(HeadingValue >= 115.0f))
{
STM_EVAL_LEDOn(LED5);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED3);
}
else if ((HeadingValue <205.0f)&&(HeadingValue >= 160.0f))
{
STM_EVAL_LEDOn(LED3);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED7);
}
else if ((HeadingValue <250.0f)&&(HeadingValue >= 205.0f))
{
STM_EVAL_LEDOn(LED4);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED7);
}
else if ((HeadingValue < 295.0f)&&(HeadingValue >= 250.0f))
{
STM_EVAL_LEDOn(LED6);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED8);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED5);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED7);
}
else if ((HeadingValue < 340.0f)&&(HeadingValue >= 295.0f))
{
STM_EVAL_LEDOn(LED8);
STM_EVAL_LEDOff(LED6);
STM_EVAL_LEDOff(LED10);
STM_EVAL_LEDOff(LED7);
STM_EVAL_LEDOff(LED9);
STM_EVAL_LEDOff(LED3);
STM_EVAL_LEDOff(LED4);
STM_EVAL_LEDOff(LED5);
}
}
else
{
/* Toggle All LEDs */
STM_EVAL_LEDToggle(LED7);
STM_EVAL_LEDToggle(LED6);
STM_EVAL_LEDToggle(LED10);
STM_EVAL_LEDToggle(LED8);
STM_EVAL_LEDToggle(LED9);
STM_EVAL_LEDToggle(LED3);
STM_EVAL_LEDToggle(LED4);
STM_EVAL_LEDToggle(LED5);
/* Delay 50ms */
Delay(5);
}
}
}
}
