6D0F - ITG3200+ADXL345

Other Hardware Sensors
1

I've searched around the internet for days and i couldn't get a stable reading from these sensors, they are just messed up like:

Gx:2.03 | Gy:-0.13 | Gz:-0.39                     
Ax:6.00 | Ay:-26.00 | Az:511.00                    
psi:-110.75 | theta:4.04 | phi:-58.83

And changing quite randomly, though they do respond to rotating and moving the chip

I'd be eternally grateful for any advice, here goes the small code I'm currently using:

#include "Wire.h"
#define GYRO_ADDR 0x68
#define ACCEL_ADDR 0x53
#define DEG2RAD 0.017453292512f
#define RAD2DEG 57.29577951309f

#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif

float q[3];
float q0, q1, q2, q3;
float exInt, eyInt, ezInt;
float twoKp, twoKi;
float integralFBx, integralFBy, integralFBz;
float now, lastupdate, samplefreq;

float offset[] = {0,0,0};

void setup()
{
  q0 = 1.0f;
  q1 = q2 = q3 = 0.0f;
  exInt = eyInt = ezInt = 0.0;
  twoKp = 1.0f;
  twoKi = 0.2f;
  now = lastupdate = 0;
  
  Serial.begin(9600);
  Wire.begin();
  delay(50);
  cbi(PORTC, 4);//disable internal
  cbi(PORTC, 5);//pullups
  Serial.print("Initiating gyroscope and accelerometer... ");
  init_gyro();
  init_accel();
  delay(1000);
  Serial.println("done.");
  Serial.print("Calibrating gyroscope... ");
  zero_gyro();
  Serial.println("done.");
  Serial.print("Offsets are: ");
  Serial.print(offset[0]);
  Serial.print(", ");
  Serial.print(offset[1]);
  Serial.print(", ");
  Serial.print(offset[2]);
  Serial.println(".");
  delay(3000);//debug
  Serial.print(27, BYTE);
  Serial.print("[2J");
}

void writeto(int dev, byte addr, byte val)
{
  Wire.beginTransmission(dev);
  Wire.send(addr);
  Wire.send(val);
  Wire.endTransmission();
}

void readfrom(byte device, byte fromaddr, int num, byte result[])
{
  Wire.beginTransmission(device);
  Wire.send(fromaddr);
  Wire.endTransmission();

  Wire.beginTransmission(device);//*
  Wire.requestFrom((int)device,num);
  int i = 0;
  while(Wire.available()) {
    result[i]= Wire.receive();
    i++;
  }
  Wire.endTransmission();//*
}

void init_gyro()
{
  writeto(GYRO_ADDR,0x3E,0x80);//reset
  writeto(GYRO_ADDR,0x3E,0x01);//clock = xgyroref
  writeto(GYRO_ADDR,0x15,0x00);//sample rate divide = 1
  writeto(GYRO_ADDR,0x16,0x18);//2000dg/s, low pass filter 256hz, internal sample rate = 8khz
  writeto(GYRO_ADDR,0x17,0x00); //no interrupt used
}

void init_accel()
{
  //writeto(ACCEL_ADDR, 0x2D, 0x00);//wake
  //writeto(ACCEL_ADDR, 0x2D, 0x10);//autosleep
  //writeto(ACCEL_ADDR, 0x2C, 0x0C);//400hz
  //writeto(ACCEL_ADDR, 0x31, 0x01);//data format
  writeto(ACCEL_ADDR, 0x2D, 0x08);//start
}

void zero_gyro()
{
  float xyz[3];
  float tmpoffset[] = {
    0,0,0      };
  for(int i=0; i < 250; i++) {
    delay(8);
    read_gyro(xyz);
    tmpoffset[0] += xyz[0];
    tmpoffset[1] += xyz[1];
    tmpoffset[2] += xyz[2];
  }
  offset[0] = tmpoffset[0]/250;
  offset[1] = tmpoffset[1]/250;
  offset[2] = tmpoffset[2]/250;
}

void read_accel(float result[])
{
  byte buffer[6];
  readfrom(ACCEL_ADDR, 0x32,6, buffer);
  result[0] = ((((int)buffer[1])<<8)|buffer[0]);
  result[1] = ((((int)buffer[3])<<8)|buffer[2]);
  result[2] = ((((int)buffer[5])<<8)|buffer[4]);
}

void read_gyro(float result[])
{
  byte buffer[6];
  readfrom(GYRO_ADDR,0x1D,6,buffer);
  result[0] = ((((int)buffer[0])<<8)|buffer[1]) - offset[0];
  result[1] = ((((int)buffer[2])<<8)|buffer[3]) - offset[1];
  result[2] = ((((int)buffer[4])<<8)|buffer[5]) - offset[2];
}

void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
  float recipNorm;
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;

  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;

  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;

    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / samplefreq);
      integralFBy += twoKi * halfey * (1.0f / samplefreq);
      integralFBz += twoKi * halfez * (1.0f / samplefreq);
      gx += integralFBx;
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f;
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }

  gx *= (0.5f * (1.0f / samplefreq));
  gy *= (0.5f * (1.0f / samplefreq));
  gz *= (0.5f * (1.0f / samplefreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

float invSqrt(float number) {
  volatile long i;
  volatile float x, y;
  volatile const float f = 1.5F;

  x = number * 0.5F;
  y = number;
  i = * ( long * ) &y;
  i = 0x5f375a86 - ( i >> 1 );
  y = * ( float * ) &i;
  y = y * ( f - ( x * y * y ) );
  return y;
}

void get_quaternion(float g[], float a[])
{
  now = micros();
  samplefreq = 1.0/((now-lastupdate)/1000000.0);
  lastupdate = now;
  AHRSupdate(g[0]/14.357*DEG2RAD, g[1]/14.357*DEG2RAD, g[2]/14.357*DEG2RAD,
  a[0]*0.0078, a[1]*0.0078, a[2]*0.0078);
  q[0] = q0;
  q[1] = q1;
  q[2] = q2;
  q[3] = q3;
}

void get_euler_angles(float angles[])
{
  angles[0] = atan2(2*q[1]*q[2]-2*q[0]*q[3], 2*q[0]*q[0]+2*q[1]*q[1]-1)*RAD2DEG;//psi
  angles[1] = -asin(2*q[1]*q[3]+2*q[0]*q[2])*RAD2DEG;//theta
  angles[2] = atan2(2*q[2]*q[3]-2*q[0]*q[1], 2*q[0]*q[0]+2*q[3]*q[3]-1)*RAD2DEG; // phi
}

void get_angles(float angles[])
{
  float e[3];
  get_euler_angles(e);
  angles[0] = e[0];
  angles[1] = e[1];
  angles[2] = e[2];

  if(angles[0] < 0) angles[0] += 360;
  if(angles[1] < 0) angles[1] += 360;
  if(angles[2] < 0) angles[2] += 360;
}

void getYawPitchRoll(float * ypr) {
  float gx, gy, gz; // estimated gravity direction
  
  gx = 2 * (q[1]*q[3] - q[0]*q[2]);
  gy = 2 * (q[0]*q[1] + q[2]*q[3]);
  gz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
  
  ypr[0] = atan2(2 * q[1] * q[2] - 2 * q[0] * q[3], 2 * q[0]*q[0] + 2 * q[1] * q[1] - 1) * 180/M_PI;
  ypr[1] = atan(gx / sqrt(gy*gy + gz*gz))  * 180/M_PI;
  ypr[2] = atan(gy / sqrt(gx*gx + gz*gz))  * 180/M_PI;
}

void loop()
{
  float g[3];
  float a[3];
  float angles[3];

  read_gyro(g);
  read_accel(a);
  get_quaternion(a,g);
  getYawPitchRoll(angles);

  Serial.print("Gx:");
  Serial.print(g[0]);
  Serial.print(" | Gy:");
  Serial.print(g[1]);
  Serial.print(" | Gz:");
  Serial.print(g[2]);
  Serial.println("                    ");
  Serial.print("Ax:");
  Serial.print(a[0]);
  Serial.print(" | Ay:");
  Serial.print(a[1]);
  Serial.print(" | Az:");
  Serial.print(a[2]);
  Serial.println("                    ");
  Serial.print("psi:");
  Serial.print(angles[0]);
  Serial.print(" | theta:");
  Serial.print(angles[1]);
  Serial.print(" | phi:");
  Serial.print(angles[2]);
  Serial.println("                    ");
  Serial.println("                    ");
  Serial.println("                    ");
  Serial.println("                    ");
  
  Serial.print(27, BYTE);
  Serial.print("[H");
  
  delay(10);
}

Thanks in advance.

2

What's wrong with those values, look perfectly OK to me.

Gyros and accelerometers are noisy, you'd expect > 1 LSB's worth of random noise at all times. You want those LSB's so they cancel out in the integration step to reduce systematic drift.

I presume 512 is fine in the accelerometer as 1g on the range you've selected?

The bulk of that code is doing quarterion geometry - have you ensured the readings from the sensors are appropriately scaled? In other words what values is AHRSUpdate() expecting - not raw sensor values surely? standard rotation quarternions are unit quarternions.