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