I put 9 axis acceleration & gyro & magnetic sensor MPU 9250 in my own digital stabilizer

Hello everyone! I’m RoboTakao.

Last time I tried 9 axis acceleration & gyro & magnetic sensor MPU 9250 with ArduinoNano. I tried putting it in the digital stabilizer, so I will report it.

Please refer to the previous post of the digital stabilizer here.
http://blog.robotakao.jp/blog-entry-227.html
http://robotakao.jp/digitalstabilizer/index.html

If you use only gyros, the angle drifts. It is expected to keep without drift by using the geomagnetic sensor.

Although I wrote last time, since MPU 9250 is roughly the same as the MPU 6050 in pin arrangement, I could use the circuit without changing it. (Since there are few pins of the header socket, it is floating, but there is no problem.)

Since the MPU 9250 is a geomagnetic sensor added to the MPU 6050, I would like to explain how to use it in Arduino.



Connection

Simply replacing the MPU 6050 with the MPU 9250 for the original digital stabilizer connection.





Description of sketch (Explanation of the geomagnetic sensor AK 8963 part)

In setup()

Enter I2C address of AK8963
const int AK8963_addr=0x0c;

Send the address of MPU9250 to I2C
Wire.beginTransmission(MPU_addr);

Send address for bypass mode setting of magnetic sensor
Wire.write(0x37);
Set to bypass mode so that magnetic sensor can be used
Wire.write(0x02);

Send the address of AK8963 to I2C
Wire.beginTransmission(AK8963_addr);

Send address for magnetic sensor setting
Wire.write(0x0a);

Set output cycle of magnetic sensor and start (0x16:100Hz 0x12:8Hz)
Wire.write(0x16);

In loop()

Address transmission at the beginning of the data of AK8963
Wire.write(0x03);

Request seven pieces of data (unless seven pieces of data are received, the next one is not updated)
Wire.requestFrom(AK8963_addr,7,true);

Docking up and down the X, Y, Z axis data (carefully in order)
MgYr=Wire.read()|(Wire.read()<<8);
MgXr=Wire.read()|(Wire.read()<<8);
MgZr=-(Wire.read()|(Wire.read()<<8));

Convert unit (micro Tesla)
MgX = MgXr / 32768.0f * 4800.0f;
MgY = MgYr / 32768.0f * 4800.0f;
MgZ = MgZr / 32768.0f * 4800.0f;/

About offset

The geomagnetic sensor is detected off the earth due to picking up the magnetism of the surrounding environment in addition to the terrestrial magnetism of the earth and due to sensor variations.
When you move the sensor to figure 8, you can see that it is misaligned like a blue plot of the graph.
Calculate the center position from the maximum and minimum values of this plot and offset it like a red plot so that the center becomes zero.



if (MgX <= MgXmin) MgXmin = MgX;
if (MgX >= MgXmax) MgXmax = MgX;
if (MgY <= MgYmin) MgYmin = MgY;
if (MgY >= MgYmax) MgYmax = MgY;
if (MgZ <= MgZmin) MgZmin = MgZ;
if (MgZ >= MgZmax) MgZmax = MgZ;

MgXoff = (MgXmax - MgXmin)/2 + MgXmin;
MgYoff = (MgYmax - MgYmin)/2 + MgYmin;
MgZoff = (MgZmax - MgZmin)/2 + MgZmin;

Angle calculation (including offset correction)

In this time, angle calculation by geomagnetism is included only around the Y axis.

if(MgX - MgXoff > 0.0){
MgYAng = atan((MgZ - MgZoff)/(MgX - MgXoff))/3.142*180 - 90;
}
}else if(MgX - MgXoff < 0.0){
MgYAng = atan((MgZ - MgZoff)/(MgX - MgXoff))/3.142*180 + 90;
}

Apply complementary filter with data from gyro and data from geomagnetic sensor

angY = 0.95*(angY - GyY*dt*0.001) - 0.05*MgYAng;


Operation check

If you do not use the geomagnetic sensor, you can see that it contains a drift of the gyro and is gradually misaligned.



Turn round and round for offset adjustment.



If you use the geomagnetic sensor, you can see that it keeps a certain angle without drifting.



Sketch

#include
#include 

const int MPU_addr=0x68;  // I2C address of the MPU-6050
const int AK8963_addr=0x0c;  // I2C address of the AK8963
int16_t AcXr,AcYr,AcZr,Tmpr,GyXr,GyYr,GyZr,MgXr,MgYr,MgZr;
int16_t Mg[7];
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ,MgXmin,MgYmin,MgZmin,MgXmax,MgYmax,MgZmax,MgXoff,MgYoff,MgZoff;
float AcXAng,AcZAng,MgYAng;
float angX = 0.0;
float angY = 0.0;
float angZ = 0.0;
int angXi = 0;
int angYi = 0;
int angZi = 0;
int angXiLast = 0;
int angYiLast = 0;
int angZiLast = 0;
int aX,aY,aZ;
int angXout, angYout,angZout;
int ZeroX = 93;
int ZeroY = 82;
int ZeroZ = 93;
float dt=30;
int djoy=3;
int joy_1 = 1;
int joy_2 = 2;
int val1,val2;
int angjoy1 =0;
int angjoy2 =0;
int dstep = 50;

Servo myservo2;
Servo myservo3;
Servo myservo4;

int Width_Min = 700;    // Minimum pulse width（ms）
int Width_Max = 2300;   // Maximum pulse width（ms）


void setup() 
{ 
  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);

  //LPF Setting
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();

  //AK8963 Setting
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x37);  // Address for setting bypass mode of magnetic sensor
  Wire.write(0x02);  // bypass mode (Magnetic sensor can be used)
  Wire.endTransmission();
  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x0a);  //Address for magnetic sensor setting
  Wire.write(0x16);  // Output cycle of the magnetic sensor (100Hz) 0x12: Output cycle of the magnetic sensor (8Hz)
  Wire.endTransmission();
  
  myservo2.attach(2,Width_Min,Width_Max);
  myservo3.attach(3,Width_Min,Width_Max);
  myservo4.attach(4,Width_Min,Width_Max);

  Serial.begin(9600);

} 

void loop() 
{
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcXr=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)    
  AcYr=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZr=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmpr=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyXr=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyYr=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZr=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x02);  // starting with register 0x02
  Wire.requestFrom(AK8963_addr,1,true);
  Wire.endTransmission(false);

  //ST1Bit=Wire.read();

  if((Wire.read() & 0x01)){   // Check if reading is ready
    Wire.beginTransmission(AK8963_addr);
    Wire.write(0x03);  // starting with register 0x02
    Wire.endTransmission(false);
    Wire.requestFrom(AK8963_addr,7,true);  // request a total of 7 registers
  
    MgYr=Wire.read()|(Wire.read()<<8);
    MgXr=Wire.read()|(Wire.read()<<8);
    MgZr=-(Wire.read()|(Wire.read()<<8));
  }

// joystic data
  val1 = analogRead(joy_1);
  val2 = analogRead(joy_2);

  if (val1 < 300){
    angjoy1 = angjoy1 - djoy;
  }
  if (val1 > 700){
    angjoy1 = angjoy1 + djoy;
  }
  if (val2 < 300){
    angjoy2 = angjoy2 - djoy;
  }
  if (val2 > 700){
    angjoy2 = angjoy2 + djoy;
  }
  
// accel data
// full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
  AcX = AcXr/16384.0;
  AcY = AcYr/16384.0;
  AcZ = AcZr/16384.0;
  
// temperture
  Tmp = Tmpr/320.00+36.53;

// gyro data
// full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
  GyX = GyXr/131.0;
  GyY = GyYr/131.0;
  GyZ = GyZr/131.0;

  AcXAng = atan(AcZ/AcY)/3.142*180;
  AcZAng = atan(AcX/AcY)/3.142*180;

// Magnetic data
  MgX = MgXr / 32768.0f * 4800.0f;// Conversion to [uT]
  MgY = MgYr / 32768.0f * 4800.0f;// Conversion to [uT]
  MgZ = MgZr / 32768.0f * 4800.0f;// Conversion to [uT] 

//Calculation of offset

  if (MgX <= MgXmin) MgXmin = MgX;
  if (MgX >= MgXmax) MgXmax = MgX;
  if (MgY <= MgYmin) MgYmin = MgY;
  if (MgY >= MgYmax) MgYmax = MgY;
  if (MgZ <= MgZmin) MgZmin = MgZ;
  if (MgZ >= MgZmax) MgZmax = MgZ;

  MgXoff = (MgXmax - MgXmin)/2 + MgXmin;
  MgYoff = (MgYmax - MgYmin)/2 + MgYmin;
  MgZoff = (MgZmax - MgZmin)/2 + MgZmin;

  if(MgX - MgXoff > 0.0){
       MgYAng = atan((MgZ - MgZoff)/(MgX - MgXoff))/3.142*180 - 90;
       }
    }else if(MgX - MgXoff < 0.0){
       MgYAng = atan((MgZ - MgZoff)/(MgX - MgXoff))/3.142*180 + 90;
    }
  
// Complementary filter
  angX = 0.95*(angX - GyX*dt*0.001*1.5) + 0.05*AcXAng;
  angZ = 0.95*(angZ + GyZ*dt*0.001*1.2) + 0.05*AcZAng;
  angY = 0.95*(angY - GyY*dt*0.001) - 0.05*MgYAng;

  angXiLast = angXi;
  angYiLast = angYi;
  angZiLast = angZi;

  angXi = (int) angX + angjoy1;
  angYi = (int) angY - angjoy2;
  angZi = (int) angZ;

  aX = angXi - angXiLast;
  aY = angYi - angYiLast;
  aZ = angZi - angZiLast;
  
  for (int k=1; k <= dstep ; k++){
    angXout = aX*k/dstep + angXiLast;
    angYout = aY*k/dstep + angYiLast;
    angZout = aZ*k/dstep + angZiLast;

    if (angXout < -40) angXout = -40;
    if (angXout > 20) angXout = 20;
    if (angYout < -80) angYout = -80;
    if (angYout > 80) angYout = 80;
    if (angZout < -70) angZout = -70;
    if (angZout > 70) angZout = 70;

    myservo2.write(ZeroY - angYout);
    myservo3.write(ZeroX - angXout);
    myservo4.write(ZeroZ - angZout);

    delayMicroseconds(dt/dstep*1000);
  }
}

Thank you! Good-bye!