/*=====================================================================================================*/
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 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)
{
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;
}
void PWM_Config( void ){
Motor_Config();
}
