Revision 0:b0f9c5ac0305, committed 2020-05-05

Comitter:

DD1993

Date:

Tue May 05 21:11:38 2020 +0000

Commit message:

initial

Changed in this revision

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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

--- /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