/*=====================================================================================================*/ 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); }
/*=====================================================================================================*/ int main( void ) { SystemInit(); LED_Config(); KEY_Config(); #if FLASH_RW_U8 Flash_EraseSector(Flash_GetSector(FLASH_SECTOR_6)); Flash_WriteDataU8(FLASH_SECTOR_6, WriteDataU8, 1024); Flash_ReadDataU8(FLASH_SECTOR_6, ReadDataU8, 1024); LED_R = (Cmp_ArrU8(WriteDataU8, ReadDataU8, 1024) == SUCCESS) ? 0 : 1; #endif #if FLASH_RW_U16 Flash_EraseSector(Flash_GetSector(FLASH_SECTOR_6)); Flash_WriteDataU16(FLASH_SECTOR_6, WriteDataU16, 512); Flash_ReadDataU16(FLASH_SECTOR_6, ReadDataU16, 512); LED_G = (Cmp_ArrU16(WriteDataU16, ReadDataU16, 512) == SUCCESS) ? 0 : 1; #endif #if FLASH_RW_U32 Flash_EraseSector(Flash_GetSector(FLASH_SECTOR_6)); Flash_WriteDataU32(FLASH_SECTOR_6, WriteDataU32, 256); Flash_ReadDataU32(FLASH_SECTOR_6, ReadDataU32, 256); LED_B = (Cmp_ArrU32(WriteDataU32, ReadDataU32, 256) == SUCCESS) ? 0 : 1; #endif Delay_10ms(100); while(1) { LED_R = !LED_R; LED_G = !LED_G; LED_B = !LED_B; Delay_10ms(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 KeyBoard_Config( void ) { LED_Config(); KEY_Config(); ADC_Config(); }