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

50
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 unsigned long now = 0, last = 0; //micros() timers for AHRS loop
float deltat = 0; //loop time in seconds 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 unsigned long lastPrint = 0; // Keep track of print time
// Vector to hold quaternion // Vector to hold quaternion
static float q[4] = {1.0, 0.0, 0.0, 0.0}; 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 // 2 - roll
static float angles[3]; static float angles[3];
float pitch_cal = 0;
float yaw_cal = 0;
float roll_cal = 0;
void setupSensor() void setupSensor()
{ {
// 1.) Set the accelerometer range // 1.) Set the accelerometer range
@ -57,7 +62,7 @@ void setup()
Serial.begin(115200); Serial.begin(115200);
while (!Serial) { while (!Serial) {
delay(1); // will pause Zero, Leonardo, etc until serial console opens delay(1);
} }
Serial.println("LSM9DS1 data read demo"); Serial.println("LSM9DS1 data read demo");
@ -70,13 +75,42 @@ void setup()
} }
Serial.println("Found LSM9DS1 9DOF"); Serial.println("Found LSM9DS1 9DOF");
// helper to just set the default scaling we want, see above!
setupSensor(); 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() void loop()
{ {
static char updated = 0;
static int loop_counter = 0; static int loop_counter = 0;
static float Gxyz[3], Axyz[3], Mxyz[3]; // Gyro/Acc/Mag data static float Gxyz[3], Axyz[3], Mxyz[3]; // Gyro/Acc/Mag data
lsm.read(); /* ask it to read in the data */ lsm.read(); /* ask it to read in the data */
@ -92,13 +126,13 @@ void loop()
mahonyQuaternionUpdate(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat, q); 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); get_angles(angles, q);
delay(SPEED); delay(SPEED);
if (loop_counter == 5) { if (loop_counter == 30) {
Serial.print("Yaw: "); Serial.print("Yaw: ");
Serial.println(angles[0]); Serial.println(angles[0]-yaw_cal);
Serial.print("Pitch: "); Serial.print("Pitch: ");
Serial.println(angles[1]); Serial.println(angles[1]-pitch_cal);
Serial.print("Roll: "); Serial.print("Roll: ");
Serial.println(angles[2]); Serial.println(angles[2]+roll_cal);
loop_counter = 0; loop_counter = 0;
} }