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
}
}