/*====================================================================================================*/
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();
}
