MPU-6050 (I2C) Library

Can someone help me port this library?
garindo
Posts: 8
Joined: Tue Sep 20, 2016 3:02 pm

Re: MPU-6050 (I2C) Library

Post by garindo » Mon May 22, 2017 11:44 pm

Hi, fingolin, for me there is no problem in sharing, I will clean my code and upload it, in arduino autolevel it works for me.

garindo
Posts: 8
Joined: Tue Sep 20, 2016 3:02 pm

Re: MPU-6050 (I2C) Library

Post by garindo » Mon May 22, 2017 11:47 pm

Bdvell, Very thanks.

Fingolin
Posts: 15
Joined: Wed May 17, 2017 1:28 pm

Re: MPU-6050 (I2C) Library

Post by Fingolin » Tue May 23, 2017 6:34 am

Now comes my code:
this is my GyroClass.h:

Code: Select all

#include <math.h>
 class Gyro{
   private:
         float AccelMultipl=2*9.81; //
         float AngleMultipl=250;
         float correction[3]={0.9890, 0.9920,0.9844}; //correction factor  for Acceleration
         float bias[3]={-0.3713,-0.0095,0.6367}; //Offset factor für Acceleration
         float Offset[3]={ 0.86433,0.99755,-0.07829}; //Offsett faktor der Drehwinkelgeber (-Mittelwert des Stillstands) //Offset for Angular Rate ~Mean if the Gyro is not Moving (minimizes drift)



   public:
   //Werte mit _accel wurden aus beschleunigungen berechnet (wird zur Sensorfusion genutzt)
   //Raw Angular-Rates: (just scaled)
        float Roll_rate=0;
        float Gier_rate=0;
        float Nick_rate=0;

       float Gier_angle=0;
  //Angles from acceleration:
        float Roll_accel=0;
        float Nick_accel=0;
  //Angles after Sensor funsion:
        float Roll_filtered=0;
        float Nick_filtered=0;
        float Gier_filtered=0; //"FILTERED" for a real filtering we actually need something like a compass which is not implemented!
        
       
  //Acceleration (Raw and Integrated) (Werte sind bis für Sensorfusion eig Nutzlos, insbesondere die Integrierten, diese sollten (hier fehlt Gravity-Offset) den Geschwindigkeiten entsprechen.
        float x_accel=0;
        float Ix_accel=0;

        float y_accel=0;
        float Iy_accel=0;

        float z_accel=0;
        float Iz_accel=0;

        float temperature;
// LastSampleTime is used for integrating correctly
        float LastSampleTime=0;

        int Adress=0x68;

    
    void init(int Adress)
      {
        /*
         * Init Function, sets Adress Wakes up the MPU and sets the LowPassFilter -> LowPass is set pretty High if the chip is dampened maybe set it lower
         */
        this->Adress=Adress;
        Wire.begin();
        Wire.beginTransmission(Adress);
        Wire.write(0x6B);  // PWR_MGMT_1 register
        Wire.write(0);     // set to zero (wakes up the MPU-6050)
        Wire.endTransmission();

        Wire.beginTransmission(Adress);
        Wire.write(0x1A);  // CONFIG register
        byte val=B00000110; //
        Wire.write(val);     // Set the LowPassFilter
        Wire.endTransmission();

        
      }
  void setConfig(int mode)
    {
    //Set the mode / range (not yet tested!)
      byte bMode;
     switch (mode){
     case 0:
       bMode=B00000000;
      this->AccelMultipl=2*9.81;
      this->AngleMultipl=250;
       break;
     case 1:
      bMode=B00001000;
       this->AccelMultipl=4*9.81;
       this->AngleMultipl=500;
        break;
     case 2:
      bMode=B00011000;
       this->AccelMultipl=8*9.81;
       this->AngleMultipl=1000;
        break;
     case 3:
      bMode=B00011000;
       this->AccelMultipl=16*9.81;
       this->AngleMultipl=2000;
        break;
     default:
      bMode=B00000000;
      this->AccelMultipl=2*9.81;
      this->AngleMultipl=250;
     break;
     }
      //Serial.println(bMode);
      Wire.beginTransmission(this->Adress);
      Wire.write(0x1B);  // starting with register 0x1B ()
      Wire.write(bMode); //1->+-500°/s
      Wire.endTransmission();
      Wire.write(0x1C);  // starting with register 0x1C ()
      Wire.write(bMode); //1->+-4g°/s
      Wire.endTransmission();
      }
      
    void getConfigs() //Not Tested!    
    {
          Wire.beginTransmission(this->Adress);
          Wire.write(0x1B);  // starting with register 0x1B (GYRI CIBFUG)
          Wire.endTransmission();
          Wire.beginTransmission(this->Adress);
          Wire.requestFrom(this->Adress,2);  // request a total of 2 registers 
          short conf=Wire.read()<<8|Wire.read();
         // Serial.println(conf,BIN);
      }

        void grabData()//grabs Data from Gyro and processes it (filteres it)
        {
          // First Get Times for the integrating stuff that happenes later
          float T_now=micros()*0.000001; //Jetzt Zeit in s
          float dT=T_now-(this->LastSampleTime); //Time that has run by (integration span)
          this->LastSampleTime=T_now;

          //Reading & Saving Raw-Values:
          Wire.beginTransmission(this->Adress);
          Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
          Wire.endTransmission();
          Wire.requestFrom(this->Adress,14);  // request a total of 14 registers
          this->x_accel=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
          this->y_accel=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
          this->z_accel=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
          this->temperature=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
          
          this->Roll_rate=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
          this->Nick_rate=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
          this->Gier_rate=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)}
          
          //Preprocessing:
        //1. Scaling works by an affin function x_scaled=a * x_raw +b
          this->x_accel=(float)this->x_accel*this->AccelMultipl/32767*this->correction[0] + this->bias[0];
          this->y_accel=(float)this->y_accel*this->AccelMultipl/32767*this->correction[1] + this->bias[1];
          this->z_accel=(float)this->z_accel*this->AccelMultipl/32767*this->correction[2] + this->bias[2];
        //2. Integrate acceleration (useless so far) //maybe used later by integrating it (twice) to determine position (should be pretty uncertain)
          this->Ix_accel+= this->x_accel*dT;
          this->Iy_accel+= this->y_accel*dT;
          this->Iz_accel+= this->z_accel*dT;
        //3. Temperature 
          this->temperature=(float) this->temperature/340.00+36.53;
        //4.Scale Angular Rates:   (Angular Rates are just adjusted by an Offset, which is determined by the mean and minimizes drift)
          this->Roll_rate=(float)this->Roll_rate*this->AngleMultipl/32767+Offset[0];
          this->Nick_rate=(float)this->Nick_rate*this->AngleMultipl/32767+Offset[1];
          this->Gier_rate=(float)this->Gier_rate*this->AngleMultipl/32767+Offset[2];


          //Roll und Nicktwerte aus Beschleunigungen:
          this->Roll_accel=RAD_TO_DEG*(atan2(-this->y_accel,-this->z_accel)); //NICK
          this->Nick_accel=RAD_TO_DEG*(atan2(this->x_accel,sqrt(this->z_accel*this->z_accel+this->y_accel*this->y_accel)));//ROLL


      //Sensor fusion is here
          //Sensor fusion uses just a complementary filter, however works pretty solid (maybe use a kalman filter later)
          
          this->Roll_filtered=0.994 *(this->Roll_filtered+this->Roll_rate*dT) + 0.006*this->Roll_accel ;
          this->Nick_filtered=0.994 *(this->Nick_filtered+this->Nick_rate*dT) + 0.006*this->Nick_accel ;

          //this->Roll_filtered -=Nick_filtered*sin(this->Gier_rate);
          //this->Nick_filtered +=Roll_filtered*sin(this->Gier_rate);
          
          this->Gier_angle+=Gier_rate*dT;
          //Gier_filtered is not filtered since for that a compass would be needed!
          this->Gier_filtered=0.98*this->Gier_angle;
    }



    void dispData(){
//Winkelbeschleunigungen:
      //Serial.print(" aX: "); Serial.print(this->x_accel); Serial.print(" aY: "); Serial.print(this->y_accel); Serial.print(" aZ: "); Serial.print(this->z_accel);

//Nick& Roll aus Acceleration:
  //Serial.print(" Nick_acc; ");Serial.print(this->Nick_accel);
  //Serial.print(" Roll_acc: ");Serial.print(this->Roll_accel);
//NICK&ROLL-ANGLE-Rates
     // Serial.print(" Roll_rate: ");Serial.print(this->Roll_rate);
     //Serial.print(" Gier_filtered: ");Serial.print(this->Gier_rate);
//NICK&ROLL ANGLES
      Serial.print(" Roll_filtered: ");Serial.print(this->Roll_filtered);     
      Serial.print(" Nick_filtered: ");Serial.print(this->Nick_filtered);
      Serial.print(" GierAngle: ");    Serial.print(this->Gier_filtered);      
    
      
//TEMPERATUR:
    //  Serial.print(" Tmp: "); Serial.print(this->temperature);  //
//NULL-Linie zur Orientierung:
//Serial.print(" Null: "); Serial.print(0);
//Serial.print(" GasMax: "); Serial.print(60);
//Serial.print(" -GasMax: "); Serial.print(-60);
      }

   };
please note, that it is work in progress! But for me I get pretty solid values.
What is important is that I use an error model for the Accelerometer which is affin (ax+b) therefore to get the best performance the scaling factors (correction,bias = a,b) must be determined, for that I use a Matlab script. (If you are interested in that feel free to ask). They differ from sensor to sensor and cannot be adopted from my code! (this will (likely) just magnify your error)

For sensor fusion I use a simple complementary filter.
If there are questions / comments / criticism about my code feel free to post them :)

Fingolin
Posts: 15
Joined: Wed May 17, 2017 1:28 pm

Re: MPU-6050 (I2C) Library

Post by Fingolin » Wed May 24, 2017 10:04 am

Does anyone know how I change the i2c pins? So that I can plug the IMU on other pins?

garindo
Posts: 8
Joined: Tue Sep 20, 2016 3:02 pm

Re: MPU-6050 (I2C) Library

Post by garindo » Wed May 31, 2017 6:29 pm

Hi fingolin,in file wire.cpp, change line:

SCL, SDA
TwoWire Wire(PB6, PB7, SOFT_STANDARD);

one think, the problem in stm32 with mpu6050 can be the reading speed i2c, 2000 microseconds in the reading routine of mpu6050 against the 600 approx with TwoWire Wire(PB6, PB7, SOFT_FAST);

// #define NO_GLOBAL_INSTANTIATION
#include<Wire.h>
TwoWire Wire2(PB6, PB7, SOFT_FAST);

const int MPU_addr=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
void setup(){
/// SCL,SDA


Wire2.begin();

Wire2.beginTransmission(MPU_addr);
Wire2.write(0x6B); // PWR_MGMT_1 register
Wire2.write(0); // set to zero (wakes up the MPU-6050)
Wire2.endTransmission();
}
void loop(){

//// read data

}

garindo
Posts: 8
Joined: Tue Sep 20, 2016 3:02 pm

Re: MPU-6050 (I2C) Library

Post by garindo » Sat Jun 03, 2017 4:44 pm

Hello...after many hours, autolevel in stm32 work right.

https://www.youtube.com/watch?v=JKp9divSNO8

User avatar
RogerClark
Posts: 7537
Joined: Mon Apr 27, 2015 10:36 am
Location: Melbourne, Australia
Contact:

Re: MPU-6050 (I2C) Library

Post by RogerClark » Sat Jun 03, 2017 10:39 pm

excellent work.

BTW you can embed vidoes, using the Youtube button with just the video ID code


Fingolin
Posts: 15
Joined: Wed May 17, 2017 1:28 pm

Re: MPU-6050 (I2C) Library

Post by Fingolin » Sun Jun 04, 2017 11:45 am

Hi Gardino,
thanks for the tipp!
Nice work I like it and you are using the same frame as I do :)
By the way, what are you using as a remote?

garindo
Posts: 8
Joined: Tue Sep 20, 2016 3:02 pm

Re: MPU-6050 (I2C) Library

Post by garindo » Sun Jun 04, 2017 6:33 pm

Very thanks Rogerclark, for the next video.
Fingolin, the system TxRx radio is radiolink at10 or at9 and you ?

Fingolin
Posts: 15
Joined: Wed May 17, 2017 1:28 pm

Re: MPU-6050 (I2C) Library

Post by Fingolin » Mon Jun 05, 2017 10:55 am

I dont have a remote yet :D that is why I am asking..

Post Reply