///////////////////////////////////////////////////////////////////////////////////////
//ARDUINO-Software Programm für eine Drohne
///////////////////////////////////////////////////////////////////////////////////////
#include <Wire.h> //Zugriff auf die Bibliothek "Wire.h", zur Kommunikation mit dem Gyroskop
#include <EEPROM.h> //Zugriff auf die Bibliothek "EEPROM.h", zum Speichern von Informationen auf dem EEPROM
#include <LiquidCrystal.h>
#include <SoftwareSerial.h>
#include <TinyGPS.h>
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//PID-Verstärkung (gain) und Grenzwerteinstellungen (limit settings)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float pid_p_gain_roll = 1.3; //Verstärkungseinstellung für den Roll-P-Controller
float pid_i_gain_roll = 0.04; //Verstärkungseinstellung für den Roll-I-Controller
float pid_d_gain_roll = 18.0; //Verstärkungseinstellung für den Roll-D-Controller
int pid_max_roll = 400; //Maximaler Ausgang des PID-Reglers (+/-)
float pid_p_gain_pitch = pid_p_gain_roll; //Verstärkungseinstellung für den Pitch-P-Controller
float pid_i_gain_pitch = pid_i_gain_roll; //Verstärkungseinstellung für den Pitch-I-Controller
float pid_d_gain_pitch = pid_d_gain_roll; //Verstärkungseinstellung für den Pitch-D-Controller
int pid_max_pitch = pid_max_roll; //Maximaler Ausgang des PID-Reglers (+/-)
float pid_p_gain_yaw = 4.0; //Verstärkungseinstellung für den Pitch-P-Controller mit 4.0
float pid_i_gain_yaw = 0.02; //Verstärkungseinstellung für den Pitch-I-Controller mit 0.02
float pid_d_gain_yaw = 0.0; //Verstärkungseinstellung für den Pitch-D-Controller mit 0.0
int pid_max_yaw = 400; //Maximaler Ausgang des PID-Reglers (+/-)
boolean auto_level = true; //Auto-Level ein (true) oder aus (false)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Einstellungen GPS-Modul
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//long lat,lon;
float lat = 28.5458,lon = 77.1703; //Variable für Breitengrad- (latitude) und Längengrad (longitude)-Objekt
SoftwareSerial gpsSerial(0,1); //rx,tx
LiquidCrystal lcd(A0,A1,A2,A3,A4,A5);
TinyGPS gps; //GPS-Objekt erzeugen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Deklaration der globalen Variablen
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
byte last_channel_1, last_channel_2, last_channel_3, last_channel_4;
byte eeprom_data[36];
byte highByte, lowByte;
volatile int receiver_input_channel_1, receiver_input_channel_2, receiver_input_channel_3, receiver_input_channel_4;
int counter_channel_1, counter_channel_2, counter_channel_3, counter_channel_4, loop_counter;
int esc_1, esc_2, esc_3, esc_4;
int throttle, battery_voltage;
int cal_int, start, gyro_address;
int receiver_input[5];
int temperature;
int acc_axis[4], gyro_axis[4];
float roll_level_adjust, pitch_level_adjust;
long acc_x, acc_y, acc_z, acc_total_vector;
unsigned long timer_channel_1, timer_channel_2, timer_channel_3, timer_channel_4, esc_timer, esc_loop_timer;
unsigned long timer_1, timer_2, timer_3, timer_4, current_time;
unsigned long loop_timer;
double gyro_pitch, gyro_roll, gyro_yaw;
double gyro_axis_cal[4];
float pid_error_temp;
float pid_i_mem_roll, pid_roll_setpoint, gyro_roll_input, pid_output_roll, pid_last_roll_d_error;
float pid_i_mem_pitch, pid_pitch_setpoint, gyro_pitch_input, pid_output_pitch, pid_last_pitch_d_error;
float pid_i_mem_yaw, pid_yaw_setpoint, gyro_yaw_input, pid_output_yaw, pid_last_yaw_d_error;
float angle_roll_acc, angle_pitch_acc, angle_pitch, angle_roll;
boolean gyro_angles_set;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Setup Routine
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup(){
//Serial.begin(57600);
//Kopie der EEPROM-Daten für einen schnellen Zugriff
for(start = 0; start <= 35; start++)eeprom_data[start] = EEPROM.read(start);
start = 0; //Start auf 0 zurücksetzen
gyro_address = eeprom_data[32]; //Gyroskop-Adresse als Variable speichern
Wire.begin(); //I2C als Master starten
TWBR = 12; //I2C-Taktgeschwindigkeit auf 400kHz setzen
//Arduino (Atmega) Pins standardmäßig als Eingänge deklariert - daher ist keine explizite Deklaration der Eingänge notwendig
DDRD |= B11110000; //Konfiguration der digitalen Porte 4, 5, 6 und 7 als Ausgang
DDRB |= B00110000; //Konfiguration der digitalen Porte 12 und 13 als Ausgang
//LED auf der Arduino als Startup-Anzeige nutzen
digitalWrite(12,HIGH); //LED ein
//Überprüfung der EEPROM-Signatur - Sicherstellung der Durchführung des Setup-Programm
while(eeprom_data[33] != 'J' || eeprom_data[34] != 'M' || eeprom_data[35] != 'B')delay(10);
//Flight-Controller braucht die MPU-6050 mit dem Gyroskop und dem Beschleunigungsmesser
//Setup ist ohne MPU-6050 abgschlossen, dann Flight-Controller-Programm stoppen
if(eeprom_data[31] == 2 || eeprom_data[31] == 3)delay(10);
set_gyro_registers(); //Spezifisches Gyroskop-Register setzen
for (cal_int = 0; cal_int < 1250 ; cal_int ++){ //Start nach 5 Sekunden
PORTD |= B11110000; //Digital port 4, 5, 6 und 7 ein
delayMicroseconds(1000); //Warte 1000us
PORTD &= B00001111; //Digital Port 4, 5, 6 und 7 aus
delayMicroseconds(3000); //Warte 3000us
}
//Kalibrierung des Gyroskops (mehrere Datenproben für einen durchschnittlichen Offset)
for (cal_int = 0; cal_int < 2000 ; cal_int ++){ //Abrufen von 2000 Werten zur Kalibrierung
if(cal_int % 15 == 0)digitalWrite(12, !digitalRead(12)); //LED-Status ändern, zum Anzeigen der Kalibrierung
gyro_signalen(); //Gyro-Ausgang auslesen
gyro_axis_cal[1] += gyro_axis[1]; //Übergabe von Rollwert an gyro_roll_cal
gyro_axis_cal[2] += gyro_axis[2]; //Übergabe von Pitchwert an gyro_pitch_cal
gyro_axis_cal[3] += gyro_axis[3]; //Übergabe von Yaw-Wert an gyro_yaw_cal
//Piepsen der ESC währenddessen vermeiden - 1000us Puls geben für die Gyro-Kalibierung
PORTD |= B11110000; //Digital Port 4, 5, 6 und 7 ein
delayMicroseconds(1000); //Warte 1000us
PORTD &= B00001111; //Digital Port 4, 5, 6 und 7 aus
delay(3); //Warte 3ms vor der nächsten Loop
}
//Division der gemessenen 2000 Werte für den durchschnittlichen Gyro-Offset
gyro_axis_cal[1] /= 2000; //Division von Roll
gyro_axis_cal[2] /= 2000; //Division von Pitch
gyro_axis_cal[3] /= 2000; //Division von Yaw
PCICR |= (1 << PCIE0); //PCIE0 ein, um PCMSK0-Scan zu aktivieren
PCMSK0 |= (1 << PCINT0); //PCINT0 (IN-PIN8) ein, um Interrupt bei Zustandänderungen auszulösen
PCMSK0 |= (1 << PCINT1); //PCINT1 (IN-PIN9) ein, um Interrupt bei Zustandswechsel auszulösen
PCMSK0 |= (1 << PCINT2); //PCINT2 (IN-PIN10) ein, um Interrupt bei Zustandswechsel auszulösen
PCMSK0 |= (1 << PCINT3); //PCINT3 (IN-PIN11) ein, um Interrupt bei Zustandänderungen auszulösen
//Warten bis Empfänger aktiv ist und Regler in unteren Position steht
while(receiver_input_channel_3 < 990 || receiver_input_channel_3 > 1020 || receiver_input_channel_4 < 1400){
receiver_input_channel_3 = convert_receiver_channel(3); //Aktuelle Empfängersignale für Gas auf den Standard 1000-2000us umrechnen
receiver_input_channel_4 = convert_receiver_channel(4); //Aktuelle Empfängersignale für Yaw auf den Standard 1000-2000us umrechnen
start ++; //Start inkrementiert während des Wartens mit jeder Loop
//Piepsen der ESC währenddessen vermeiden - 1000us Puls geben währenddessen auf die Eingänge des Empfängers gewartet wird
PORTD |= B11110000; //Digital Port 4, 5, 6 und 7 ein
delayMicroseconds(1000); //Warte 1000us
PORTD &= B00001111; //Digital Port 4, 5, 6 und 7 aus
delay(3); //Warte 3ms vor der nächsten Loop
if(start == 125){ //Alle 125 Loops (500ms)
digitalWrite(12, !digitalRead(12)); //Ändern des LED-Status
start = 0; //Erneut bei 0 starten
}
}
start = 0; //Start auf 0 zurücksetzen
//Batteriespannung in Variable battery_voltage
//65 ist die Spannunbgskompensation für die Diode
//12.6V = ~5V @Analog 0 & 12.6V = 1023 analogRead(0) & 1260/1023 = 1.2317
//battery_voltage bekommt 1050, wenn die Batteriespannung 10.5V beträgt
battery_voltage = (analogRead(0) + 65) * 1.2317;
loop_timer = micros(); //Timer für die nächste Loop
//Durchführung erfolgt, dann LED aus
digitalWrite(12,LOW); //LED aus
///GPS-Modul
Serial.begin(9600); //Seriell verbinden
//Seriell.println("The GPS Received Signal:");
gpsSerial.begin(9600); //GPS-Sensor anschließen
lcd.begin(16,2);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Hauptprogramm-Loop
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop(){
//65.5 = 1 deg/sec (Datenblatt der MPU-6050 für mehr Informationen)
gyro_roll_input = (gyro_roll_input * 0.7) + ((gyro_roll / 65.5) * 0.3); //Gyro-PID-Input ist deg/sec
gyro_pitch_input = (gyro_pitch_input * 0.7) + ((gyro_pitch / 65.5) * 0.3);//Gyro-PID-Input ist deg/sec
gyro_yaw_input = (gyro_yaw_input * 0.7) + ((gyro_yaw / 65.5) * 0.3); //Gyro-PID-Input ist deg/sec
////////////////////////////////////////////////////////////////////////////////////////////////////
//IMU-Code (https://youtu.be/4BoIE8YQwM8 und https://youtu.be/j-kE0AMEWy4)
////////////////////////////////////////////////////////////////////////////////////////////////////
//Gyro-Winkel-Berechnungen
//0.0000611 = 1 / (250Hz / 65.5)
angle_pitch += gyro_pitch * 0.0000611; //Berechnung des zurückgelegten Neigungswinkel & mit angle_pitch addieren
angle_roll += gyro_roll * 0.0000611; //Berechnung des gefahrenen Rollwinkels & mit angle_roll addieren
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) Arduino sin-Funktion ist im Bogenmaß
angle_pitch -= angle_roll * sin(gyro_yaw * 0.000001066); //IMU hat gegiert, dann Übertragung von Roll-Winkel an Pitch-Winkel
angle_roll += angle_pitch * sin(gyro_yaw * 0.000001066); //IMU hat gegiert, dann Übertragung von Pitch-Winkel an Roll-Winkel
//Beschleunigungsmesser-Winkelberechnungen
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Berechnung des gesamten Beschleunigungsvektors
if(abs(acc_y) < acc_total_vector){ //Verhindern, dass asin-Funktion kein NaN erzeugt
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Neigungswinkel berechnen
}
if(abs(acc_x) < acc_total_vector){ //Verhindern, dass asin-Funktion kein NaN erzeugt
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Rollwinkel berechnen
}
//MPU-6050 "Wasserwaage" platzieren und die Werte zur Kalibrierung notieren
angle_pitch_acc -= 0.0; //Beschleunigungsmesser-Kalibrierwert für Pitch
angle_roll_acc -= 0.0; //Beschleunigungsmesser-Kalibrierwert für Roll
angle_pitch = angle_pitch * 0.9996 + angle_pitch_acc * 0.0004; //Drift-Korrektur des Gyro-Pitchwinkels mit dem Beschleunigungssensor-Pitchwinkel
angle_roll = angle_roll * 0.9996 + angle_roll_acc * 0.0004; //Drift-Korrektur des Gyro-Rollwinkels mit dem Beschleunigungssensor-Rollwinkel
pitch_level_adjust = angle_pitch * 15; //Berechnung des Korrektur-Pitchwinkels
roll_level_adjust = angle_roll * 15; //Berechnung des Korrektur-Rollwinkels
if(!auto_level){ //Drohne nicht im Auto-Level-Modus
pitch_level_adjust = 0; //Pitchwinkel-Korrektur auf 0
roll_level_adjust = 0; //Rollwinkel-Korrektur auf 0
}
//Motorstart - Sschritt 1: Gas gering und Gier links
if(receiver_input_channel_3 < 1050 && receiver_input_channel_4 < 1050)start = 1;
//Schritt 2: Gier-Stick zurück in Mittelstellung - Motoren starten
if(start == 1 && receiver_input_channel_3 < 1050 && receiver_input_channel_4 > 1450){
start = 2; //Drohne starten
angle_pitch = angle_pitch_acc; //Gyro-Pitchwinkel = Beschleunigungsmesser-Pitchwinkel
angle_roll = angle_roll_acc; //Gyro-Rollwinkel = Beschleunigungsmesser-Rollwinkel
gyro_angles_set = true; //IMU-Startflag auf true
//PID-Regler für stroßfreien Start resetten
pid_i_mem_roll = 0;
pid_last_roll_d_error = 0;
pid_i_mem_pitch = 0;
pid_last_pitch_d_error = 0;
pid_i_mem_yaw = 0;
pid_last_yaw_d_error = 0;
}
//Motoren stoppen: Gas gering und Gier rechts
if(start == 2 && receiver_input_channel_3 < 1050 && receiver_input_channel_4 > 1950)start = 0;
//PID-Sollwert in Grad pro Sekunge durch Roll-Empfängereingang bestimmen (bei der Division durch 3 beträgt die max. Rollrate ca. 164 Grad pro Sekunde ((500-8)/3=164d/s))
pid_roll_setpoint = 0;
//Totband von 16us für bessere Ergebnisse
if(receiver_input_channel_1 > 1508)pid_roll_setpoint = receiver_input_channel_1 - 1508;
else if(receiver_input_channel_1 < 1492)pid_roll_setpoint = receiver_input_channel_1 - 1492;
pid_roll_setpoint -= roll_level_adjust; //Subtraktion der Winkelkorrektur von dem standartisierten Empfänger-Roll-Eingangswert
pid_roll_setpoint /= 3.0; //Divison des Sollwertes für den PID-Roll-Regler durch 3 zur Umrechnung von Winkel in Grad
//PID-Sollwert in Grad pro Sekunge durch Pitch-Empfängereingang bestimmen (bei der Division durch 3 beträgt die max. Pitchrate ca. 164 Grad pro Sekunde ((500-8)/3=164d/s))
pid_pitch_setpoint = 0;
//Totband von 16us für bessere Ergebnisse
if(receiver_input_channel_2 > 1508)pid_pitch_setpoint = receiver_input_channel_2 - 1508;
else if(receiver_input_channel_2 < 1492)pid_pitch_setpoint = receiver_input_channel_2 - 1492;
pid_pitch_setpoint -= pitch_level_adjust; //Subtraktion der Winkelkorrektur von dem standartisierten Empfänger-Pitch-Eingangswert
pid_pitch_setpoint /= 3.0; //Divison des Sollwertes für den PID-Pitch-Regler durch 3 zur Umrechnung von Winkel in Grad
//PID-Sollwert in Grad pro Sekunde durch Gier-Empfänger-Eingang bestimmen (bei der Division durch 3 beträgt die max. Gierrate ca. 164 Grad pro Sekunde ((500-8)/3=164d/s))
pid_yaw_setpoint = 0;
//Totband von 16us für bessere Ergebnisse
if(receiver_input_channel_3 > 1050){ //Do not yaw when turning off the motors.
if(receiver_input_channel_4 > 1508)pid_yaw_setpoint = (receiver_input_channel_4 - 1508)/3.0;
else if(receiver_input_channel_4 < 1492)pid_yaw_setpoint = (receiver_input_channel_4 - 1492)/3.0;
}
calculate_pid(); //PID-Eingänge bekannt - Berechnung der PID-Ausgänge
//Batteriespannung für Kompensation notwendig & Komplementärfilter zum Reduzieren von Rauschen & 0.09853 = 0.08 * 1.2317
battery_voltage = battery_voltage * 0.92 + (analogRead(0) + 65) * 0.09853;
//LED ein, wenn Batteriespannung zu gering
if(battery_voltage < 1000 && battery_voltage > 600)digitalWrite(12, HIGH);
throttle = receiver_input_channel_3; //Gassignal = Basissignal
if (start == 2){ //Motoren starten
if (throttle > 1800) throttle = 1800; //Spielraum für Kontrolle bei Vollgas
esc_1 = throttle - pid_output_pitch + pid_output_roll - pid_output_yaw; //Berechnung ESC-1-Impuls (vorne-rechts)
esc_2 = throttle + pid_output_pitch + pid_output_roll + pid_output_yaw; //Berechnung ESC-2-Impuls (hinten-rechts)
esc_3 = throttle + pid_output_pitch - pid_output_roll - pid_output_yaw; //Berechnung ESC-3-Impuls (hinten-links)
esc_4 = throttle - pid_output_pitch - pid_output_roll + pid_output_yaw; //Berechnung ESC-4-Impuls (vorne-links)
if (battery_voltage < 1240 && battery_voltage > 800){ //Batterie angeschlossen?
esc_1 += esc_1 * ((1240 - battery_voltage)/(float)3500); //Kompensation des ESC-1-Impuls für Spannungsabfall
esc_2 += esc_2 * ((1240 - battery_voltage)/(float)3500); //Kompensation des ESC-2-Impuls für Spannungsabfall
esc_3 += esc_3 * ((1240 - battery_voltage)/(float)3500); //Kompensation des ESC-3-Impuls für Spannungsabfall
esc_4 += esc_4 * ((1240 - battery_voltage)/(float)3500); //Kompensation des ESC-4-Impuls für Spannungsabfall
}
if (esc_1 < 1100) esc_1 = 1100; //Motoren laufen lassen
if (esc_2 < 1100) esc_2 = 1100; //Motoren laufen lassen
if (esc_3 < 1100) esc_3 = 1100; //Motoren laufen lassen
if (esc_4 < 1100) esc_4 = 1100; //Motoren laufen lassen
if(esc_1 > 2000)esc_1 = 2000; //Begrenzung des ESC-1-Impulse auf 2000us
if(esc_2 > 2000)esc_2 = 2000; //Begrenzung des ESC-2-Impulse auf 2000us
if(esc_3 > 2000)esc_3 = 2000; //Begrenzung des ESC-3-Impulse auf 2000us
if(esc_4 > 2000)esc_4 = 2000; //Begrenzung des ESC-4-Impulse auf 2000us
}
else{
esc_1 = 1000; //Start nicht 2, dann 1000us-Impuls für ess-1
esc_2 = 1000; //Start nicht 2, dann 1000us-Impuls für ess-2.
esc_3 = 1000; //Start nicht 2, dann 1000us-Impuls für ess-3.
esc_4 = 1000; //Start nicht 2, dann 1000us-Impuls für ess-4.
}
////////////////////////////////////////////////////////////////////////////////////////////////////
//Erstellen der Impulse für ESC's (https://youtu.be/fqEkVcqxtU8)
////////////////////////////////////////////////////////////////////////////////////////////////////
//WICHTIG: Durch die Winkelberechnung wird die Schleifenzeit sehr wichtig und diese beträgt im vorliegenden Code 4000us!!!
if(micros() - loop_timer > 4050)digitalWrite(12, HIGH); //LED ein, wenn Schleifenzeit > 4050us
//Bildwiederholrate beträgt 250Hz - alle ESC's brauchen alle 4ms einen Impuls
while(micros() - loop_timer < 4000); //Warten bis 4000us überschritten ist
loop_timer = micros(); //Timer für nächste Loop setzen
PORTD |= B11110000; //Digital outputs 4,5,6 und 7 ein
timer_channel_1 = esc_1 + loop_timer; //Berechnung (Zeit) der fallenden Flanke des ESC-1-Impulse
timer_channel_2 = esc_2 + loop_timer; //Berechnung (Zeit) der fallenden Flanke des ESC-2-Impulse
timer_channel_3 = esc_3 + loop_timer; //Berechnung (Zeit) der fallenden Flanke des ESC-3-Impulse
timer_channel_4 = esc_4 + loop_timer; //Berechnung (Zeit) der fallenden Flanke des ESC-4-Impulse
//Freie Zeit von 1000us nutzen --> aktuelle Gyro + Empfängerdaten nehmen und in Grad pro Sekunde für PID-Berechnungen skalieren
gyro_signalen();
while(PORTD >= 16){ //Loop solange durchführen bis Ausgänge 4,5,6 und 7 aus sin
esc_loop_timer = micros(); //Aktuelle Zeit auslesen
if(timer_channel_1 <= esc_loop_timer)PORTD &= B11101111; //Digital Ausgang 4 aus, wenn Zeit abgelaufen ist
if(timer_channel_2 <= esc_loop_timer)PORTD &= B11011111; //Digital Ausgang 5 aus, wenn Zeit abgelaufen ist
if(timer_channel_3 <= esc_loop_timer)PORTD &= B10111111; //Digital Ausgang 6 aus, wenn Zeit abgelaufen ist
if(timer_channel_4 <= esc_loop_timer)PORTD &= B01111111; //Digital Ausgang 7 aus, wenn Zeit abgelaufen ist
}
///GPS-Modul
while(gpsSerial.available()){ //GPS-Daten prüfen
if(gps.encode(gpsSerial.read())){ //GPS-Daten verschlüsseln
gps.f_get_position(&lat,&lon);
//Displayposition
lcd.clear();
lcd.setCursor(1,0);
lcd.print("GPS Signal");
//Serial.print("Position: ");
//Serial.print("Latitude:");
//Serial.print(lat,6);
//Serial.print(";");
//Serial.print("Longitude:");
//Serial.println(lon,6);
lcd.setCursor(1,0);
lcd.print("LAT:");
lcd.setCursor(5,0);
lcd.print(lat);
//Serial.print(lat)
//Serial.print(" ");
lcd.setCursor(0,1);
lcd.print(",LON:");
lcd.setCursor(5,1);
lcd.print(lon);
}
}
String latitude = String(lat,6);
String longitude = String(lon,6);
Serial.println(latitude+";"+longitude);
delay(1000);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Routine, wenn Eingänge 8, 9, 10 oder 11 ihren Zustand ändern --> Auslesen des Empfängersignals (https://youtu.be/bENjl1KQbvo)
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ISR(PCINT0_vect){
current_time = micros();
//Channel 1=========================================
if(PINB & B00000001){ //Eingang 8 high?
if(last_channel_1 == 0){ //Eingang 8 von 0 auf 1
last_channel_1 = 1; //Erinnerung an aktuellen Eingangszustand
timer_1 = current_time; //Setzen von timer_1 auf current_time
}
}
else if(last_channel_1 == 1){ //Eingang 8 nicht ein und nicht von 0 auf 1
last_channel_1 = 0; //Erinnerung an aktuellen Eingangszustand
receiver_input[1] = current_time - timer_1; //Channel 1 = current_time - timer_1
}
//Channel 2=========================================
if(PINB & B00000010 ){ //Eingang 9 high?
if(last_channel_2 == 0){ //Eingang 9 von 0 auf 1
last_channel_2 = 1; //Erinnerung an aktuellen Eingangszustand
timer_2 = current_time; //Setzen von timer_2 auf current_time
}
}
else if(last_channel_2 == 1){ //Eingang 9 nicht ein und nicht von 0 auf 1
last_channel_2 = 0; //Erinnerung an aktuellen Eingangszustand
receiver_input[2] = current_time - timer_2; //Channel 2 = current_time - timer_2
}
//Channel 3=========================================
if(PINB & B00000100 ){ //Eingang 10 high?
if(last_channel_3 == 0){ //Eingang 10 von 0 auf 1
last_channel_3 = 1; //Erinnerung an aktuellen Eingangszustand
timer_3 = current_time; //Setzen von timer_3 auf current_time
}
}
else if(last_channel_3 == 1){ //Eingang 10 nicht ein und nicht von 0 auf 1
last_channel_3 = 0; //Erinnerung an aktuellen Eingangszustand
receiver_input[3] = current_time - timer_3; //Channel 3 = current_time - timer_3
}
//Channel 4=========================================
if(PINB & B00001000 ){ //Eingang 11 high?
if(last_channel_4 == 0){ //Eingang 11 von 0 auf 1
last_channel_4 = 1; //Erinnerung an aktuellen Eingangszustand
timer_4 = current_time; //Setzen von timer_4 auf current_time
}
}
else if(last_channel_4 == 1){ //Eingang 11 nicht ein und nicht von 0 auf 1
last_channel_4 = 0; //Erinnerung an aktuellen Eingangszustand
receiver_input[4] = current_time - timer_4; //Channel 4 = current_time - timer_4
}
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Subroutine zum Auslesen des Gyro
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void gyro_signalen(){
//MPU-6050 auslesen
if(eeprom_data[31] == 1){
Wire.beginTransmission(gyro_address); //Start der Kommunikation mit dem Gyro
Wire.write(0x3B); //Start des Lesens von Register 43h und automatische Erhöhung bei jedem Lesen
Wire.endTransmission(); //Ende der Übertragung
Wire.requestFrom(gyro_address,14); //Abfrage von 14 Bytes vom Gyro
receiver_input_channel_1 = convert_receiver_channel(1); //Aktuelle Empfängersignale für Pitch auf den Standard 1000-2000us konvertieren
receiver_input_channel_2 = convert_receiver_channel(2); //Aktuelle Empfängersignale für Roll auf den Standard 1000-2000us konvertieren
receiver_input_channel_3 = convert_receiver_channel(3); //Aktuelle Empfängersignale für Gas auf den Standard 1000-2000us konvertieren
receiver_input_channel_4 = convert_receiver_channel(4); //Aktuelle Empfängersignale für Gier auf den Standard 1000-2000us konvertieren
while(Wire.available() < 14); //Warten bis 14 Bytes empfangen sind
acc_axis[1] = Wire.read()<<8|Wire.read(); //Hinzufügen des Low- und High-Bytes in die Variable acc_x
acc_axis[2] = Wire.read()<<8|Wire.read(); //Hinzufügen des Low- und High-Bytes in die Variable acc_y
acc_axis[3] = Wire.read()<<8|Wire.read(); //Hinzufügen des Low- und High-Bytes in die Variable acc_z
temperature = Wire.read()<<8|Wire.read(); //Hinzufügen des Low- und High-Bytes zur Temperatur-Variable
gyro_axis[1] = Wire.read()<<8|Wire.read(); //Lesen der High- und Low-Teil der Winkeldaten
gyro_axis[2] = Wire.read()<<8|Wire.read(); //Lesen der High- und Low-Teil der Winkeldaten
gyro_axis[3] = Wire.read()<<8|Wire.read(); //Lesen der High- und Low-Teil der Winkeldaten
}
if(cal_int == 2000){
gyro_axis[1] -= gyro_axis_cal[1]; //Kompensation nur nach der Kalibrierung
gyro_axis[2] -= gyro_axis_cal[2]; //Kompensation nur nach der Kalibrierung
gyro_axis[3] -= gyro_axis_cal[3]; //Kompensation nur nach der Kalibrierung
}
gyro_roll = gyro_axis[eeprom_data[28] & 0b00000011]; //gyro_roll auf die richtige Achse setzen (gespreichert im EEPROM)
if(eeprom_data[28] & 0b10000000)gyro_roll *= -1; //gyro_roll invertieren, wenn MSB von EEPROM-Bit 28 gesetzt ist
gyro_pitch = gyro_axis[eeprom_data[29] & 0b00000011]; //gyro_pitch auf die richtige Achse setzen (gespeichert im EEPROM)
if(eeprom_data[29] & 0b10000000)gyro_pitch *= -1; //gyro_pitch invertieren, wenn MSB von EEPROM-Bit 29 gesetzt ist
gyro_yaw = gyro_axis[eeprom_data[30] & 0b00000011]; //gyro_yaw auf die richtige Achse setzen (gespeichert im EEPROM)
if(eeprom_data[30] & 0b10000000)gyro_yaw *= -1; //gyro_yaw invertieren, wenn MSB von EEPROM-Bit 30 gesetzt ist
acc_x = acc_axis[eeprom_data[29] & 0b00000011]; //acc_x auf die richtige Achse setzen (gespreichert im EEPROM)
if(eeprom_data[29] & 0b10000000)acc_x *= -1; //acc_x invertieren, wenn MSB von EEPROM-Bit 29 gesetzt ist
acc_y = acc_axis[eeprom_data[28] & 0b00000011]; //acc_y auf die richtige Achse setzen (gespreichert im EEPROM)
if(eeprom_data[28] & 0b10000000)acc_y *= -1; //acc_y invertieren, wenn MSB von EEPROM-Bit 28 gesetzt ist
acc_z = acc_axis[eeprom_data[30] & 0b00000011]; //acc_z auf die richtige Achse setzen (gespreichert im EEPROM)
if(eeprom_data[30] & 0b10000000)acc_z *= -1; //acc_z invertieren, wenn MSB von EEPROM-Bit 30 gesetzt ist
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Subroutine zur Berechnung der PID-Ausgänge (https://youtu.be/JBvnB0279-Q )
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void calculate_pid(){
//Roll-Berechnungen
pid_error_temp = gyro_roll_input - pid_roll_setpoint;
pid_i_mem_roll += pid_i_gain_roll * pid_error_temp;
if(pid_i_mem_roll > pid_max_roll)pid_i_mem_roll = pid_max_roll;
else if(pid_i_mem_roll < pid_max_roll * -1)pid_i_mem_roll = pid_max_roll * -1;
pid_output_roll = pid_p_gain_roll * pid_error_temp + pid_i_mem_roll + pid_d_gain_roll * (pid_error_temp - pid_last_roll_d_error);
if(pid_output_roll > pid_max_roll)pid_output_roll = pid_max_roll;
else if(pid_output_roll < pid_max_roll * -1)pid_output_roll = pid_max_roll * -1;
pid_last_roll_d_error = pid_error_temp;
//Pitch-Berechnungen
pid_error_temp = gyro_pitch_input - pid_pitch_setpoint;
pid_i_mem_pitch += pid_i_gain_pitch * pid_error_temp;
if(pid_i_mem_pitch > pid_max_pitch)pid_i_mem_pitch = pid_max_pitch;
else if(pid_i_mem_pitch < pid_max_pitch * -1)pid_i_mem_pitch = pid_max_pitch * -1;
pid_output_pitch = pid_p_gain_pitch * pid_error_temp + pid_i_mem_pitch + pid_d_gain_pitch * (pid_error_temp - pid_last_pitch_d_error);
if(pid_output_pitch > pid_max_pitch)pid_output_pitch = pid_max_pitch;
else if(pid_output_pitch < pid_max_pitch * -1)pid_output_pitch = pid_max_pitch * -1;
pid_last_pitch_d_error = pid_error_temp;
//Yaw-Berechnungen
pid_error_temp = gyro_yaw_input - pid_yaw_setpoint;
pid_i_mem_yaw += pid_i_gain_yaw * pid_error_temp;
if(pid_i_mem_yaw > pid_max_yaw)pid_i_mem_yaw = pid_max_yaw;
else if(pid_i_mem_yaw < pid_max_yaw * -1)pid_i_mem_yaw = pid_max_yaw * -1;
pid_output_yaw = pid_p_gain_yaw * pid_error_temp + pid_i_mem_yaw + pid_d_gain_yaw * (pid_error_temp - pid_last_yaw_d_error);
if(pid_output_yaw > pid_max_yaw)pid_output_yaw = pid_max_yaw;
else if(pid_output_yaw < pid_max_yaw * -1)pid_output_yaw = pid_max_yaw * -1;
pid_last_yaw_d_error = pid_error_temp;
}
//Umwandlung der Empfängersignale in einen standardisierten 1000–1500–2000ms Wert (Verwendung der gespeicherten Daten aus EEPROM)
int convert_receiver_channel(byte function){
byte channel, reverse; //Deklaration von lokalen Variablen
int low, center, high, actual;
int difference;
channel = eeprom_data[function + 23] & 0b00000111; //Kanalzuweisung der jeweiligen Funktion
if(eeprom_data[function + 23] & 0b10000000)reverse = 1; //Reverse-Kanal, wenn der höchtwertige Bit erreicht ist
else reverse = 0; //Kein höchstwertiges Bit, dann keine Umkehrung
actual = receiver_input[channel]; //Lesen des aktuellen Empfängerwerts für entsprechende Funktion
low = (eeprom_data[channel * 2 + 15] << 8) | eeprom_data[channel * 2 + 14]; //Speichern des Low-Werts für den jeweiligen Empfänger-Eingangssignals
center = (eeprom_data[channel * 2 - 1] << 8) | eeprom_data[channel * 2 - 2]; //Speichern des Center-Werts für den jeweiligen Empfänger-Eingangssignals
high = (eeprom_data[channel * 2 + 7] << 8) | eeprom_data[channel * 2 + 6]; //Speichern des High-Werts für den jeweiligen Empfänger-Eingangssignals
if(actual < center){ //Aktuelle Empfängerwert ist kleiner als der Mittelwert
if(actual < low)actual = low; //Begrenzung des niedrigsten Wertes (Wert aus der Einrichtung)
difference = ((long)(center - actual) * (long)500) / (center - low); //Berechnung und Skalierung des aktuellen Wertes auf 1000-2000us
if(reverse == 1)return 1500 + difference; //Wenn der Kanal invertiert ist
else return 1500 - difference; //Wenn der Kanal nicht invertiert ist
}
else if(actual > center){ //Aktuelle Empfängerwert ist größer als der Mittelwert
if(actual > high)actual = high; //Begrenzung des niedrigsten Wertes (Wert aus der Einrichtung)
difference = ((long)(actual - center) * (long)500) / (high - center); //Berechnung und Skalierung des aktuellen Wertes auf 1000-2000us
if(reverse == 1)return 1500 - difference; //Wenn der Kanal invertiert ist
else return 1500 + difference; //Wenn der Kanal nicht invertiert ist
}
else return 1500;
}
void set_gyro_registers(){
//Einstellung der MPU-6050
if(eeprom_data[31] == 1){
Wire.beginTransmission(gyro_address); //Start der Kommunikation mit der gefundenen Adresse (nach Suche)
Wire.write(0x6B); //Schreiben in das PWR_MGMT_1 Register (6B hex)
Wire.write(0x00); //Register-Bits auf 00000000 setzen zur Aktivierung des Gyro
Wire.endTransmission(); //Ende der Übertragung des Gyro
Wire.beginTransmission(gyro_address); //Start der Kommunikation mit der gefundenen Adresse (nach Suche)
Wire.write(0x1B); //Schreiben in das GYRO_CONFIG Register (1B hex)
Wire.write(0x08); //RegisterBits auf 00001000 setzen (500dps full scale)
Wire.endTransmission(); //Ende der Übertragung des Gyro
Wire.beginTransmission(gyro_address); //Start der Kommunikation mit der gefundenen Adresse (nach Suche)
Wire.write(0x1C); //Schreiben in das ACCEL_CONFIG Register (1A hex)
Wire.write(0x10); //Register-Bits auf 00010000 setzen (+/- 8g full scale range)
Wire.endTransmission(); //Ende der Übertragung des Gyro
//Stichprobenartige Registerprüfung zur Überprüfung der Werte (sind die Werte korrekt?)
Wire.beginTransmission(gyro_address); //Start der Kommunikation mit der gefundenen Adresse (nach Suche)
Wire.write(0x1B); //Start des Lesens in Register 0x1B
Wire.endTransmission(); //Ende der Übertragung
Wire.requestFrom(gyro_address, 1); //1 Byte vom Gyro anfordern
while(Wire.available() < 1); //Warten bis 6 Bytes empfangen sind
if(Wire.read() != 0x08){ //Überprüfung, ob der Wert 0x08 ist
digitalWrite(12,HIGH); //LED ein
while(1)delay(10); //Für immer in Loop warten
}
Wire.beginTransmission(gyro_address); //Start der Kommunikation mit der gefundenen Adresse (nach Suche)
Wire.write(0x1A); //Schreiben in das CONFIG Register (1A hex)
Wire.write(0x03); //RegisterBits auf 00000011 setzen (Set Digital Low Pass Filter to ~43Hz)
Wire.endTransmission(); //Ende der Übertragung des Gyro
}
}
