void check_func(int argc, char *argv[]){
u8 Sta = ERROR;
printf("Check.....\n");
Sta = nRF_Check();
if(Sta == ERROR){
printf("Fail.....\n");
}else{
printf("Success!!!!!!\n");
}
}
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);
}
int main(void)
{
int status;
memset(RX_BUF,0,4);
led_init();
nRF24L01_init();
motor_init(); //电机初始化
QuadCopter_init(&QuadCopter);
uart_init(115200);
status = nRF_Check(); /*检测NRF模块与MCU的连接*/
if(status == SUCCESS) /*判断连接状态*/
uart_printf("\r\n NRF与MCU连接成功！\r\n");
else
uart_printf("\r\n NRF与MCU连接失败，请重新检查接线。\r\n");
nRF_RX_Mode();
LED_OFF;
while(1)
{
nRF_RX_Mode();
nRF_Rx_Dat(RX_BUF);
status=RX_BUF[2];
switch(status)
{
case Q_ON:
LED_ON;
QuadCopter.Status=Q_ON;
QuadCopter.BaseSpeed=300;
motor_speed(QuadCopter.BaseSpeed,QuadCopter.BaseSpeed,QuadCopter.BaseSpeed,QuadCopter.BaseSpeed ) ;
break;
case Q_UP:
QuadCopter_up(&QuadCopter);
break;
case Q_DOWN:
QuadCopter_down(&QuadCopter);
break;
case Q_OFF:
LED_OFF;
QuadCopter.Status=Q_OFF;
motor_speed(0,0,0,0 ) ;
break;
default:
break;
}
}
}
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;
}
/*=====================================================================================================*/
int main(void)
{
u8 Sta = ERROR;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
System_Init();
test_printf();
/* Throttle Config */
if (KEY == 1) {
LED_B = 0;
Motor_Control(PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX);
}
while (KEY == 1);
LED_B = 1;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while (Sta == ERROR)
Sta = nRF_Check();
/* Sensor Init */
if (Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
/* Systick Config */
if (SysTick_Config(SystemCoreClock / SampleRateFreg)) { // SampleRateFreg = 500 Hz
while (1);
}
/* Wait Correction */
while (SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while (!KEY) {
LED_B = ~LED_B;
Delay_10ms(1);
Transport_Send(TxBuf[0]);
printf("Roll%d,Pitch%d,Yaw%d,CH1 %u(%d),CH2 %u(%d),CH3 %u(%d),CH4 %u(%d),CH5 %u()\r\n",
(int)AngE.Roll, (int)AngE.Pitch, (int)AngE.Yaw,
PWM1_InputCaptureValue, global_rc_roll,
PWM2_InputCaptureValue, global_rc_pitch,
PWM3_InputCaptureValue, global_rc_thr,
PWM4_InputCaptureValue, global_rc_yaw,
PWM5_InputCaptureValue);
}
LED_B = 1;
/* Final State Machine */
while (1) {
LED_G = ~LED_G;
switch (FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
do {
Sta = nRF_Tx_Data(TxBuf[0]);
} while (Sta == MAX_RT);
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf[0]);
if (Sta == RX_DR) {
Transport_Recv(RxBuf[0]);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
CTRL_FlightControl();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf[0] + 20, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
Transport_Send(TxBuf[0]);
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}
/*=====================================================================================================*/
int main( void )
{
u8 Sta = ERROR;
FSM_Mode FSM_State = FSM_Rx;
/* System Init */
System_Init();
/* Throttle Config */
if(KEY == 1) {
LED_B = 0;
Motor_Control(PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX, PWM_MOTOR_MAX);
}
while(KEY == 1);
LED_B = 1;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
/* nRF Check */
while(Sta == ERROR)
Sta = nRF_Check();
/* Sensor Init */
if(Sensor_Init() == SUCCESS)
LED_G = 0;
Delay_10ms(10);
/* Systick Config */
if(SysTick_Config(420000)) { // 168MHz / 420000 = 400Hz = 2.5ms
while(1);
}
/* Wait Correction */
while(SensorMode != Mode_Algorithm);
/* Lock */
LED_R = 1;
LED_G = 1;
LED_B = 1;
while(!KEY) {
LED_B = ~LED_B;
Delay_10ms(1);
Transport_Send(TxBuf[0]);
RS232_VisualScope(USART3, TxBuf[0]+2, 8);
}
LED_B = 1;
/* Final State Machine */
while(1) {
LED_G = ~LED_G;
switch(FSM_State) {
/************************** FSM Tx ****************************************/
case FSM_Tx:
// FSM_Tx
nRF_TX_Mode();
do {
Sta = nRF_Tx_Data(TxBuf[0]);
} while(Sta == MAX_RT);
// FSM_Tx End
FSM_State = FSM_Rx;
break;
/************************** FSM Rx ****************************************/
case FSM_Rx:
// FSM_Rx
nRF_RX_Mode();
Sta = nRF_Rx_Data(RxBuf[0]);
if(Sta == RX_DR) {
Transport_Recv(RxBuf[0]);
}
// FSM_Rx End
FSM_State = FSM_CTRL;
break;
/************************** FSM CTRL **************************************/
case FSM_CTRL:
// FSM_CTRL
CTRL_FlightControl();
// FSM_CTRL End
FSM_State = FSM_UART;
break;
/************************** FSM UART ***************************************/
case FSM_UART:
// FSM_USART
RS232_VisualScope(USART3, TxBuf[0]+2, 8);
// FSM_USART End
FSM_State = FSM_DATA;
break;
/************************** FSM DATA **************************************/
case FSM_DATA:
// FSM_DATA
Transport_Send(TxBuf[0]);
// FSM_DATA End
FSM_State = FSM_Tx;
break;
}
}
}