Turingon/LSM9DS1-Adafruit-AHRS
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

First iteration of the implementation

Browse source
This commit is contained in:
Turingon 2024-06-22 11:41:15 +02:00
parent 76141a90aa
commit 3950a7f3f9
6 changed files with 991 additions and 801 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

129
adafruit-AHRS.cpp
View file

@ -1,129 +0,0 @@
/* The implementation
*/
// VERY IMPORTANT!
//These are the previously determined offsets and scale factors for the gyroscope, accelerometer and magnetometer, using the mega
//The AHRS will not work without the correct calibration parameters
//Gyroscope calibration parameters
G_offset[3] = {75, 31, 142};
//Accelerometer calibration parameters
float A_B[3]
{ -133.33, 72.29, -291.92};
float A_Ainv[3][3]
{ { 1.00260, 0.00404, 0.00023},
{ 0.00404, 1.00708, 0.00263},
{ 0.00023, 0.00263, 0.99905}
};
//Magnetometer calibration parameters
float M_B[3]
{ -922.31, 2199.41, 373.17};
float M_Ainv[3][3]
{ { 1.04492, 0.03452, -0.01714},
{ 0.03452, 1.05168, 0.00644},
{ -0.01714, 0.00644, 1.07005}
};
// local magnetic declination in degrees in Hamburg
float declination = +4.08;
float p[] = {0, 1, 0}; //Y marking on sensor board points toward yaw = 0
// MAHONY PART
// These are the free parameters in the Mahony filter and fusion scheme,
// Kp for proportional feedback, Ki for integral
// Kp is not yet optimized. Ki is not used.
#define Kp 50
static float q[4] = {1.0, 0.0, 0.0, 0.0};
static float yaw, pitch, roll; //Euler angle output
// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
int get_heading(float acc[3], float mag[3], float p[3])
{
float W[3], N[3]; //derived direction vectors
// cross "Up" (acceleration vector, g) with magnetic vector (magnetic north + inclination) with to produce "West"
vector_cross(acc, mag, W);
vector_normalize(W);
// cross "West" with "Up" to produce "North" (parallel to the ground)
vector_cross(W, acc, N);
vector_normalize(N);
// compute heading in horizontal plane, correct for local magnetic declination
int heading = round(atan2(vector_dot(W, p), vector_dot(N, p)) * 180 / M_PI + declination);
heading = -heading; //conventional nav, heading increases North to East
heading = (heading + 720)%360; //apply compass wrap
return heading;
}
// subtract offsets and correction matrix to accel and mag data
void get_scaled_IMU(float Axyz[3], float Mxyz[3], float ax, float ay, float az, float mx, float my, float mz) {
byte i;
float temp[3];
Axyz[0] = ax;
Axyz[1] = ay;
Axyz[2] = az;
Mxyz[0] = mx;
Mxyz[1] = my;
Mxyz[2] = mz;
//apply offsets (bias) and scale factors from Magneto
for (i = 0; i < 3; i++) temp[i] = (Axyz[i] - A_B[i]);
Axyz[0] = A_Ainv[0][0] * temp[0] + A_Ainv[0][1] * temp[1] + A_Ainv[0][2] * temp[2];
Axyz[1] = A_Ainv[1][0] * temp[0] + A_Ainv[1][1] * temp[1] + A_Ainv[1][2] * temp[2];
Axyz[2] = A_Ainv[2][0] * temp[0] + A_Ainv[2][1] * temp[1] + A_Ainv[2][2] * temp[2];
vector_normalize(Axyz);
//apply offsets (bias) and scale factors from Magneto
for (int i = 0; i < 3; i++) temp[i] = (Mxyz[i] - M_B[i]);
Mxyz[0] = M_Ainv[0][0] * temp[0] + M_Ainv[0][1] * temp[1] + M_Ainv[0][2] * temp[2];
Mxyz[1] = M_Ainv[1][0] * temp[0] + M_Ainv[1][1] * temp[1] + M_Ainv[1][2] * temp[2];
Mxyz[2] = M_Ainv[2][0] * temp[0] + M_Ainv[2][1] * temp[1] + M_Ainv[2][2] * temp[2];
vector_normalize(Mxyz);
// Apply right-handedness (this is a property of the IMU)
Axyz[0] = - Axyz[0];
}
// basic vector operations
void vector_cross(float a[3], float b[3], float out[3])
{
out[0] = a[1] * b[2] - a[2] * b[1];
out[1] = a[2] * b[0] - a[0] * b[2];
out[2] = a[0] * b[1] - a[1] * b[0];
}
float vector_dot(float a[3], float b[3])
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
void vector_normalize(float a[3])
{
float mag = sqrt(vector_dot(a, a));
a[0] /= mag;
a[1] /= mag;
a[2] /= mag;
}
/*
This tilt-compensated code assumes that the Adafruit LSM9DS1 sensor board is oriented with Y pointing
to the North, X pointing West, and Z pointing up.
The code compensates for tilts of up to 90 degrees away from horizontal.
Facing vector p is the direction of travel and allows reassigning these directions.
It should be defined as pointing forward,
parallel to the ground, with coordinates {X, Y, Z} (in magnetometer frame of reference).
*/

53
calibration/mega_calibration.cpp
View file

@ -511,7 +511,12 @@ void loop()
Serial.println("\nWhere Hcalc = Ainv*(H-B)\n");
Serial.println("Code initialization statements...");
Serial.print("\nfloat B[3] = {");
if (sensor==0) {
Serial.print("\nfloat M_B[3] = {");
} else {
Serial.print("\nfloat A_B[3] = {");
}
Serial.print(B[0], 2); // Print B values with 2 decimal places
Serial.print(",");
Serial.print(B[1], 2);
@ -519,7 +524,13 @@ void loop()
Serial.print(B[2], 2);
Serial.println("};");
Serial.println("\nfloat Ainv[3][3] = {");
if (sensor==0) {
Serial.println("\nfloat M_Ainv[3][3] = {");
} else {
Serial.println("\nfloat A_Ainv[3][3] = {");
}
Serial.print(" {");
Serial.print(A_1[0], 5);
Serial.print(",");
@ -550,44 +561,6 @@ void loop()
Serial.println(1.0 - sqrt(xs / nlines) / hm, 6); // Print RMS residual with 6 decimal places
// printf("\r\nCorrection matrix Ainv, using Hm=%8.1f:\r\n",hm);
// for(i = 0; i < 3; i++)
// printf("%9.5lf %9.5lf %9.5lf\r\n", A_1[i*3], A_1[i*3+1], A_1[i*3+2]);
// printf("\r\nWhere Hcalc = Ainv*(H-B)\r\n");
// printf("\r\nCode initialization statements...\r\n");
// printf("\r\n float B[3]\r\n {%8.2lf,%8.2lf,%8.2lf};\r\n",B[0],B[1],B[2]);
// printf("\r\n float Ainv[3][3]\r\n {{%9.5lf,%9.5lf,%9.5lf},\r\n",A_1[0],A_1[1],A_1[2]);
// printf(" {%9.5lf,%9.5lf,%9.5lf},\r\n",A_1[3],A_1[4],A_1[5]);
// printf(" {%9.5lf,%9.5lf,%9.5lf}};\r\n",A_1[6],A_1[7],A_1[8]);
// printf("\r\nRMS corrected vector length %9.6f\n",sqrt(xs/nlines));
// printf("\r\nRMS residual %9.6f\n",1.0-sqrt(xs/nlines)/hm);
// dump corrected measurements if desired; I'm not removing the possibility of doing this
// printf(" output filename for corrected values (csv) ");
// scanf("%s",&filename);
// if(strlen(filename) > 1) {
// fp2 = fopen(filename, "w");
// float xc,yc,zc;
// xs=0;
// for(i = 0; i<nlines; i++){
// x = raw[3*i] -B[0];
// y = raw[3*i+1]-B[1];
// z = raw[3*i+2]-B[2];
// xc = A_1[0]*x + A_1[1]*y + A_1[2]*z;
// yc = A_1[3]*x + A_1[4]*y + A_1[5]*z;
// zc = A_1[6]*x + A_1[7]*y + A_1[8]*z;
// xs += xc*xc + yc*yc + zc*zc;
// fprintf(fp2,"%8.2f,%8.2f,%8.2f\n",xc,yc,zc);
// }
// fclose(fp2);
// }
free(D);
free(S);
free(C);

1256
calibration/sample_serial_output.txt
View file

File diff suppressed because it is too large Load diff

105
sample_main.cpp Normal file
View file

@ -0,0 +1,105 @@
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h> // not used in this demo but required!
#include "LSM9DS1_Adafruit_AHRS.h"
// i2c
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
#define LSM9DS1_SCK A5
#define LSM9DS1_MISO 12
#define LSM9DS1_MOSI A4
#define LSM9DS1_XGCS 6
#define LSM9DS1_MCS 5
// You can also use software SPI
//Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(LSM9DS1_SCK, LSM9DS1_MISO, LSM9DS1_MOSI, LSM9DS1_XGCS, LSM9DS1_MCS);
// Or hardware SPI! In this case, only CS pins are passed in
//Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1(LSM9DS1_XGCS, LSM9DS1_MCS);
unsigned long now = 0, last = 0; //micros() timers for AHRS loop
float deltat = 0; //loop time in seconds
#define SPEED 100 // ms between angle prints
unsigned long lastPrint = 0; // Keep track of print time
// Vector to hold quaternion
static float q[4] = {1.0, 0.0, 0.0, 0.0};
// We save the orientation here:
// 0 - yaw
// 1 - pitch
// 2 - roll
static float angles[3];
void setupSensor()
{
// 1.) Set the accelerometer range
lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G, lsm.LSM9DS1_ACCELDATARATE_10HZ);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_4G, lsm.LSM9DS1_ACCELDATARATE_119HZ);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_8G, lsm.LSM9DS1_ACCELDATARATE_476HZ);
//lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_16G, lsm.LSM9DS1_ACCELDATARATE_952HZ);
// 2.) Set the magnetometer sensitivity
lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_8GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_12GAUSS);
//lsm.setupMag(lsm.LSM9DS1_MAGGAIN_16GAUSS);
// 3.) Setup the gyroscope
lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_500DPS);
//lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_2000DPS);
}
void setup()
{
Serial.begin(115200);
while (!Serial) {
delay(1); // will pause Zero, Leonardo, etc until serial console opens
}
Serial.println("LSM9DS1 data read demo");
// Try to initialise and warn if we couldn't detect the chip
if (!lsm.begin())
{
Serial.println("Oops ... unable to initialize the LSM9DS1. Check your wiring!");
while (1);
}
Serial.println("Found LSM9DS1 9DOF");
// helper to just set the default scaling we want, see above!
setupSensor();
}
void loop()
{
static char updated = 0;
static int loop_counter = 0;
static float Gxyz[3], Axyz[3], Mxyz[3]; // Gyro/Acc/Mag data
lsm.read(); /* ask it to read in the data */
/* Get a new sensor event */
sensors_event_t a, m, g, temp;
lsm.getEvent(&a, &m, &g, &temp);
loop_counter++;
get_scaled_IMU(Gxyz, Axyz, Mxyz, g.gyro.x, g.gyro.y, g.gyro.z, a.acceleration.x, a.acceleration.y, a.acceleration.z, m.magnetic.x, m.magnetic.y, m.magnetic.z);
now = micros();
deltat = (now - last) * 1.0e-6; //seconds since last update
last = now;
mahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat, q);
get_angles(angles, q);
delay(SPEED);
if (loop_counter == 5) {
Serial.print("Yaw: ");
Serial.println(angles[0]);
Serial.print("Pitch: ");
Serial.println(angles[1]);
Serial.print("Roll: ");
Serial.println(angles[2]);
loop_counter = 0;
}
}

239
src/LSM9DS1_Adafruit_AHRS.cpp Normal file
View file

@ -0,0 +1,239 @@
/* The implementation
*/
// VERY IMPORTANT!
//These are the previously determined offsets and scale factors for the gyroscope, accelerometer and magnetometer, using the mega
//The AHRS will not work without the correct calibration parameters
#include "LSM9DS1_Adafruit_AHRS.h"
#include <math.h>
//Gyroscope calibration parameters
float G_offset[3] = {-0.04, 0.00, 0.03};
//Accelerometer calibration parameters
float A_B[3] = {-0.28,-0.18,-0.06};
float A_Ainv[3][3] = {
{1.04504,0.02970,-0.01990},
{0.02970,1.04075,0.00254},
{-0.01990,0.00254,0.96972}
};
//Magnetometer calibration parameters
float M_B[3] = {28.29,18.91,-5.36};
float M_Ainv[3][3] = {
{1.19555,0.03164,0.11033},
{0.03164,1.30165,-0.00935},
{0.11033,-0.00935,1.06658}
};
// local magnetic declination in degrees in Hamburg
float declination = +4.08;
float p[] = {0, 1, 0}; //Y marking on sensor board points toward yaw = 0
// MAHONY PART
// These are the free parameters in the Mahony filter and fusion scheme,
// Kp for proportional feedback, Ki for integral
#define Kp 50
#define Ki 0.0
static float yaw, pitch, roll; //Euler angle (actually Tait-Bryan, just shifted by 90') output
void get_angles(float orientation[3], float q[4]) {
roll = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2]));
pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3]));
yaw = atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - ( q[2] * q[2] + q[3] * q[3]));
// to degrees
yaw *= 180.0 / 3.14159;
pitch *= 180.0 / 3.14159;
roll *= 180.0 / 3.14159;
yaw = -(yaw + declination);
if (yaw < 0) yaw += 360.0;
if (yaw >= 360.0) yaw -= 360.0;
orientation[0] = yaw;
orientation[1] = pitch;
orientation[2] = roll;
}
void get_scaled_IMU(float Gxyz[3], float Axyz[3], float Mxyz[3], float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
char i;
float temp[3];
Gxyz[0] = gx - G_offset[0];
Gxyz[1] = gy - G_offset[1];
Gxyz[2] = gz - G_offset[2];
Axyz[0] = ax;
Axyz[1] = ay;
Axyz[2] = az;
Mxyz[0] = mx;
Mxyz[1] = my;
Mxyz[2] = mz;
for (i = 0; i < 3; i++) temp[i] = (Axyz[i] - A_B[i]);
Axyz[0] = A_Ainv[0][0] * temp[0] + A_Ainv[0][1] * temp[1] + A_Ainv[0][2] * temp[2];
Axyz[1] = A_Ainv[1][0] * temp[0] + A_Ainv[1][1] * temp[1] + A_Ainv[1][2] * temp[2];
Axyz[2] = A_Ainv[2][0] * temp[0] + A_Ainv[2][1] * temp[1] + A_Ainv[2][2] * temp[2];
vector_normalize(Axyz);
for (int i = 0; i < 3; i++) temp[i] = (Mxyz[i] - M_B[i]);
Mxyz[0] = M_Ainv[0][0] * temp[0] + M_Ainv[0][1] * temp[1] + M_Ainv[0][2] * temp[2];
Mxyz[1] = M_Ainv[1][0] * temp[0] + M_Ainv[1][1] * temp[1] + M_Ainv[1][2] * temp[2];
Mxyz[2] = M_Ainv[2][0] * temp[0] + M_Ainv[2][1] * temp[1] + M_Ainv[2][2] * temp[2];
vector_normalize(Mxyz);
// Apply right-handedness (this is a property of the IMU)
Axyz[0] = - Axyz[0];
Gxyz[0] = - Gxyz[0];
}
void mahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat, float q[4])
{
// Vector to hold integral error for Mahony method
static float eInt[3] = {0.0, 0.0, 0.0};
// short name local variable for readability
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];
float norm;
float hx, hy, hz; //observed West horizon vector W = AxM
float ux, uy, uz, wx, wy, wz; //calculated A (Up) and W in body frame
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Measured horizon vector = a x m (in body frame)
hx = ay * mz - az * my;
hy = az * mx - ax * mz;
hz = ax * my - ay * mx;
// Normalise horizon vector
norm = sqrt(hx * hx + hy * hy + hz * hz);
if (norm == 0.0f) return; // Handle div by zero
norm = 1.0f / norm;
hx *= norm;
hy *= norm;
hz *= norm;
// Estimated direction of Up reference vector
ux = 2.0f * (q2q4 - q1q3);
uy = 2.0f * (q1q2 + q3q4);
uz = q1q1 - q2q2 - q3q3 + q4q4;
// estimated direction of horizon (West) reference vector
wx = 2.0f * (q2q3 + q1q4);
wy = q1q1 - q2q2 + q3q3 - q4q4;
wz = 2.0f * (q3q4 - q1q2);
// Error is the summed cross products of estimated and measured directions of the reference vectors
// It is assumed small, so sin(theta) ~ theta IS the angle required to correct the orientation error.
ex = (ay * uz - az * uy) + (hy * wz - hz * wy);
ey = (az * ux - ax * uz) + (hz * wx - hx * wz);
ez = (ax * uy - ay * ux) + (hx * wy - hy * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
// Apply I feedback
gx += Ki * eInt[0];
gy += Ki * eInt[1];
gz += Ki * eInt[2];
}
// Apply P feedback
gx = gx + Kp * ex;
gy = gy + Kp * ey;
gz = gz + Kp * ez;
//update quaternion with integrated contribution
// small correction 1/11/2022, see https://github.com/kriswiner/MPU9250/issues/447
gx = gx * (0.5*deltat); // pre-multiply common factors
gy = gy * (0.5*deltat);
gz = gz * (0.5*deltat);
float qa = q1;
float qb = q2;
float qc = q3;
q1 += (-qb * gx - qc * gy - q4 * gz);
q2 += (qa * gx + qc * gz - q4 * gy);
q3 += (qa * gy - qb * gz + q4 * gx);
q4 += (qa * gz + qb * gy - qc * gx);
// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;
}
// basic vector operations
void vector_cross(float a[3], float b[3], float out[3])
{
out[0] = a[1] * b[2] - a[2] * b[1];
out[1] = a[2] * b[0] - a[0] * b[2];
out[2] = a[0] * b[1] - a[1] * b[0];
}
float vector_dot(float a[3], float b[3])
{
return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}
void vector_normalize(float a[3])
{
float mag = sqrt(vector_dot(a, a));
a[0] /= mag;
a[1] /= mag;
a[2] /= mag;
}
/*
This tilt-compensated code assumes that the Adafruit LSM9DS1 sensor board is oriented with Y pointing
to the North, X pointing West, and Z pointing up.
The code compensates for tilts of up to 90 degrees away from horizontal.
Facing vector p is the direction of travel and allows reassigning these directions.
It should be defined as pointing forward,
parallel to the ground, with coordinates {X, Y, Z} (in magnetometer frame of reference).
*/
// Returns a heading (in degrees) given an acceleration vector a due to gravity, a magnetic vector m, and a facing vector p.
int get_heading(float acc[3], float mag[3], float p[3])
{
float W[3], N[3]; //derived direction vectors
// cross "Up" (acceleration vector, g) with magnetic vector (magnetic north + inclination) with to produce "West"
vector_cross(acc, mag, W);
vector_normalize(W);
// cross "West" with "Up" to produce "North" (parallel to the ground)
vector_cross(W, acc, N);
vector_normalize(N);
// compute heading in horizontal plane, correct for local magnetic declination
int heading = round(atan2(vector_dot(W, p), vector_dot(N, p)) * 180 / M_PI + declination);
heading = -heading; //conventional nav, heading increases North to East
heading = (heading + 720)%360; //apply compass wrap
return heading;
}

10
adafruit-AHRS.h → src/LSM9DS1_Adafruit_AHRS.h
View file

@ -13,15 +13,15 @@ SCL ------------ 9
SDA ------------ 8
*/
#ifndef ADAFRUIT-AHRS_INCLUDE
#define ADAFRUIT-AHRS_INCLUDE
float A_B[3], A_Ainv[3][3], M_B[3], M_Binv[3][3], declination, p[3];
#ifndef LSM9DS1_ADAFRUIT_AHRS_INCLUDE
#define LSM9DS1_ADAFRUIT_AHRS_INCLUDE
int get_heading(float acc[3], float mag[3], float p[3]);
void get_scaled_IMU(float Axyz[3], float Mxyz[3], float ax, float ay, float az, float mx, float my, float mz);
void get_scaled_IMU(float Gxyz[3], float Axyz[3], float Mxyz[3], float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
void mahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat, float q[4]);
void vector_cross(float a[3], float b[3], float out[3]);
float vector_dot(float a[3], float b[3]);
void vector_normalize(float a[3]);
void get_angles(float orientation[3], float q[4]);
#endif