MPU 6050 Beschleunigungssensor - Programm zu langsam

[Atvilar]

Hallo,

Ich bin dabei einen Datalogger zu bauen, der aktuelle Zeit, Temperatur(en), Ausrichtung gegenüber Startpunkt und Beschleunigungen auf eine SD Karte speichern soll.
Als Prozessor verwende ich einen Arduino Uno, Der Gyro ist ein IMU6050.
Das Demoprogramm funktioniert super.

Nach einer Implementierung in mein Programm stellte ich fest, dass das Programm nur sehr langsame Winkeländerungen richtig wahrnimmt.
Mein Gedanke ist, dass mein Programm so langsam ist, dass die IMU verhältnismäßig kurz angesprochen wird. Während der anderen Zeit gehen dann Lageänderungen verloren.
Da die Kiste aber dazu gedacht ist, Stöße und Würfe zu loggen, ist das ein Problem. :)

Ich habe versucht ein Programm eines Balancer - Roboters zu verfolgen, aber ohne Erfolg.

hier mein Code:



/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// PRE SETUP

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// LIBRARIES

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

#include "Wire.h"
#include "OneWire.h"
#include "DallasTemperature.h"

//////////////////////////////////////////////////////////////////////////////////////////////////// GYROSCOPE

#include <Wire.h>
#include <MPU6050.h>

//////////////////////////////////////////////////////////////////////////////////////////////////// SD CARD READER

#include <SPI.h>
#include <SD.h>

//////////////////////////////////////////////////////////////////////////////////////////////////// Clock

#include <virtuabotixRTC.h> //Library used

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// DEFINE I0

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

#define ONE_WIRE_BUS_1 A0
OneWire ourWire1(ONE_WIRE_BUS_1);
DallasTemperature T_ext(&ourWire1);

//////////////////////////////////////////////////////////////////////////////////////////////////// GYROSCOPE

MPU6050 mpu;

//////////////////////////////////////////////////////////////////////////////////////////////////// SD CARD READER

File myFile;

//////////////////////////////////////////////////////////////////////////////////////////////////// Clock

virtuabotixRTC myRTC(6, 7, 8); //If you change the wiring change the pins here also

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// DEFINE VARIABLES

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

float T_ext_RawValue =0;

float T_ext_RawHigh = 98.4; // Rohwert gemessen bei 100°C
float T_ext_RawLow = -0.4; // Rohwert gemessen bei 0°C
float T_ext_ReferenceHigh = 98.3; // Sollwert bei 100°C
float T_ext_ReferenceLow = 0; // Sollwert bei 0°C

//////////////////////////////////////////////////////////////////////////////////////////////////// INTERNAL TEMP SENSOR (GYRO)

float T_int_RawValue =0;

float T_int_RawHigh = 100; // Rohwert gemessen bei 100°C Soll: 28,4 ..... Ist: 31,45
float T_int_RawLow = 0; // Rohwert gemessen bei 0°C
float T_int_ReferenceHigh = 100; // Sollwert bei 100°C
float T_int_ReferenceLow = 0; // Sollwert bei 0°C

//////////////////////////////////////////////////////////////////////////////////////////////////// POSITION (GYRO)

// Timers
unsigned long timer = 0;
float timeStep = 0.01;

// Pitch, Roll and Yaw values
float pitch = 0; // y
float roll = 0; // x
float yaw = 0; // z







/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// PRE CALCULATION

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

float T_ext_RawRange = T_ext_RawHigh - T_ext_RawLow;
float T_ext_ReferenceRange = T_ext_ReferenceHigh - T_ext_ReferenceLow;

//////////////////////////////////////////////////////////////////////////////////////////////////// INTERNAL TEMP SENSOR (GYRO)

float T_int_RawRange = T_int_RawHigh - T_int_RawLow;
float T_int_ReferenceRange = T_int_ReferenceHigh - T_int_ReferenceLow;





/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// SETUP

void setup() {

//////////////////////////////////////////////////////////////////////////////////////////////////// SERIAL PORT

delay(1000);
Serial.begin(9600);
Serial.println("Serial port etablished");

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

T_ext.begin();
Serial.println("External temperature sensor set up");

//////////////////////////////////////////////////////////////////////////////////////////////////// GYROSCOPE

Serial.println("Initialize MPU6050");

while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
delay(500);
}

// Calibrate gyroscope. The calibration must be at rest.
// If you don't want calibrate, comment this line.
mpu.calibrateGyro();

// Set threshold sensivty. Default 3.
// If you don't want use threshold, comment this line or set 0.
mpu.setThreshold(3);

//////////////////////////////////////////////////////////////////////////////////////////////////// SD CARD READER

Serial.print("Initializing SD card...");

if (!SD.begin(4)) {
Serial.println("initialization failed!");
while (1);
}
Serial.println("initialization done.");

// open the file. note that only one file can be open at a time,
// so you have to close this one before opening another.
myFile = SD.open("Log.txt", FILE_WRITE);
// close the file:
myFile.close();

//////////////////////////////////////////////////////////////////////////////////////////////////// PRINT HEADER TO SERIAL PORT

Serial.println(" Date / Time / T EXT / T INT / Rot X / Rot Y / Rot Z / Acc X / Acc Y");

}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// LOOP

void loop() {

//////////////////////////////////////////////////////////////////////////////////////////////////// EXTERNAL TEMP SENSOR

T_ext.requestTemperatures();
float T_ext_RawValue = T_ext.getTempCByIndex(0);

float T_ext_CorrectedValue = (((T_ext_RawValue - T_ext_RawLow) * T_ext_ReferenceRange) / T_ext_RawRange) + T_ext_ReferenceLow;

//////////////////////////////////////////////////////////////////////////////////////////////////// INTERNAL TEMP SENSOR (GYRO)

float T_int_RawValue = mpu.readTemperature();
float T_int_CorrectedValue = (((T_int_RawValue - T_int_RawLow) * T_int_ReferenceRange) / T_int_RawRange) + T_int_ReferenceLow;

//////////////////////////////////////////////////////////////////////////////////////////////////// POSITION (GYRO)

timer = millis();

// Read normalized values
Vector norm = mpu.readNormalizeGyro();

// Calculate Pitch, Roll and Yaw
pitch = pitch + norm.YAxis * timeStep;
roll = roll + norm.XAxis * timeStep;
yaw = yaw + norm.ZAxis * timeStep;

// Wait to full timeStep period
delay((timeStep*1000) - (millis() - timer));

//////////////////////////////////////////////////////////////////////////////////////////////////// ACCELERATION (GYRO)

// Read normalized values
Vector normAccel = mpu.readNormalizeAccel();

// Calculate Pitch & Roll
int acc_pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis*normAccel.YAxis + normAccel.ZAxis*normAccel.ZAxis))*180.0)/M_PI;
int acc_roll = (atan2(normAccel.YAxis, normAccel.ZAxis)*180.0)/M_PI;

//////////////////////////////////////////////////////////////////////////////////////////////////// Clock

myRTC.updateTime();

//////////////////////////////////////////////////////////////////////////////////////////////////// PRINT TO SERIAL PORT

Serial.print(myRTC.dayofmonth);
Serial.print(".");
Serial.print(myRTC.month);
Serial.print(".");
Serial.print(myRTC.year);
Serial.print(" / ");
Serial.print(myRTC.hours);
Serial.print(":");
Serial.print(myRTC.minutes);
Serial.print(":");
Serial.print(myRTC.seconds);
Serial.print(" / ");
Serial.print(T_ext_CorrectedValue, 1);
Serial.print("°C / ");
Serial.print(T_int_CorrectedValue);
Serial.print("°C / ");
Serial.print(roll);
Serial.print("° / ");
Serial.print(pitch);
Serial.print("° / ");
Serial.print(yaw);
Serial.print("° / ");
Serial.print(acc_roll);
Serial.print("g / ");
Serial.print(acc_pitch);
Serial.print("g / ");



Serial.println(" ");

//////////////////////////////////////////////////////////////////////////////////////////////////// PRINT TO SD CARD

myFile = SD.open("Log.txt", FILE_WRITE);

// if the file opened okay, write to it:
if (myFile) {
myFile.print(myRTC.dayofmonth);
myFile.print(".");
myFile.print(myRTC.month);
myFile.print(".");
myFile.print(myRTC.year);
myFile.print(" ");
myFile.print(myRTC.hours);
myFile.print(":");
myFile.print(myRTC.minutes);
myFile.print(":");
myFile.print(myRTC.seconds);
myFile.print(" ");
myFile.print(T_ext_CorrectedValue, 1);
myFile.print(" ");
myFile.print(T_int_CorrectedValue);
myFile.print(" ");
myFile.print(roll);
myFile.print(" ");
myFile.print(pitch);
myFile.print(" ");
myFile.print(yaw);
myFile.print(" ");
myFile.print(acc_roll);
myFile.print(" ");
myFile.print(acc_pitch);
myFile.print(" ");


myFile.println(" ");
// close the file:
myFile.close();
} else {
// if the file didn't open, print an error:
Serial.println("error opening Log.txt");
}



}


Die übertriebenen Comments sind für mich nur eine Lesehilfe, die kommen beim fertigen Code natürlich raus.
Habt ihr eine Idee, wie ich der IMU belastbare Werte entlocken kann?

Vielen Dank,
Atvilar

[Klebwax]

Die übertriebenen Comments sind für mich nur eine Lesehilfe, die kommen beim fertigen Code natürlich raus.

Dem fertigen Code sind sie egal, die ignoriert der Compiler vollständig. Für mich sind sie aber keine Lesehilfe sondern behindern mich beim Lesen. Ebenso die vielen Leerzeilen. Man muß dauernd scrollen, um ein paar Zeilen im Zusammenhang zu sehen. Ich hab mir den Code daher nicht angesehen und kann dir also auch nicht sagen, wo dein Problem liegt.

MfG Klebwax

[Atvilar]

Hallo Klebwax,

Ich spreche nicht vom Compiler, sondern vom Program selbst.
alles was die MPU macht ist: schau dir 0,01 sec die Bewegungen an, addiere sie zusammen, berechne daraus deine aktuelle Position.
Dann geht das Programm anderen Aufgaben nach. Bis sich das Program das nächste mal an der IMU ankommt, werden Bewegungen ignoriert.

LG Atvilar

[HaWe]

hallo,
1., da es ein Arduino-Programm ist, wäre das Arduino-Unterforum der bessere Ort zum Posten.
2., wenn du parallele Ausführung von Sensor-Auslesen + Logging und restlichen Programmfunktionen willst, die sich gegenseitig nicht behindern, empfiehlt sich Multithreading (1 Thread für i2c und den MPU samt Logging, 1 Thread oder mehrere weitere für den Rest). Das funktioniert auf dem Arduino Due und Zero mit der Scheduler Lib und mit dem ESP32 per std::thread - dann wird dabei nichts mehr verpasst!
3., ich würde für den MPU eine Lib verwenden, die den internen motion processor verwendet (z.B. MPU6050_DMP6_using_DMP_V6.12, dabei I2C-Clock auf 400000)

PS:
(https://github.com/jrowberg/i2cdevli...rduino/MPU6050, bin mir nicht sicher, welche du verwendest)

[Rabenauge]

Ich sehe, dass du den Dallas-Temperatursensor benutzt.
Wenn der im Parasit-Modus läuft, ist er sehr langsam! Der braucht dann nämlich eine gewisse Zeit, um sich aufzuladen.
Da die Dinger im normalen Modus (eigene Stromversorgung) auch nicht mehr können, als eben die Temperatur auszugeben, benutz ich die kaum noch...
Weiter sehe ich, dass deine zahlreichen seriellen Ausgaben auch sehr langsam erfolgen- dafür gibts nur selten nen sinnvollen Grund.
Auch das verzögert den Programmablauf deutlich.
Schreiben auf SD-Karte ist ebenfalls nicht eben schnell...

Du kannst ja mal ermitteln, wie lange deine loop() für einen Durchlauf braucht: am Anfang die aktuellen micros() auslesen, am Ende wieder...und dir die Differenz ausgeben lassen.
Mit millis() geht das natürlich auch....micros() hat aber ne höhere Auflösung.

[Atvilar]

Hallo,

@ HaWe
1) Danke, ich war mir nicht sicher ob ich auf Sensor oder Arduino posten sollte.
2) Multithreading, so etwas habe ich mir gedacht. muss mich da aber erst einlesen, wie ich das hinbekomme! (hab Arduino Uno)
3) Lib Möchte ich verhindern, da in einer weiteren Ausbaustufe 2 Libs kollidieren würde (Ich glaube die Beiden verwenden die gleiche Clock oder so)

@ Rabenauge:
Den Dallas Sensor benutze ich, weil ich den gerade zur Hand hatte. Er läuft NICHT im Parasit-Modus
Die Seriellen Ausgaben benutze ich zum Debugging und Kalibrieren, auch die kommen später raus. Trotzdem: Ohne Seriellen Ausgaben + SD Speicherung: 369 952 micros / loop
Schreiben auf die SD ist aber nicht zu vermeiden. wäre eine Lange "Wurst" schneller?
1 Loop mit Seriellen Ausgaben dauert ca. 422 488 micros

Edit:
Ohne die beiden T Berechnungen (Dallas + MPU) komme ich auf 40 056 micros / Loop.
Außerdem habe ich die baudrate auf 230400 erhöht.
Nichtsdestotrotz muss der Loop noch schneller sein.

Danke für die Tipps!

[HaWe]

Mit dem Uno geht MT nicht bzw. nicht vernünftig, da viel zu wenig RAM. Der Arduino Zero/M0 hat aber denselben Formfaktor, hat aber ~15x soviel RAM und ~3x soviel Takt.
Deine Bedenken wegen "Clock" vesretehe ich nicht....:confused:
Der Flaschenhals wird aber sicher das schnelle Schreiben auf SD sein.

[Rabenauge]

Du kannst versuchen, die Ausgaben, die auf die Karte geschrieben werden, erst ein paar Datensätze lang zu puffern.
Das erspart zumindest das dauernde öffnen und schliessen der Datei...da musst du halt schauen, wie weit du mit dem Arbeitsspeicher kommst.

Weiters würde ich versuchen, möglichst viele der Fliesskomma-Berechnungen zu umschiffen (je mehr davon verschwindet, umso besser).

[wkrug]

Wie wärs, wenn Du die Gyro Abfragen in einen Timer Interrupt legst, der alle 5 bis 10ms ausgelöst wird.
Dort zyklisch die Gyro Werte addieren und in der Hauptroutine die Position daraus berechnen.
Komplexe Berechnungen sollten da aber dann vermieden werden ( Zeitfaktor ).

Zum anderen würde ich statt der float Variablen lieber int Typen wählen, wo das möglich ist, weil die ein Controller wesentlich schneller verrechnen kann.

[HaWe]

Ist die Frage, was genau gespeichert werden soll: die nackten raw-Werte direkt aus den Registern (6DOF, zusammengesetzt zu 3x integer für Gyro und 3x integer für Accel.) - oder schon fertig ausgerechnete Winkel.
Eulerwinkel (bzw. yaw, pitch, roll) werden aber nur mit floats vernünftig berechnet. Wenn also abspeichern, dann entweder direkt die raw-Werte aus den Registern (ohne weitere Umrechnungen), oder die daraus bereits fertig ausgerechneten geometrischen Winkel und dann erst zu int gerundet:
z.B. die float-Eulerwinkel mit 100 multiplizieren und dann runden (edit: je nachdem, int16_t oder uint16_t). Dann hat man gegenüber float nur die halbe Anzahl an Bytes auf SD zu schreiben und statt 6 Werte für 6DOF nur 3 ausgerechnete Euler-Winkel.

[HaWe]

PS:
es gibt auch SAMD51 (ARM Cortex M4) Platinen im Uno-Formfaktor, programmierbar per Arduino genau wie auch UNO oder DUE. Vorteil: 120MHz Takt plus extra fpu Coprozessor für schnelle float-Berechnungen, da sind dann auch komplizierte float-Berechnungen rattenschnell:
https://www.adafruit.com/product/3382
https://www.adafruit.com/product/4000 .

Aber auch der MPU6050 hat ja bereits einen onboard-digital-motion-processor (dmp), der komplizierte Berechnungen onboard, also schon ohne Arduino, ausführen kann, nur muss der gesondert angesprochen werden, was längst nicht alle MPU6050 Libs auch tatsächlich tun, im Gegensatz zu dieser hier, wie schon erwähnt: https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 .