diff --git a/adafruit-AHRS.cpp b/adafruit-AHRS.cpp deleted file mode 100644 index 65c45dd..0000000 --- a/adafruit-AHRS.cpp +++ /dev/null @@ -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). -*/ - diff --git a/calibration/mega_calibration.cpp b/calibration/mega_calibration.cpp index 5b6dcb2..9a27851 100644 --- a/calibration/mega_calibration.cpp +++ b/calibration/mega_calibration.cpp @@ -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 +#include +#include +#include // 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; + } + +} diff --git a/src/LSM9DS1_Adafruit_AHRS.cpp b/src/LSM9DS1_Adafruit_AHRS.cpp new file mode 100644 index 0000000..1bb2fc7 --- /dev/null +++ b/src/LSM9DS1_Adafruit_AHRS.cpp @@ -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 + + + +//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; +} \ No newline at end of file diff --git a/adafruit-AHRS.h b/src/LSM9DS1_Adafruit_AHRS.h similarity index 59% rename from adafruit-AHRS.h rename to src/LSM9DS1_Adafruit_AHRS.h index f5b2950..1b3fb1a 100644 --- a/adafruit-AHRS.h +++ b/src/LSM9DS1_Adafruit_AHRS.h @@ -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 \ No newline at end of file