Quadcopter Stabilizing (I2C Gyro Acc)

Other Hardware Sensors
1

I am using an I2C 10DOF IMU from ebay. (adxl345 and L3G4200d are what I am using on it) I have successfully used the accelerometer to get angles by mapping the values to -90, 90. However, I am having issues with the Gyroscope (L3G4200d). I based the code off the one that worked for the ADXL345. I found an example code that gave some values to send to initialize it, that I couldn't find in the data sheet. I found the multiplier for the numbers and used it to get degrees/second. However, the x and y values come out the same.

I also tried to change it to an angle, but I can't get that to work either. (The previous problem is more important)

#include <Wire.h>

#define GYRO (0x69)
#define TO_READ (4)


byte buff_1[4]; //buffer array to recieve data from device


float MDPS = 0.00875; //datasheet said this was the multiplier

void setup()
{
  Serial.begin(9600);
  
  Wire.begin();
  
  //setup Gyro (L3G4200)
  writeTo(GYRO, 0x20, 0b00001111); //couldn't find this in data
  //sheet, but it was in an example code
  writeTo(GYRO, 0x21, 0b00000000);
  writeTo(GYRO, 0x22, 0b00001000);
  writeTo(GYRO, 0x23, 0b00000000);  
}


void loop()
{
   //Gyro values
  
  int x_Gyro, y_Gyro;  
  
  //I send it a buffer to give back with data
  readFrom(GYRO, 0b1101000, TO_READ, buff_1);   
  
  //bitwise operations to shift MSB over 8 and tack on LSB
  x_Gyro = ((((int)buff_1[1]) << 8) | buff_1[0]) * MDPS;

  y_Gyro = ((((int)buff_1[3]) << 8) | buff_1[2]) * MDPS;
    
  Serial.print(" Gyro: x = ");
  Serial.print(x_Gyro);
  Serial.print(" y = ");
  Serial.print(y_Gyro);
  Serial.write(10);
  //delay(200);
}

//basic I2C write function
void writeTo(int device, byte address, byte val)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.write(val);
  Wire.endTransmission();
}

//this takes the device address, the info address, the number of 
//bytes you want, and a buff array (to send the data back with) 
// and gives back a buff array
void readFrom(int device, byte address, int num, byte buff[])
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.endTransmission();
  Wire.beginTransmission(device);
  Wire.requestFrom(device, num);
  
  int i = 0;
  while(Wire.available())
  {
    buff[i] = Wire.read();
    i++;
  }
  
  Wire.endTransmission();
}

Moderator edit: quote tags swapped for code tags. AWOL

2

UPDATE:

I was able to get the correct values by grabbing the MSB and LSB separately instead of in a buff array. I don't know why it worked.

I tried to make a simple integration, but the values skyrocket immediately.

#include <Wire.h>

#define GYRO (0x69)
#define TO_READ (4)


byte buff_1[4]; //buffer array to recieve data from device


float MDPS = 0.00875; //datasheet said this was the multiplier
float GyroAngleX = 0.0; 
float GyroAngleY = 0.0;
float DT = 0.01; 
float xFix, yFix; 

void setup()
{
  Serial.begin(9600);
  
  Wire.begin();
  
  //setup Gyro (L3G4200)
  writeTo(GYRO, 0x20, 0b00001111); //couldn't find this in data
  //sheet, but it was in an example code
  writeTo(GYRO, 0x21, 0b00000000);
  writeTo(GYRO, 0x22, 0b00001000);
  writeTo(GYRO, 0x23, 0b00000000);
  
  /*
  byte xMSB = readFrom(GYRO, 0x29, 1);
  byte xLSB = readFrom(GYRO, 0x28, 1);
  xFix = ((xMSB << 8) | xLSB) * MDPS;

  byte yMSB = readFrom(GYRO, 0x2B, 1);
  byte yLSB = readFrom(GYRO, 0x2A, 1);
  yFix = ((yMSB << 8) | yLSB) * MDPS;
  
  Serial.print(xFix);
  Serial.println(yFix);
  Serial.println(" ");
   */
  
}


void loop()
{
   
  
  //Gyro values
  
  float x, y;
  
  //time between loops
  DT = micros();
  DT = DT / 1000000; //change to seconds
  
   
  byte xMSB = readFrom(GYRO, 0x29, 1);
  byte xLSB = readFrom(GYRO, 0x28, 1);
  x = ((xMSB << 8) | xLSB) * MDPS; //shift MSB over 8 bits and 
  //tack on the LSB, then muliply it by MDPS to get
  // degrees per second

  byte yMSB = readFrom(GYRO, 0x2B, 1);
  byte yLSB = readFrom(GYRO, 0x2A, 1);
  y = ((yMSB << 8) | yLSB) * MDPS;
  
  
  
  //degrees = previous degree plus degrees-per-second times seconds
  GyroAngleX = (GyroAngleX + (x) * DT);
  
  GyroAngleY = (GyroAngleY + (y) * DT);
  
  //x = GyroAngleX;
  //y = GyroAngleY;
  
  
  
  Serial.print(" Gyro: x = ");
  Serial.print(GyroAngleX);
  Serial.print(" y = ");
  Serial.print(GyroAngleY);
  Serial.write(10);
  delay(1);
}



//basic I2C write function
void writeTo(int device, byte address, byte val)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.write(val);
  Wire.endTransmission();
}


//this takes the device address, the info address, and 1
//, returns a byte
int readFrom(int device, byte address, int num)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.endTransmission();
  Wire.beginTransmission(device);
  Wire.requestFrom(device, num);
  
  int val;
  val = Wire.read();
  return val;
  
  Wire.endTransmission();
}

Moderator edit: Again, quote tags swapped for code tags.

3

Update:

I think I figured it out. I went ahead and wrote a new code using functions to try to make it neater. I combined the Gyro and Accelerometer readings to make angles using a basic complementary filter. It doesn't quite zero out, I may try to do a better calibration method, or possibly try to figure out some type of Kalman filter.

@Moderator, sorry about using quote tags before, this time I will use code tags.

#include <Wire.h>

#define GYRO (0x69) //gyro address
#define ACC (0x53) //acclerometer address

int xAcc, yAcc; //raw data from sensor
int xAccFix = 0, yAccFix = 0; //calibration data

float xAccAngle, yAccAngle; //angle from accelerometer data

int xGyro, yGyro; //raw data from gyro
int xGyroFix, yGyroFix;

float MDPS = 0.0000875; //change raw to degrees per second
float xGyroAngle, yGyroAngle;
float dt = 0.01; //time between loops

float xAngle = 0.0, yAngle = 0.0;

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  
  AccSetup(); //send setup values to accelerometer
  GyroSetup(); //same for gyro
  
  AccFix(); //calibration values
  GyroFix();
  
   
}


void loop()
{
  
  dt = micros() / 1000000; //time for loop
  
  
  AccGetVals(); //gets values
  
  xAcc = xAcc - xAccFix; //uses calibration values
  yAcc = yAcc - yAccFix;
  
  xAccAngle = map(xAcc, -256, 256, -90, 90); //changes to angle
  yAccAngle = map(yAcc, -256, 256, -90, 90);
    
  
  GyroGetVals();
  
  
  //"integrates" the angular velocities
  xGyroAngle = (xAngle + (xGyro - xGyroFix) * dt); 
  yGyroAngle = (yAngle + (yGyro - yGyroFix) * dt);
  
  //combines it with the accelerometer angle
  xAngle = (0.8) * xGyroAngle + (0.2) * xAccAngle;
  yAngle = (0.8) * yGyroAngle + (0.2) * yAccAngle;
  
  
  Serial.print(xAngle);
  Serial.print("     ");
  Serial.print(yAngle);
  Serial.print("\n"); //newline
  
  
  /*
  Serial.print(xAccAngle);
  Serial.print("   ");
  Serial.print(xGyroAngle);
  Serial.print("       ");
  Serial.print(yAccAngle);
  Serial.print("   ");
  Serial.print(yGyroAngle);
  Serial.print("\n");
    */
}


void AccSetup()
{
  i2cWrite(ACC, 0x2D, 0);
  i2cWrite(ACC, 0x2D, 16);
  i2cWrite(ACC, 0x2D, 8);
}

void GyroSetup()
{
  i2cWrite(GYRO, 0x20, 0b00001111);
  i2cWrite(GYRO, 0x21, 0b00000000);
  i2cWrite(GYRO, 0x22, 0b00001000);
  i2cWrite(GYRO, 0x23, 0b00000000);
}

void AccFix()
{
    
  int i = 1;
  while (i <= 3)
  {
    AccGetVals();
    xAccFix = xAccFix + xAcc;
    yAccFix = yAccFix + yAcc;
    i++;
  }
  
  xAccFix = xAccFix / 3;
  yAccFix = yAccFix / 3;
  
  
  Serial.print(xAccFix);
  Serial.print("    ");
  Serial.println(yAccFix);
  
}

void GyroFix()
{
  
  xGyroFix = 0;
  yGyroFix = 0;
  
  int i = 1;
  while (i <= 3)
  {
    GyroGetVals();
    xGyroFix = xGyroFix + xGyro;
    yGyroFix = yGyroFix + yGyro;
    i++;
  }
  
  xGyroFix = xGyroFix / 3;
  yGyroFix = yGyroFix / 3;
  
}

void AccGetVals()
{
  byte MSB = i2cread(ACC, 0x33);
  byte LSB = i2cread(ACC, 0x32);
  xAcc = ((MSB << 8) | LSB);
  
  MSB = i2cread(ACC, 0x35);
  LSB = i2cread(ACC, 0x34);
  yAcc = ((MSB << 8) | LSB);
  
}

void GyroGetVals()
{
  byte MSB = i2cread(GYRO, 0x29);
  byte LSB = i2cread(GYRO, 0x28);
  xGyro = ((MSB << 8) | LSB) * MDPS;
  
  MSB = i2cread(GYRO, 0x2B);
  LSB = i2cread(GYRO, 0x2A);
  yGyro = ((MSB << 8) | LSB) * MDPS;
  
}

  
  

void i2cWrite(int device, byte address, byte value)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.write(value);
  Wire.endTransmission();
}


int i2cread (int device, byte address)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.endTransmission();
  Wire.beginTransmission(device);
  Wire.requestFrom(device, 1);
  
  int value = Wire.read();
  
  return value;
  
  Wire.endTransmission();
}
4

I finally made some progress on both the code and the quad. It does not fly yet, but hopefully that is just because I don't have enough propellers (one was destroyed). I was able to change the code around until it would stabilize reasonably well on one axis, so my hope is when I get the new propellers I will only have to do minor tweaks.

For those interested, here is the code (I haven't commented it, so if you have questions about what a certain part is for, just ask):

#include <Wire.h>
#include <Servo.h>

#define GYRO (0x69)
#define ACC (0x53)

int currMicros, pastMicros, count = 0;

int xAcc, yAcc;
int xAccFix = 0, yAccFix = 0;


float xAccAngle, yAccAngle;

int xGyro, yGyro;
int xGyroFix, yGyroFix;

float MDPS = 0.0000875;
float xGyroAngle, yGyroAngle;
float dt = 0.01;

Servo myservo, myservo1, myservo2, myservo3;

float xFix = 0, yFix = 0;
float xAngle = 0.0, yAngle = 0.0;

int signalAngleX = 0, signalAngleY = 0;
int throttle = 0;
float xVal_1, xVal_2, yVal_1, yVal_2;

int shutdownCount;


void setup()
{
  Serial3.begin(9600);
  Wire.begin();
  
  rotorStartup();
  
  
  
  AccSetup();
  GyroSetup();
  
  AccFix();
  GyroFix();
  
}

void loop()
{
  
  pastMicros = currMicros;
  currMicros = micros();
  
  dt = (float) (currMicros - pastMicros) / 1000000;
  
  getSignal();
    
  getAngles();

    if (xAngle < (signalAngleX - 1.5) )
  {
    
    if (xFix < 0)
    {      
      xFix = 0;
    }
    else if (xAngle < (signalAngleX - 5) )
    {
      if (xAngle < (signalAngleX - 10) )
      {
        if (xAngle < (signalAngleX - 15) )
        {
          xFix = 5;
        }
        else xFix = xFix + 1.25;
      }
      else xFix = xFix + 0.25;
    }
    else xFix = xFix + 0.0625;
  }
  
  
  else if (xAngle > (signalAngleX + 1.5) )
  {
       
    if (xFix > 0)
    {      
      xFix = 0;
    }
    else if (xAngle > (signalAngleX + 5) )
    {
      if (xAngle > (signalAngleX + 10) )
      {
        if (xAngle > (signalAngleX + 15) )
        {
          xFix = -5;
        }
        else xFix = xFix - 1.25;
      }
      else xFix = xFix - 0.25;
    }
    else xFix = xFix - 0.0625;
  }
  
  else xFix = 0;
  
  
  
  if (yAngle < (signalAngleY - 1.5) )
  {
    
    if (yFix < 0)
    {      
      yFix = 0;
    }
    else if (yAngle < (signalAngleY - 5) )
    {
      if (yAngle < (signalAngleY - 10) )
      {
        if (yAngle < (signalAngleY - 15) )
        {
          yFix = 5;
        }
        else yFix = yFix + 1.25;
      }
      else yFix = yFix + 0.25;
    }
    else yFix = yFix + 0.0625;
  }
  
  
  else if (yAngle > (signalAngleY + 1.5) )
  {
       
    if (yFix > 0)
    {      
      yFix = 0;
    }
    else if (yAngle > (signalAngleY + 5) )
    {
      if (yAngle > (signalAngleY + 10) )
      {
        if (yAngle > (signalAngleY + 15) )
        {
          yFix = -5;
        }
        else yFix = yFix - 1.25;
      }
      else yFix = yFix - 0.25;
    }
    else yFix = yFix - 0.0625;
  }
  
  else yFix = 0;
  
  count = count + 1;
  if (count >= 25)
  {
    yFix = 0;
    xFix = 0;
    
    count = 0;
  }
  
  if ((yFix > 5) or (yFix < -5))
  {
    yFix = 0;
  }
  
  if ((xFix > 5) or (xFix < -5))
  {
    xFix = 0;
  }

   
    xVal_1 = throttle + xFix;
    xVal_2 = throttle - xFix;
       
    yVal_1 = throttle + yFix;
    yVal_2 = throttle - yFix;
     
  myservo.write(xVal_1);
  myservo1.write(xVal_2);
  myservo2.write(yVal_1);
  myservo3.write(yVal_2);

}



void getSignal()
{
  int x, y;
  
  if (Serial3.available())
  {
    throttle =   Serial3.read();

    x = constrain(Serial3.read(), 0, 30);
    signalAngleX = map(x, 0, 30, -15, 15);

    y = constrain(Serial3.read(), 0, 30);

    signalAngleY = map(y, 0, 30, -15, 15);
    
  }
  
  else if (throttle > 50)
  {
    shutdownCount = shutdownCount + 1;
    if (shutdownCount > 25)
    {
      throttle = throttle - 1;
      shutdownCount = 0;
    }
  }
  
}

void getAngles()
{
  AccGetVals();
  
  xAcc = xAcc - xAccFix;
  yAcc = yAcc - yAccFix;
  
  xAccAngle = map(xAcc, -256, 256, -90, 90);
  yAccAngle = map(yAcc, -256, 256, -90, 90);
    
  GyroGetVals();
  
  xGyroAngle = (xAngle + (xGyro - xGyroFix) * dt); 
  yGyroAngle = (yAngle + (yGyro - yGyroFix) * dt);
  
  xAngle = (0.9) * xGyroAngle + (0.1) * xAccAngle;
  yAngle = (0.9) * yGyroAngle + (0.1) * yAccAngle;
  
  
}


void AccSetup()
{
  i2cWrite(ACC, 0x2D, 0);
  i2cWrite(ACC, 0x2D, 16);
  i2cWrite(ACC, 0x2D, 8);
}

void GyroSetup()
{
  i2cWrite(GYRO, 0x20, 0b00001111);
  i2cWrite(GYRO, 0x21, 0b00000000);
  i2cWrite(GYRO, 0x22, 0b00001000);
  i2cWrite(GYRO, 0x23, 0b00000000);
}

void AccFix()
{
    
  int i = 1;
  while (i <= 3)
  {
    AccGetVals();
    xAccFix = xAccFix + xAcc;
    yAccFix = yAccFix + yAcc;
    i++;
  }
  
  xAccFix = xAccFix / 3;
  yAccFix = yAccFix / 3;
  
  
}

void GyroFix()
{
  
  xGyroFix = 0;
  yGyroFix = 0;
  
  int i = 1;
  while (i <= 3)
  {
    GyroGetVals();
    xGyroFix = xGyroFix + xGyro;
    yGyroFix = yGyroFix + yGyro;
    i++;
  }
  
  xGyroFix = xGyroFix / 3;
  yGyroFix = yGyroFix / 3;
  
}

void AccGetVals()
{
  byte MSB = i2cread(ACC, 0x33);
  byte LSB = i2cread(ACC, 0x32);
  xAcc = ((MSB << 8) | LSB);
  
  MSB = i2cread(ACC, 0x35);
  LSB = i2cread(ACC, 0x34);
  yAcc = ((MSB << 8) | LSB);
  
}

void GyroGetVals()
{
  byte MSB = i2cread(GYRO, 0x29);
  byte LSB = i2cread(GYRO, 0x28);
  xGyro = ((MSB << 8) | LSB) * MDPS;
  
  MSB = i2cread(GYRO, 0x2B);
  LSB = i2cread(GYRO, 0x2A);
  yGyro = ((MSB << 8) | LSB) * MDPS;
  
}

  
  

void i2cWrite(int device, byte address, byte value)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.write(value);
  Wire.endTransmission();
}


int i2cread (int device, byte address)
{
  Wire.beginTransmission(device);
  Wire.write(address);
  Wire.endTransmission();
  Wire.beginTransmission(device);
  Wire.requestFrom(device, 1);
  
  int value = Wire.read();
  
  return value;
  
  Wire.endTransmission();
}

void rotorStartup()
{
  myservo.attach(35);
  myservo1.attach(37);
  myservo2.attach(23);
  myservo3.attach(25);
  
  int val = 10;
  myservo.write(val);
  myservo1.write(val);
  myservo2.write(val);
  myservo3.write(val);
  
  delay(1000);
  
  val = 10;
  
  while (val < 65)
  {
    myservo.write(val);
    myservo1.write(val);
    myservo2.write(val);
    myservo3.write(val);
    
    val++;
    delay(50);
  }
  
  
}
5

My question is why map the accel to -90 , 90, isn't the angle related to cos of gravity vector? Or is it for some other purpose?

6

silverxxx:
My question is why map the accel to -90 , 90, isn't the angle related to cos of gravity vector? Or is it for some other purpose?

Using the map function simplifies it. For angles under ?/6, the trig isn't really necessary (look up small angle approximation). I assume it will never tilt more than 30°, so it shouldn't matter. (I don't plan on doing tricks) I also assume that it is faster for it to do a map function than to do trig.

7

you already made ??your first flight with your Quadcopter? Post a video for us!
I want made a quad too, and I follow you for this!
8)