Projekt 86: Low-Cost-Drohne mit Ardunio
Autoren: Sergej Vogel, Janis Ostermann
Betreuer: Prof. Schneider
→ zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)
Einleitung
Im Rahmen des Moduls "Angewandte Elektrotechnik" aus dem Studiengang "Business and Systemsengineering" der Hochschule Hamm-Lippstadt in Lippstadt konnten die Studenten ihr bereits erlerntes Wissen unter Beweis stellen und praktische Erfahrungen sammeln, indem sie eine elektrotechnisches Projekt realisieren. Thema dieses Projekts soll die Auslegung und Konstruktion einer kostengünstigen Drohne bei Zuhilfenahme eines Arduino Mikrocontrollers sein. Über die Entwicklung der Hardware hinaus soll zusätzlich ein rudimentäres Programm zur Präsentation der Funktionsweise der Drohne entwickelt werden. Genauere Anforderungen sind unter dem Punkt "Erwartungen an die Projektlösung" einsehbar.
Erwartungen an die Projektlösung
- Recherche bisheriger Lösungen
- Entwurf der Drone, deren elektrischer Schaltung (Sensoren, Aktoren), Konstruktion und Beschaffung der Bauteile
- Realisierung des Aufbaus
- Darstellung der regelungstechnischen Theorie
- Systemidentifikation (Übertragungsfunktion der Regelstrecke bestimmen)
- Vergleichen und bewerten Sie verschiedene Regleransätze (P, PI, PID und andere).
- Test und wiss. Dokumentation
- Machen Sie ein tolles Videos, welches die Funktion visualisiert.
- Live Vorführung während der Abschlusspräsentation
Kür: Modellbasierte Programmierung der Hardware via Simulink
Projekt
Zielsetzung
Das Ziel des Projektes war es die Drohne von Null auf zu konzipieren und zusammenzubauen. Dabei sollte der Kostenaspekt mit beachtet werden. Ein weiteres Ziel stellt das geregelte Schweben der Drohne auf einer bestimmten Höhe dar.
Projektplan
Das Projekt „Low-Cost-Drohne“ sollte in einer Zeitspanne von 16 Wochen realisiert werden. Um einen reibungslosen Ablauf zu gewährleisten, wird das Projekt in folgende Projektschritte gegliedert, die in bestimmten Zeitspannen zu bewältigen sind. Daraus resultiert der Projektplan (siehe Abbildung 1).
Projektdurchführung
Die Recherche rund um den Bereich "Drohne" stellt den Start in das Projekt dar. Da die Studenten seitens der Hochschule finanziell unterstützt wurden, gab es eine zeitliche Vorgabe zur Erstellung einer Einkaufsliste, mit den Teilen die für das jeweilige Projekt benötigt werden. Deshalb war es wichtig, die Hauptrecherche (zur Realisierung der Drohne) schnell abzuschließen. In dieser Phase haben sich mehrere Lösungen zur Realisierung der Drohne angeboten. Diese waren zwar einzeln für sich nicht überzeugend, aber durch die Entnahme einiger Teilbereiche und Indizierung eigener Ideen ist ein individueller Realisierungsplan entstanden. Dabei dient das YouTube-Video[1] als Inspiration und zentrale Grundlage für den technischen Bereich. Im Softwarebereich stehen ebenfalls viele Beispiele und Bibliotheken zur Verfügung. Doch auch da gibt es keine ideale Lösung für die Steuerung und Regelung der Drohne. Somit besteht auch hier die Aufgabe, Bibliotheken/Teillösungen mit einander zu kombinieren bzw. aufeinander abzustimmen.
Entwurf der Drohne
In der Recherchephase war es zunächst geplant, den Rahmen der Drohne selbstständig mittels CAD zu entwerfen und mit dem 3-D-Druckverfahren zu fertigen. Da sich die Realisierung des Rahmens als enormer zeitlicher Aufwand herausgestellt hat, wurde die Idee verworfen. Stattdessen wird eine vorgefertigte CAD Konstruktion eines Drohnenrahmen [2] verwendet, bei der die Größenverhältnisse anzupassen sind, da:
- der 3-D-Drucker nur einen begrenzten Druckraum besitzt, weswegen die Gesamtkonstruktion verkleinert werden muss und
- die Bauteile wie Rotoren und Akku bereits bestellt sind, weswegen die Auflageflächen nochmals angepasst werden müssen.
Um den Abstand der Drohne zum Boden zu messen, soll ein Ultraschallsensor zum Einsatz kommen, der mittig der Drohne, mit der Ausrichtung nach unten zum Boden, angebracht werden soll. Das Problem liegt darin, dass der Sensor erst einen Abstand von mindestens 2cm messen kann. Aus diesem Grund werden längere Standbeine benötigt, die aus einer weiteren CAD Konstruktion [3] entnommen und auf unserer Konstruktion angepasst werden. Das Ergebnis ist die CAD-Konstruktion in der Abbildung 2.
Verwendete Komponenten
In der Tabelle 1 werden alle Komponenten aufgeführt, die für den Zusammenbau der Drohne, sowie für einen Bau einer Sicherungsplattform (Erläuterung folgt im Abschnitt "3.5.2 Umsetzung der Drohne"), benötigt werden.
Kosten der Drohne
In der Tabelle 2 werden die Kosten für die Bauteile aufgeführt, die für den Bau der Drohne benötigt werden.
| Anzahl | Bauteilbezeichnung | Kosten Pro Teil in € | Gesamtkosten in € |
|---|---|---|---|
| 1 | Arduino Uno R3 | 22,99 | 22,99 |
| 1 | Ultraschall-Sensor | 2,95 | 6,29 |
| 1 | MPU-6050 Gyroskop Beschleunigungssensor | 3,40 | 3,40 |
| 2 | Brushless Motor CW | 6,29 | 12,58 |
| 2 | Brushless Motor CCW | 6,29 | 12,58 |
| 1 | Lipo-Akku | 23,75 | 23,75 |
| 1 | Rotoren-Pack | 2,72 | 2,72 |
| 4 | Afro Mini ESC | 6,33 | 25,32 |
| 1 | XT60 Stecker (male) Anschlusskabel für LiPo, Quadrocopter | 3,89 | 3,89 |
| 1 | Ladekabel XT60 für Lipo Akku | 4,95 | 4,95 |
| 1 | 3D-Druck-Rahmen | 0,00 | 0,00 |
| 1 | Taster | 0,00 | 0,00 |
| 1 | Schrauben | 3,09 | 3,09 |
| - | Summe | - | 118,22 |
Schaltung (Sensoren/Aktoren)
Alle Sensoren und Motorcontroller (ESCs) sind direkt mit dem Arduino Uno verbunden. Dazu werden Kabel durch die Kabeldurchführungen zwischen den verschiedenen Ebenen der Drohne verlegt. Die Aktoren, bzw. die Motor Controller sind zusammen mit dem Arduino UNO in einer Parallelschaltung mit dem Lithium Polymer Akku verbunden. Die Sensoren erhalten ihre Spannung über den 5 Volt Pin des Arduino. So können auch noch Sensordaten ausgelesen werden wenn der Arduino per Serieller Schnittstelle mit dem PC verbunden ist, während der Akku von der Drohne getrennt ist. Um Problemen durch Potentialunterschiede vorzubeugen, sind alle Komponenten über einen gemeinsamen Ground verbunden. Die ESCs erhalten ein PWM (Pulsweiten Moulation) Signal über die PWM fähigen Digital-Pins des Arduino. Die ESCs regeln einen Drehstrom in Abhängigkeit vom PWM Signal, um so die Geschwindigkeit der Motoren zu regulieren. Der Ultraschallsensor ist an das 5 Volt Netz des Arduino angeschlossen und überträgt seine Daten über die Digital Pins 11 Und 12 an den Arduino. Die Kommunikation des MPU-6050 Sensors wird über die I2C Schnittstelle des Arduino realisiert. Dazu wird der Sensor mit den SLC und SDA Pins des Mikrocontrollers verbunden. Das zur Kommunikation benötigte Interrupt Signal liegt an Interrupt Pin 1 des Arduions an. Der Modus-Wechsel-Taster ist über Interrupt Pin 2 und den Ground Pin an den Arduino angebunden. Beim Druck des Tasters wird der Interrupt Pin auf das Potential des Ground gezogen. Der Interrupt Pin ist mit einem Pull Up Widerstand auf 5 Volt gelegt. Dadurch Erkennt der Arduino das drücken des Tasters und löst eine Interrupt Routine aus.
Umsetzung der Drohne
Für die Realisierungsphase wird das Drohnengerüst benötigt. Hierzu wird die Drohne innerhalb der CAD-Konstruktion in ihre Einzelkomponenten zerlegt. Nun können die einzelnen Komponenten so positioniert werden, dass sich das Drucken von Hilfsstrukturen reduziert. Das hat den Vorteil, dass die Komponenten eine bessere Struktur erhalten und sich zusätzlich die Druckzeit, sowie die Nachbearbeitung reduziert. Sind die Einzelteile fertig, so werden sie von der Hilfstruktur befreit und geschliffen. Im letzten Schritt werden die Komponenten zu einem Drohnenrahmen zusammengeschraubt (siehe Abbildung 3). Gut zu erkennen sind die nutzbaren Flächen, die im weiteren Verlauf mit den Bauteilen wie Motoren, Akku, Arduino und Sensoren bestückt werden. Doch zuvor werden einige Richtlinien bzw. Punkte für den Zusammenbau aufgestellt, die dabei helfen sollen, die Drohne direkt beim ersten Versuch so zu fertigen, dass weitere Veränderungen vorgebeugt werden. Hier einige Richtlinien:
- Die Komponenten wie den Arduino, den Lipo-Akku und die Sensoren so anordnen, dass sich der Gewichtsschwerpunkt in der Mitte der Drohne befindet
- Rotoren haben dadurch in etwa dieselbe Belastung
- Im Hinblick auf die Bestimmung der Drohnenlage und die Messung des Abstandes zum Boden ergibt es einen Vorteil, wenn sich die Sensoren zentral an der Drohne befinden
- Verkabelung kurz halten (sieht ordentlich aus und keine Gefahr das die Kabel in die Rotoren geraten)
- Der Startknopf soll so positioniert sein, dass beim Drücken des Knopfes keine Gefahr für die Hände entsteht
- Komponenten so anbringen, dass diese schnell und problemlos ausgetauscht werden können
Die Struktur des Gerüstes ist für die Einhaltung der Richtlinien bestens geeignet. So gibt es in der Mitte des Gerüstes eine obere und eine untere Auflagefläche, wo die weiteren Bauteile angebracht werden können, um den Schwerpunkt der Drohne zentral zu halten. Dies ist auch der Plan.
Der Arduino soll einfach zugänglich sein, da die meisten Kabel dort zusammenlaufen und somit ein einfacher bzw. schneller Zugriff von Vorteil ist. Aus diesem Grund wird der Arduino auf die obere Ablagefläche positioniert und mit einem Kabelbinder fixiert (siehe Abbildung 4)
Die beiden Auflageflächen sind durch kleine Balken mit einander verbunden und grenzen dadurch einen optimalen Raum für den Akku ein. So befindet sich der Akku in der Mitte der Drohne (siehe Abbildung 5) und kann durch kurze Leitungen sowohl den Arduino, die ESCs und die Rotoren oben, als auch die Sensoren unten mit Strom versorgen.
Wie bereits erwähnt sollen der Gyro-Sensor (Beschleunigungs- und Lagesensor) und der Ultraschallsensor (für die Messung des Abstandes zum Boden) unterhalb der zweiten Platte angebracht werden. Da sich dort aber bereits Versorgungsleitungen befinden, wird eine weitere Fläche benötigt, damit die Sensoren parallel zum Boden angebracht werden können. Dafür wird die obere Auflagenfläche aus der CAD-Konstruktion entnommen und nochmals mit dem 3-D-Druckverfahren angefertigt. Daraufhin werden die Sensoren mit Schrauben an der Platte befestigt und die Platte wird mit Kabelbindern mit der nun mittleren Auflagefläche verbunden (siehe Abbildung 6).
Die Rotoren haben bereits einen vordefinierten Platz und die dazugehörigen ESCs werden jeweils unterhalb des Drohnenarms mit Kabelbindern befestigt (siehe Abbildung 7 und Abbildung 8).
Zum Schluss wird der Startknopf der Drohne neben dem Akku angebracht (siehe Abbildung 9), sodass kein Verletzungsrisiko besteht, sobald man die Drohne startet.
Durch die Zusammenführung der Komponenten entsteht eine vollwertige Drohne (siehe Abbildung 10) deren Schwerpunkt sich im Zentrum befindet, die Komponenten leicht zugänglich sind und somit jederzeit ohne viel Aufwand ausgewechselt werden können.
Für den Softwareteil wird zusätzlich eine Sicherheitsplattform gebaut (siehe Abbildung 10), die die Bewegungen der Drohne einschränken soll, falls diese nicht wie gewünscht reagiert.
Softwareentwicklung
Die Softwareentwicklung findet in der Arduino IDE, der Softwareentwicklungsumgebung von Arduino, statt. Es wird zunächst eine Recherche zu bestehenden Bibliotheken durchgeführt. Diese werden im Anschluss dazu verwendet um mit den verschiedenen Komponenten der Drohne zu kommunizieren. Ansätze zur Implementierung geben dabei die Beispielprogramme, welche in den Bibliotheken enthalten sind. Für die Realisierung der verschiedenen Zustände der Drohne wird sich der Switch/Case Funktion bedient. Mit Hilfe dieser wird ein Automat implementiert, welcher durch betätigen des Tasters seinen Modus wechselt.
Weitere Informationen sind den Kommentaren im Code zu entnehmen.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Bibliotheken
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "HCSR04.h"
#include <ESC.h>
#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Defines
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define MOTOR_PIN1 (6) // ESC Pin von Motor 1 -> BEI UNS Nr. 3 VORNE RECHTS
#define MOTOR_PIN2 (9) // ESC Pin von Motor 2 -> BEI UNS Nr. 4 HINTEN RECHTS
#define MOTOR_PIN3 (10) // ESC Pin von Motor 3 -> BEI UNS Nr. 1 HINTEN LINKS
#define MOTOR_PIN4 (5) // ESC Pin von Motor 4 -> BEI UNS Nr. 2 VORNE LINKS
#define SPEED_MIN (1440) // Set the Minimum ESC Speed in microseconds -> mindestens 1060 -> Drehen ab 1440
#define SPEED_MAX (1550) // Set the Maximum ESC Speed in microseconds -> Bis zu 1860
#define SPEED_ARM (1000) // Set the Arm ESC Speed in microseconds -> noch austesten
#define ULTRASONIC_TRIGGER_PIN (12) // Trigger Pin von Ultraschallsensor
#define ULTRASONIC_ECHO_PIN (11) // Echo Pin von Ultraschallsensor
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//FLug Variablen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
MPU6050 mpu; // Neues Objekt MPU Sensor aus der MPU Bibliothek erzeugen
UltraSonicDistanceSensor distanceSensor(ULTRASONIC_TRIGGER_PIN, ULTRASONIC_ECHO_PIN);
int State = 0, TextShown = 0;
volatile unsigned long alteZeit=0, entprellZeit=100; // Interrupt Taster Variablen
bool EnalbedFlying = 1; // bei 1 springt Stateflow von State 2 sofort in den State 3 in dem die Motoren gestartete werden
int throttle = 1400; // Gas zwischen 1080 und maximal 1980
double throttleWithPID = 0;
double dist_offset, winkel_offset_roll = 5.13, winkel_offset_pitch = 2.75, winkel_offset_yaw = 0;
double maDist, rawDist, DistDiff; // Ultraschallsensor Filter Variablen
int cal_int;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//ESC Library Variablen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double esc_1, esc_2, esc_3, esc_4;
ESC myESC1 (MOTOR_PIN1, SPEED_MIN, SPEED_MAX, SPEED_ARM); // Neues Objekt ESC aus der ESC Bibliothek erzeugen
ESC myESC2 (MOTOR_PIN2, SPEED_MIN, SPEED_MAX, SPEED_ARM); // Neues Objekt ESC aus der ESC Bibliothek erzeugen
ESC myESC3 (MOTOR_PIN3, SPEED_MIN, SPEED_MAX, SPEED_ARM); // Neues Objekt ESC aus der ESC Bibliothek erzeugen
ESC myESC4 (MOTOR_PIN4, SPEED_MIN, SPEED_MAX, SPEED_ARM); // Neues Objekt ESC aus der ESC Bibliothek erzeugen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//PID Variablen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double pid_p_gain_roll = 0.3; //Gain setting for the roll P-controller
double pid_i_gain_roll = 0;//.006;//0.04; //Gain setting for the roll I-controller
double pid_d_gain_roll = 0.2;//.06;//18.0; //Gain setting for the roll D-controller
int pid_max_roll = 20; //Maximum output of the PID-controller (+/-)
double pid_pitch_setpoint = 0;
double pid_p_gain_pitch = pid_p_gain_roll; //Gain setting for the pitch P-controller.
double pid_i_gain_pitch = pid_i_gain_roll; //Gain setting for the pitch I-controller.
double pid_d_gain_pitch = pid_d_gain_roll; //Gain setting for the pitch D-controller.
int pid_max_pitch = pid_max_roll; //Maximum output of the PID-controller (+/-)
double pid_roll_setpoint = 0;
double pid_p_gain_yaw = 0.025; //1.0; //4.0 //Gain setting for the pitch P-controller. //4.0
double pid_i_gain_yaw = 0.0; //0.02; //Gain setting for the pitch I-controller. //0.02
double pid_d_gain_yaw = 0.0; //Gain setting for the pitch D-controller.
int pid_max_yaw = 400; //Maximum output of the PID-controller (+/-)
double pid_yaw_setpoint = 0;
double pid_p_gain_throttle = 3; //1.0; //4.0 //Gain setting for the pitch P-controller. //4.0
double pid_i_gain_throttle = 15; //0.02; //Gain setting for the pitch I-controller. //0.02
double pid_d_gain_throttle = 0.5; //Gain setting for the pitch D-controller.
double pid_max_throttle = 200; //Maximum output of the PID-controller (+/-)
double pid_min_throttle = 0; //Maximum output of the PID-controller (+/-)
double pid_dist_setpoint = 30;
double angle_roll = 0, angle_pitch = 0, angle_yaw = 0;
double pid_angle_roll = 0, pid_angle_pitch = 0, pid_angle_yaw = 0;
double angle_roll_raw = 0, angle_pitch_raw = 0, angle_yaw_raw = 0;
double last_angle_roll_raw = 0, last_angle_pitch_raw = 0, last_angle_yaw_raw = 0;
double pid_output_roll = 0, pid_output_pitch = 0, pid_output_yaw = 0;
PID rollPID(&pid_angle_roll, &pid_output_roll, &pid_roll_setpoint, pid_p_gain_roll, pid_i_gain_roll, pid_d_gain_roll, DIRECT); // Neues Objekt PID Regler aus der PID Bibliothek erzeugen
PID pitchPID(&pid_angle_pitch, &pid_output_pitch, &pid_pitch_setpoint, pid_p_gain_pitch, pid_i_gain_pitch, pid_d_gain_pitch, DIRECT); // Neues Objekt PID Regler aus der PID Bibliothek erzeugen
//PID yawPID(&pid_angle_yaw, &pid_output_yaw, &pid_yaw_setpoint, pid_p_gain_yaw, pid_i_gain_yaw, pid_d_gain_yaw, DIRECT); /// Neues Objekt PID Regler aus der PID Bibliothek erzeugen
//PID throttlePID(&maDist, &throttleWithPID, &pid_dist_setpoint, pid_p_gain_throttle, pid_i_gain_throttle, pid_d_gain_throttle, DIRECT); // Neues Objekt PID Regler aus der PID Bibliothek erzeugen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//MPU Variablen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(115200);
//Interrupt Pin
pinMode(3, INPUT); // Pin 2 ist INT0
digitalWrite(3, HIGH); // interner Pull up Widerstand auf 5V
attachInterrupt(1, interruptKnopfRoutine, LOW); // Pin 2 (INT 0) geht auf 0V (LOW) dann interruptRoutine aufrufen
//LED einschalten um Setup zu signalisieren
pinMode(13, OUTPUT);
digitalWrite(13,HIGH); //Turn on the warning led.
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(71);
mpu.setYGyroOffset(47);
mpu.setZGyroOffset(-74);
mpu.setXAccelOffset(-1287); // 1688 factory default for my test chip
mpu.setYAccelOffset(-1412);
mpu.setZAccelOffset(1788);
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
Serial.println("Kalibrierung gestartet! Drohne nicht bewegen!");
for (cal_int = 0; cal_int < 100 ; cal_int ++){ //Take 100 readings for calibration.
rawDist = distanceSensor.measureDistanceCm(); //Messung der Distanz zum Boden in cm
maDist = movingAverage(rawDist);
dist_offset += maDist; //Ad distance value to dist_offset
delay(3); //Wait 3 milliseconds before the next loop.
}
//Aus 100 Messung den Mittelwert bilden
dist_offset /= 100;
Serial.println("Kalibrierung beendet!");
pitchPID.SetOutputLimits(pid_max_pitch*(-1), pid_max_pitch);
rollPID.SetOutputLimits(pid_max_roll*(-1), pid_max_roll);
// yawPID.SetOutputLimits(pid_max_yaw*(-1), pid_max_yaw); // Yaw Regler ist momentan noch nicht in die ESC Berechnung eingebunden
// throttlePID.SetOutputLimits(pid_min_throttle, pid_max_throttle); // Höhen Regler ist momentan noch nicht in die ESC Berechnung eingebunden
pitchPID.SetMode(AUTOMATIC);
rollPID.SetMode(AUTOMATIC);
// yawPID.SetMode(AUTOMATIC); // Yaw Regler ist momentan noch nicht in die ESC Berechnung eingebunden
// throttlePID.SetMode(AUTOMATIC); // Höhen Regler ist momentan noch nicht in die ESC Berechnung eingebunden
pitchPID.SetControllerDirection(REVERSE);
rollPID.SetControllerDirection(REVERSE);
// yawPID.SetControllerDirection(REVERSE); // Yaw Regler ist momentan noch nicht in die ESC Berechnung eingebunden
// throttlePID.SetControllerDirection(DIRECT); // Höhen Regler ist momentan noch nicht in die ESC Berechnung eingebunden
State = 0; //Initial State auf 0 zurücksetzen
//LED ausschalten wenn Setup beendet wurde
digitalWrite(13,LOW); //Turn on the warning led.
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Hauptprogramm
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
if (mpuInterrupt && fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
fifoCount = mpu.getFIFOCount();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
angle_yaw_raw = (ypr[0] * 180/M_PI);
angle_pitch_raw = (ypr[1] * 180/M_PI);
angle_roll_raw = (ypr[2] * 180/M_PI);
pid_angle_yaw = angle_yaw_raw-winkel_offset_yaw;
pid_angle_pitch = angle_pitch_raw-winkel_offset_pitch;
pid_angle_roll = angle_roll_raw-winkel_offset_roll;
}
rawDist = distanceSensor.measureDistanceCm(); //Messung der Distanz zum Boden in cm
maDist = movingAverage(rawDist)-dist_offset; //Filterung der Distanz Messwerte mit gleitendem Mittelwertfilter
switch (State) {
////////////////////////////////////////////////////////////////////////////////////////////////////// Ruhezustand
case 0:
if(TextShown == 0){
Serial.println("State: Stop");
Serial.println("\nPress Button to Start!");
TextShown = 1;
}
myESC1.stop(); // tell ESC to stop
myESC2.stop(); // tell ESC to stop
myESC3.stop(); // tell ESC to stop
myESC4.stop(); // tell ESC to stop
break;
////////////////////////////////////////////////////////////////////////////////////////////////////// Arm-Zustand
case 1:
if(TextShown == 0){
Serial.println("State: Arm");
TextShown = 1;
}
myESC1.arm();
myESC2.arm();
myESC3.arm();
myESC4.arm();
delay(3000); // Wait for a while
break;
////////////////////////////////////////////////////////////////////////////////////////////////////// PID-Initialisierung
case 2:
if(TextShown == 0){
Serial.println("State: Initialisiere Flug");
TextShown = 1;
}
Serial.print("Höhe in cm: ");Serial.print((int)maDist);
Serial.print(" \t\tWinkelX in Grad (Roll): ");Serial.print(pid_angle_roll);
Serial.print("\tWinkelY in Grad (Pitch): ");Serial.println(pid_angle_pitch);
if(EnalbedFlying == 1){
State = 3;
TextShown = 0;
}
break;
////////////////////////////////////////////////////////////////////////////////////////////////////// Flug
case 3:
if(TextShown == 0){
Serial.println("State: Flug gestartet");
TextShown = 1;
}
// PID Parameter zurücksetzen
pid_roll_setpoint = 0;
pid_pitch_setpoint = 0;
pid_yaw_setpoint = 0;
calculate_pid(); //PID Regler Output berechnen
esc_1 = (pid_output_roll/2) - (pid_output_pitch/2);// - pid_output_yaw; // Power für esc 1 berechnen (vorne-rechts - CCW)
esc_2 = (pid_output_roll/2) + (pid_output_pitch/2);// + pid_output_yaw; // Power für esc 2 berechnen (hinten-rechts - CW)
esc_3 = ((pid_output_roll/2)*(-1)) + (pid_output_pitch/2);// - pid_output_yaw; // Power für esc 3 berechnen (hinten-links - CCW)
esc_4 = ((pid_output_roll/2)*(-1)) - (pid_output_pitch/2);// + pid_output_yaw; // Power für esc 4 berechnen (vorne-links - CW)
if (esc_1 < -30) esc_1 = -30; //Limit the esc-1 power. unteres Limit
if (esc_2 < -30) esc_2 = -30; //Limit the esc-1 power. unteres Limit
if (esc_3 < -30) esc_3 = -30; //Limit the esc-1 power. unteres Limit
if (esc_4 < -30) esc_4 = -30; //Limit the esc-1 power. unteres Limit
if(esc_1 > 30)esc_1 = 30; //Limit the esc-1 power. oberes Limit
if(esc_2 > 30)esc_2 = 30; //Limit the esc-2 power. oberes Limit
if(esc_3 > 30)esc_3 = 30; //Limit the esc-3 power. oberes Limit
if(esc_4 > 30)esc_4 = 30; //Limit the esc-4 power. oberes Limit
esc_1 = (esc_1);
esc_2 = (esc_2);
esc_3 = (esc_3);
esc_4 = (esc_4);
myESC1.speed(esc_1+throttle); // tell ESC to go to the oESC speed value
myESC2.speed(esc_2+throttle); // tell ESC to go to the oESC speed value
myESC3.speed(esc_3+throttle); // tell ESC to go to the oESC speed value
myESC4.speed(esc_4+throttle); // tell ESC to go to the oESC speed value
Serial.print("Höhe in cm: ");Serial.print((int)maDist);
Serial.print("\tESC 1: ");Serial.print(esc_1+throttle);
Serial.print("\tESC 2: ");Serial.print(esc_2+throttle);
Serial.print("\tESC 3: ");Serial.println(esc_3+throttle);
Serial.print("\tESC 4: ");Serial.println(esc_4+throttle);
delay(10);
break;
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Interrupt Funktion
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void interruptKnopfRoutine() {
if((millis() - alteZeit) > entprellZeit) { // innerhalb der entprellZeit nichts machen
Serial.println("Knopf erkannt!");
TextShown = 0;
if(EnalbedFlying == 1){
if (State < 3) {
State += 1;
}
else {
State = 0;
}
}
else{
if (State < 2) {
State += 1;
}
else {
State = 0;
}
}
}
alteZeit = millis(); // letzte Schaltzeit merken
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//PID Parameter Berechnung
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void calculate_pid(){
pitchPID.Compute();
rollPID.Compute();
// yawPID.Compute();
// throttlePID.Compute();
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Moving Average Funktion
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float movingAverage(float M) {
#define LM_SIZE 3
static float LM[LM_SIZE]; // LastMeasurements
static byte index = 0;
static float sum = 0;
static byte count = 0;
// keep sum updated to improve speed.
sum -= LM[index];
LM[index] = M;
sum += LM[index];
index++;
index = index % LM_SIZE;
if (count < LM_SIZE) count++;
return sum / count;
}
Ergebnis
Hardware
Die Zielsetzung zur Hardware wurde vollständig erfüllt. Es mag zwar klingen, dass eine 118,22€ teure Drohne keine „Low-Cost-Drohne“ ist, jedoch sollte bedacht werden, dass es sich um einen Prototypen handelt und sich die Kosten der Drohne durch eine Massenfertigung deutlich reduzierten lassen. Des Weiteren wurde die Drohne so umgesetzt, dass sich einzelne Komponenten einfach austauschen lassen, falls diese beschädigt oder defekt sein sollten. Damit das Verletzungsrisiko durch die Drohne minimal ist, wurde eine Plattform aufgebaut, die die Bewegungen der Drohne einschränkt.
Software
Die Software der Drohne wurde den Anforderungen entsprechend entwickelt. Die Aktoren werden mit der Hilfe der ESC Bibliothek angesteuert. Des weiteren ist es möglich die Lage der Drohne und den Abstand zum Boden durch die gewonnenen Sensordaten zu bestimmen. Durch die Filterung und Sensordatenfusion sind die Messwerte sehr genau und zeigen wenig Latenz auf. Somit sollte eine Lage- und Höhenregelung möglich sein. Die Regelung der Drohne ist in Form eines PID Reglers umgesetzt. Dieser ist in der Software implementiert. Für das erfolgreiche Regeln der Drohne ist jedoch noch eine Auslegung der PID-Regelparameter nötig. Dies war aus Mangel an Zeit zum Ende des Projektes nicht mehr ausreichend umsetzbar.
Ausblick
In den nächsten Weiterentwicklungsschritten der Drohne kann durch die Arbeit an folgenden Punkten eine Weiterentwicklung des Projektes erfolgen:
- Auslegung der PID Regelparameter
- Implementierung von Steig- und Sinkflug bei dem autonomen Abheben der Drohne
- Controller zur Steuerung der Drohne von Außen
- Tiefenentladungsschutz für den LiPo Akku
Zusammenfassung
Um eine gewisse Orientierung zu erhalten, wurde zunächst Recherche betrieben und der Projektablaufplan aufgestellt. Im Anschluss darauf gelang es die notwendigen Bauteile zu erfassen und zu bestellen. In der ersten Hauptphase wurde die Hardware anhand von recherchierten Unterlagen und eigener Ideen realisiert. Dafür gab es eigene Richtlinien die bei der Umsetzung zu beachten waren um ein erfolgreiches Umsetzten des Projektes zu sichern. In der zweiten Hauptphase ging es um die Entwicklung der Software. Diese wurde ebenfalls durch das Kombinieren von bestehenden Lösungen und eigenen Entwicklungen umgesetzt. Das Ergebnis ist eine erfolgreiche Auswertung der Sensoren und die Ansteuerung der Aktoren, welches den Grundstein zur Regelung der Drohne legt.
Insgesamt betragen die Kosten für den Bau der Drohne 118,22€. Dies sind jedoch lediglich die Kosten für den Prototypen und welche sich in einer Massenfertigung deutlich reduzieren ließen.
Lessons Learned
Die Realisierung einer kostengünstigen Drohne stellte eine enorme Herausforderung dar. Dabei konnte und musste man bereits erlerntes Wissen mit neuen Erfahrungen kombinieren, während man durchgehend unter Zeitdruck stand. Das simuliert mehr oder weniger das Leben eines Ingenieurs. Das erlernte Grundwissen aus dem Mechatronik Bachelor sowie dem Business and Systems Engineering Master in den Bereichen der Elektrotechnik und Softwareentwicklung stellte eine gute Grundlage für das Projekt dar. Des Weiteren konnten durch kleine Fehler und Unachtsamkeiten (leider auf Kosten der Zeit) Erfahrungen gesammelt werden.
Im Detail:
- Teillösungen mit einander kombinieren, aufeinander aufbauen und eigene Ideen einfließen lassen, um daraus eine neue Lösung zu erarbeiten
- Planung der optimalen Umsetzung der Hardware und damit verbundene unterschiedliche Betrachtungswinkel um nicht nur den funktionstechnischen Aufbau zu realisieren, sondern ihn auch so zu konzipieren, dass defekte Teile schnell und problemlos ausgetauscht werden können
- Praktisches Arbeiten und Umgehen mit Werkzeugen z.B.: beim Sägen, Bohren und Löten
- Den Sicherheitsaspekt in solchen Projekten ernst nehmen und nicht zu vernachlässigen
- Schutz von elektrischen Komponenten wie z.B. einem Tiefenentladungsschutz
- Beachtung von elektromagnetischen Einflüssen z.B. auf die die Sensoren
YouTube Video
Das YouTube Video als visueller Beleg steht unter folgendem Link zur Verfügung:
Literatur
→ zurück zur Übersicht: WS 18/19: Angewandte Elektrotechnik (BSE)