![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
code to fly a quadrocopter
main.cpp
- Committer:
- DD1993
- Date:
- 2020-05-05
- Revision:
- 0:b0f9c5ac0305
File content as of revision 0:b0f9c5ac0305:
#include "mbed.h" #include "pwmdeg.h" #include "degpwm.h" #include "PID_roll.h" #include "PID_pitch.h" #include "PID_yaw.h" #include "mpu_output.h" #include "motor_mixer.h" #define pitch_output RawSerial pc_main(USBTX, USBRX); mpu_output main_mpu_output; pwmdeg pwmdeg_main; Degpwm Degpwm_main; motormixer motormixer_main; Timer test; //Interrupt Handler InterruptIn roll_int(A0); //Channel 1 InterruptIn pitch_int(A1); //Channel 2 InterruptIn throttle_int(D11); //Channel 3 InterruptIn yaw_int(D12); //Channel 4 //Variablen ChannelIn volatile uint16_t _pulsewidth_ch1; volatile uint16_t _pulsewidth_ch2; volatile uint16_t _pulsewidth_ch3; volatile uint16_t _pulsewidth_ch4; //Timer ChannelIn Timer _t_ch1; Timer _t_ch2; Timer _t_ch3; Timer _t_ch4; //Init von PID Regler //Beispiel: PIDClass pid_roll(AngleKp, AngleKi, RateKp, RateKd); //Roll PID_ROLL_Class pid_roll(4, 0.0007, 0.020, 0); //Pitch PID_PITCH_Class pid_pitch(4, 0.0007, 0.020, 0); //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35); //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35); //PID_PITCH_Class pid_pitch(4, 0.0011, 0.06, 12.35); //Yaw PID_YAW_Class pid_yaw(4, 0.000, 0.000, 0.0); //PID_YAW_Class pid_yaw(4, 0.0002, 0.015, 0.0); Stärker einstellen!!!! float Rate = 0, dt = 10; //Main Funktionen void pwm_in_main(); void pitch_rc_main(); void roll_rc_main(); void yaw_rc_main(); void motors_ready_main(); void pwm_read_main(); void rise_ch1(); void fall_ch1(); void rise_ch2(); void fall_ch2(); void rise_ch3(); void fall_ch3(); void rise_ch4(); void fall_ch4(); //PwmIn Variablen int ele_in_main = 0; //Channel 1 int ail_in_main = 0; //Channel 2 int thr_in_main = 0; //Channel 3 int rud_in_main = 0; //Channel 4 //Übergabevariablen //Channel1 float roll_error = 0; int roll_error_pwm_ready = 0; //Channel2 float pitch_error = 0; int pitch_error_pwm_ready = 0; //Channel3 float yaw_error = 0; int yaw_error_pwm_ready = 0; //Channel4 int motor_all_pwm_ready = 0; ////////////////////////////////////MAIN/////////////////////////////////////////////////////////////////////// int main() { NVIC_SetPriority(EXTI3_IRQn, 0x01); //Priorität von Interrupt des Sensors dekrementieren!!! main_mpu_output.Init(); //Sensor initalisieren main_mpu_output.dmpDataUpdate(); //MBED_ASSERT(main_mpu_output.Init() == true); motormixer_main.motor_all_period(2.5); //Einstellen Periode Motoren //motormixer_main.motor_calibration(2.5); //Kalibrieren der Motoren //ISR Funktionen RC ChannelIn roll_int.rise(&rise_ch1); pitch_int.rise(&rise_ch2); throttle_int.rise(&rise_ch3); yaw_int.rise(&rise_ch4); while(1) { test.start(); //Alle Channel einlesen pwm_in_main(); //PWM Signale des RC in Grad umrechnen, Roll-Error abspeichern (danach in PWM) roll_rc_main(); //PWM Signale des RC in Grad umrechnen, Pitch-Error abspeichern (danach in PWM) pitch_rc_main(); //PWM Signale des RC in Grad umrechnen, Yaw-Error abspeichern (danach in PWM) yaw_rc_main(); //Werte in Mototrmixer schreiben --> ansteuern der Motoren motors_ready_main(); //pc_main.printf("hier\n"); test.stop(); int zeit = test.read_ms(); //pc_main.printf("zeit: %d\n", zeit); test.reset(); } } ///////////////////ISR von Rise Channel 1 Roll///////////////////////////////////// void rise_ch1() { _t_ch1.start(); roll_int.fall(&fall_ch1);//Interrupt Fall } void fall_ch1() { _t_ch1.stop(); if(_t_ch1.read_us()>=1000&&_t_ch1.read_us()<=2000) { _pulsewidth_ch1 = _t_ch1.read_us(); //pc_main.printf("ch1: %d\n", _pulsewidth_ch1); } _t_ch1.reset(); } ///////////////////ISR von Rise Channel 2 Pitch///////////////////////////////////// void rise_ch2() { _t_ch2.start(); pitch_int.fall(&fall_ch2);//Interrupt Fall } void fall_ch2() { _t_ch2.stop(); if(_t_ch2.read_us()>=1000&&_t_ch2.read_us()<=2000) { _pulsewidth_ch2 = _t_ch2.read_us(); //pc_main.printf("ch1: %d\n", _pulsewidth_ch2); } _t_ch2.reset(); } ///////////////////ISR von Rise Channel 3 Throttle///////////////////////////////////// void rise_ch3() { _t_ch3.start(); throttle_int.fall(&fall_ch3);//Interrupt Fall } void fall_ch3() { _t_ch3.stop(); if(_t_ch3.read_us()>=1000&&_t_ch3.read_us()<=2000) { _pulsewidth_ch3 = _t_ch3.read_us(); //pc_main.printf("ch1: %d\n", _pulsewidth_ch1); } _t_ch3.reset(); } ///////////////////ISR von Rise Channel 4 YAW///////////////////////////////////// void rise_ch4() { _t_ch4.start(); yaw_int.fall(&fall_ch4);//Interrupt Fall } void fall_ch4() { _t_ch4.stop(); if(_t_ch4.read_us()>=1000&&_t_ch4.read_us()<=2000) { _pulsewidth_ch4 = _t_ch4.read_us(); //pc_main.printf("ch4: %d\n", _pulsewidth_ch4); } _t_ch4.reset(); } ///////PWM In abspeichern/////////////////////////////// void pwm_in_main() { ail_in_main = _pulsewidth_ch1; ele_in_main = _pulsewidth_ch2; thr_in_main = _pulsewidth_ch3; rud_in_main = _pulsewidth_ch4; //pc_main.printf("throttle: %d|pitch: %d|roll: %d|rud: %d \n", thr_in_main, ele_in_main, ail_in_main, rud_in_main); } ///////////Roll Rechnen und Abspeichern/////////////////////////////////////////////////////////////////////////////////// void roll_rc_main() { //Roll-Error abspeichern (in PWM) float Setpoint_Roll = pwmdeg_main.roll(ail_in_main), CurrentPosition_Roll = main_mpu_output.sensor_roll(); roll_error = pid_roll.update_roll(Setpoint_Roll, CurrentPosition_Roll, Rate, dt); //pc_main.printf("Output pitch error: %d\n", pitch_error); roll_error_pwm_ready = Degpwm_main.roll_pwm(roll_error); //pc_main.printf("Output roll error pwm: %d\n", roll_error_pwm_ready); } ///////////Pitch Rechnen und Abspeichern/////////////////////////////////////////////////////////////////////////////////// void pitch_rc_main() { float Setpoint_Pitch = pwmdeg_main.pitch_forward(ele_in_main), CurrentPosition_Pitch = main_mpu_output.sensor_pitch(); pitch_error = pid_pitch.update_pitch(Setpoint_Pitch, CurrentPosition_Pitch, Rate, dt); //pc_main.printf("Output pitch error: %f\n", pitch_error); pitch_error_pwm_ready = Degpwm_main.pitch_pwm(pitch_error); //pc_main.printf("Output pitch error pwm: %d\n", pitch_error_pwm_ready); #ifdef pitch_output pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output pitch error pwm: %d|pitch error: %f\n", Setpoint_Pitch, CurrentPosition_Pitch, pitch_error_pwm_ready, pitch_error); #endif } ///////////Yaw Rechnen und Abspeichern/////////////////////////////////////////////////////////////////////////////////// void yaw_rc_main() { //Yaw Error abspeichern (in PWM) float SetAccel_Yaw = pwmdeg_main.yaw(rud_in_main), CurrentAccel_Yaw = main_mpu_output.sensor_yaw(); yaw_error = pid_yaw.update_yaw(SetAccel_Yaw, CurrentAccel_Yaw, Rate, dt); yaw_error_pwm_ready = Degpwm_main.yaw_pwm(yaw_error); //pc_main.printf("Output yaw error pwm: %d\n", yaw_error_pwm_ready); //pc_main.printf("SetAccel: %f| CurrentAccel: %f| Output yaw error pwm: %d|yaw error: %f\n", SetAccel_Yaw, CurrentAccel_Yaw, yaw_error_pwm_ready, yaw_error); } ///////////Übergabe an Motormixer/////////////////////////////////////////////////////////////////////////////////// void motors_ready_main() { //yaw_error_pwm_ready = 0; //Throttle abspeichern (in PWM) if(thr_in_main > 1111) { motor_all_pwm_ready = thr_in_main; //PWM Werte in Motormixer und in fertige Motoren in Variablen speichern //Motor1 motormixer_main.motor1_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready); //Motor2 motormixer_main.motor2_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready); //Motor3 motormixer_main.motor3_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready); //Motor4 motormixer_main.motor4_ready(motor_all_pwm_ready, roll_error_pwm_ready, pitch_error_pwm_ready, yaw_error_pwm_ready); } else { //pc_main.printf("throttle: %d\n", throttle_in.pulsewidth()); motormixer_main.motor_all_off(); } }