1
$\begingroup$

I am trying to manually calibrate the on-board accelerometer of an APM 2.6 controller.

I am using the following code (I found this somewhere, don't remember where) with Arduino 1.0.5 (in Windows environment) to fetch the accelerometer and gyro data:

  #include <SPI.h>
  #include <math.h>

  #define ToD(x) (x/131)
  #define ToG(x) (x*9.80665/16384)

  #define xAxis 0
  #define yAxis 1
  #define zAxis 2

  #define Aoffset 0.8

  int time=0;
  int time_old=0;

  const int ChipSelPin1 = 53;

  float angle=0;
  float angleX=0;
  float angleY=0;
  float angleZ=0;



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

     pinMode(40, OUTPUT);
     digitalWrite(40, HIGH);


     SPI.begin();  
     SPI.setClockDivider(SPI_CLOCK_DIV16); 

     SPI.setBitOrder(MSBFIRST); 
     SPI.setDataMode(SPI_MODE0);


      pinMode(ChipSelPin1, OUTPUT);

     ConfigureMPU6000();  // configure chip
    }

void loop()

{
 Serial.print("Acc X ");


  Serial.print(AcceX(ChipSelPin1));
  Serial.print("   ");
  Serial.print("Acc Y ");
  Serial.print(AcceY(ChipSelPin1));
  Serial.print("   ");
  Serial.print("Acc Z ");  
  Serial.print(AcceZ(ChipSelPin1));  
  Serial.print(" Gyro X ");  
  Serial.print(GyroX(ChipSelPin1)); 
  Serial.print(" Gyro Y ");  
  Serial.print(GyroY(ChipSelPin1)); 
  Serial.print(" Gyro Z ");  
  Serial.print(GyroZ(ChipSelPin1)); 
  Serial.println();
}


void SPIwrite(byte reg, byte data, int ChipSelPin) {
  uint8_t dump;
  digitalWrite(ChipSelPin,LOW);
  dump=SPI.transfer(reg);
  dump=SPI.transfer(data);
  digitalWrite(ChipSelPin,HIGH);
}


uint8_t SPIread(byte reg,int ChipSelPin) {
  uint8_t dump;
  uint8_t return_value;
  uint8_t addr=reg|0x80;
  digitalWrite(ChipSelPin,LOW);
  dump=SPI.transfer(addr);
  return_value=SPI.transfer(0x00);
  digitalWrite(ChipSelPin,HIGH);
  return(return_value);
}


int AcceX(int ChipSelPin) {
  uint8_t AcceX_H=SPIread(0x3B,ChipSelPin);
  uint8_t AcceX_L=SPIread(0x3C,ChipSelPin);
  int16_t AcceX=AcceX_H<<8|AcceX_L;
  return(AcceX);
}


int AcceY(int ChipSelPin) {
  uint8_t AcceY_H=SPIread(0x3D,ChipSelPin);
  uint8_t AcceY_L=SPIread(0x3E,ChipSelPin);
  int16_t AcceY=AcceY_H<<8|AcceY_L;
  return(AcceY);
}


int AcceZ(int ChipSelPin) {
  uint8_t AcceZ_H=SPIread(0x3F,ChipSelPin);
  uint8_t AcceZ_L=SPIread(0x40,ChipSelPin);
  int16_t AcceZ=AcceZ_H<<8|AcceZ_L;
  return(AcceZ);
}


int GyroX(int ChipSelPin) {
  uint8_t GyroX_H=SPIread(0x43,ChipSelPin);
  uint8_t GyroX_L=SPIread(0x44,ChipSelPin);
  int16_t GyroX=GyroX_H<<8|GyroX_L;
  return(GyroX);
}


int GyroY(int ChipSelPin) {
  uint8_t GyroY_H=SPIread(0x45,ChipSelPin);
  uint8_t GyroY_L=SPIread(0x46,ChipSelPin);
  int16_t GyroY=GyroY_H<<8|GyroY_L;
  return(GyroY);
}


int GyroZ(int ChipSelPin) {
  uint8_t GyroZ_H=SPIread(0x47,ChipSelPin);
  uint8_t GyroZ_L=SPIread(0x48,ChipSelPin);
  int16_t GyroZ=GyroZ_H<<8|GyroZ_L;
  return(GyroZ);
}


//--- Function to obtain angles based on accelerometer readings ---//
float AcceDeg(int ChipSelPin,int AxisSelect) {
  float Ax=ToG(AcceX(ChipSelPin));
  float Ay=ToG(AcceY(ChipSelPin));
  float Az=ToG(AcceZ(ChipSelPin));
  float ADegX=((atan(Ax/(sqrt((Ay*Ay)+(Az*Az)))))/PI)*180;
  float ADegY=((atan(Ay/(sqrt((Ax*Ax)+(Az*Az)))))/PI)*180;
  float ADegZ=((atan((sqrt((Ax*Ax)+(Ay*Ay)))/Az))/PI)*180;
  switch (AxisSelect)
  {
    case 0:
    return ADegX;
    break;
    case 1:
    return ADegY;
    break;
    case 2:
    return ADegZ;
    break;
  }
}


//--- Function to obtain angles based on gyroscope readings ---//
float GyroDeg(int ChipSelPin, int AxisSelect) {
  time_old=time;
  time=millis();
  float dt=time-time_old;
  if (dt>=1000)
  {
    dt=0;
  }
  float Gx=ToD(GyroX(ChipSelPin));
  if (Gx>0 && Gx<1.4)
  {
    Gx=0;
  }
  float Gy=ToD(GyroY(ChipSelPin));
  float Gz=ToD(GyroZ(ChipSelPin));
  angleX+=Gx*(dt/1000);
  angleY+=Gy*(dt/1000);
  angleZ+=Gz*(dt/1000);
  switch (AxisSelect)
  {
    case 0:
    return angleX;
    break;
    case 1:
    return angleY;
    break;
    case 2:
    return angleZ;
    break;
  }
}


void ConfigureMPU6000()
{
  // DEVICE_RESET @ PWR_MGMT_1, reset device
  SPIwrite(0x6B,0x80,ChipSelPin1);
  delay(150);

  // TEMP_DIS @ PWR_MGMT_1, wake device and select GyroZ clock
  SPIwrite(0x6B,0x03,ChipSelPin1);
  delay(150);

  // I2C_IF_DIS @ USER_CTRL, disable I2C interface
  SPIwrite(0x6A,0x10,ChipSelPin1);
  delay(150);

  // SMPRT_DIV @ SMPRT_DIV, sample rate at 1000Hz
  SPIwrite(0x19,0x00,ChipSelPin1);
  delay(150);

  // DLPF_CFG @ CONFIG, digital low pass filter at 42Hz
  SPIwrite(0x1A,0x03,ChipSelPin1);
  delay(150);

  // FS_SEL @ GYRO_CONFIG, gyro scale at 250dps
  SPIwrite(0x1B,0x00,ChipSelPin1);
  delay(150);

  // AFS_SEL @ ACCEL_CONFIG, accel scale at 2g (1g=8192)
  SPIwrite(0x1C,0x00,ChipSelPin1);
  delay(150);
}

My objective use to calibrate the accelerometers (and gyro), so that I can use them without having to depend on Mission Planner.

I'm reading values like:

Acc X 288   Acc Y -640   Acc Z 16884 Gyro X -322 Gyro Y 26 Gyro Z 74
Acc X 292   Acc Y -622   Acc Z 16854 Gyro X -320 Gyro Y 24 Gyro Z 79
Acc X 280   Acc Y -626   Acc Z 16830 Gyro X -328 Gyro Y 23 Gyro Z 71
Acc X 258   Acc Y -652   Acc Z 16882 Gyro X -314 Gyro Y 22 Gyro Z 78
Acc X 236   Acc Y -608   Acc Z 16866 Gyro X -321 Gyro Y 17 Gyro Z 77
Acc X 238   Acc Y -642   Acc Z 16900 Gyro X -312 Gyro Y 26 Gyro Z 74
Acc X 226   Acc Y -608   Acc Z 16850 Gyro X -321 Gyro Y 26 Gyro Z 68
Acc X 242   Acc Y -608   Acc Z 16874 Gyro X -325 Gyro Y 27 Gyro Z 69
Acc X 236   Acc Y -576   Acc Z 16836 Gyro X -319 Gyro Y 19 Gyro Z 78
Acc X 232   Acc Y -546   Acc Z 16856 Gyro X -321 Gyro Y 24 Gyro Z 68
Acc X 220   Acc Y -624   Acc Z 16840 Gyro X -316 Gyro Y 30 Gyro Z 77
Acc X 252   Acc Y -594   Acc Z 16874 Gyro X -320 Gyro Y 19 Gyro Z 59
Acc X 276   Acc Y -622   Acc Z 16934 Gyro X -317 Gyro Y 34 Gyro Z 69
Acc X 180   Acc Y -564   Acc Z 16836 Gyro X -320 Gyro Y 28 Gyro Z 68
Acc X 250   Acc Y -596   Acc Z 16854 Gyro X -329 Gyro Y 33 Gyro Z 70
Acc X 220   Acc Y -666   Acc Z 16888 Gyro X -316 Gyro Y 19 Gyro Z 71
Acc X 278   Acc Y -596   Acc Z 16872 Gyro X -307 Gyro Y 26 Gyro Z 78
Acc X 270   Acc Y -642   Acc Z 16898 Gyro X -327 Gyro Y 28 Gyro Z 72
Acc X 260   Acc Y -606   Acc Z 16804 Gyro X -308 Gyro Y 31 Gyro Z 64
Acc X 242   Acc Y -650   Acc Z 16906 Gyro X -313 Gyro Y 31 Gyro Z 78
Acc X 278   Acc Y -628   Acc Z 16898 Gyro X -309 Gyro Y 22 Gyro Z 67
Acc X 250   Acc Y -608   Acc Z 16854 Gyro X -310 Gyro Y 23 Gyro Z 75
Acc X 216   Acc Y -634   Acc Z 16814 Gyro X -307 Gyro Y 27 Gyro Z 83
Acc X 228   Acc Y -604   Acc Z 16904 Gyro X -326 Gyro Y 17 Gyro Z 75
Acc X 270   Acc Y -634   Acc Z 16898 Gyro X -320 Gyro Y 31 Gyro Z 77

From what I understand, SPIread(...,...) returns an analog voltage value from the data pins of the sensor, which happens to be proportional to the acceleration values. Right?

My question is: How do I go about calibrating the accelerometer?

What I've tried till date: I've tried the "place horizontal... place nose down... left side, right side" technique used by mission planner.

Basically, when placed on horizontal position, the sensor is experiencing +1g on it's Z axis and 0g in X and Y axis. Left/right side provides ±1g on Y axis and nose down/up provides ±1g on X axis.

Now for every orientation, I've passed the raw sensor data through a LPF and then computed the mean, median and SD of this sensor data over 100 iterations. I store this mean, median and SD value in the EEPROM for each axis (one for +1g and one for 0g).

Now, when I use the sensor, I load the stats from the EEPROM, match the mean/median and standard deviation with the current reading of 4/5 iterations.

Here I'm working under the assumption that the values between 0g and +1g (and anything above 1g) can be interpolated/extrapolated from the data using a linear plot.

  • Is this the correct approach for calibration?
  • Can you suggest a better way?
  • I noticed that the maxima/minima for each axis is different. Is this the expected outcome or is there something wrong in the code?
  • What do I do with the gyro? How to calibrate for angular acceleration?
Improve this question
$\endgroup$
0

1 Answer 1

Reset to default
1
$\begingroup$

You can use least square approach and solve this problem using Gauss-Newton method. Following blog post sums it up very well.

http://chionophilous.wordpress.com/2011/08/26/accelerometer-calibration-iii-improving-accuracy-with-least-squares-and-the-gauss-newton-method/

Improve this answer
$\endgroup$
1
  • $\begingroup$ I love people who document their work in elaborate blogs. :) Gauss Newton would be a nice approach, trying that next... $\endgroup$
    – metsburg
    Dec 18, 2013 at 6:41

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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