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;
}
}
}
