Incorrect angle measurement on a motorbike

[fralbo]

Hello, I have a strange situation using your library to read inclination angle on a motorbike. Below I made the same experiment at the same time on the same part of a road, in red with my smartphone using HyperIMU application and in blue with an Arduino Beetle connected to a GY-521 MPU6050 board. As you can see when I get a turn, driving at regular speed, the red curve is pretty horizontal meaning I keep the same inclination angle which is what I made. But strangely, the measure I get on MPU6050 seems to slightly drift to zero. I precise that everything is fine when I move to the board on my desk. The problem only occurs when I use it on my motorbike and I guess it could be caused by a "high" gravity force during the turn which could cause the anti-drift feature to overload the gyro reading. As you can see the offset the problem causes during the turn is more or less found on the next turn (h).

At the moment, I didn't make the same test with another library but do you think it could be caused by your lib or internally in the MPU6050 ?

My code is the following one:

/**
 * From https://github.com/jrowberg/i2cdevlib/issues/441
 */
#include <math.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <filters.h>
#include <Servo.h>

// ---------------------------------------------------------------------------
#define YAW      0
#define PITCH    1
#define ROLL     2

#define G   (double)9.80665
// --------------------- MPU6050 variables ------------------------------------
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// Orientation/motion vars
Quaternion q;        // [w, x, y, z]         quaternion container
VectorFloat gravity; // [x, y, z]            gravity vector
float ypr[3];        // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

int8_t logPitch[1400];
//int8_t logAZ[700];
int logIdx = 0;
unsigned long logTime;

// ---------------------------------------------------------------------------
#define LED_PIN 13

#define OFFSET  (90)

/* The following values depends on the MPU6050 and must be defined for each chip.*/
// MPU moto
#define XACCEL_OFFSET   1921
#define YACCEL_OFFSET   1713
#define ZACCEL_OFFSET   1117
#define XGYRO_OFFSET    102
#define YGYRO_OFFSET    -166
#define ZGYRO_OFFSET    -200
/*/
// MPU DFRobot
#define XACCEL_OFFSET   -1331
#define YACCEL_OFFSET   535
#define ZACCEL_OFFSET   883
#define XGYRO_OFFSET    45
#define YGYRO_OFFSET    29
#define ZGYRO_OFFSET    89
/*/
bool blinkState = false;

Servo servo;
int16_t ms;
int8_t global_fifo_count = 0; //made global so can monitor from outside GetIMUHeadingDeg() fcn
int8_t rollSide = 1, blinkDiv;
float angle, ay, az;
const float cutoff_freq   = 7.0;  //Cutoff frequency in Hz
const float sampling_time = 0.005; //Sampling time in seconds.
IIR::ORDER  order  = IIR::ORDER::OD2; // Order (OD1 to OD4)

Filter f_az(cutoff_freq, sampling_time, order);
Filter f_angle(cutoff_freq, sampling_time, order);

/**
   Setup configuration
*/
void setup() {
//  Serial.begin(115200);
//  while (!Serial);

  //memset(logPitch, 0, sizeof(logPitch));
  logIdx = 0;

  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)

  pinMode(LED_PIN, OUTPUT); // LED

  mpu.initialize();

  // Load and configure the DMP
  //Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();

  // Returns 0 if it worked
  if (devStatus == 0) {
    // Turn on the DMP, now that it's ready
    //Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // MPU calibration: set YOUR offsets here.
    mpu.setXAccelOffset(XACCEL_OFFSET);
    mpu.setYAccelOffset(YACCEL_OFFSET);
    mpu.setZAccelOffset(ZACCEL_OFFSET);
    mpu.setXGyroOffset(XGYRO_OFFSET);
    mpu.setYGyroOffset(YGYRO_OFFSET);
    mpu.setZGyroOffset(ZGYRO_OFFSET);

    //mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8);

    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.println(F(")"));
  }

  servo.attach(9);
  servo.write(OFFSET);
  delay(1000);

  digitalWrite(LED_PIN, 1);
  delay(10);
  digitalWrite(LED_PIN, 0);
  delay(50);
  digitalWrite(LED_PIN, 1);
  delay(10);
  digitalWrite(LED_PIN, 0);
  delay(50);
  digitalWrite(LED_PIN, 1);
  delay(10);
  digitalWrite(LED_PIN, 0);

  delay(2000);
  logTime = millis() + 50;
  //Serial.println();
}

int n = 10;

/**
   Main program loop
*/
void loop() {
  int8_t i;

  if (Serial.available()) {
    int i;
    digitalWrite(LED_PIN, 1);
    delay(1000);
    for (i = 0; i < sizeof(logPitch); i++) {
      Serial.println(logPitch[i]);
      //Serial.print("\t");
      //Serial.println(logAZ[i]);
      delay(1);
    }
    digitalWrite(LED_PIN, 0);
    delay(10000);
    mpu.resetFIFO();
  }

  // if programming failed, don't try to do anything
  if (!dmpReady) {
    Serial.println(F("DMP Initialization failed "));
    return;
  }

  static unsigned long MPUSampletime;
  if ((millis() - MPUSampletime) >= 9) { // After FIFO read no new data will be there for at least 9 ms
    MPUSampletime = millis();
    if (mpu.dmpPacketAvailable())
    {
      //retreive the most current yaw value from IMU
      angle = (f_angle.filterIn(GetIMUHeadingDeg()) * 180.0) / M_PI;
      //az = f_az.filterIn((float)mpu.getAccelerationZ() / 4096.0 * G);

/*      
      Serial.print(-90);
      Serial.print("\t");
      Serial.print(90);
      Serial.print("\t");
      //Serial.print(millis());
      //Serial.print("\t");
      Serial.print(ay);
      Serial.print("\t");
      Serial.print(az);
      Serial.print("\t");
      Serial.print((angle * 90.0) / M_PI);
      Serial.println();
      */
      if ((millis() > logTime ) && (logIdx < (sizeof(logPitch) -1))) {
        logPitch[logIdx] = (int8_t)angle;
        //logAZ[logIdx] = (int8_t)(az * 10.0);
        logIdx++;

        logTime = millis() + 50;
        // blink LED to indicate activity
        if (!((blinkDiv++) % 5)) {
          blinkState = !blinkState;
          digitalWrite(LED_PIN, blinkState);
        }
      }
      //Serial.println(angle + OFFSET);
      servo.write(angle + OFFSET);

    }
  }
}

float GetIMUHeadingDeg()
{
  // At least one data packet is available

  mpuIntStatus = mpu.getIntStatus();
  fifoCount = mpu.getFIFOCount();// get current FIFO count

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024)
  {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    //Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  }
  else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT))
  {
    // read all available packets from FIFO
    while (fifoCount >= packetSize) // Lets catch up to NOW, in case someone is using the dreaded delay()!
    {
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      // track FIFO count here in case there is > 1 packet available
      // (this lets us immediately read more without waiting for an interrupt)
      fifoCount -= packetSize;
    }
    global_fifo_count = mpu.getFIFOCount(); //should be zero here

    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  }

  float pitch = -ypr[PITCH];
  return pitch;
}

[georgegohl888]

I have the same issue in an aircraft, during a turn the value drifts due to the accelerometes detecing the gravity force being level and correcting the gyro, though it is not actually level.

[fralbo]

Yes and I'm surprised that most of these chips and library are noticed to be usable on drones as its exactly the same problem on such vehicles as well.

I think we should only use gyro values, not quaternions or AHRS algorithm.

After we need a way to fix long-term drift of gyro integration. I though about magnetometer. Detecting a straight direction means that roll is equal to 0. That's enough for motorbike but for an aircraft, we also have to correct the pitch. Maybe with an altimeter.

Regards.

Le dim. 8 août 2021 à 20:21, georgegohl888 @.***> a écrit :

I have the same issue in an aircraft, during a turn the value drifts due to the accelerometes detecing the gravity force being level and correcting the gyro, though it is not actually level.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/jrowberg/i2cdevlib/issues/586#issuecomment-894835727, or unsubscribe https://github.com/notifications/unsubscribe-auth/ADWOCCMSIQGDGR4FRUGKEZLT33DLFANCNFSM45L33IEQ . Triage notifications on the go with GitHub Mobile for iOS https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675 or Android https://play.google.com/store/apps/details?id=com.github.android&utm_campaign=notification-email .

[ElreboCM]

@fralbo Do you happen to know what algorithm hyperIMU uses for calculating the inclination?

[fralbo]

Not exactly but it usually relies on Kalman filters or something like that which is basically a mix on gyrometers and accelerometers. That's fine for all vehicule turning flat but for motorbike or drone its absolutely wrong. So I now only use gyrometers as explained above.

[ElreboCM]

My impression was that hyperIMU does what you would expect.

Below I made the same experiment at the same time on the same part of a road, in red with my smartphone using HyperIMU application and in blue with an Arduino Beetle connected to a GY-521 MPU6050 board. As you can see when I get a turn, driving at regular speed, the red curve is pretty horizontal meaning I keep the same inclination angle which is what I made.

Then it would be great to use the same algorithm with gyro and acc values from the mpu6050.

[fralbo]

I tried and got the same results. Accelerometers are definitely not usable for banking vehicule.

[ElreboCM]

@fralbo Do you have a code example. I would be really interested.

[ZHomeSlice]

There is a possible solution

I have given 2 options and the second is easier but the first is necessary to explain why.

DMP has 2 modes DMP_FEATURE_LP_QUAT (Gyro Only) DMP_FEATURE_6X_LP_QUAT (Accelerometer and Gyro)

When I created the DMP Image instance I set the DMP_Feature to the Accelerometer and Gyro Quaternion calculations.

So How do I Change this :)

Well, it's easy... I hope This is for V6.12 of the DMP image. Just after loading the DMP (Next line of code) image into the MPU6050 Memory but before starting it up if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail <<< Loads the DMP Image

Let's make some changes as follows:

#define CFG_8                   (2718)
#define CFG_LP_QUAT             (2712)
unsigned char regs[4];
// Disable DMP_FEATURE_6X_LP_QUAT 
        memset(regs, 0xA3, 4);
        mpu_write_mem(CFG_8, 4, regs);
// Enable DMP_FEATURE_LP_QUAT 
        regs[0] = 0xC0;
        regs[1] = 0xC2;
        regs[2] = 0xC4;
        regs[3] = 0xC6;
        mpu_write_mem(CFG_LP_QUAT, 4, regs);

//This is the function example needed to write to a specific byte of memory in the DMP Image loaded into the MPU6050
int mpu_write_mem(unsigned short mem_addr, unsigned short length, unsigned char *data){
    unsigned char tmp[2];

    if (!data)     return -1;
    if (!st.chip_cfg.sensors)  return -1;

    tmp[0] = (unsigned char)(mem_addr >> 8);
    tmp[1] = (unsigned char)(mem_addr & 0xFF);

    I2Cdev::writeBytes(devAddr,0x6D, 2,tmp) // sets the mem location to write to in the DMP Image
    I2Cdev::writeBytes(devAddr,0x6F, 2,data) //What to change
    return 0;
}

The above code is derived from the Invensense example and additional modifications will likely be needed to get the writeBytes functions to work with Jeff's library.

---

Alternative Easier option

replace Bank 10 of DMP V6.12 (Lines 311 to 327) with the following changes already added to the DMP image prior to loading (Not Tested!! Sorry.)

/* bank # 10 */
0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6,
0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD,
0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB,
0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C,
0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8,
0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9,
0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2,
0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84,
0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0,
0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0xA3, 0xA3,
0xA3, 0xA3, 0xB2, 0x8B, 0xC0, 0xC2, 0xC4, 0xC6, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3,
0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9,
0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3,
0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28,
0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90,
0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,

Z

[fralbo]

@fralbo Do you have a code example. I would be really interested.

The code is in my first Post.

[fralbo]

@ZHomeSlice that's very interesting. I switched to ICM-20948, I'll take a look to see if there is a similar option.