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

Added first things to the code

Browse source
This commit is contained in:
Turingon 2024-06-18 14:43:56 +02:00
parent fe1b558ad8
commit 76141a90aa
1 changed files with 30 additions and 18 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
adafruit-AHRS.cpp
View file

@ -2,10 +2,13 @@
*/
// VERY IMPORTANT!
//These are the previously determined offsets and scale factors for accelerometer and magnetometer, using MPU9250_cal and Magneto
//The compass will NOT work well or at all if these are not correct
//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
//Accel scale 16457.0 to normalize
//Gyroscope calibration parameters
G_offset[3] = {75, 31, 142};
//Accelerometer calibration parameters
float A_B[3]
{ -133.33, 72.29, -291.92};
@ -15,7 +18,7 @@ float A_Ainv[3][3]
{ 0.00023, 0.00263, 0.99905}
};
//Mag scale 3746.0 to normalize
//Magnetometer calibration parameters
float M_B[3]
{ -922.31, 2199.41, 373.17};
@ -25,24 +28,25 @@ float M_Ainv[3][3]
{ -0.01714, 0.00644, 1.07005}
};
// TILT COMPENSATION CODE
// local magnetic declination in degrees in Hamburg
float declination = +4.08;
/*
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).
*/
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])
{
@ -114,4 +118,12 @@ void vector_normalize(float a[3])
a[2] /= mag;
}
// END OF TILT COMPENSATION CODE
/*
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).
*/