code to fly a quadrocopter

Dependencies:   mbed

Files at this revision

API Documentation at this revision

Comitter:
DD1993
Date:
Tue May 05 21:11:38 2020 +0000
Commit message:
initial

Changed in this revision

MPU6050_DMP_Nucleo-I2Cdev.lib Show annotated file Show diff for this revision Revisions of this file
Motors/motor_mixer.cpp Show annotated file Show diff for this revision Revisions of this file
Motors/motor_mixer.h Show annotated file Show diff for this revision Revisions of this file
PID/PID_pitch.cpp Show annotated file Show diff for this revision Revisions of this file
PID/PID_pitch.h Show annotated file Show diff for this revision Revisions of this file
PID/PID_roll.cpp Show annotated file Show diff for this revision Revisions of this file
PID/PID_roll.h Show annotated file Show diff for this revision Revisions of this file
PID/PID_yaw.cpp Show annotated file Show diff for this revision Revisions of this file
PID/PID_yaw.h Show annotated file Show diff for this revision Revisions of this file
PWM/degpwm.cpp Show annotated file Show diff for this revision Revisions of this file
PWM/degpwm.h Show annotated file Show diff for this revision Revisions of this file
PWM/pwmdeg.cpp Show annotated file Show diff for this revision Revisions of this file
PWM/pwmdeg.h Show annotated file Show diff for this revision Revisions of this file
config.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r b0f9c5ac0305 MPU6050_DMP_Nucleo-I2Cdev.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050_DMP_Nucleo-I2Cdev.lib	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/DD1993/code/Quadrocopter_Nucleo/#05d50bce9cd9
diff -r 000000000000 -r b0f9c5ac0305 Motors/motor_mixer.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motors/motor_mixer.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,140 @@
+#include "motor_mixer.h"
+
+RawSerial pc_motor_mixer(USBTX, USBRX);
+PwmOut motor1_out(D5);
+PwmOut motor2_out(D6);
+PwmOut motor3_out(D9);
+PwmOut motor4_out(D10);
+
+//Definition der Ausgänge für die Motoren
+/*
+                FRONT
+
+    CW M3(D9)           CCW M1(D5)
+        \                   /
+         \                 /
+          -----------------      
+         /                 \
+        /                   \
+    CCW M2(D6)          CW M4(D10)
+    
+                BACK
+
+motor1 = rcgas + roll - pitch + yaw
+motor2 = rcgas - roll + pitch + yaw
+motor3 = rcgas − roll - pitch - yaw
+motor4 = rcgas + roll + pitch - yaw
+*/
+
+void motormixer::motor_all_period(int period)
+{
+    motor1_out.period_ms(period);
+    motor2_out.period_ms(period);
+    motor3_out.period_ms(period);
+    motor4_out.period_ms(period);
+}
+
+
+void motormixer::motor_all_off()
+{
+    motor1_out.pulsewidth_us(1100);
+    motor2_out.pulsewidth_us(1100);
+    motor3_out.pulsewidth_us(1100);
+    motor4_out.pulsewidth_us(1100);
+}
+
+/////////////////////////MOTOR1//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor1_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor1_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready - pitch_error_pwm_ready + yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor1_pwm_ready < 1200) {motor1_out.pulsewidth_us(1200);}
+    else if (motor1_pwm_ready > 1759) {motor1_out.pulsewidth_us(1759);}
+    
+    else {
+
+        motor1_out.pulsewidth_us(motor1_pwm_ready);
+        //pc_motor_mixer.printf("Output Motor1: %d\n", motor1_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOR2//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor2_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor2_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready + pitch_error_pwm_ready + yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor2_pwm_ready < 1200) {motor2_out.pulsewidth_us(1200);}
+    else if (motor2_pwm_ready > 1759) {motor2_out.pulsewidth_us(1759);}
+    
+    else {
+
+        //pc_motor_mixer.printf("Output Motor2: %d\n", motor2_pwm_ready);
+        motor2_out.pulsewidth_us(motor2_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOR3//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor3_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor3_pwm_ready = motor_all_pwm_ready - roll_error_pwm_ready - pitch_error_pwm_ready - yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor3_pwm_ready < 1200) {motor3_out.pulsewidth_us(1200);}
+    else if (motor3_pwm_ready > 1759) {motor3_out.pulsewidth_us(1759);}
+    
+    else {
+
+        motor3_out.pulsewidth_us(motor3_pwm_ready);
+        //pc_motor_mixer.printf("Output Motor3: %d\n", motor3_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOR4//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor4_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready)
+{
+
+    int motor4_pwm_ready = motor_all_pwm_ready + roll_error_pwm_ready + pitch_error_pwm_ready - yaw_error_pwm_ready;
+
+    //Wert an Motor-Regler senden
+    if (motor4_pwm_ready < 1200) {motor4_out.pulsewidth_us(1200);}
+    else if (motor4_pwm_ready > 1759) {motor4_out.pulsewidth_us(1759);}
+    
+    else {
+
+        //pc_motor_mixer.printf("Output Motor4: %d\n", motor4_pwm_ready);
+        motor4_out.pulsewidth_us(motor4_pwm_ready);
+    }
+}
+
+/////////////////////////MOTOREN KALIBRIEREN//////////////////////////////////////////////////////////////////////////////////////////
+void motormixer::motor_calibration(int period)
+{
+    pc_motor_mixer.printf("Periode einstellen\n");
+    motor1_out.period_ms(period);
+    motor2_out.period_ms(period);
+    motor3_out.period_ms(period);
+    motor4_out.period_ms(period);
+    pc_motor_mixer.printf("Done\n");
+
+    pc_motor_mixer.printf("Vollast...3Sec warten...\n");
+    motor1_out.pulsewidth_us(1759);
+    motor2_out.pulsewidth_us(1759);
+    motor3_out.pulsewidth_us(1759);
+    motor4_out.pulsewidth_us(1759);
+    
+    wait(5);
+    
+    pc_motor_mixer.printf("Keine Last...3Sec warten...\n");
+    motor1_out.pulsewidth_us(1111);
+    motor2_out.pulsewidth_us(1111);
+    motor3_out.pulsewidth_us(1111);
+    motor4_out.pulsewidth_us(1111);
+    pc_motor_mixer.printf("Done\n");
+    
+    wait(5);
+}
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 Motors/motor_mixer.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motors/motor_mixer.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,22 @@
+#ifndef motor_mixer_H
+#define motor_mixer_H
+
+#include "mbed.h"
+
+class motormixer
+{
+public:
+
+    void motor_all_period(int period);
+    void motor_calibration(int period);  //sets the period time of the motors
+    void motor_all_off();
+    void motor1_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready);
+    void motor2_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready);
+    void motor3_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready);
+    void motor4_ready(int motor_all_pwm_ready, int roll_error_pwm_ready, int pitch_error_pwm_ready, int yaw_error_pwm_ready);
+
+private:
+
+};
+
+#endif
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PID/PID_pitch.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_pitch.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,37 @@
+#include "PID_pitch.h"
+
+RawSerial pc_pid_pitch(USBTX, USBRX);
+ 
+PID_PITCH_Class::PID_PITCH_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){
+ 
+        Angle_Kp=AngleKp;
+        Angle_Ki=AngleKi;
+        Rate_Kp=RateKp;
+        Rate_Kd=RateKd;
+}
+ 
+float PID_PITCH_Class::update_pitch(float Setpoint, float CurrentPosition, float Rate, float dt){
+      
+      AngleError = 0;
+      AngleError = Setpoint - CurrentPosition;
+      //pc_pid_pitch.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError);
+      
+      Angle_I +=AngleError*dt;
+      //pc_pid_pitch.printf("Angle_I: %f\n", Angle_I);
+      
+      if(Angle_I>1023.0f) Angle_I=1023.0f;
+      else if(Angle_I<-1023.0f) Angle_I= -1023.0f;
+      
+      
+      
+      RateError = (AngleError*Angle_Kp)- Rate;
+      
+      Rate_D = (RateError-PreviousError)/dt;
+
+      Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd));
+  
+      PreviousError = RateError;
+
+      
+      return Output;
+}
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PID/PID_pitch.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_pitch.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,24 @@
+#ifndef PID_PITCH_H
+#define PID_PITCH_H
+ 
+#include "mbed.h"
+ 
+class PID_PITCH_Class
+{
+ 
+   public:
+   
+   PID_PITCH_Class(float AngleKp,float AngleKi, float RateKp, float RateKd);
+   
+   float update_pitch(float Setpoint, float CurrentPosition, float Rate, float dt);
+   
+   private:
+   
+        float Angle_Kp,Angle_Ki,Rate_Kp,Rate_Kd;
+        float AngleError,RateError;
+        float Angle_I,Rate_D;
+        float Output,WindUp;
+        float PreviousError;
+ 
+};
+#endif
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PID/PID_roll.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_roll.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,33 @@
+#include "PID_roll.h"
+
+RawSerial pc_pid_roll(USBTX, USBRX);
+ 
+PID_ROLL_Class::PID_ROLL_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){
+ 
+        Angle_Kp=AngleKp;
+        Angle_Ki=AngleKi;
+        Rate_Kp=RateKp;
+        Rate_Kd=RateKd;
+}
+ 
+float PID_ROLL_Class::update_roll(float Setpoint, float CurrentPosition, float Rate, float dt){
+      
+      AngleError = 0;
+      AngleError = Setpoint - CurrentPosition;
+      //pc_pid.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError);
+      
+      Angle_I +=AngleError*dt;
+      
+      if(Angle_I>100.0f) Angle_I=100.0f;
+      else if(Angle_I<-100.0f) Angle_I= -100.0f;
+      
+      RateError = (AngleError*Angle_Kp)- Rate;
+      
+      Rate_D = (RateError-PreviousError)/dt;
+      
+      Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd));
+      
+      PreviousError = RateError;
+      
+      return Output;
+}
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PID/PID_roll.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_roll.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,24 @@
+#ifndef PID_ROLL_H
+#define PID_ROLL_H
+ 
+#include "mbed.h"
+ 
+class PID_ROLL_Class
+{
+ 
+   public:
+   
+   PID_ROLL_Class(float AngleKp,float AngleKi, float RateKp, float RateKd);
+   
+   float update_roll(float Setpoint, float CurrentPosition, float Rate, float dt);
+   
+   private:
+   
+        float Angle_Kp,Angle_Ki,Rate_Kp,Rate_Kd;
+        float AngleError,RateError;
+        float Angle_I,Rate_D;
+        float Output,WindUp;
+        float PreviousError;
+ 
+};
+#endif
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PID/PID_yaw.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_yaw.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,33 @@
+#include "PID_yaw.h"
+
+RawSerial pc_pid_yaw(USBTX, USBRX);
+ 
+PID_YAW_Class::PID_YAW_Class(float AngleKp,float AngleKi, float RateKp, float RateKd){
+ 
+        Angle_Kp=AngleKp;
+        Angle_Ki=AngleKi;
+        Rate_Kp=RateKp;
+        Rate_Kd=RateKd;
+}
+ 
+float PID_YAW_Class::update_yaw(float Setpoint, float CurrentPosition, float Rate, float dt){
+      
+      AngleError = 0;
+      AngleError = Setpoint - CurrentPosition;
+      //pc_pid.printf("Setpoint: %f| CurrentPosition: %f| angle: %f\n", Setpoint, CurrentPosition, AngleError);
+      
+      Angle_I +=AngleError*dt;
+      
+      if(Angle_I>1023.0f) Angle_I=1023.0f;
+      else if(Angle_I<-1023.0f) Angle_I= -1023.0f;
+      
+      RateError = (AngleError*Angle_Kp)- Rate;
+      
+      Rate_D = (RateError-PreviousError)/dt;
+      
+      Output = (float)((RateError*Rate_Kp) + (Angle_I*Angle_Ki) + (Rate_D*Rate_Kd));
+      
+      PreviousError = RateError;
+      
+      return Output;
+}
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PID/PID_yaw.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID/PID_yaw.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,24 @@
+#ifndef PID_YAW_H
+#define PID_YAW_H
+ 
+#include "mbed.h"
+ 
+class PID_YAW_Class
+{
+ 
+   public:
+   
+   PID_YAW_Class(float AngleKp,float AngleKi, float RateKp, float RateKd);
+   
+   float update_yaw(float Setpoint, float CurrentPosition, float Rate, float dt);
+   
+   private:
+   
+        float Angle_Kp,Angle_Ki,Rate_Kp,Rate_Kd;
+        float AngleError,RateError;
+        float Angle_I,Rate_D;
+        float Output,WindUp;
+        float PreviousError;
+ 
+};
+#endif
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PWM/degpwm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/degpwm.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,76 @@
+#include "degpwm.h"
+
+RawSerial pc_degpwm(USBTX, USBRX);
+
+
+ //////////////////////PITCH FORWARD_DEG_TO_PWM//////////////////////////////////////////////////////////////
+int Degpwm::pitch_pwm(float pitch_error)
+    {
+        float pitch_e = pitch_error;
+        float pitch_error_pwm = 0.0;
+        int pitch_error_pwm_ready = 0;
+        
+        //Umrechnung von Grad zu PWM Wert
+        if (pitch_e < -50 || pitch_e > 50){}
+        
+        else{
+    
+            pitch_error_pwm = pitch_e *400/50;
+            //pc_degpwm.printf("Output Pitch Errorpwm: %f\n", pitch_e);
+            pitch_error_pwm_ready = pitch_error_pwm;
+            //pc_degpwm.printf("Output Pitch Error: %f| PWM: %d\n", pitch_error, pitch_error_pwm_ready);
+         }
+        
+        return pitch_error_pwm_ready; // Rücgabe des Korrekturwerts in PWM
+    }
+    
+ //////////////////////ROLL_DEG_TO_PWM//////////////////////////////////////////////////////////////
+int Degpwm::roll_pwm(float roll_error)
+    {
+        float pitch_e = roll_error;
+        float roll_error_pwm = 0.0;
+        int roll_error_pwm_ready = 0;
+        
+        //Umrechnung von Grad zu PWM Wert
+        if (pitch_e < -50 || pitch_e > 50){}
+        
+        else{
+    
+            roll_error_pwm = pitch_e *400/50;
+            //pc_degpwm.printf("Output Pitch Errorpwm: %f\n", pitch_e);
+            roll_error_pwm_ready = roll_error_pwm;
+            //pc_degpwm.printf("Output Pitch Error: %f| PWM: %d\n", pitch_error, pitch_error_pwm_ready);
+         }
+        
+        return roll_error_pwm_ready; // Rücgabe des Korrekturwerts in PWM
+    }
+ 
+ //////////////////////YAW_ACCEL_TO_PWM//////////////////////////////////////////////////////////////   
+int Degpwm::yaw_pwm(float yaw_error)
+    {
+        float yaw_e = yaw_error;
+        float yaw_error_pwm = 0;
+        int yaw_error_pwm_ready = 0;
+     
+        //Umrechnung von Grad zu PWM Wert
+        if (yaw_e < -1200) {
+            yaw_error_pwm = -1200 *370/170;
+            yaw_error_pwm_ready = yaw_error_pwm;
+            }
+            
+        else if (yaw_e > 1200) {
+            yaw_error_pwm = 1200 *370/170;
+            yaw_error_pwm_ready = yaw_error_pwm;
+            }
+        
+        else{
+            yaw_error_pwm = yaw_e *370/170;
+            yaw_error_pwm_ready = yaw_error_pwm;
+         
+            //pc_degpwm.printf("Output yaw Error: %f| PWM: %d\n", yaw_error, yaw_error_pwm_ready);
+         }
+        
+        return yaw_error_pwm_ready; // Rückgabe des Korrekturwerts in PWM
+        
+    }
+ 
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PWM/degpwm.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/degpwm.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,17 @@
+#ifndef MBED_DEGPWM_H
+#define MBED_DEGPWM_H
+
+#include "mbed.h"
+
+
+class Degpwm {
+    
+public:
+    int pitch_pwm(float pitch_error);
+    int roll_pwm(float roll_error);
+    int yaw_pwm(float yaw_error);
+
+protected:
+ 
+};
+#endif
diff -r 000000000000 -r b0f9c5ac0305 PWM/pwmdeg.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/pwmdeg.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,94 @@
+#include "pwmdeg.h"
+
+RawSerial pc2(USBTX, USBRX);
+
+//////////////////////PITCH FORWARD//////////////////////////////////////////////////////////////
+float pwmdeg::pitch_forward(int pitch_in)
+{
+    int wert = pitch_in;
+    //pc2.printf("ele_kalsse: %d \n", wert);
+    float dPWM = 0;
+    int pwm_mittel = 1479;
+    float pitch_forwarddeg = 0;
+    float grad = 0;
+
+    dPWM = wert - pwm_mittel;
+    grad = dPWM * 50/400;
+
+    //pc2.printf("ele: %f | wert: %d\n", dPWM, wert);
+
+    if(wert >= 1460 && wert <= 1500) {
+        pitch_forwarddeg = 0;
+        //pc2.printf("ele: %f | wert: %d\n", dPWM, wert);
+    }
+
+    else {
+        pitch_forwarddeg = grad;
+
+    }
+    //pc2.printf("ele: %f | wert: %d\n", dPWM, wert);
+    //pc2.printf("Grad von RC: %f| Wert ele: %d \n", pitch_forwarddeg, wert);
+    return pitch_forwarddeg;
+
+}
+
+
+
+
+
+//////////////////////ROLL//////////////////////////////////////////////////////////////
+float pwmdeg::roll(int ail_in)
+{
+    int wert = ail_in;
+    float rolldeg = 0;
+    int pwm_mittel = 1500;
+    float grad = 0;
+    float dPWM = 0;
+
+    dPWM = wert - pwm_mittel;
+    grad = dPWM * 50/400;
+
+
+
+    if(wert >= 1480 && wert <= 1520) {
+        rolldeg = 0;
+
+    }
+
+    else {
+        rolldeg = grad;
+        //pc2.printf("grad: %f", grad);
+
+    }
+
+    //pc2.printf("Grad von RC: %f| Wert ele: %d \n", rolldeg, wert);
+    return rolldeg;
+
+}
+
+//////////////////////YAW//////////////////////////////////////////////////////////////
+float pwmdeg::yaw(int rud_in)
+{
+    int wert = rud_in;
+    int pwm_mittel = 1492;
+    int grad = 0;
+    int dPWM = 0;
+    int yawaccel = 0;
+    //pc2.printf("grad: %f\n", wert);
+
+    dPWM = wert - pwm_mittel;
+    grad = dPWM * 170/380;
+
+    if(wert >= 1470 && wert <= 1500) {
+        yawaccel = 0;
+    }
+
+    else {
+        yawaccel = grad;
+    }
+
+
+    //pc2.printf("Grad/s von RC: %d| Wert yaw: %d \n", yawaccel, wert);
+    return yawaccel;
+
+}
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 PWM/pwmdeg.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PWM/pwmdeg.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,17 @@
+#ifndef MBED_PWMDEG_H
+#define MBED_PWMDEG_H
+
+#include "mbed.h"
+
+
+class pwmdeg {
+    
+public:
+    float pitch_forward(int pitch_in);
+    float roll(int roll_in);
+    float yaw(int yaw_in);
+    
+protected:
+ 
+};
+#endif
diff -r 000000000000 -r b0f9c5ac0305 config.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/config.h	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,18 @@
+// FIFO rate = 200Hz / (1 + this value)
+// For example, 0x01 is 100Hz, 0x03 is 50Hz.
+// 0x00 to 0x09
+#define IMU_FIFO_RATE_DIVIDER 0x09
+
+// Sample rate = 1kHz / (1 + this valye)
+// For example, 4 is 200Hz.
+#define IMU_SAMPLE_RATE_DIVIDER 3
+
+// measuring range of gyroscope (±n deg/s)
+// But other value doesn't yet support.
+#define MPU6050_GYRO_FS MPU6050_GYRO_FS_2000
+
+// measuring range of acceleration sensor (±n g)
+// But other value doesn't yet support.
+#define MPU6050_ACCEL_FS MPU6050_ACCEL_FS_2
+
+#define PC_BAUDRATE 115200
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,290 @@
+#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();
+    }
+
+}
\ No newline at end of file
diff -r 000000000000 -r b0f9c5ac0305 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue May 05 21:11:38 2020 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0
\ No newline at end of file