Hello,
ich habe mir euren Code für das Sensormodule (Kompass, Acc, Gyro) durchgesehen,
da ich ziemlich Probleme hatte vernünftige Werte vom Kompass zu erhalten. Das
hat sich mittlerweile zum Glück erledigt, von eurem Code und Vorgehensweise habe
ich jedoch recht viel profitiert. Deswegen versuche ich Euch auch an meinen
Erkenntnissen teilzuhaben.
1. Schaut euch mal die SoftTimer Library an. Die ist wirklich gut geschrieben,
einfach zu handhaben und fehlerfrei. Die funktioniert so wie man sich das
vorstellen würde. Damit kann man Tasks, die in regelmäßigen Abständen ablaufen,
sehr schön modelieren. Der Sourcecode wird dadurch verständlicher, leichter
modifizierbar und übersichtlicher.
2. Meines Erachtens macht ihr die Abfrage vom Gyro falsch. Der Gyro mißt laufend
die Drehgeschwindigkeit um 3 Achsen. Will man nun wissen, wie weit sich ein
Körper gedreht hat, muss man die Messwerte summieren und mit dem Zeitintervall
multiplizieren. Soweit ist ja noch alles klar. Nur darf man natürlich keine
Messwerte überlesen, das Zeitintervall sollte möglichst klein sein, damit das
Messergebnis genau wird und man sollte immer einen Augenblick später die Daten
vom Gyro einlesen, wenn dieser neue Daten ermittelt hat. Keine einfache Aufgabe,
möchte man die Leseschleife selber timen. Aber es gibt einen Trick:
Man initialisiert den Gyro für 400Hz data output rate und er soll im Streaming
mode die Daten in den FIFO Buffer (32 Werte maximal) schreiben.
Fragt man nun den Gyro ca. alle 20ms ab, sind 7 bis 9 Werte im FIFO Buffer. Will
man die Werte auslesen, liest man zuerst das FIFO_SRC_REG, dort steht nämlich
wie viele Werte gerade im FIFO Buffer gespeichert sind und ob vielleicht ein
Überlauf passiert ist. Im nächsten Schritt liest man die n Werte aus dem FIFO
Buffer aus, und summiert sie.
Möchte man, so wie ich das hier mache, auf einen Rutsch bis zu 64 Bytes von der
I2C Schnittstelle einlesen, muss man die internen Buffer in der
Arduino Library
Wire vergrößern (die alles andere als gut programmiert ist, das ist aber eine
andere Geschichte)...
hier ein Ausschnitt aus meiner Gyro Lese Routine:
Code:
struct {
uint8_t out_x_l;
uint8_t out_x_h;
uint8_t out_y_l;
uint8_t out_y_h;
uint8_t out_z_l;
uint8_t out_z_h;
} gyroData[32];
uint8_t fifoSrcReg = 0;
read(FIFO_SRC_REG, &fifoSrcReg, sizeof(fifoSrcReg)); // read the FIFO_SRC_REG
// FIFO_SRC_REG
// 7: Watermark status. (0: FIFO filling is lower than WTM level; 1: FIFO filling is equal or higher than WTM level)
// 6: Overrun bit status. (0: FIFO is not completely filled; 1:FIFO is completely filled)
// 5: FIFO empty bit. (0: FIFO not empty; 1: FIFO empty)
// 4..0: FIFO stored data level
//Serial.print("FIFO_SRC_REG: "); Serial.println(fifoSrcReg, HEX);
uint8_t countOfData = (fifoSrcReg & 0x1F) + 1;
*pCountOfSamples = countOfData;
if (bitRead(fifoSrcReg, 6)==1)
printf_P(PSTR("FIFO overrun FIFO_SRC_REG= 0x%02xn"), fifoSrcReg);
memset(gyroData, 0, sizeof(gyroData[0])*32);
read(0xA8, (uint8_t *)gyroData, sizeof(gyroData[0])*countOfData); // the first bit of the register address specifies we want automatical address increment
gyroRaw.xAxis = gyroRaw.yAxis = gyroRaw.zAxis = 0;
for (uint8_t i=0; i<countOfData; i++)
{
gyroRaw.xAxis += ((gyroData[i].out_x_h << 8) | gyroData[i].out_x_l) + offsetX;
gyroRaw.yAxis += ((gyroData[i].out_y_h << 8) | gyroData[i].out_y_l) + offsetY;
gyroRaw.zAxis += ((gyroData[i].out_z_h << 8) | gyroData[i].out_z_l) + offsetZ;
}
//printf("L3G4200D raw data: %5d %5d %5dn", gyroRaw.XAxis, gyroRaw.YAxis, gyroRaw.ZAxis);
Warum ist der Code mit dem Quaternionen von Madgwick auskommentiert? Funktioniert das doch nicht so gut?
lg
Hannes
p.s. ich verwende das GY80 Modul als Input für einen Autopiloten für Segelschiffe. Die 400 sm von Tonga nach Fiji hat er schon geschafft...
