139 lines
No EOL
4.1 KiB
C++
139 lines
No EOL
4.1 KiB
C++
#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 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};
|
|
// We save the orientation here:
|
|
// 0 - yaw
|
|
// 1 - pitch
|
|
// 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
|
|
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);
|
|
}
|
|
|
|
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");
|
|
|
|
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 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 == 30) {
|
|
Serial.print("Yaw: ");
|
|
Serial.println(angles[0]-yaw_cal);
|
|
Serial.print("Pitch: ");
|
|
Serial.println(angles[1]-pitch_cal);
|
|
Serial.print("Roll: ");
|
|
Serial.println(angles[2]+roll_cal);
|
|
loop_counter = 0;
|
|
}
|
|
|
|
} |