void GPS_REV_TASK(void *p_arg)
{
uint8_t data_temp;
(void)p_arg;
USART_ITConfig(USART2, USART_IT_RXNE, DISABLE);
comClearRxFifo(COM2);
GpsRevCnt = 0;
while(1)
{
BSP_OS_TimeDlyMs(10000);
comClearRxFifo(COM2);
GpsRevCnt = 0;
USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);
while(GpsRevCnt < GPS_MAX_REV_SIZE)
{
while(comGetChar(COM2, &data_temp))
{
GpsRevBuf[GpsRevCnt++] = data_temp;
}
BSP_OS_TimeDlyMs(10);
}
USART_ITConfig(USART2, USART_IT_RXNE, DISABLE);
GPS_Analysis(&GpsCurInfo, GpsRevBuf);
}
}
void GPS_STO_TASK(void *p_arg)
{
(void)p_arg;
while(1)
{
BSP_OS_TimeDlyMs(1000);
}
}
static void AppTaskStart (void *p_arg)
{
OS_ERR err;
(void)p_arg;
BSP_Tick_Init();
bsp_Init();
CPU_Init();
//Mem_Init(); //内存管理模块
// Math_Init();
#if OS_CFG_STAT_TASK_EN > 0u
OSStatTaskCPUUsageInit(&err);
#endif
#ifdef CPU_CFG_INT_DIS_MEAS_EN
CPU_IntDisMeasMaxCurReset();
#endif
AppTimerCreate();
AppObjCreate();
AppTaskCreate();
while (1)
{
//GPIOG->ODR ^= GPIO_Pin_10; //添加LED指示灯，指示星务计算机工作状态 2014/10/18
BSP_OS_TimeDlyMs(2000);
//printf("living!\r\n");
//bsp_LedToggle(1);
//bsp_LedToggle(2);
//bsp_LedToggle(3);
}
}
void AppSampleTask(void *p_arg)
{
(void)p_arg;
/* 监测MPU6050信息 */
while(SensorCheckCnt < 5)
{
if(i2c_CheckDevice(MPU6050_SLAVE_ADDRESS) == 0)
{
bsp_InitMPU6050(); //初始化MPU6050
MPU6050Flag |= NORMAL;
}
else
{
MPU6050Flag &= (~NORMAL);
}
/* 监测HMC5883L信息 */
if (i2c_CheckDevice(HMC5883L_SLAVE_ADDRESS) == 0)
{
bsp_InitHMC5883L(); /* 初始化HMC5883L */
HMC5883LFlag |= NORMAL;
}
else
{
HMC5883LFlag &= (~NORMAL);
}
SensorCheckCnt++;
}
if(SensorCheckCnt == 5)
{
}
while(1)
{
/* 传感器采集数据 */
MPU6050_ReadData();
HMC5883L_ReadData();
MPU6050_DataDeal(); //处理MPU6050获取的数据得到：MPU6050_H结构体
HMC5883L_DataDeal(); //处理HMC5883L获取的数得到：HMC5883L_H结构体
if(HMC5883LFlag & CALI_MODE)
{
}
if(MPU6050Flag & CALI_MODE)
{
if(!(MPU6050FlagOld & CALI_MODE)) //进入校验模式
{
CaliTime = 200;
MPU6050_H.Accel_X_Offset = g_tMPU6050.Accel_X;
MPU6050_H.Accel_Y_Offset = g_tMPU6050.Accel_Y;
MPU6050_H.Accel_Z_Offset = g_tMPU6050.Accel_Z - 65536 / 4;
MPU6050_H.GYRO_X_Offset = g_tMPU6050.GYRO_X;
MPU6050_H.GYRO_Y_Offset = g_tMPU6050.GYRO_Y;
MPU6050_H.GYRO_Z_Offset = g_tMPU6050.GYRO_Z;
}
if(CaliTime == 0)
{
MPU6050Flag &= ~(CALI_MODE);
}
MPU6050_H.Accel_X_Offset = (float)(g_tMPU6050.Accel_X + MPU6050_H.Accel_X_Offset) / 2;
MPU6050_H.Accel_Y_Offset = (float)(g_tMPU6050.Accel_Y + MPU6050_H.Accel_Y_Offset) / 2;
MPU6050_H.Accel_Z_Offset = (float)(g_tMPU6050.Accel_Z + MPU6050_H.Accel_Z_Offset - 65536 / 4) / 2;
MPU6050_H.GYRO_X_Offset = (float)(g_tMPU6050.GYRO_X + MPU6050_H.GYRO_X_Offset) / 2;
MPU6050_H.GYRO_Y_Offset = (float)(g_tMPU6050.GYRO_Y + MPU6050_H.GYRO_Y_Offset) / 2;
MPU6050_H.GYRO_Z_Offset = (float)(g_tMPU6050.GYRO_Z + MPU6050_H.GYRO_Z_Offset) / 2;
CaliTime--;
if(CaliTime > 200)
CaliTime = 200;
}
if((HMC5883LFlag & NORMAL) && (MPU6050Flag & NORMAL))
{
FlyMain();
}
if(WifiStatus == CONNECTING)
{
if((WIFI_Period++) % 5000 == 0)
{
Mem_Clr(send_data_buf, sizeof(send_data_buf));
sprintf(send_data_buf, "sy:s1:d:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%fd:%f",angleAx_temp, angleAy_temp, angleAz_temp,MPU6050_H.Accel_X,MPU6050_H.Accel_Y,MPU6050_H.Accel_Z, \
MPU6050_H.GYRO_X,MPU6050_H.GYRO_Y,MPU6050_H.GYRO_Z,HMC5883L_H.X,HMC5883L_H.Y,HMC5883L_H.Z,MPU6050_H.Accel_X_Offset,MPU6050_H.Accel_Y_Offset,MPU6050_H.Accel_Z_Offset, \
MPU6050_H.GYRO_X_Offset,MPU6050_H.GYRO_Y_Offset,MPU6050_H.GYRO_Z_Offset);
ESP8266_send_data(send_data_buf);
bsp_LedToggle(2);
}
// if(CurrentGeneralReinforce != SetGeneralReinforce)
// {
// if(SetGeneralReinforce > 100) //防止增益超出范围
// SetGeneralReinforce = 100;
//
// for(index=0; index<4; index++)
// {
// PwmTemp = (uint32_t)(CurrentPwmValue[index] * SetGeneralReinforce / 100); //计算出当前的PWM实际值
// if(PwmTemp > 100) //防止PWM占空比超出范围
// PwmTemp = 100;
//
// bsp_SetPWMDutyCycle(PwmTemp, index+1);
// }
//
// CurrentGeneralReinforce = SetGeneralReinforce; //增益赋值
// }
for(index=0; index<4; index++)
{
if(CurrentPwmValue[index] != SetPwmValue[index])
{
if(SetPwmValue[index] > 100) //防止增益超出范围
SetPwmValue[index] = 100;
bsp_SetPWMDutyCycle(SetPwmValue[index], index+1);
CurrentPwmValue[index] = SetPwmValue[index];
}
// if(SetPwmDirection[index] != CurrentPwmDirection[index])
// SetPwmDirection[index] = CurrentPwmDirection[index];
}
}
MPU6050FlagOld = MPU6050Flag;
BSP_OS_TimeDlyMs(2);
}
}
void OUTPUT_TASK(void *p_arg)
{
char i;
char timeout = 0;
(void)p_arg;
while((!MTQ_POWER_PIN()) && timeout < 50) //第一次启动等待至POWER拉高再进行处理
{
timeout++;
BSP_OS_TimeDlyMs(10);
}
if(MTQ_POWER_PIN())
{
MTQ_WAKEN;
}
PwmPwrStatusOld = MTQ_POWER_PIN(); //将Pwm引脚的电平值赋值给PwmOld
while(1)
{
if(PwmPwrStatusOld != MTQ_POWER_PIN()) //Pwm使能信号发生改变
{
if(MTQ_POWER_PIN()) //电源开启
{
MTQ_WAKEN; //芯片醒来
for(i=0; i<3; i++)
{
PwmOutPut[i].PwmSetDuty = PwmOutPut[i].PwmSetDutyOld;
}
}
else //电源关闭
{
for(i=0; i<3; i++)
{
PwmOutPut[i].PwmSetDutyOld = PwmOutPut[i].PwmSetDuty;
PwmOutPut[i].PwmCurDuty = 0;
bsp_SetPWMDutyCycle(PwmOutPut[i].PwmCurDuty, i+1);
}
MTQ_SLEEP; //关闭芯片
}
}
if(MTQ_POWER_PIN() && MTQ_SLEEP_PIN()) //MTQ打开
{
for(i=0; i<3; i++)
{
if(PwmOutPut[i].PwmSetDir != PwmOutPut[i].PwmCurDir || PwmOutPut[i].PwmSetDuty != PwmOutPut[i].PwmCurDuty)
{
PwmOutPut[i].PwmCurDuty = PwmOutPut[i].PwmSetDuty;
PwmOutPut[i].PwmCurDir = PwmOutPut[i].PwmSetDir;
if(PwmOutPut[0].PwmCurDir)
MTQ1_DIR_NAG;
else
MTQ1_DIR_POS;
if(PwmOutPut[1].PwmCurDir)
MTQ2_DIR_NAG;
else
MTQ2_DIR_POS;
if(PwmOutPut[2].PwmCurDir)
MTQ3_DIR_NAG;
else
MTQ3_DIR_POS;
if(PwmOutPut[i].PwmCurDir)
{
bsp_SetPWMDutyCycle(PwmOutPut[i].PwmCurDuty, i+1);
}
else
{
bsp_SetPWMDutyCycle(100 - PwmOutPut[i].PwmCurDuty, i+1);
}
}
}
}
/*
if(MotorCurOutput != MotorSetOutput)
{
MotorCurOutput = MotorSetOutput;
MotorChange(MotorSetOutput);
}
*/
PwmPwrStatusOld = MTQ_POWER_PIN();
BSP_OS_TimeDlyMs(1);
}
}
void BSP_Ser_Init (CPU_INT32U baud_rate)
{
FlagStatus tc_status;
GPIO_InitTypeDef gpio_init;
USART_InitTypeDef usart_init;
USART_ClockInitTypeDef usart_clk_init;
/* ------------------ INIT OS OBJECTS ----------------- */
BSP_OS_SemCreate(&BSP_SerTxWait, 0, "Serial Tx Wait");
BSP_OS_SemCreate(&BSP_SerRxWait, 0, "Serial Rx Wait");
BSP_OS_SemCreate(&BSP_SerLock, 1, "Serial Lock");
#if (BSP_CFG_SER_CMD_HISTORY_LEN > 0u)
BSP_SerCmdHistory[0] = (CPU_CHAR)'\0';
#endif
/* ----------------- INIT USART STRUCT ---------------- */
usart_init.USART_BaudRate = baud_rate;
usart_init.USART_WordLength = USART_WordLength_8b;
usart_init.USART_StopBits = USART_StopBits_1;
usart_init.USART_Parity = USART_Parity_No ;
usart_init.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
usart_init.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
usart_clk_init.USART_Clock = USART_Clock_Disable;
usart_clk_init.USART_CPOL = USART_CPOL_Low;
usart_clk_init.USART_CPHA = USART_CPHA_2Edge;
usart_clk_init.USART_LastBit = USART_LastBit_Disable;
BSP_PeriphEn(BSP_PERIPH_ID_USART2);
BSP_PeriphEn(BSP_PERIPH_ID_IOPD);
BSP_PeriphEn(BSP_PERIPH_ID_AFIO);
GPIO_PinRemapConfig(GPIO_Remap_USART2, ENABLE);
/* ----------------- SETUP USART2 GPIO ---------------- */
/* Configure GPIOD.5 as push-pull. */
gpio_init.GPIO_Pin = GPIO_Pin_5;
gpio_init.GPIO_Speed = GPIO_Speed_50MHz;
gpio_init.GPIO_Mode = GPIO_Mode_AF_PP;
GPIO_Init(GPIOD, &gpio_init);
/* Configure GPIOD.6 as input floating. */
gpio_init.GPIO_Pin = GPIO_Pin_6;
gpio_init.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOD, &gpio_init);
/* ------------------ SETUP USART2 -------------------- */
USART_Init(USART2, &usart_init);
USART_ClockInit(USART2, &usart_clk_init);
USART_Cmd(USART2, ENABLE);
USART_ITConfig(USART2, USART_IT_TC, DISABLE);
USART_ITConfig(USART2, USART_IT_TXE, DISABLE);
tc_status = USART_GetFlagStatus(USART2, USART_FLAG_TC);
while (tc_status == SET) {
USART_ClearITPendingBit(USART2, USART_IT_TC);
USART_ClearFlag(USART2, USART_IT_TC);
BSP_OS_TimeDlyMs(10);
tc_status = USART_GetFlagStatus(USART2, USART_FLAG_TC);
}
BSP_IntVectSet(BSP_INT_ID_USART2, BSP_Ser_ISR_Handler);
BSP_IntEn(BSP_INT_ID_USART2);
}
void OUTPUT_TASK(void *p_arg)
{
char timeout = 0;
(void)p_arg;
//PwmCurDuty_1 = PwmSetDuty_1;
//PwmCurDuty_2 = PwmSetDuty_2;
//PwmCurDuty_3 = PwmSetDuty_3;
while((!MTQ_POWER_PIN()) && timeout < 50) //第一次启动等待至POWER拉高再进行处理
{
timeout++;
BSP_OS_TimeDlyMs(100);
}
if(MTQ_POWER_PIN())
{
MTQ_WAKEN;
}
PwmPwrStatusOld = MTQ_POWER_PIN(); //将Pwm引脚的电平值赋值给PwmOld
while(1)
{
if(PwmPwrStatusOld != MTQ_POWER_PIN()) //Pwm使能信号发生改变
{
if(MTQ_POWER_PIN()) //电源开启
{
MTQ_WAKEN; //芯片醒来
PwmSetDuty_1 = PwmCurDutyOld_1;
PwmSetDuty_2 = PwmCurDutyOld_2;
PwmSetDuty_3 = PwmCurDutyOld_3;
}
else //电源关闭
{
PwmCurDutyOld_1 = PwmCurDuty_1;
PwmCurDutyOld_2 = PwmCurDuty_2;
PwmCurDutyOld_3 = PwmCurDuty_3;
PwmCurDuty_1 = 0;
PwmCurDuty_2 = 0;
PwmCurDuty_3 = 0;
bsp_SetPWMDutyCycle(PwmCurDuty_1, 1);
bsp_SetPWMDutyCycle(PwmCurDuty_2, 2);
bsp_SetPWMDutyCycle(PwmCurDuty_3, 3);
MTQ_SLEEP; //关闭芯片
}
}
if(MTQ_POWER_PIN() && MTQ_SLEEP_PIN()) //MTQ打开
{
if(PwmCurDuty_1 != PwmSetDuty_1)
{
PwmCurDuty_1 = PwmSetDuty_1;
bsp_SetPWMDutyCycle(PwmCurDuty_1, 1);
}
if(PwmCurDuty_2 != PwmSetDuty_2)
{
PwmCurDuty_2 = PwmSetDuty_2;
bsp_SetPWMDutyCycle(PwmCurDuty_2, 2);
}
if(PwmCurDuty_3 != PwmSetDuty_3)
{
PwmCurDuty_3 = PwmSetDuty_3;
bsp_SetPWMDutyCycle(PwmCurDuty_3, 3);
}
}
/*
if(MotorCurOutput != MotorSetOutput)
{
MotorCurOutput = MotorSetOutput;
MotorChange(MotorSetOutput);
}
*/
PwmPwrStatusOld = MTQ_POWER_PIN();
BSP_OS_TimeDlyMs(100);
}
}