void SYS_INIT(void) { LED_INIT(); LED_FLASH(); Moto_Init(); LED_FLASH(); Tim3_Init(500); Nvic_Init(); Uart1_Init(115200); ANO_TC_I2C2_INIT(0xA6,400000,1,1,3,3); MPU6050_Init(); Spi1_Init(); Nrf24l01_Init(MODEL_TX2,40); if(Nrf24l01_Check()) Uart1_Put_String("NRF24L01 IS OK !\r\n"); else Uart1_Put_String("NRF24L01 IS NOT OK !\r\n"); ADC1_Init(); FLASH_Unlock(); EE_INIT(); EE_READ_ACC_OFFSET(); EE_READ_GYRO_OFFSET(); EE_READ_PID(); Tim3_Control(1); }
void SYS_INIT(void) { LED_INIT(); //LED及串口IO 初始化 LED_FLASH(); //LED闪烁 Tim3_Init(500); //中断初始化 //1000=1MS,500=0.5MS Moto_Init(); //PWM // Uart1_Init(115200); //串口初始化,飞控上几乎无用 Spi1_Init(); //SPI初始化 Nvic_Init(); //中断初始化 Nrf24l01_Init(MODEL_TX2,40); //2401中断初始化 主发送 通道 40 // if(Nrf24l01_Check()) Uart1_Put_String("NRF24L01 IS OK !\r\n"); //检测2401是否初始化成功 // else Uart1_Put_String("NRF24L01 IS NOT OK !\r\n"); InitMPU6050(); ADC1_Init(); //检测电池电压 FLASH_Unlock(); //保存飞飞控参数 EE_INIT(); EE_READ_ACC_OFFSET(); EE_READ_GYRO_OFFSET(); EE_READ_PID(); PID_ROL.P = PID_PIT.P = 5; //用于初始化pid,如用匿名上位机写入pid,则屏蔽 PID_ROL.D = PID_PIT.D = 0.1; PID_YAW.P = 0.5; PID_YAW.D = 0.05; }
void Sys_Init() { NVIC_Config(); Usart1_Config(); DMA_Config(); SPI1_Init(); Nrf24l01_Init(3,40); LED_Config(); LED_OFF(); // I2C_MPU_Init(); // MPU6050_Init(); }
int main(void) { u8 i; u8 RecvBuf[32]; u8 SendBuf[32]; u8 offline = 0; u8 recv_flag = 0; u32 pd2ms=0,pd20ams=0,pd20bms=0,pd100ms=0; SystemInit(); RCC_Configuration(); NVIC_Configuration(); GPIO_Configuration(); USART1_Configuration(); dbgPrintf(" Init Ticktack !\r\n"); cycleCounterInit(); SysTick_Config(SystemCoreClock / 1000); for(i=0;i<2;i++) { OP_LED1;OP_LED2;OP_LED3;OP_LED4; delay_ms(500); OP_LED1;OP_LED2;OP_LED3;OP_LED4; delay_ms(500); } FilterInit(); controllerInit(); dbgPrintf(" Init eeprom!\r\n"); FLASH_Unlock(); EE_Init(); EE_Read_ACC_GYRO_Offset(); /* 如果PID丢失或者错误,将下面两行注释去掉,重新编译烧写,运行一遍,可将PID还原,然后重新注释,再烧写一遍 */ //EE_Write_PID(); //EE_Write_Rate_PID(); EE_Read_PID(); EE_Read_Rate_PID(); dbgPrintf(" Init adc!\r\n"); ADC_DMA_Init(); dbgPrintf(" Init NRF24L01 !\r\n"); SPI_NRF_Init(); Nrf24l01_Init(); NRF24l01_SetRxMode(); while(Nrf24l01_Check()) { dbgPrintf("NRF24L01 Fault !\r\n"); delay_ms(500); } dbgPrintf("NRF24L01 Is Detected !\r\n"); dbgPrintf("Init MPU6050...\r\n"); IIC_Init(); MPU6050_initialize(); dbgPrintf("Init Motor...\r\n"); Motor_Init(); Motor_SetPwm(0,0,0,0); pd20bms = TIMIRQCNT + 10*ITS_PER_MS; while(1) { if(TIMIRQCNT>pd2ms + 2*ITS_PER_MS-1) // every 4ms { GetEulerAngle(); if(lock_flag==UNLOCK) AttitudeToMotors(angle.y,angle.x,angle.z); else { MOTOR1=0; MOTOR2=0; MOTOR3=0; MOTOR4=0; } Motor_SetPwm(MOTOR1,MOTOR2,MOTOR3,MOTOR4); pd2ms = TIMIRQCNT; } if(TIMIRQCNT>pd20ams + 20*ITS_PER_MS-1) // every 20ms { if(NRF24l01_Recv(RecvBuf)>10) { if((RecvBuf[RecvBuf[2]+3]==CheckSum(RecvBuf, RecvBuf[2]+3))&&(RecvBuf[0]==0xAA)) { if(RecvBuf[1]!=0xC0) OP_LED1; offline=0; switch(RecvBuf[1]) { case 0xC0: //control Getdesireddata(RecvBuf); OP_LED2; break; case 0x10: //W PID SetPID(RecvBuf); break; case 0x11: //W Attitude SetAccGyroOffset(RecvBuf); break; case 0x12: //W Control offset break; case 0x14: //W Rate PID SetRatePID(RecvBuf); break; case 0x20: //R PID recv_flag = RESEND; GetPID(SendBuf); break; case 0x21: //R Attitude recv_flag = RESEND; GetAccGyroOffset(SendBuf); break; case 0x22: //R Control offset break; case 0x24: //R Rate PID recv_flag = RESEND; GetRatePID(SendBuf); break; case 0x40: //校准姿态 EnableCalibration(); break; case 0x41: //校准遥控器零点 break; default: break; } } } pd20ams = TIMIRQCNT; } if(TIMIRQCNT>pd20bms + 20*ITS_PER_MS-1) // every 20ms { if(recv_flag==0) GetState(SendBuf); else recv_flag--; NRF_SendData(SendBuf); OP_LED3; pd20bms = TIMIRQCNT; } if(TIMIRQCNT>pd100ms + 100*ITS_PER_MS-1) // every 100ms { if(offline>20) lock_flag = LOCK; offline++; // OP_LED4; pd100ms = TIMIRQCNT; } } }