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?