//函数名：Controler()
//输入：无
//输出: 无
//描述：飞机控制函数主体，被定时器调用
//作者：马骏
//备注：没考上研，心情不好
void Controler(void)
{
static char Counter_Cnt=0;
Counter_Cnt++;
DMP_Routing(); //DMP 线程 所有的数据都在这里更新
DMP_getYawPitchRoll(); //读取 姿态角
/*******************向上位机发送姿态信息，如果要在PC上位机看实时姿态，需要把这里解注释****************/
/*******************PC姿态显示，跟串口debug不能共存****************/
Send_AtitudeToPC();
if(Counter_Cnt==5)
{
Counter_Cnt=0;
Nrf_Irq(); //从2.4G接收控制目标参数
//ReceiveDataFormUART();//从蓝牙透传模块接收控制目标参数，和2.4G接收控制只能选其一
PID_Calculate(); //=2时控制一次,频率500HZ
}
}
//函数名：Controler()
//输入：无
//输出: 无
//描述：飞机控制函数主体，被定时器调用
//作者：马骏
//备注：没考上研，心情不好
void Controler(void)
{
static char Counter_Cnt=0;
Counter_Cnt++;
DMP_Routing(); //DMP 线程 所有的数据都在这里更新
DMP_getYawPitchRoll(); //读取 姿态角
/*******************向上位机发送姿态信息，如果要在PC上位机看实时姿态,宏开关控制***************/
#ifndef Debug
Send_AtitudeToPC();
#else
#endif
if(Counter_Cnt==5)
{
Counter_Cnt=0;
Nrf_Irq(); //从2.4G接收控制目标参数
//ReceiveDataFormUART();//从蓝牙透传模块接收控制目标参数，和2.4G接收控制只能选其一
PID_Calculate(); //=2时控制一次,频率200HZ
}
}
void loop()
{
static uint32_t nextTick = 0;
while(millis()<nextTick){}
nextTick = millis()+TICK_FRAME_PERIOD; //循环间隔FRAME
//处理蓝牙命令
CommandProcess();
//读取遥控命令
Comm_Process();
if(GetFrameCount()%10 == 0)
{
//读取姿态传感器数据
//读取欧拉角
#ifdef IMU_SW //软件姿态解算
//IMUSO3Thread();
#else
DMP_Routing(); //DMP 线程 所有的数据都在这里更新
#endif
//imu校准
// if(imuCaliFlag)
// {
// if(IMU_Calibrate())
// {
// imuCaliFlag=0;
// gParamsSaveEEPROMRequset=1; //请求记录到EEPROM
// imu.caliPass=1;
// }
// }
//PID二环角速度
//CtrlAttiRate();
//控制电机
//CtrlMotor();
}
if(GetFrameCount()%20 == 0)
{
//处理遥控数据
//PID一环角度控制
//CtrlAttiAng();
}
//10HZ,每100ms一次
//if(getTickCount()%100 == 0)
if(GetFrameCount()%1000 == 0)
{
//检测电池电量
BatteryCheck();
//printf("Convert result is %d\n", GetBatteryAD());
//遥控通信丢失处理
//更新LED灯状态
UpdateLED();
}
if(GetFrameCount()%100 == 0)
{
//printf("yaw=%d, roll=%d, pitch=%d \n",(int)imu.yaw, (int)imu.roll, (int)imu.pitch);
//printf("\n");
}
//故障保护
if(GetFrameCount() > 6000 && GetFrameCount() < 8000)
{
Motor_Start();
MotorPwmOutput(0,0,85,0);
}
// else if(GetFrameCount() >= 8000 && GetFrameCount() < 10000)
// {
// MotorPwmOutput(40,40,40,40);
// }
// else if(GetFrameCount() >= 10000 && GetFrameCount() < 12000)
// {
// MotorPwmOutput(60,60,60,60);
// }
// else if(GetFrameCount() >= 12000 && GetFrameCount() < 14000)
// {
// MotorPwmOutput(80,80,80,80);
// }
// else if(GetFrameCount() >= 14000 && GetFrameCount() < 16000)
// {
// MotorPwmOutput(100,100,100,100);
// }
else if(GetFrameCount() >= 40000 && !falg)
{
falg = true;
//MotorPwmOutput(0,0,0,0);
Motor_Stop();
}
IncFrameCount(1);
}
