__interrupt void PID_CONTROL() { TA1CCTL1 &=~ CCIFG; switch(CpuTimePoll) { //第一个3ms给控制 每6ms控制一次 case 0 : ANGLE_CALCULATE(); //四元数转欧拉角 这个执行了1.2MS左右 PID_ROLL(); //间接计算PID占空比 PID时间 不长 PID_PITCH(); PID_YALL(); PWM_CALCULATE(); //PID输出值转换为四路电机的PWM占空比 PWM_SET(); //PWM占空比设置 //以上执行2.2ms break; //第二给3ms给接收控制信息 发送周期为15ms其中包括一个3ms的接收 case 1 : if(time1 % 4 == 0) { Data_Send_PID1(); } else if(time1 % 4 == 1) { Data_Send_MotoPWM(); //设置电机 } else if(time1 % 4 == 2) { Data_Send_Status(); } else if(time1 % 4 == 3) { Data_Send_Senser(); } if(time1++ == 3) time1 = 0; break; } if(CpuTimePoll++ == 1) CpuTimePoll = 0; }
void Send_Data(void) { if(Send_Status==2) { Data_Send_Status(); } else if(Send_Status==4) { Data_Send_RCData(); } else if(Send_Status==6) { Data_Send_MotoPWM(); } else if(Send_Status==8) { Data_Send_Senser(); } else if(Send_Status==10) { if(SendPidDataFlag==1) { Data_Send_PID1(); } } else if(Send_Status==12) { if(SendPidDataFlag==1) { Data_Send_PID2(); SendPidDataFlag=0; } Send_Status=0; } Send_Status++; }
void Data_Exchange(void) { #ifdef DATA_TRANSFER_USE_SPI_NRF Nrf_Check_Event(); u8 sta = Nrf_Get_FIFOSta(); if ((sta & (1 << 4)) == 0) return; #endif if (Send.DataF1) { Send.DataF1 = 0; Data_Send_F1(); } if (Send.DataF2) { Send.DataF2 = 0; Data_Send_F2(); } if (Send.DataF3) { Send.DataF3 = 0; Data_Send_F3(); } if (Send.RCData && Send.DataF4) { Send.DataF4 = 0; Data_Send_F4(); } if (Send.Status) { Send.Status = 0; Data_Send_Status(); } else if (Send.GpsData) { Send.GpsData = 0; //Data_Send_GpsData(); } if (Send.Senser) { Send.Senser = 0; Data_Send_Senser(); } if (Send.PID1) { Send.PID1 = 0; Data_Send_PID1(); } else if (Send.PID2) { Send.PID2 = 0; Data_Send_PID2(); } else if (Send.PID3) { Send.PID3 = 0; Data_Send_PID3(); } else if (Send.PID4) { Send.PID4 = 0; Data_Send_PID4(); } else if (Send.PID5) { Send.PID5 = 0; Data_Send_PID5(); } else if (Send.PID6) { Send.PID6 = 0; Data_Send_PID6(); } if (Send.RCData && Ex_ON_OFF.RCData) { Send.RCData = 0; Data_Send_RCData(); } if (Send.Offset) { Send.Offset = 0; Data_Send_OFFSET(); } if (Send.MotoPwm) { Send.MotoPwm = 0; Data_Send_MotoPWM(); } }
void Data_Exchange(void) { #ifdef DATA_TRANSFER_USE_SPI_NRF Nrf_Check_Event(); u8 sta = Nrf_Get_FIFOSta(); if((sta & (1<<4))==0) return; #endif if(Send_Status) { Send_Status = 0; Data_Send_Status(); } else if(Send_Senser) { Send_Senser = 0; Data_Send_Senser(); } else if(Send_PID1) { Send_PID1 = 0; Data_Send_PID1(); } else if(Send_PID2) { Send_PID2 = 0; Data_Send_PID2(); } else if(Send_PID3) { Send_PID3 = 0; Data_Send_PID3(); } else if(Send_PID4) { Send_PID4 = 0; Data_Send_PID4(); } else if(Send_PID5) { Send_PID5 = 0; Data_Send_PID5(); } else if(Send_PID6) { Send_PID6 = 0; Data_Send_PID6(); } else if(Send_RCData) { Send_RCData = 0; Data_Send_RCData(); } else if(Send_Offset) { Send_Offset = 0; Data_Send_OFFSET(); } else if(Send_MotoPwm) { Send_MotoPwm = 0; Data_Send_MotoPWM(); } }
//Send_Senser = 1; //Send_Status = 1; // Send_MotoPwm = 1; void Data_Exchange(void) { #ifdef DATA_TRANSFER_USE_SPI_NRF Nrf_Check_Event();//当使用 24l01时 必须 使用此函数 清 寄存器 u8 sta = Nrf_Get_FIFOSta();//清FIFO if ((sta & (1 << 4)) == 0) return; #endif if (Send_Status) { Send_Status = 0; //Data_Send_Status(); } else if (Send_GpsData) { Send_GpsData = 0; //Data_Send_GpsData(); } if (Send_Senser) { Send_Senser = 0; //Data_Send_Senser(); } if (Send_PID1==1) { Send_PID1 = 2; Data_Send_PID1(0); } else if (Send_PID1==2) { Send_PID1 = 0; Data_Send_PID1(1); } else if (Send_PID2==1) { Send_PID2 = 0; Data_Send_PID2(0); } else if (Send_PID2==2) { Send_PID2 = 0; Data_Send_PID2(1); } else if (Send_PID3==1) { Send_PID3 = 0; Data_Send_PID3(0); } else if (Send_PID3==2) { Send_PID3 = 0; Data_Send_PID3(1); } if (Send_RCData) { Send_RCData = 0; // Data_Send_RCData(); } else if (Send_Offset) { Send_Offset = 0; //Data_Send_OFFSET(); } if (Send_MotoPwm) { Send_MotoPwm = 0; // Data_Send_MotoPWM(); } }