Chris Elsholz / Mbed 2 deprecated Quadrocopter

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed TextLCD

Fork of Quadrocopter by Marco Friedmann

main.cpp@19:ac98ad146067, 2017-09-11 (annotated)

Committer:
MarcoF89
Date:
Mon Sep 11 11:07:54 2017 +0000
Revision:
19:ac98ad146067
Parent:
18:78e7de067ddb
Child:
20:76e25a3c8684
Kalmanfilter von c++ in c gewandelt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarcoF89 13:5f0a2103c707 1 #include <Timer.h>
MarcoF89 13:5f0a2103c707 2 #include <math.h>
MarcoF89 15:742683a8efda 3 #include "mbed.h"
MarcoF89 15:742683a8efda 4 #include "stdio.h"
MarcoF89 13:5f0a2103c707 5 #include "deklaration.h"
MarcoF89 13:5f0a2103c707 6 #include "messen.h"
chriselsholz 18:78e7de067ddb 7 #include "filter/Kalman.h"
MarcoF89 15:742683a8efda 8
MarcoF89 15:742683a8efda 9 #define RAD 57.29577951
MarcoF89 15:742683a8efda 10
MarcoF89 15:742683a8efda 11 uint16_t zeit;
MarcoF89 15:742683a8efda 12 uint32_t zeit2;
MarcoF89 15:742683a8efda 13
MarcoF89 15:742683a8efda 14 uint8_t k;
MarcoF89 15:742683a8efda 15
MarcoF89 19:ac98ad146067 16
MarcoF89 15:742683a8efda 17
MarcoF89 15:742683a8efda 18
MarcoF89 13:5f0a2103c707 19
MarcoF89 6:27a09e8bebfb 20 int main()
MarcoF89 15:742683a8efda 21 {
MarcoF89 15:742683a8efda 22 gain_g = 0;
MarcoF89 15:742683a8efda 23 z_off = 0;
MarcoF89 15:742683a8efda 24 zeit = 0;
MarcoF89 15:742683a8efda 25 k = 0;
MarcoF89 15:742683a8efda 26 drift_z = 0;
MarcoF89 15:742683a8efda 27
MarcoF89 13:5f0a2103c707 28 Motor1.period_ms(2);
MarcoF89 11:8457b851e3e1 29 Motor2.period_ms(2);
MarcoF89 11:8457b851e3e1 30 Motor3.period_ms(2);
MarcoF89 11:8457b851e3e1 31 Motor4.period_ms(2);
MarcoF89 12:4a4dad7a3432 32 initialisierung_gyro();
chriselsholz 16:59d80bf88bf8 33 initialisierung_acc();
MarcoF89 19:ac98ad146067 34 Kalman(&Q_angle, &Q_bias, &R_measure, &bias, P);
MarcoF89 12:4a4dad7a3432 35 if (taster2)
MarcoF89 12:4a4dad7a3432 36 {
MarcoF89 13:5f0a2103c707 37 viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
MarcoF89 12:4a4dad7a3432 38 }
MarcoF89 12:4a4dad7a3432 39 if (taster3)
MarcoF89 12:4a4dad7a3432 40 {
MarcoF89 13:5f0a2103c707 41 anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
MarcoF89 11:8457b851e3e1 42 }
MarcoF89 15:742683a8efda 43 pc.printf("Druecke Taster1 fuer den Start\n\r");
chriselsholz 17:de729cb71f1e 44 n1=n2=n3=n4=700;
chriselsholz 16:59d80bf88bf8 45
chriselsholz 16:59d80bf88bf8 46 if (taster1)
chriselsholz 16:59d80bf88bf8 47 {
chriselsholz 17:de729cb71f1e 48 uint16_t flanke1,hilfe1=0,flanke2,hilfe2=0,flanke3,hilfe3=0,flanke4,hilfe4=0;
chriselsholz 16:59d80bf88bf8 49 pc.printf("Taster-Modus aktiv\n\r");
chriselsholz 17:de729cb71f1e 50 n1=n2=n3=n4=625;
chriselsholz 16:59d80bf88bf8 51 while(1)
chriselsholz 16:59d80bf88bf8 52 {
chriselsholz 16:59d80bf88bf8 53 flanke2 = taster2;
chriselsholz 16:59d80bf88bf8 54 if ((flanke2 != 0) && (hilfe2 == 0))
chriselsholz 16:59d80bf88bf8 55 {
chriselsholz 17:de729cb71f1e 56 n1+=50;
chriselsholz 17:de729cb71f1e 57 n2+=50;
chriselsholz 17:de729cb71f1e 58 n3+=50;
chriselsholz 17:de729cb71f1e 59 n4+=50;
chriselsholz 16:59d80bf88bf8 60 }
chriselsholz 16:59d80bf88bf8 61 hilfe2=flanke2;
chriselsholz 16:59d80bf88bf8 62 flanke3 = taster3;
chriselsholz 16:59d80bf88bf8 63 if ((flanke3 != 0) && (hilfe3 == 0))
chriselsholz 16:59d80bf88bf8 64 {
chriselsholz 17:de729cb71f1e 65 n1-=50;
chriselsholz 17:de729cb71f1e 66 n2-=50;
chriselsholz 17:de729cb71f1e 67 n3-=50;
chriselsholz 17:de729cb71f1e 68 n4-=50;
chriselsholz 16:59d80bf88bf8 69 }
chriselsholz 16:59d80bf88bf8 70 hilfe3=flanke3;
chriselsholz 17:de729cb71f1e 71 flanke4 = taster4;
chriselsholz 17:de729cb71f1e 72 if ((flanke4 != 0) && (hilfe4 == 0))
chriselsholz 17:de729cb71f1e 73 {
chriselsholz 17:de729cb71f1e 74 n1=n2=n3=n4=625;
chriselsholz 16:59d80bf88bf8 75 }
chriselsholz 17:de729cb71f1e 76 hilfe4=flanke4;
chriselsholz 16:59d80bf88bf8 77 Motor1.pulsewidth_us(n1);
chriselsholz 16:59d80bf88bf8 78 Motor2.pulsewidth_us(n2);
chriselsholz 16:59d80bf88bf8 79 Motor3.pulsewidth_us(n3);
chriselsholz 18:78e7de067ddb 80 Motor4.pulsewidth_us(n4);
chriselsholz 18:78e7de067ddb 81
chriselsholz 17:de729cb71f1e 82 pc.printf("Drehzahl= %d\r= %d",n1);
chriselsholz 16:59d80bf88bf8 83 }
chriselsholz 16:59d80bf88bf8 84 }
chriselsholz 16:59d80bf88bf8 85
MarcoF89 13:5f0a2103c707 86 while(1)
MarcoF89 13:5f0a2103c707 87 {
chriselsholz 18:78e7de067ddb 88 if (taster1)
MarcoF89 11:8457b851e3e1 89 {
MarcoF89 15:742683a8efda 90 while(1)//(!taster4)
MarcoF89 15:742683a8efda 91 {
MarcoF89 15:742683a8efda 92 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
MarcoF89 15:742683a8efda 93 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 15:742683a8efda 94 drift_gyro(&drift_z, &drift_x, &drift_y, &timer, &timer2, &gain_g, &roll_g, &pitch_g, &z_off, &x_off, &y_off);
MarcoF89 15:742683a8efda 95 pc.printf("\n\rOffgesamt:\n\rZ = %3.5f\tY = %3.5f\tZ = %3.5f\t\n\r", z_off, x_off, y_off);
MarcoF89 15:742683a8efda 96 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 15:742683a8efda 97
MarcoF89 15:742683a8efda 98 /********//******/
MarcoF89 15:742683a8efda 99 /*Messen*//*Gyro*/
MarcoF89 15:742683a8efda 100 /********//******/
MarcoF89 15:742683a8efda 101 timer.stop();
MarcoF89 15:742683a8efda 102 timer2.stop();
MarcoF89 15:742683a8efda 103 timer.reset();
MarcoF89 15:742683a8efda 104 timer2.reset();
MarcoF89 15:742683a8efda 105 gain_g = 0;
MarcoF89 15:742683a8efda 106 pitch_g = 0;
MarcoF89 15:742683a8efda 107 roll_g = 0;
MarcoF89 15:742683a8efda 108 timer.start();
MarcoF89 15:742683a8efda 109 timer2.start();
MarcoF89 15:742683a8efda 110 int i = 0;
MarcoF89 15:742683a8efda 111 while(1)
MarcoF89 15:742683a8efda 112 {
MarcoF89 15:742683a8efda 113 i++;
MarcoF89 15:742683a8efda 114 zeit = timer.read_us();
MarcoF89 15:742683a8efda 115 timer.reset();
MarcoF89 15:742683a8efda 116 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
chriselsholz 18:78e7de067ddb 117 gain_g = gain_g + ((z_g - z_off) * 1/16.4);
chriselsholz 18:78e7de067ddb 118 pitch_g = pitch_g + ((y_g - y_off) * 1/16.4);
chriselsholz 18:78e7de067ddb 119 roll_g = roll_g + ((x_g - x_off) * 1/16.4);
MarcoF89 15:742683a8efda 120 y = y_a / 16384.00;
MarcoF89 15:742683a8efda 121 x = x_a / 16384.00;
MarcoF89 15:742683a8efda 122 z = z_a / 16384.00;
MarcoF89 15:742683a8efda 123 roll_a = atan2(y, sqrt(x * x + z * z)) * RAD;
MarcoF89 15:742683a8efda 124 pitch_a = atan2(-x, z) * RAD;
MarcoF89 19:ac98ad146067 125
MarcoF89 15:742683a8efda 126 if (timer2.read_ms() >= 2000)
MarcoF89 15:742683a8efda 127 {
MarcoF89 15:742683a8efda 128 gain_g -= drift_z;
MarcoF89 15:742683a8efda 129 pitch_g -= drift_y;
MarcoF89 15:742683a8efda 130 roll_g -= drift_x;
MarcoF89 15:742683a8efda 131 timer2.reset();
MarcoF89 15:742683a8efda 132 }
MarcoF89 15:742683a8efda 133 gain = gain_g;
MarcoF89 15:742683a8efda 134 pitch = pitch_g * 0.9 + pitch_a * 0.1;
MarcoF89 15:742683a8efda 135 roll = roll_g * 0.9 + roll_a * 0.1;
MarcoF89 15:742683a8efda 136 if (i == 2000)
MarcoF89 15:742683a8efda 137 {
MarcoF89 15:742683a8efda 138 pc.printf("gain: %2.5f\tpitch: %2.5f\troll: %2.5f\t\n\r",pitch, roll, gain);
MarcoF89 15:742683a8efda 139 i = 0;
MarcoF89 15:742683a8efda 140 }
MarcoF89 15:742683a8efda 141
MarcoF89 15:742683a8efda 142 }
MarcoF89 11:8457b851e3e1 143 }
MarcoF89 11:8457b851e3e1 144 }
MarcoF89 10:16ca5e9ee0dc 145 }
MarcoF89 13:5f0a2103c707 146 }