/*====================================================================================================*/ void KDWM_Loop( void ) { while(1) { LED_R_Reset(); LED_G_Set(); LED_B_Set(); delay_ms(200); LED_R_Set(); LED_G_Reset(); LED_B_Set(); delay_ms(200); LED_R_Set(); LED_G_Set(); LED_B_Reset(); delay_ms(200); while(KEY_Read()) { LED_R_Set(); LED_G_Set(); LED_B_Set(); delay_ms(200); LED_R_Reset(); LED_G_Reset(); LED_B_Reset(); delay_ms(200); } } }
/* * ===================================================================================== * * Filename: my_sensor.c * * Description: * * Version: 1.0 * Created: 2014/6/15 21:20:17 * Revision: none * Compiler: gcc * * Author: YOUR NAME (), * Organization: * * ===================================================================================== */ #include "my_sensor.h" #include "AHRS.h" #include "global.h" __IO uint16_t gSendDataQueue[SendDataLength]; uint16_t gTempGetData[SendDataLength]; volatile uint16_t gMPU6050Counter = MPU6050_Hang; volatile uint16_t gMPU6050InitDelayCounter = MPU6050InitDelay_Hang; volatile uint8_t gMPU6050Swap = 0; volatile uint8_t* gMPU6050DataPointer = 0; uint8_t gMPU6050InitQueue1[MPU6050InitQueueLength1] = MPU6050InitQueueValue1; uint8_t gMPU6050InitQueue2[MPU6050InitQueueLength2] = MPU6050InitQueueValue2; volatile uint8_t gMPU6050ScanQueue[(MPU6050ScanQueueLength1 + MPU6050ScanQueueLength2)]; void MPU6050_RawProcess(void)/*{{{*/ { if(gMPU6050Swap == 1) { gMPU6050RawAccel.x = gMPU6050DataPointer[XL] + (gMPU6050DataPointer[XH] << 8); gMPU6050RawAccel.y = gMPU6050DataPointer[YL] + (gMPU6050DataPointer[YH] << 8); gMPU6050RawAccel.z = gMPU6050DataPointer[ZL] + (gMPU6050DataPointer[ZH] << 8); } else if(gMPU6050Swap == 2) { gMPU6050RawGyro.x = gMPU6050DataPointer[XL] + (gMPU6050DataPointer[XH] << 8); gMPU6050RawGyro.y = gMPU6050DataPointer[YL] + (gMPU6050DataPointer[YH] << 8); gMPU6050RawGyro.z = gMPU6050DataPointer[ZL] + (gMPU6050DataPointer[ZH] << 8); //END OF ONE SCAN SEQUENCE gControl = 1; } }/*}}}*/ void calib_sensor(void)/*{{{*/ { if(gMPU6050RawAccel.z < - 3000) { gAHRSStat = AHRS_Cali_Accel;//plane placed upside down,goto Accel Cali gLEDTurnTreshB = LEDTurnTresh_Mode1; gLEDCounterG = LED_Hang; LED_G_Set(LED_On); gLEDCounterR = LED_Hang; LED_R_Set(LED_Off); gAHRSCounter = ACCEL_LPF_TRESH + 2; //gsPrintfActualLength = sprintf(gsPrintfBuffer,"Accel Calibration\r\n"); //UARTsendString(Tool_USART0,(uint8_t*)gsPrintfBuffer); } else { gAHRSStat = AHRS_Cali_Gyro; gLEDTurnTreshG = LEDTurnTresh_Mode2; gLEDTurnTreshB = LEDTurnTresh_Mode2; gLEDTurnTreshR = LEDTurnTresh_Mode2; //gsPrintfActualLength = sprintf(gsPrintfBuffer,"Gyro Calibration\r\n"); //UARTsendString(Tool_USART0,(uint8_t*)gsPrintfBuffer); } }/*}}}*/
/*====================================================================================================*/ void UM_GPIO_Config( void ) { GPIO_InitTypeDef GPIO_InitStruct; /* Enable all GPIO Clk *******************************************************/ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE); /* GPIO all analog input *****************************************************/ GPIO_InitStruct.GPIO_Pin = GPIO_Pin_All; GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AN; GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_Init(GPIOB, &GPIO_InitStruct); GPIO_Init(GPIOC, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = GPIO_Pin_All & (~(GPIO_Pin_13 | GPIO_Pin_14)); GPIO_Init(GPIOA, &GPIO_InitStruct); /* GPIO Pin ******************************************************************/ GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStruct.GPIO_Pin = LED_R_PIN; GPIO_Init(LED_R_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = LED_G_PIN; GPIO_Init(LED_G_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = LED_B_PIN; GPIO_Init(LED_B_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN; GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStruct.GPIO_Pin = KEY_U_PIN; GPIO_Init(KEY_U_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = KEY_D_PIN; GPIO_Init(KEY_D_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = KEY_R_PIN; GPIO_Init(KEY_R_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = KEY_P_PIN; GPIO_Init(KEY_P_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.GPIO_Pin = KEY_L_PIN; GPIO_Init(KEY_L_GPIO_PORT, &GPIO_InitStruct); LED_R_Set(); LED_G_Set(); LED_B_Set(); }
/*====================================================================================================*/ void GPIO_Config( void ) { GPIO_InitTypeDef GPIO_InitStruct; /* GPIO Clk ******************************************************************/ LED_R_GPIO_CLK_ENABLE(); LED_G_GPIO_CLK_ENABLE(); LED_B_GPIO_CLK_ENABLE(); KEY_WU_GPIO_CLK_ENABLE(); KEY_BO_GPIO_CLK_ENABLE(); /* GPIO Pin ******************************************************************/ /* LED */ GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Pin = LED_R_PIN; HAL_GPIO_Init(LED_R_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.Pin = LED_G_PIN; HAL_GPIO_Init(LED_G_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.Pin = LED_B_PIN; HAL_GPIO_Init(LED_B_GPIO_PORT, &GPIO_InitStruct); /* KEY */ GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Pin = KEY_WU_PIN; HAL_GPIO_Init(KEY_WU_GPIO_PORT, &GPIO_InitStruct); GPIO_InitStruct.Pin = KEY_BO_PIN; HAL_GPIO_Init(KEY_BO_GPIO_PORT, &GPIO_InitStruct); /* GPIO Pin ******************************************************************/ LED_R_Set(); LED_G_Set(); LED_B_Set(); }