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@25:a8a3cbc57c61, 2017-09-18 (annotated)

Committer:
MarcoF89
Date:
Mon Sep 18 10:42:00 2017 +0000
Revision:
25:a8a3cbc57c61
Parent:
24:aaa5b4703555
Child:
27:67b388f6ac2e
Aufger?umt und Kalmanfilter f?r alle Achsen angewendet

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 25:a8a3cbc57c61 8
MarcoF89 15:742683a8efda 9 #define RAD 57.29577951
MarcoF89 25:a8a3cbc57c61 10
MarcoF89 6:27a09e8bebfb 11 int main()
MarcoF89 15:742683a8efda 12 {
MarcoF89 15:742683a8efda 13 z_off = 0;
MarcoF89 15:742683a8efda 14 drift_z = 0;
MarcoF89 15:742683a8efda 15
MarcoF89 13:5f0a2103c707 16 Motor1.period_ms(2);
MarcoF89 11:8457b851e3e1 17 Motor2.period_ms(2);
MarcoF89 11:8457b851e3e1 18 Motor3.period_ms(2);
MarcoF89 11:8457b851e3e1 19 Motor4.period_ms(2);
MarcoF89 12:4a4dad7a3432 20 initialisierung_gyro();
chriselsholz 16:59d80bf88bf8 21 initialisierung_acc();
MarcoF89 25:a8a3cbc57c61 22 Kalman_pitch();
MarcoF89 25:a8a3cbc57c61 23 Kalman_yaw();
MarcoF89 25:a8a3cbc57c61 24 Kalman_roll();
MarcoF89 25:a8a3cbc57c61 25 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a);
MarcoF89 25:a8a3cbc57c61 26 wait(1);
chriselsholz 23:c99a4bd60609 27 if (taster2)
chriselsholz 23:c99a4bd60609 28 {
chriselsholz 23:c99a4bd60609 29 viberationen(&rauschen, &Motor1, &Motor2, &Motor3, &Motor4, &taster4);
chriselsholz 23:c99a4bd60609 30 }
chriselsholz 23:c99a4bd60609 31 if (taster3)
chriselsholz 23:c99a4bd60609 32 {
chriselsholz 23:c99a4bd60609 33 anlernen(&Motor1, &Motor2, &Motor3, &Motor4, &taster1, &taster2, &taster4);
chriselsholz 23:c99a4bd60609 34 }
chriselsholz 23:c99a4bd60609 35
chriselsholz 23:c99a4bd60609 36 pc.printf("Druecke Taster1 fuer den Start\n\r");
chriselsholz 23:c99a4bd60609 37 n1=n2=n3=n4=700;
chriselsholz 23:c99a4bd60609 38 Motor1.pulsewidth_us(n1);
chriselsholz 23:c99a4bd60609 39 Motor2.pulsewidth_us(n2);
chriselsholz 23:c99a4bd60609 40 Motor3.pulsewidth_us(n3);
chriselsholz 23:c99a4bd60609 41 Motor4.pulsewidth_us(n4);
chriselsholz 23:c99a4bd60609 42
MarcoF89 13:5f0a2103c707 43 while(1)
MarcoF89 13:5f0a2103c707 44 {
chriselsholz 18:78e7de067ddb 45 if (taster1)
MarcoF89 11:8457b851e3e1 46 {
MarcoF89 25:a8a3cbc57c61 47 while(1)
MarcoF89 25:a8a3cbc57c61 48 {
MarcoF89 15:742683a8efda 49 pc.printf("\n\rOffset und Driftberechnung wird durchgefuehrt, halte die Drohne still");
MarcoF89 15:742683a8efda 50 offset_gyro(&z_off, &x_off, &y_off);
MarcoF89 24:aaa5b4703555 51 //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 52 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 53 pc.printf("\n\rDrift:Z: %3.10f\tX: %3.10f\tY: %3.10f\n\r", drift_z, drift_x, drift_y);
MarcoF89 25:a8a3cbc57c61 54
MarcoF89 15:742683a8efda 55 timer.reset();
MarcoF89 15:742683a8efda 56 timer.start();
MarcoF89 15:742683a8efda 57 int i = 0;
MarcoF89 15:742683a8efda 58 while(1)
MarcoF89 15:742683a8efda 59 {
MarcoF89 15:742683a8efda 60 i++;
MarcoF89 25:a8a3cbc57c61 61 dt = timer.read_us() * 0.000001; //Zeit zwischen zwei Messpunkten
MarcoF89 15:742683a8efda 62 timer.reset();
MarcoF89 25:a8a3cbc57c61 63 aktuell_roh(&z_g, &x_g, &y_g, &z_a, &x_a, &y_a); //Rohdaten einlesen
MarcoF89 25:a8a3cbc57c61 64
MarcoF89 25:a8a3cbc57c61 65 y = y_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 66 x = x_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 67 z = z_a / 16384.00; //Umwandlung in G-Kraft
MarcoF89 25:a8a3cbc57c61 68
MarcoF89 25:a8a3cbc57c61 69 newAngle_pitch = atan2(-x, z) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 70 newRate_pitch = ((y_g - y_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 24:aaa5b4703555 71
MarcoF89 25:a8a3cbc57c61 72 newAngle_roll = atan2(y, sqrt(x * x + z * z)) * RAD; //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 73 newRate_roll = ((x_g - x_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 25:a8a3cbc57c61 74
MarcoF89 25:a8a3cbc57c61 75 newAngle_yaw = ((z_g - z_off) * 1/16.4); //Umwandlung der G-Kraft in °
MarcoF89 25:a8a3cbc57c61 76 newRate_yaw = ((z_g - z_off) * 1/16.4); //Offset subtrahiert +++ Umwandlung in °/s
MarcoF89 25:a8a3cbc57c61 77
MarcoF89 25:a8a3cbc57c61 78 pitch = getPitch(&newAngle_pitch, &newRate_pitch, &dt);
MarcoF89 25:a8a3cbc57c61 79 yaw = getYaw(&newAngle_yaw, &newRate_yaw, &dt);
MarcoF89 25:a8a3cbc57c61 80 roll = getRoll(&newAngle_roll, &newRate_roll, &dt);
MarcoF89 25:a8a3cbc57c61 81
MarcoF89 15:742683a8efda 82 if (i == 2000)
MarcoF89 15:742683a8efda 83 {
MarcoF89 25:a8a3cbc57c61 84 pc.printf(" roll: %2.5f\t yaw: %2.5f\t pitch: %2.5f\t rohgyro: %2.5f\n\r",roll, yaw, pitch, newRate_pitch);
MarcoF89 15:742683a8efda 85 i = 0;
MarcoF89 15:742683a8efda 86 }
MarcoF89 25:a8a3cbc57c61 87 Motorsteurung(&Motor1, &Motor2, &Motor3, &Motor4, &taster2, &taster3, &taster4, &n1, &n2, &n3, &n4);
MarcoF89 15:742683a8efda 88 }
MarcoF89 11:8457b851e3e1 89 }
MarcoF89 11:8457b851e3e1 90 }
MarcoF89 10:16ca5e9ee0dc 91 }
MarcoF89 25:a8a3cbc57c61 92 }