Dominik Diedrich / Mbed 2 deprecated drone_quado_x

code to fly a quadrocopter

Dependencies:   mbed

PWM/pwmdeg.cpp@0:b0f9c5ac0305, 2020-05-05 (annotated)

Committer:
DD1993
Date:
Tue May 05 21:11:38 2020 +0000
Revision:
0:b0f9c5ac0305
initial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DD1993 0:b0f9c5ac0305 1 #include "pwmdeg.h"
DD1993 0:b0f9c5ac0305 2
DD1993 0:b0f9c5ac0305 3 RawSerial pc2(USBTX, USBRX);
DD1993 0:b0f9c5ac0305 4
DD1993 0:b0f9c5ac0305 5 //////////////////////PITCH FORWARD//////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 6 float pwmdeg::pitch_forward(int pitch_in)
DD1993 0:b0f9c5ac0305 7 {
DD1993 0:b0f9c5ac0305 8 int wert = pitch_in;
DD1993 0:b0f9c5ac0305 9 //pc2.printf("ele_kalsse: %d \n", wert);
DD1993 0:b0f9c5ac0305 10 float dPWM = 0;
DD1993 0:b0f9c5ac0305 11 int pwm_mittel = 1479;
DD1993 0:b0f9c5ac0305 12 float pitch_forwarddeg = 0;
DD1993 0:b0f9c5ac0305 13 float grad = 0;
DD1993 0:b0f9c5ac0305 14
DD1993 0:b0f9c5ac0305 15 dPWM = wert - pwm_mittel;
DD1993 0:b0f9c5ac0305 16 grad = dPWM * 50/400;
DD1993 0:b0f9c5ac0305 17
DD1993 0:b0f9c5ac0305 18 //pc2.printf("ele: %f | wert: %d\n", dPWM, wert);
DD1993 0:b0f9c5ac0305 19
DD1993 0:b0f9c5ac0305 20 if(wert >= 1460 && wert <= 1500) {
DD1993 0:b0f9c5ac0305 21 pitch_forwarddeg = 0;
DD1993 0:b0f9c5ac0305 22 //pc2.printf("ele: %f | wert: %d\n", dPWM, wert);
DD1993 0:b0f9c5ac0305 23 }
DD1993 0:b0f9c5ac0305 24
DD1993 0:b0f9c5ac0305 25 else {
DD1993 0:b0f9c5ac0305 26 pitch_forwarddeg = grad;
DD1993 0:b0f9c5ac0305 27
DD1993 0:b0f9c5ac0305 28 }
DD1993 0:b0f9c5ac0305 29 //pc2.printf("ele: %f | wert: %d\n", dPWM, wert);
DD1993 0:b0f9c5ac0305 30 //pc2.printf("Grad von RC: %f| Wert ele: %d \n", pitch_forwarddeg, wert);
DD1993 0:b0f9c5ac0305 31 return pitch_forwarddeg;
DD1993 0:b0f9c5ac0305 32
DD1993 0:b0f9c5ac0305 33 }
DD1993 0:b0f9c5ac0305 34
DD1993 0:b0f9c5ac0305 35
DD1993 0:b0f9c5ac0305 36
DD1993 0:b0f9c5ac0305 37
DD1993 0:b0f9c5ac0305 38
DD1993 0:b0f9c5ac0305 39 //////////////////////ROLL//////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 40 float pwmdeg::roll(int ail_in)
DD1993 0:b0f9c5ac0305 41 {
DD1993 0:b0f9c5ac0305 42 int wert = ail_in;
DD1993 0:b0f9c5ac0305 43 float rolldeg = 0;
DD1993 0:b0f9c5ac0305 44 int pwm_mittel = 1500;
DD1993 0:b0f9c5ac0305 45 float grad = 0;
DD1993 0:b0f9c5ac0305 46 float dPWM = 0;
DD1993 0:b0f9c5ac0305 47
DD1993 0:b0f9c5ac0305 48 dPWM = wert - pwm_mittel;
DD1993 0:b0f9c5ac0305 49 grad = dPWM * 50/400;
DD1993 0:b0f9c5ac0305 50
DD1993 0:b0f9c5ac0305 51
DD1993 0:b0f9c5ac0305 52
DD1993 0:b0f9c5ac0305 53 if(wert >= 1480 && wert <= 1520) {
DD1993 0:b0f9c5ac0305 54 rolldeg = 0;
DD1993 0:b0f9c5ac0305 55
DD1993 0:b0f9c5ac0305 56 }
DD1993 0:b0f9c5ac0305 57
DD1993 0:b0f9c5ac0305 58 else {
DD1993 0:b0f9c5ac0305 59 rolldeg = grad;
DD1993 0:b0f9c5ac0305 60 //pc2.printf("grad: %f", grad);
DD1993 0:b0f9c5ac0305 61
DD1993 0:b0f9c5ac0305 62 }
DD1993 0:b0f9c5ac0305 63
DD1993 0:b0f9c5ac0305 64 //pc2.printf("Grad von RC: %f| Wert ele: %d \n", rolldeg, wert);
DD1993 0:b0f9c5ac0305 65 return rolldeg;
DD1993 0:b0f9c5ac0305 66
DD1993 0:b0f9c5ac0305 67 }
DD1993 0:b0f9c5ac0305 68
DD1993 0:b0f9c5ac0305 69 //////////////////////YAW//////////////////////////////////////////////////////////////
DD1993 0:b0f9c5ac0305 70 float pwmdeg::yaw(int rud_in)
DD1993 0:b0f9c5ac0305 71 {
DD1993 0:b0f9c5ac0305 72 int wert = rud_in;
DD1993 0:b0f9c5ac0305 73 int pwm_mittel = 1492;
DD1993 0:b0f9c5ac0305 74 int grad = 0;
DD1993 0:b0f9c5ac0305 75 int dPWM = 0;
DD1993 0:b0f9c5ac0305 76 int yawaccel = 0;
DD1993 0:b0f9c5ac0305 77 //pc2.printf("grad: %f\n", wert);
DD1993 0:b0f9c5ac0305 78
DD1993 0:b0f9c5ac0305 79 dPWM = wert - pwm_mittel;
DD1993 0:b0f9c5ac0305 80 grad = dPWM * 170/380;
DD1993 0:b0f9c5ac0305 81
DD1993 0:b0f9c5ac0305 82 if(wert >= 1470 && wert <= 1500) {
DD1993 0:b0f9c5ac0305 83 yawaccel = 0;
DD1993 0:b0f9c5ac0305 84 }
DD1993 0:b0f9c5ac0305 85
DD1993 0:b0f9c5ac0305 86 else {
DD1993 0:b0f9c5ac0305 87 yawaccel = grad;
DD1993 0:b0f9c5ac0305 88 }
DD1993 0:b0f9c5ac0305 89
DD1993 0:b0f9c5ac0305 90
DD1993 0:b0f9c5ac0305 91 //pc2.printf("Grad/s von RC: %d| Wert yaw: %d \n", yawaccel, wert);
DD1993 0:b0f9c5ac0305 92 return yawaccel;
DD1993 0:b0f9c5ac0305 93
DD1993 0:b0f9c5ac0305 94 }