First iteration of the implementation
This commit is contained in:
parent
76141a90aa
commit
3950a7f3f9
6 changed files with 991 additions and 801 deletions
|
@ -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).
|
||||
*/
|
||||
|
|
@ -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);
|
||||
|
|
File diff suppressed because it is too large
Load diff
105
sample_main.cpp
Normal file
105
sample_main.cpp
Normal 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
239
src/LSM9DS1_Adafruit_AHRS.cpp
Normal 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;
|
||||
}
|
|
@ -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
|
Loading…
Reference in a new issue