/*=====================================================================================================*/
void System_Init( void )
{
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
I2C_Config();
Motor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +3.5f;
PID_Pitch.Ki = +0.004f;
PID_Pitch.Kd = +4.0f;
PID_Roll.Kp = +3.5f;
PID_Roll.Ki = +0.004f;
PID_Roll.Kd = +4.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.25f;
Delay_10ms(2);
}
/*=====================================================================================================*/
void System_Init(void)
{
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
PWM_Capture_Config();
Sensor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +1.5f;
PID_Pitch.Ki = +0.002f;
PID_Pitch.Kd = +1.0f;
PID_Roll.Kp = +1.5f;
PID_Roll.Ki = +0.002f;
PID_Roll.Kd = +1.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.0f;
Delay_10ms(2);
}
void main()
{
nRF24L01_Config();
SetRX_Mode();
if(MAX_RT)
{
SPI_RW_Reg(FLUSH_TX,0);
}
SPI_RW_Reg(WRITE_REG+STATUS,0xFF);// clear RX_DR or TX_DS or MAX_RT interrupt flag
SPI_RW_Reg(FLUSH_RX,0);
SCON =0x40;
TMOD = 0x20;
TH1 = 0xfd;
TL1 = 0xfd;
TR1 = 1;
EX1=1;
IT1=0;
EA=1;
init_1602();
while(1)
{
Control();
display();
if(RxBuf[0]==0xAA)
{
RxBuf[0]=0;
//PH_dat=RxBuf[1];
temp_dat=RxBuf[2];
temp_dot=RxBuf[3];
beep_flag=RxBuf[4];
}
delay1ms(10);
}
}
void main()
{
TxBuf[0]=0x00;
Read_18B20_Temperature();
Read_18B20_Temperature();
delay1ms(200);
Read_18B20_Temperature();
Read_18B20_Temperature();
delay1ms(200);
Read_18B20_Temperature();
Read_18B20_Temperature();
delay1ms(200);
nRF24L01_Config(); //初始化NRF24L01
SPI_RW_Reg(FLUSH_TX,0);
EX1=1;
IT1=1;
EA=1;
init_1602();
while(1)
{
Read_18B20_Temperature();
keyscan();
Control();
display();
TxBuf[0]=0xAA;
TxBuf[2]=Tem_dispbuf[4];
TxBuf[3]=Tem_dispbuf[0];
TxBuf[4]=beep_flag;
nRF24L01_TxPacket(TxBuf); //发送数据
delay1ms(10);
}
}
void system_init(void)
{
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
PWM_Capture_Config();
Sensor_Config();
nRF24L01_Config();
#if configSD_BOARD
SDIO_Config();
#endif
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +4.0f;
PID_Pitch.Ki = 0;//0.002f;
PID_Pitch.Kd = +1.5f;
PID_Roll.Kp = +4.0f;
PID_Roll.Ki = 0;//0.002f;
PID_Roll.Kd = 1.5f;
PID_Yaw.Kp = +5.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +15.0f;
Delay_10ms(10);
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
#if configFLIGHT_CONTROL_BOARD
/* nRF Check */
while ( nRF_Check()== ERROR );
/* Sensor Init */
while(Sensor_Init() == ERROR);
#endif
Delay_10ms(10);
/* Lock */
LED_R = 0;
LED_G = 1;
LED_B = 1;
sys_status = SYSTEM_INITIALIZED;
}
/*=====================================================================================================*/
void QCopterFC_Init( void )
{
u8 Sta = ERROR;
SystemInit();
LED_Config();
KEY_Config();
RS232_Config();
Motor_Config();
Sensor_Config();
nRF24L01_Config();
PID_Init(&PID_Yaw);
PID_Init(&PID_Roll);
PID_Init(&PID_Pitch);
PID_Pitch.Kp = +1.5f;
PID_Pitch.Ki = +0.002f;
PID_Pitch.Kd = +1.0f;
PID_Roll.Kp = +1.5f;
PID_Roll.Ki = +0.002f;
PID_Roll.Kd = +1.0f;
PID_Yaw.Kp = +0.0f;
PID_Yaw.Ki = +0.0f;
PID_Yaw.Kd = +0.0f;
RF_SendData.Packet = 0x00;
/* Throttle Config */
if(KEY == KEY_ON) {
LED_B = 0;
BLDC_CtrlPWM(BLDC_PWM_MAX, BLDC_PWM_MAX, BLDC_PWM_MAX, BLDC_PWM_MAX);
}
while(KEY == KEY_ON);
LED_B = 1;
BLDC_CtrlPWM(BLDC_PWM_MIN, BLDC_PWM_MIN, BLDC_PWM_MIN, BLDC_PWM_MIN);
/* nRF Check */
while(Sta == ERROR)
Sta = nRF_Check();
Delay_10ms(10);
/* Sensor Init */
if(Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
}
void system_init(void)
{
LED_Config();
Serial_Config(Serial_Baudrate);
Motor_Config();
PWM_Capture_Config();
//IMU Config
Sensor_Config();
nRF24L01_Config();
//SD Config
if ((SD_status = SD_Init()) != SD_OK)
system.status = SYSTEM_ERROR_SD;
PID_Init(&PID_Pitch, 4.0, 0.0, 1.5);
PID_Init(&PID_Roll, 4.0, 0.0, 1.5);
PID_Init(&PID_Yaw, 5.0, 0.0, 15.0);
Delay_10ms(10);
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while (nRF_Check() == ERROR);
/* Sensor Init */
while (Sensor_Init() == ERROR);
Delay_10ms(10);
/* Lock */
SetLED(LED_R, DISABLE);
SetLED(LED_G, ENABLE);
SetLED(LED_B, ENABLE);
//Check if no error
if (system.status != SYSTEM_ERROR_SD)
system.status = SYSTEM_INITIALIZED;
}