DIY Quadcopter Gyro etc Problem

Hallöchen,

ich baue mir z.Zt. eine Drohne bzw. einen Quadcopter.
Da ich alles selbst zusammenschraube und programmiere, möchte ich keine fertigen Skripts nutzen.
(Bis auf die, die jetzt schon benutze und vielleicht 2 oder 3 mehr).
Im Moment bin ich dabei die Funktion für das Stabilisieren der Drohne zu schreiben und habe auch schon große Fortschritte erziehlt.
Jedoch ist mein Problem, daß die Lesegeschwindigkeit des Gyros MPU-6050 zu langsam ist damit der Copter die Balance hält.

Habe schon gegoogelt wie blöd, aber leider nix gefunden.

Habe den Code soweit es ging ohne delay() gebastelt und gemerkt, daß die Werte trotzdem nur 1x/s aktualisiert bzw. eingelesen werden.
Ich brauche aber eine schnellere Frequenz: ca. 10x/s.
Habe schon in die Kalman.h und die Servo.h geschaut, aber nichts entsprechendes gefunden.
Ich versuchte die Wire.h zu modifizieren, aber ich finde sie erst gar nicht!

Im Prinzip funktioniert mein Sketch ja, aber die ESCs bzw Motoren reagieren auf Grund des Timings zu langsam/spät!

Bin also ein wenig ratlos... und wäre sehr dankbar wenn mir jemand einen Tipp geben könnte.
LG, Florian

Danke im Voraus!

Anbei mein Sketch:

// Libs includieren

#include <Wire.h>

#include <Servo.h>

#include "Kalman.h"

#include <TinyGPS.h>

#include <SoftwareSerial.h>

// Servos definieren  
 
Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;

// Drehzahl min, max, mid - Werte

const int thrMax = 540;
const int thrMin = 27;
const int thrMid = 100;  // Muss noch getestet werden

// Servosteuerung Pinbelegung

const int motxf = 6;
const int motxr = 10;
const int motyf = 5;
const int motyr = 9;

Kalman kalmanX; 
Kalman kalmanY;

// IMU Data 
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

int accXangle, accYangle; // Angle calculate using the accelerometer
int temp; // Temperature
int gyroXangle, gyroYangle; // Angle calculate using the gyro
int compAngleX, compAngleY; // Calculate the angle using a complementary filter
int kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

// X- und Y-Achsen definieren

const int defAngX = 180;  // zum Kalibrieren leer lassen
// int defAngY = 180;  // zum Kalibrieren leer lassen


uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data 

unsigned long fix_age;

SoftwareSerial GPS(2,3);
TinyGPS gps;
void gpsdump(TinyGPS &gps);
bool feedgps();
void getGPS();
long lat, lon;
float LAT, LON;



void setup()

{
  
// ESCs Pinbelegung definieren & vorbereiten
  
  
esc1.attach(motxf);
esc1.write(thrMin);
delay(1000);
esc2.attach(motxr);
esc2.write(thrMin);
delay(1000);
esc3.attach(motyf);
esc3.write(thrMin);
delay(1000);
esc4.attach(motyr);
esc4.write(thrMin);
delay(1000);
  
  GPS.begin(9600);
  
  Serial.begin(9600);
  Wire.begin();
  i2cData[0] = 4; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode 
  
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
  
  delay(100); // Wait for sensor to stabilize
  
  // Set kalman and gyro starting angle 
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) 
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
  
  timer = micros();



}
 
void loop()

{
  

  

int throttle = 50;


if (accXangle < 177)   //gyro,comp,kal    X Angle ???
  {
    while(accXangle<177)
    {
     
      esc1.write(throttle-10);
      
      } 
    
    delay(50);
  }else if(accXangle > 183)
  { 
    while(accXangle>183)
    {
   
    esc1.write(throttle+10);
      
    }
   delay(50); 
  }else
  {
   esc1.write(throttle); 
delay(50);
  }

  
  
  
  
  /*
  if (accXangle < 177)
  {
    esc2.write(throttle+3);
    delay(50);
  }else if(accXangle > 183)
  { 
    esc2.write(throttle-3); 
   delay(50); 
  }else
  {
   esc2.write(throttle); 
delay(50);
  }
  
  
  
  if (accYangle < 177)
  {
    esc3.write(throttle-3);
    delay(50);
  }else if(accYangle > 183)
  { 
    esc3.write(throttle+3); 
   delay(50); 
  }else
  {
   esc3.write(throttle); 
delay(50);
  }
  
  if (accYangle < 177)
  {
    esc4.write(throttle+3);
    delay(50);
  }else if(accYangle > 183)
  { 
    esc4.write(throttle-3); 
   delay(50); 
  }else
  {
   esc4.write(throttle); 
delay(50);
  }
*/
  

  // Update all the values   
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);  
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
  // atan2 outputs the value of -? to ? (radians)
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  
  int gyroXrate = (int)gyroX/131.0;
  int gyroYrate = -((int)gyroY/131.0);
  gyroXangle += gyroXrate*((int)(micros()-timer)/1000000); // Calculate gyro angle without any filter  
  gyroYangle += gyroYrate*((int)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((int)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((int)(micros()-timer)/1000000);
  
  compAngleX = (0.93*(compAngleX+(gyroXrate*(int)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(int)(micros()-timer)/1000000)))+(0.07*accYangle);
  
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (int)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (int)(micros()-timer)/1000000);
  timer = micros();
  
  temp = ((int)tempRaw + 12412.0) / 340.0;
   
  long lat, lon;
  unsigned long fix_age, time, date, speed, course;
  unsigned long chars;
  unsigned short sentences, failed_checksum;

  // retrieves +/- lat/long in 100000ths of a degree
  gps.get_position(&lat, &lon, &fix_age);

  getGPS();
  
  Serial.print("Latitude : ");
  Serial.print(LAT/100000,7);
  Serial.print(" :: Longitude : ");
  Serial.println(LON/100000,7);
  



  

}



void getGPS(){
  bool newdata = false;
  unsigned long start = millis();
  // Every 1 seconds we print an update
  while (millis() - start < 1000)
  {
    if (feedgps ()){
      newdata = true;
    }
  }
  if (newdata)
  {
    gpsdump(gps);
  }
}

bool feedgps(){
  while (GPS.available())
  {
    if (gps.encode(GPS.read()))
      return true;
  }
  return 0;
}

void gpsdump(TinyGPS &gps)
{
  //byte month, day, hour, minute, second, hundredths;
  gps.get_position(&lat, &lon);
  LAT = lat;
  LON = lon;
  {
    feedgps(); // If we don't feed the gps during this long routine, we may drop characters and get checksum errors
  }
}

Dein Problem sind die Delays, die du im Loop hast. Dadurch dass in jeder if - Abfrage ein Delay kommt, muss alles danach in der Loop Schleife warten. Da das wie gesagt in jeder ist, hast du ein "`Gesamtdelay" bis die Loop Schleife wieder anfängt von ungefähr 4*50ms (kann auch sein, dass ich mich verzählt hab;) ). Sowas ist immer zu vermeiden, vor allem dann wenn du zeitkritische Operationen ausführst.

Wenn du nicht willst das eine Funktion ständig ausgeführt wird, ohne die restlichen Funktionen aufzuhalten, kannst du sowas nutzen:

unsigned long millis_now = 0; //Zeitvariablen definieren
unsigned long last_run_time = 0;

void loop()
{
millis_now = millis(); // aktuelle Zeit
 if (millis_now  - last_run_time > 5) //immer nur ausführen, wenn das letzte ausfuehren länger als 5ms her ist (jetzige Zeit - letzter Ausfuehrungszeitpunkt)
 {
 last_run_time=millis_now ; //setze letzten Ausfuehrungspunkt zu der jetzigen Zeit, da wir ja jetzt die Funktion ausfuehren
 action();
 }
}

Ich hoffe mal, du verstehst was ich meine. :wink:

EDIT: Hab mich geirrt, dass war wohl nicht dein Hauptproblem. Die getGPS(); Funktion hat ne hübsche While-Schleife, die immer eine Sekunde lang ausgeführt wird. Änder die Schleife so:

unsigned long last_gps_time= 0;
void getGPS(){
  bool newdata = false;
  
  // Every 1 seconds we print an update
  if(millis() -last_gps_time >1000)
  {
last_gps_time = millis();
    if (feedgps ()){
      gpsdump(gps);
    }
  }
}

Da dort ein While stand, wurde die Schleife solang ausgeführt, bis die Bedingung erfüllt wurde, also eine Sekunde lang :wink:

Hi nussecke!
Danke für Deine Antwort!

Aber ich habe den Sketch ohne die delays() probiert. Dennoch bekomme ich die Werte des Gyros immer nur 1x/s.

Hier der Basiscode für den Gyro:

#include <Wire.h>
#include "Kalman.h" // Source: https://github.com/TKJElectronics/KalmanFilter

Kalman kalmanX; // Create the Kalman instances
Kalman kalmanY;

/* IMU Data */
int16_t accX, accY, accZ;
int16_t tempRaw;
int16_t gyroX, gyroY, gyroZ;

double accXangle, accYangle; // Angle calculate using the accelerometer
double temp; // Temperature
double gyroXangle, gyroYangle; // Angle calculate using the gyro
double compAngleX, compAngleY; // Calculate the angle using a complementary filter
double kalAngleX, kalAngleY; // Calculate the angle using a Kalman filter

uint32_t timer;
uint8_t i2cData[14]; // Buffer for I2C data

void setup() {  

  Serial.begin(9600);
  Wire.begin();
  i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
  i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
  i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
  i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
  while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
  while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode 
  
  while(i2cRead(0x75,i2cData,1));
  if(i2cData[0] != 0x68) { // Read "WHO_AM_I" register
    Serial.print(F("Error reading sensor"));
    while(1);
  }
  
  delay(100); // Wait for sensor to stabilize
  
  /* Set kalman and gyro starting angle */
  while(i2cRead(0x3B,i2cData,6));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  
  kalmanX.setAngle(accXangle); // Set starting angle
  kalmanY.setAngle(accYangle);
  gyroXangle = accXangle;
  gyroYangle = accYangle;
  compAngleX = accXangle;
  compAngleY = accYangle;
  
  timer = micros();
}

void loop() {
  
  
  /* Update all the values */  
  while(i2cRead(0x3B,i2cData,14));
  accX = ((i2cData[0] << 8) | i2cData[1]);
  accY = ((i2cData[2] << 8) | i2cData[3]);
  accZ = ((i2cData[4] << 8) | i2cData[5]);
  tempRaw = ((i2cData[6] << 8) | i2cData[7]);  
  gyroX = ((i2cData[8] << 8) | i2cData[9]);
  gyroY = ((i2cData[10] << 8) | i2cData[11]);
  gyroZ = ((i2cData[12] << 8) | i2cData[13]);
  
  // atan2 outputs the value of -? to ? (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We then convert it to 0 to 2? and then from radians to degrees
  accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
  accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
  
  double gyroXrate = (double)gyroX/131.0;
  double gyroYrate = -((double)gyroY/131.0);
  gyroXangle += gyroXrate*((double)(micros()-timer)/1000000); // Calculate gyro angle without any filter  
  gyroYangle += gyroYrate*((double)(micros()-timer)/1000000);
  //gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
  //gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
  
  compAngleX = (0.93*(compAngleX+(gyroXrate*(double)(micros()-timer)/1000000)))+(0.07*accXangle); // Calculate the angle using a Complimentary filter
  compAngleY = (0.93*(compAngleY+(gyroYrate*(double)(micros()-timer)/1000000)))+(0.07*accYangle);
  
  kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
  kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
  timer = micros();
  
  temp = ((double)tempRaw + 12412.0) / 340.0;
  

  
  delay(1);
}

Serialmonitor gibt trotzdem nur sekündlich die Werte aus! :frowning:

geezy82:
Jedoch ist mein Problem, daß die Lesegeschwindigkeit des Gyros MPU-6050 zu langsam ist damit der Copter die Balance hält.

Dein Problem ist eher, dass Du Endlosschleifen schreibst, die nie wieder aufhören zu laufen, schau Dir z.B. mal folgenden Codeabschnitt an:

while(accXangle<177)
{
esc1.write(throttle-10);
}

D.h. wenn beim Eintritt in die Schleife die Bedinung "accXangle<177" erfüllt ist, wird irgendwas gemacht. Und zwar wieder und wieder und wieder. Da sich an der Bedingung innerhalb der Schleife nichts ändert, also insbesondere kein neuer Wert an accXangle zugewiesen wird, kommt der Code nie mehr aus dieser Schleife heraus, nachdem er dort einmal hineingeraten ist.

Und von solchen Endlosschleifen hast Du reichlich im Code verbastelt. Nicht nur eine, sondern viele.

Dein ganzer Sketch ist momentan mehr auf einer geistig-philosophischen als auf einer fliegerischen Ebene zu sehen, Dein Projekt könnte gut das Motto tragen:

Die Drohne - auf der Suche nach dem Gleichgewicht zwischen Gegenwart und Ewigkeit.

geezy82:
...
Serialmonitor gibt trotzdem nur sekündlich die Werte aus! :frowning:

Bei dem Code den du hier gepostet hast?
Da sehe ich kein Print.. :wink:
Rein interessenhalber würde ich zwei Ausgaben je loop schleife- Schleife mal testen, um die while(i2cRead(0x3B,i2cData,14)); daran könnts vielleicht liegen, da das ja bis auf die atan2 Funktion die einzige Funktion ist worans dann sonst noch liegen könnte.

Serial.println("Vorher");
while(i2cRead(0x3B,i2cData,14));
Serial.println("Nacher");

Oder alternativ du lässt das while außenrum weg, weil falls nix ankommt hälts nur deine Funktion auf.

Hallo jurs!

Danke für Deine Antwort!

Der Code funktioniert so wie er geschrieben ist. Die while-Schleife läuft nicht endlos!
Das Problem ist, daß die Werte des Gyros nur sekündlich ausgelesen werden.
Ich habe keine signifikanten delays() eingebaut die diese Verzögerung rechtfertigen.
Es ist doch logisch, daß ich mit if-Anweisung auslese, ob der Winkel stimmt und dann mit while korrigiere.
Das ganze dauert maximal den 50ms. Aber da mein Gyro nur 1x/s ausgelesen wird, wird dementsprechend auch die RPM
nur sekündlich korrigiert.

@nussecke: Vielen Dank! Ich probiers mal mit Deinem Code! :slight_smile:

Naja Jurs hat schon recht, angenommen dein Quadrocopter ist extrem schräg, dann versucht er erst den einen Rotor anzupassen und dann erst den nächsten. Das Problem ist hier das dein Quadrocopter eventuell ungleichmäßig belastet ist und die Verstellung von +-10 nicht ausreicht um ihn zurück die Ausgangslage zu bringen, dann würde die Whileschleife dauerhaft ausgeführt werden und die anderen rotoren würden nix mitbekommen. Ein anderes Problem bei deiner Art der Regelung ist, dass das System sollte es funktionieren stark überschwingt oder fast gar nicht reagiert, weil der Puls bei einer kleinen wie auch großen Abweichung gleich groß ist.
Also wäre es hier ratsam z.b. mindestens einen P-Regler benutzen, also eine Winkelabhängige (new_throttle = throttle +winkelabweichung*verstärkungsfaktor) Verstellung deiner Stellgröße (Hier das PWM Signal ans ESC) und das dann alle beispielsweise 10 ms wieder neu anzupassen ohne While schleife, solang bis das System sich wieder in einer Gleichgewichtslage befindet.

Mal ne andere Frage, wo hastn eigentlich die readi2c Funktion her, hast du dir die selbst programmiert, weil in der WIRE Library hab ich die nicht gefunden. Wenn ja poste bitte mal deinen Code dazu.

Mal aus Neugier: wie verhindert man bei Testen, dass einem der Quadrocopter Manhack-artig ins Gesicht fliegt? Man kann ja auch nicht einfach aus der Ferne den Strom abdrehen machen wenn was nicht klappt

Gute Frage, hab ich mir damals auch gestellt als ich an so nem Teil gebastelt hab. Wollte am Anfang auch nicht raus zum Testen und die Autos in der Umgebung in Gefahr bringen, daher hab ich erstmal nach ner Indoorlösung gesucht
Hab dann zur komplizierten Lösung gegriffen und mittels einem Kugelgelenk an einem in Hochrichtung beweglich Stab, der in in einem Rohr axial geführt wurd, den Quad befestigt sowie einen axialen Anschlag für den Stab vorgesehn, damit der Quad nicht abhaut. Damit konnten die Freiheitsgrade gewährleistet unddie einzelnen Achsen erstmal kalibrieren werden. Dann ist das Teil eigentlich wenig gefährlich, wenns erstmal halbwegs in Ruhe schwebt. Danach mit ner 10m Schur und Anker draußen am Boden befestigt, um Freifliegen zu testen.
Wenn das mal klappt und die Regelung bisschen Störgrößenverträglich ist geht mit der Steuerung eigentlich nicht mehr soviel schief, wenn der Pilot keine Wahnsinnsmanöver fliegen will :wink:
Strom abdrehen geht in der Tat nicht, aber du kannst über die Fernbedienung das Steuersignal zu den ESC auf das minimale PWM zu reduzieren und die Rotoren darüber stoppen. Aber wer lässt sein Flieger freiwillig dann abstürzen. :wink:

Ich bin darin jetzt echt kein Experte, aber so hab ich das Problem gelöst.

Hi @ all,

Sorry, daß ich jetzt erst antworte, war ein langer Tag.

@nussecke: Naja, Jurs hat schon irgendwie recht. Aber bin ja auch noch Noob, von daher verbessere ich meinen Code auch gern Euren Vorschlägen folgend. Hatte schon mit for-Schleife experimentiert und auch eine Verschachtelung von if-while-for. Hat aber alles nicht so geklappt - bisher. Habe gestern kurz Deinen Code versucht und die Rate ist tatsächlich besser - Vielen Dank nochmal! :slight_smile:
PS: Den Code "readi2c" habe ich von hier: GitHub - TKJElectronics/KalmanFilter: This is a Kalman filter used to calculate the angle, rate and bias from from the input of an accelerometer/magnetometer and a gyroscope.

@Serenifly: Da mein Budget nicht so groß ist, muß ich mit Deckung auskommen. ;D
Der Quad hat mich beim ersten Mal ganz gut erwischt, aber außer einer kleinen Schnittwunde am Knie ist nix passiert.
Mittlerweile habe ich ja auch die Min- und Max-RPM herausgefunden (27-540). Daher kann ich meinen Code dementsprechend
anpassen, ohne daß ich mir weitere Verletzungen zuziehe.

Servo.h

Servo esc1;
Servo esc2;
Servo esc3;
Servo esc4;

Wofür brauchst du bei einem Quadcopter einen Servo? Ich dachte, Servos bräuchte man höchstens bei einem HELIcopter für die einzelnen Rotorblätter! Oder hast du einen Roboterarm am Quadcopter?

ich vermute mal mit esc1 bis 4 sind die vier Motorregler gemeint.

Genau!
Ich steuere die ESCs mit der Servo.h-Lib. Funktioniert prima!
Allerdings musste ich die Min/Max-Werte anpassen (statt 0-1023 > 27-540).

Bin z.Zt. bissel im Stress, aber sobald ich wieder Freizeit habe, experimentiere ich weiter und teile Euch meine Fortschritte mit.