int main(void) { Periph_clock_enable(); GPIO_Config(); LEDon; Delay_ms(10); //short blink LEDoff; Delay_ms(50); Usart4Init(); ADC_Config(); MPU6050_Init(); Timer2_Config(); Timer3_Config();//RC control timer NVIC_Configuration(); EXTI_Config(); //engineInit(); ///????to initialize all variables; configLoad(); MPU6050_Gyro_calibration(); while(1) { engineProcess(); while(stop==0) {}//Closed loop waits for interrupt } }
void RC_Config(void) { GPIO_InitTypeDef GPIO_InitStructure; __disable_irq(); GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable , ENABLE); // PA15 must be initialized after PA15/PB3 are made available with GPIO_Remap_SWJ_JTAGDisable GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15; // PA15 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; // Set to Input GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // GPIO Speed GPIO_Init(GPIOA, &GPIO_InitStructure); //EXTI IN GPIO Config GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_4; // PB3-Pitch, PB4-Roll GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD; // Set to Input Pull Down GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; // GPIO Speed GPIO_Init(GPIOB, &GPIO_InitStructure); GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; // PC2-Yaw GPIO_Init(GPIOC, &GPIO_InitStructure); GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource3); GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource4); GPIO_EXTILineConfig(GPIO_PortSourceGPIOC, GPIO_PinSource2); EXTI_InitTypeDef EXTI_InitStructure; EXTI_InitStructure.EXTI_Line = EXTI_Line3 | EXTI_Line4 | EXTI_Line2; EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling; EXTI_InitStructure.EXTI_LineCmd = ENABLE; EXTI_Init(&EXTI_InitStructure); EXTI_GenerateSWInterrupt(EXTI_Line3 | EXTI_Line4 | EXTI_Line2); EXTI_ClearITPendingBit(EXTI_Line3 | EXTI_Line4 | EXTI_Line2); NVIC_EnableIRQ(EXTI3_IRQn); // Enable interrupt NVIC_EnableIRQ(EXTI4_IRQn); // Enable interrupt NVIC_EnableIRQ(EXTI2_IRQn); // Enable interrupt Timer3_Config(); //RC control timer __enable_irq(); }
int main(void) { Delay_ms(100); Periph_clock_enable(); GPIO_Config(); Usart4Init(); I2C_Config(); ADC_Config(); MPU6050_Init(); Timer1_Config(); Timer8_Config(); Timer2_Config(); Timer5_Config(); Timer4_Config(); Timer3_Config();//RC control timer NVIC_Configuration(); EXTI_Config(); TIM_Cmd(TIM5, ENABLE); TIM_CtrlPWMOutputs(TIM5, ENABLE); for (i = 1 ; i < 1 ; i++) ; //small delay before starting Timer4 TIM_Cmd(TIM4, ENABLE); TIM_CtrlPWMOutputs(TIM4, ENABLE); Delay_ms(100); for (i = 0; i < configDataSize; i++) //reads configuration from eeprom { ReadFromEEPROM(i); configData[i] = EepromData; Delay_ms(5); } I2C_AcknowledgeConfig(I2C2, ENABLE); Delay_ms(100); while (1) { LEDon; DEBUG_LEDon; while (ConfigMode == 1) { TimerOff(); //Configuration loop } MPU6050_ACC_get();//Getting Accelerometer data acc_roll_angle = -(atan2(accADC_x, accADC_z)) + (configData[11] - 50.00) * 0.0035; //Calculating pitch ACC angle+callibration acc_pitch_angle = +(atan2(accADC_y, accADC_z)); //Calculating roll ACC angle MPU6050_Gyro_get();//Getting Gyroscope data acc_roll_angle_vid = ((acc_roll_angle_vid * 99.00) + acc_roll_angle) / 100.00; //Averaging pitch ACC values acc_pitch_angle_vid = ((acc_pitch_angle_vid * 99.00) + acc_pitch_angle) / 100.00; //Averaging roll ACC values sinus = sinusas[(int)(rc4)]; //Calculating sinus cosinus = sinusas[90 - (int)(rc4)]; //Calculating cosinus ROLL = -gyroADC_z * sinus + gyroADC_y * cosinus; roll_angle = (roll_angle + ROLL * dt) + 0.0002 * (acc_roll_angle_vid - roll_angle); //Roll Horizon //ROLL=-gyroADC_z*sinus+gyroADC_y*cosinus; yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw pitch_angle_true = ((pitch_angle_true + gyroADC_x * dt) + 0.0002 * (acc_pitch_angle_vid - pitch_angle_true)); //Pitch Horizon ADC1Ch1_vid = ((ADC1Ch1_vid * 99.00) + (readADC1(1) / 4000.00)) / 100.00; //Averaging ADC values ADC1Ch1_vid = 0.00; rc4_avg = ((rc4_avg * 499.00) + (rc4)) / 500.00; //Averaging RC4 values pitch_angle = pitch_angle_true - rc4_avg / 57.3; //Adding angle pitch_angle_correction = pitch_angle * 150.0; if (pitch_angle_correction > 2.0) { pitch_angle_correction = 2.0; } if (pitch_angle_correction < -2.0) { pitch_angle_correction = -2.0; } pitch_setpoint = pitch_setpoint + pitch_angle_correction; //Pitch return to zero after collision roll_angle_correction = roll_angle * 200.0; if (roll_angle_correction > 2.0) { roll_angle_correction = 2.0; } if (roll_angle_correction < -2.0) { roll_angle_correction = -2.0; } roll_setpoint = roll_setpoint + roll_angle_correction; //Roll return to zero after collision ADC1Ch13_vid = ((ADC1Ch13_vid * 99.00) + ((readADC1(13) - 2000) / 4000.00)) / 100.00; //Averaging ADC values if (configData[10] == '0') { yaw_angle = (yaw_angle + gyroADC_z * dt) + 0.01 * (ADC1Ch13_vid - yaw_angle); //Yaw AutoPan } if (configData[10] == '1') { yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw RCPan } yaw_angle_correction = yaw_angle * 50.0; if (yaw_angle_correction > 1.0) { yaw_angle_correction = 1.0; } if (yaw_angle_correction < -1.0) { yaw_angle_correction = -1.0; } yaw_setpoint = yaw_setpoint + yaw_angle_correction; //Yaw return to zero after collision pitch_PID();//Pitch axis pid roll_PID(); //Roll axis pid yaw_PID(); //Yaw axis pid printcounter++; //Print data to UART if (printcounter >= 100) { //sprintf (buff, " %d %d %c Labas\n\r", ACCread[0], ACCread[1], ACCread[2]); //sprintf (buff, " %x %x %x %x %x %x Labas\n\r", ACCread[0], ACCread[1], ACCread[2], ACCread[3], ACCread[4], ACCread[5]); //sprintf (buff, "Labas %d %d\n\r", ACCread[0], ACCread[1]); //sprintf (buff, "%3.1f %f\n\r", ADC1Ch1_vid*57.3, sinus); //sprintf (buff, "Labas %f %f %f \n\r", accADC_x, accADC_y, accADC_z); //sprintf (buff, "%3.1f %3.1f \n\r", acc_roll_angle_vid*57.3, acc_pitch_angle_vid *57.3); //sprintf (buff, "%3.1f %3.1f \n\r", pitch_angle*57.3, roll_angle*57.3); //sprintf (buff, "%d\n\r", rc4); //USART_PutString(buff); printcounter = 0; } stop = 0; LEDoff; watchcounter = 0; while (stop == 0) {} //Closed loop waits for interrupt } }