0

I've been trying to build a tilt compensated compass for an autonomous Arduino car I'm building. I'm having issues with the consistency of my data.

This is the code I'm using.

#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps_V6_12.h>
#define Magnetometer_addr 0x1E
MPU6050 mpu;

bool dmpReady = false;  
uint8_t mpuIntStatus;   
uint8_t devStatus;      
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64]; 
float prevoius_corrected_reading,corrected_reading;

Quaternion q;           
VectorFloat gravity;   
float ypr[3];         

bool mpuInterrupt = false;  

float Xerror = 0.01;
float Yerror = -0.385;
float Zerror = -0.17;

int X0, X1, Z0, Z1,Y0, Y1;
float X, Y, Z;
float compass_heading;



void setup() {
  Serial.begin(9600);
  Wire.begin();

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x01);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x02);
  Wire.write(0x00);
  Wire.endTransmission();

  mpu.initialize();  
  //Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  devStatus = mpu.dmpInitialize();

    mpu.setXGyroOffset(51);
    mpu.setYGyroOffset(8);
    mpu.setZGyroOffset(21);
    mpu.setXAccelOffset(1150); 
    mpu.setYAccelOffset(-50); 
    mpu.setZAccelOffset(1060); 

    if (devStatus == 0) {

        mpu.CalibrateAccel(6);
        mpu.CalibrateGyro(6);
        //Serial.println();
        mpu.PrintActiveOffsets();
        mpu.setDMPEnabled(true);
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        //Serial.print(F("DMP Initialization failed (code "));
        //Serial.print(devStatus);
        //Serial.println(F(")"));
    }

  delay(20);

}

void loop() {

  compass_heading = request_compass_data();

  Serial.print("Raw compass heading is \t");
  Serial.print(compass_heading);

  Serial.print("\t\t");

  request_accel_data();

  Serial.print("YAW\t:");
  Serial.print(ypr[0] * 180/M_PI);
  Serial.print("\tROLL\t:");
  Serial.print(ypr[1] * 180/M_PI);
  Serial.print("\tPITCH\t:");
  Serial.print(ypr[2] * 180/M_PI);

  prevoius_corrected_reading = corrected_reading;

  Serial.print("\tCorrected compass heading is \t");
  float Xhor =  X*cos(ypr[2]) + Z*sin(ypr[2]);
  float Yhor =  Y*cos(ypr[1]) - Z*sin(ypr[1])*cos(ypr[2]) + X*sin(ypr[1])*sin(ypr[2]);

  corrected_reading = rad_to_deg(rad_ang(Yhor, Xhor));
  Serial.print(corrected_reading);

  Serial.print("\tFiltered compass heading is \t");

  Serial.print(0.1 * corrected_reading + 0.9 * prevoius_corrected_reading);
  Serial.println();



}

float rad_ang(float a, float b){
  float ang = atan2(a, b);
  if(ang<0){ang += 2*PI;}
  if(ang>2*PI){ang -= 2*PI;}
  return ang;
}

float rad_to_deg(float rad_in){
  return rad_in / PI * 180;
}

float request_compass_data(){
  //return compass heading
  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x03);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  X0 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x04);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  X1 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x05);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Z0 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x06);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Z1 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x07);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Y0 = Wire.read();
  }

  Wire.beginTransmission(Magnetometer_addr);
  Wire.write(0x08);
  Wire.endTransmission();
  Wire.requestFrom(Magnetometer_addr,1);
  if (Wire.available()>=1){
  Y1 = Wire.read();
  }

  X0 = X0<<8;
  Z0 = Z0<<8;
  Y0 = Y0<<8;

  X = X1 + X0;
  X -= Xerror;
  Y = Y1 + Y0;
  Y -= Yerror;
  Z = Z1 + Z0;
  Z -= Zerror;

  //Calibrated manually
  X = X * 0.00092 - Xerror;
  Y = Y * 0.00092 - Yerror;
  Z = Z * 0.00092 - Zerror;
  /*
  Serial.print("XYZ\t");
  Serial.print(X);
  Serial.print("\t");
  Serial.print(Y);
  Serial.print("\t");
  Serial.println(Z);
*/
  float dir = rad_to_deg(rad_ang(Y, X));

  return dir;

}

void request_accel_data(){
        if (!dmpReady) return;
    mpuInterrupt = true;

    while (!mpuInterrupt && fifoCount < packetSize) {
        if (mpuInterrupt && fifoCount < packetSize) {
          fifoCount = mpu.getFIFOCount();
        }  

    }

    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    //Serial.print("INT STATUS:\t");
    //Serial.print(mpuIntStatus);
    fifoCount = mpu.getFIFOCount();
    //Serial.print("\tFIFO COUNT:\t");
    //Serial.print(fifoCount);

    if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) {

        mpu.resetFIFO();
        fifoCount = mpu.getFIFOCount();
        Serial.println(F("FIFO overflow!"));

    } 

    else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);  
        mpu.resetFIFO();

    }
}

When I have the below sensor orientation my data output(corrected_reading) varies as shown in the below graph on one complete rotation.I'm happy with this output as there is only very little inconsistency and the tilt compensation works fine too.

Data variation

Sensor Orientation

However keeping them in the above orientation within my car is not practically possible in my case. So I tried this orientation. Sensor Orientation 2

But no matter how I adjust my code to suit this orientation I always end up getting very inconsistent data like below.

second Graph third graph

I'm sure that there is some mistake i'm making in the second orientation i used.The biggest problem is that the raw magnetometer output(compass_heading) also becomes inconsistent in this case.

Please give me your suggestions as to what changes I should make in order to get a smooth output with the second orientation. Thanks in advance.

Microcontroller - Arduino Uno Magnetometer - HMC 5983 Accelerometer - MPU 6050

0

As it is not possible to reproduce the errors and analyse the program in detail, the thing which jumped at me that you have no gliding error correction (or its hidden in a library) for the compass (The MPU6050 has probably one as FIFO is called but you should look it up). The method is easy to explain:

  • You take an array of lets say 20 measure points.
  • You apply FIFO First value in is the last out
  • you do gliding average over the first 15 values -> this is the orientation output
  • the last 2 are used for value validation:
    • if you get a value out of a defined data range AND the next is in the range then ditch the before value (error corection)
    • if you get a value out of a defined data range AND the next 4 values are also in the range compared tothe first out of range this will cause the array to empty and start a new gliding average with these values. (Might not be the case when driving a car but in plane models this can happen -> 180 degree turns)

Its up to you how many values you take as a base reference and how many lead to an aprupt direction change. As we are in hobby electronics you can not expect high quality value delivery sowe have to use math to get to a desired result.
Regarding the changed orientation of the magnetometer.
EDIT
In my experience the turning of the magnetometer upside down resultedin weird behavior and more unstable readings. I guess this is due to the elctronics shielding now from the top rather than the bottom.
For reading and error handling I looked [into this library code]((https://github.com/keepworking/Mecha_QMC5883L/blob/master/MechaQMC5883.cpp) and adapted the read routine to my needs. Also lookat the examplesfor valuable input.

| improve this answer | |
  • Yeah I'll try that..and the orientation is changed by 180° about the y axis in the second case I have mentioned..so how can changing x and y solve the problem? – AfiJaabb Apr 25 at 16:07
  • The question is did you turn the hardware sensor 90 degree or flip upside down (there is a difference!) – Codebreaker007 Apr 25 at 17:49
  • I have turned it 180 degrees.Ideally the y axis will be oh the same direction it was while the z and x axes will be reversed. – AfiJaabb Apr 26 at 4:59
  • See my edit and the link – Codebreaker007 Apr 26 at 8:13

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.