Guten Abend,

ich versuche einem Roboter einen Kompass zu geben, damit dieser eine Orientierung hat.
Mit dieser Orientierung kann ich dann beispielsweise sagen, dass sich um 180° drehen soll.
Bis jetzt konnte ich einen yaw winkel im bereich von 0 - 359° errechnen.

Das Problem ist, dass die Ausrichtung sich ändert, wenn ich das Modul auf
eine alte Ausrichtung zurück drehe. Wenn ich das MPU Modul auf 180° drehe
und dieses dann in die entgegengesetzte Richtung drehe (also weiter 180°) dann habe ich immer unterschiedliche
Abweichungen von bis zu 30°. Die Richtungen die beispielsweise 90° anzeigen, ändern sich ständig.
Nicht falsch verstehe:
Wenn das Modul still steht dann ist der Wert relativ stabil. Ich habe lediglich einen Drift von 1° alle 20 Sekunden.
Besteht mein Problem etwa in diesem Drift?

Was muss ich tun, um diese Problem zu beheben?
Ich habe den Code und die Infos von dieser Seite:

http://blascarr.com/lessons/imu-visualization/

Ansonsten ist hier auch noch mein vollständiger Code:
Code:
#include "MadgwickAHRS.h"
#include "IMU_MPU9250.h"
 
float roll;
float pitch;
float yaw;
long Serialtime;
long last_time;
long microsPerReading;
 
Madgwick filter;
MPU9250 imu;
 
 
void updateIMU(float *roll, float *pitch, float *yaw){
 
  if (imu.readByte(MPU9250_ADDRESS, INT_STATUS) && 0x01){    imu.readAccelData(imu.accelCount);
    imu.getAres();
 
    imu.ax = (float)imu.accelCount[0]*imu.aRes;
    imu.ay = (float)imu.accelCount[1]*imu.aRes;
    imu.az = (float)imu.accelCount[2]*imu.aRes;
    imu.readGyroData(imu.gyroCount);
    imu.getGres();
 
    imu.gx = (float)imu.gyroCount[0]*imu.gRes;
    imu.gy = (float)imu.gyroCount[1]*imu.gRes;
    imu.gz = (float)imu.gyroCount[2]*imu.gRes;
  }
 
  imu.updateTime();
  imu.delt_t = millis() - imu.count;
  if (imu.delt_t > microsPerReading){
    filter.updateIMU(imu.gx, imu.gy, imu.gz, imu.ax, imu.ay, imu.az);
    *roll = filter.getRoll();
    *pitch = filter.getPitch();
    *yaw = filter.getYaw();
    imu.count = millis();
  }
}
void setup() {
  Wire.begin();
  Serial.begin(9600);
  filter.begin(25);
   
  microsPerReading = 1000 / 25;
  Serialtime = 50;
  last_time = millis();

  imu.gyroBias[0] = 0.0f;
  imu.gyroBias[1] = 0.0f;
  imu.gyroBias[2] = 0.0f;

  imu.accelBias[0] = 0.0f;
  imu.accelBias[1] = 0.0f;
  imu.accelBias[2] = 0.0f;
   
  imu.calibrateMPU9250(imu.gyroBias, imu.accelBias);
  imu.initMPU9250();
}
 
void loop() {
  updateIMU(&roll, &pitch, &yaw);
  if (millis() - last_time > Serialtime) {
    Serial.print("Orientation ");
    Serial.print(yaw);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);
    last_time = millis();
  }
}