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

Did some calibration

Browse source
This commit is contained in:
Turingon 2024-06-22 13:11:26 +02:00
parent 3950a7f3f9
commit ca3c84493b
1 changed files with 43 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

52
sample_main.cpp
View file

@ -22,7 +22,7 @@ Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();
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
#define SPEED 50 // 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};
@ -32,6 +32,11 @@ static float q[4] = {1.0, 0.0, 0.0, 0.0};
// 2 - roll
static float angles[3];
float pitch_cal = 0;
float yaw_cal = 0;
float roll_cal = 0;
void setupSensor()
{
// 1.) Set the accelerometer range
@ -57,7 +62,7 @@ void setup()
Serial.begin(115200);
while (!Serial) {
delay(1); // will pause Zero, Leonardo, etc until serial console opens
delay(1);
}
Serial.println("LSM9DS1 data read demo");
@ -70,13 +75,42 @@ void setup()
}
Serial.println("Found LSM9DS1 9DOF");
// helper to just set the default scaling we want, see above!
setupSensor();
static float Gxyz[3], Axyz[3], Mxyz[3];
delay(100);
Serial.println("CALIBRATING HOLD STILL");
delay(500);
for (int i=0; i < 50; i++) {
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);
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);
yaw_cal += (angles[0]);
pitch_cal += abs(angles[1]);
roll_cal += abs(angles[2]);
}
last = 0;
yaw_cal /= 50;
pitch_cal /= 50;
roll_cal /= 50;
Serial.println("CALIBRATION DONE!");
Serial.println(pitch_cal);
Serial.println(roll_cal);
}
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 */
@ -92,14 +126,14 @@ void loop()
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) {
if (loop_counter == 30) {
Serial.print("Yaw: ");
Serial.println(angles[0]);
Serial.println(angles[0]-yaw_cal);
Serial.print("Pitch: ");
Serial.println(angles[1]);
Serial.println(angles[1]-pitch_cal);
Serial.print("Roll: ");
Serial.println(angles[2]);
Serial.println(angles[2]+roll_cal);
loop_counter = 0;
}
}
}