gx = ((gix * GYRO_RANGE) / GYRO_SCALE); // DPS
gx *= DEG_TO_RAD; // radians per second
The algorithm doesn't want to calculate orientation angles based only on the accelerometer, it still uses a gyroscope, otherwise I can't explain the drift I see.
/*
This sketch shows to initialize the filter and update it with the imu output. It also shows how to get the euler angles of the estimated heading orientation.
*/
/* NOTE: The accelerometer MUST be calibrated for the fusion to work. Adjust the
AX, AY, AND AZ offsets until the sensor reads (0,0,0) at rest.
*/
#define AHRS_VERSION 13.1
#define PROCESSING
// #define DEBUG
#include <Wire.h>
// library Adafruit LSM303
#include <Adafruit_LSM303.h>
// library L3DG20
#include <L3G.h>
// #include <basicMPU6050.h> // Library for IMU sensor. See this link: https://github.com/RCmags/basicMPU6050
#include <imuFilter.h> // https://github.com/RCmags/imuFilter/
/* // Gyro settings:
#define LP_FILTER 3 // Low pass filter. Value from 0 to 6
#define GYRO_SENS 0 // Gyro sensitivity. Value from 0 to 3
#define ACCEL_SENS 0 // Accelerometer sensitivity. Value from 0 to 3
#define ADDRESS_A0 LOW // I2C address from state of A0 pin. A0 -> GND : ADDRESS_A0 = LOW
// A0 -> 5v : ADDRESS_A0 = HIGH
// Accelerometer offset:
constexpr int AX_OFFSET = 0; // Use these values to calibrate the accelerometer. The sensor should output 1.0g if held level.
constexpr int AY_OFFSET = 0; // These values are unlikely to be zero.
constexpr int AZ_OFFSET = 0;
//-- Set the template parameters:
basicMPU6050<LP_FILTER, GYRO_SENS, ACCEL_SENS, ADDRESS_A0,
AX_OFFSET, AY_OFFSET, AZ_OFFSET
>imu; */
// =========== Settings ===========
imuFilter fusion;
#define G (9.80665)
#define DEG_TO_RAD (M_PI/180)
#define RAD_TO_DEG (180/M_PI)
#define GYRO_RANGE 2000.0f
// #define GYRO_SCALE 28571.4f
#define GYRO_SCALE 32768.0f
#define ACCEL_RANGE 8.0f
#define ACCEL_SCALE 32768.0f
#define STATUS_LED PC13
#define SERIAL_BAUD (115200)
#define GAIN 0.9 /* Fusion gain, value between 0 and 1 - Determines orientation correction with respect to gravity vector.If set to 1 the gyroscope is dissabled. If set to 0 the accelerometer is dissabled (equivant to gyro-only) */
#define SD_ACCEL 0.1 /* Standard deviation of acceleration. Accelerations relative to (0,0,1)g outside of this band are suppresed. Accelerations within this band are used to update the orientation. [Measured in g-force] */
#define FUSION true /* Enable sensor fusion. Setting to "true" enables gravity correction */
int16_t aix, aiy, aiz, gix, giy, giz;// raw
float ax, ay, az, gx, gy, gz, mx, my, mz;// G | deg/s | uGauss
float axX, ayY, azZ, gxX, gyY, gzZ, mxX, myY, mzZ;// m/s^2 | rad/s | microtesla (uT)
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
Adafruit_LSM303 lsm303;
// Adafruit_LSM303 lsm303(&Wire1);
L3G gyro;
void setup() {
Serial.begin(SERIAL_BAUD);
Wire.begin();
Serial.print("AHRS ver: "); Serial.println(AHRS_VERSION, 1);
pinMode(STATUS_LED, OUTPUT); // Status LED
digitalWrite(STATUS_LED, HIGH); // stm32f411 led OFF
// Calibrate imu
// imu.setup();
// imu.setBias();
// Adafruit LSM303
initAccelMag();
// L3DG20
initGyro();
// read accel + mag data
lsm303.read();
// read gyro data
gyro.read();
calibrateSensor(gyroBias, accelBias);
#ifdef DEBUG
Serial.print("gyroBias[0]: ");
Serial.println(gyroBias[0]);
Serial.print("gyroBias[1]: ");
Serial.println(gyroBias[1]);
Serial.print("gyroBias[2]: ");
Serial.println(gyroBias[2]);
Serial.print("accelBias[0]: ");
Serial.println(accelBias[0]);
Serial.print("accelBias[1]: ");
Serial.println(accelBias[1]);
Serial.print("accelBias[2]: ");
Serial.println(accelBias[2]);
delay(2000);
#endif
aix = lsm303.accelData.x - accelBias[0]; // RAW X
aiy = lsm303.accelData.y - accelBias[1]; // RAW Y
aiz = lsm303.accelData.z + accelBias[2]; // RAW Z
ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // G
// ax *= G; // in 9.8 m/s/s
ay = ((aiy * ACCEL_RANGE) / ACCEL_SCALE); // G
// ay *= G; // in 9.8 m/s/s
az = ((aiz * ACCEL_RANGE) / ACCEL_SCALE); // G
// az *= G; // in 9.8 m/s/s
#if FUSION
// Set quaternion with gravity vector
// fusion.setup( imu.ax(), imu.ay(), imu.az() );
fusion.setup( ax, -az, ay );
#else
// Just use gyro
fusion.setup();
#endif
}
void loop() {
// read accel + mag data
lsm303.read();
// read gyro data
gyro.read();
aix = lsm303.accelData.x - accelBias[0]; // RAW
aiy = lsm303.accelData.y - accelBias[1]; // RAW
aiz = lsm303.accelData.z + accelBias[2]; // RAW
ax = ((aix * ACCEL_RANGE) / ACCEL_SCALE); // 1G
// ax *= G; // in 9.8 m/s/s
ay = ((aiy * ACCEL_RANGE) / ACCEL_SCALE); // 1G
// ay *= G; // in 9.8 m/s/s
az = ((aiz * ACCEL_RANGE) / ACCEL_SCALE); // 1G
// az *= G; // in 9.8 m/s/s
// Serial.print("ax: ");
// Serial.println(ax);
// Serial.print("ay: ");
// Serial.println(ay);
// Serial.print("az: ");
// Serial.println(az);
gix = gyro.g.x - gyroBias[0];// RAW
giy = gyro.g.y - gyroBias[1];// RAW
giz = gyro.g.z - gyroBias[2];// RAW
gx = ((gix * GYRO_RANGE) / GYRO_SCALE); // DPS
gx *= DEG_TO_RAD; // radians per second
gy = ((giy * GYRO_RANGE) / GYRO_SCALE); // DPS
gy *= DEG_TO_RAD; // radians per second
gz = ((giz * GYRO_RANGE) / GYRO_SCALE); // DPS
gz *= DEG_TO_RAD; // radians per second
#ifdef DEBUG
Serial.print("aix: ");
Serial.println(aix);
Serial.print("aiy: ");
Serial.println(aiy);
Serial.print("aiz: ");
Serial.println(aiz);
// Serial.print("ax: ");
// Serial.println(ax);
// Serial.print("ay: ");
// Serial.println(ay);
// Serial.print("az: ");
// Serial.println(az);
Serial.print("gix: ");
Serial.println(gix);
Serial.print("giy: ");
Serial.println(giy);
Serial.print("giz: ");
Serial.println(giz);
// Serial.print("gx: ");
// Serial.println(gx);
// Serial.print("gy: ")
// Serial.println(gy);
// Serial.print("gz: ");
// Serial.println(gz);
delay(200);
#endif
// Update filter:
#if FUSION
/* NOTE: GAIN and SD_ACCEL are optional parameters */
fusion.update( gx, -gz, -gy, ax, az, ay, GAIN, SD_ACCEL );
#else
// Only use gyroscope
fusion.update( imu.gx(), imu.gy(), imu.gz() );
#endif
/* // Display angles:
Serial.print( fusion.pitch() );
Serial.print( " " );
Serial.print( fusion.yaw() );
Serial.print( " " );
Serial.print( fusion.roll() );
// timestep
Serial.print( " " );
Serial.print( fusion.timeStep(), 6 );
Serial.println(); */
#ifdef PROCESSING
Serial.print("Orientation: ");
Serial.print(fusion.pitch() * RAD_TO_DEG); // roll
Serial.print(" ");
Serial.print(fusion.roll() * RAD_TO_DEG); // pitch
Serial.print(" ");
Serial.println(fusion.yaw() * RAD_TO_DEG); // yaw
#endif
}
bool initAccelMag()
// void initAccelMag()
{
bool successAccelMag = false;
if (!lsm303.begin())
{
successAccelMag = false;
#ifdef DEBUG
Serial.println("Failed to autodetect accel + mag LSM303!");
#endif
while (1);
} else {
successAccelMag = true;
#ifdef DEBUG
Serial.println("Accel + mag LSM303 FOUND!");
#endif
}
lsm303.enable_temperatureSensor();
// tempRaw = lsm303.get_temperatureData();
// lsm303.setMagGain(Adafruit_LSM303::LSM303_MAGGAIN_1_3);
lsm303.setMagGain(Adafruit_LSM303::LSM303_MAGGAIN_8_1);
// 0x08 = 0b00001000 2g
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x08);
// 0x18 = 0b00011000 4g
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x18);
// 0x28 = 0b00101000 8g
lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x28);
// 0x38 = 0b00111000 16g
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG4_A, 0x38);
// 0 Boot - 00 HP-Filter - 0 filtered data selection - 0 HP filter enabled for int2 source - 0 HP filter enabled for int1 source -
// 00 HP Filter Cut off Frequ
// 0b00000000
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0x00);
// 0b10000100 - 0x84
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0x84);
// 0b11111111 - 0xF
// lsm303.write8(LSM303_ADDRESS_ACCEL, Adafruit_LSM303::LSM303_REGISTER_ACCEL_CTRL_REG2_A, 0xFF);
lsm303.enable_temperatureSensor();
return successAccelMag;
}
bool initGyro()
// void initGyro()
{
bool successGyro = false;
if (!gyro.init())
{
successGyro = false;
#ifdef DEBUG
Serial.println("Failed to autodetect gyro L3GD20!");
#endif
while (1);
} else {
successGyro = true;
#ifdef DEBUG
Serial.println("Gyro L3DG20 FOUND!");
#endif
}
// enableDefault
// 0x5F = 0b01011111
// DR = 01 (190 Hz ODR); BW = 01 (25 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
gyro.enableDefault();
// gyro.writeReg(L3G::CTRL_REG4, 0x00); // 250 dps full scale 0b0000000
// gyro.writeReg(L3G::CTRL_REG4, 0x10); // 500 dps full scale 0b00100000
gyro.writeReg(L3G::CTRL_REG4, 0x20); // 2000 dps full scale 0b00110000
// gyro.writeReg(L3G::CTRL_REG1, 0x0F); // normal power mode, all axes enabled, ODR 95Hz, Cut-Off 12.5 Hz - 0b00001111
// 0x6F = 0b01101111
// DR = 01 (190 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
// gyro.writeReg(L3G::CTRL_REG1, 0x6F);
// 0xAF = 0b10101111
// DR = 10 (380 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
// gyro.writeReg(L3G::CTRL_REG1, 0xAF);
// 0xBF = 0b10111111
// DR = 10 (380 Hz ODR); BW = 11 (100 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled)
gyro.writeReg(L3G::CTRL_REG1, 0xBF);
return successGyro;
}
// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void calibrateSensor(float * dest1, float * dest2)
{
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
// uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
// uint16_t accelsensitivity = 16384; // = 16384 LSB/g
uint16_t gyrosensitivity = GYRO_RANGE;
uint16_t accelsensitivity = ACCEL_RANGE;
fifo_count = 100;
packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
// Form signed 16-bit integer for each sample in FIFO
accel_temp[0] = (int16_t) lsm303.accelData.x;
accel_temp[1] = (int16_t) lsm303.accelData.y;
accel_temp[2] = (int16_t) lsm303.accelData.z;
gyro_temp[0] = (int16_t) gyro.g.x;
gyro_temp[1] = (int16_t) gyro.g.y;
gyro_temp[2] = (int16_t) gyro.g.z;
// Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[0] += (int32_t) accel_temp[0];
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];
}
// Normalize sums to get average count biases
accel_bias[0] /= (int32_t) packet_count;
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;
if(accel_bias[1] > 0L) {accel_bias[1] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[1] += (int32_t) accelsensitivity;}
// Push gyro biases to hardware registers
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
// Output scaled accelerometer biases for manual subtraction in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}
It would be great if you tried to run this code with similar sensors or others, for example icm20948 or bno055.
Let me know if you need a graphic attitude indicator in the processing language, I will send it to you in an archive.
I also plan to test the algorithm based on raw data from the gyroscope and accelerometer from the flight gear simulator, as I did here
I would be glad for a little help.