Did some calibration
This commit is contained in:
parent
3950a7f3f9
commit
ca3c84493b
1 changed files with 43 additions and 9 deletions
|
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
Loading…
Reference in a new issue