Module Pmodnav - IMU 9 axes + Baromètre LSM9DS1 / LPS25HB

/************************************************************************
*
* Test du module Pmod IMU 9 axes + Baromètre (basé sur le programme de Jim Lindblom)
*
*************************************************************************
* Description: Pmod_NAV
* Toutes les données (accéléromètre, gyroscope, magnétomètre) sont affichées
* dans le moniteur série
*
* Matériel
* 1. Arduino Uno
* 2. Module Pmod NAV (télécharger la bibliothèque
* https://github.com/sparkfun/SparkFun_LSM9DS1_Arduino_Library)
* Licence Beerware
*
* Câblage
* Module<----------> Arduino
* J1 broche 6 3.3V
* J1 broche 5 GND
* J1 broche 4 A5
* J1 broche 2 A4
*
*
* Schéma publié sous licence CC Attribution-ShareALike (réalisé avec Fritzing)
*
* N'importez pas le programme par un copier/coller dans l'IDE de l'arduino.
* Utilisez le lien ci-dessous pour télécharger le code source.
* 

************************************************************************/
// Appel des bibliothèques
#include <Wire.h>
#include <SparkFunLSM9DS1.h>

// Déclaration des adresses du module
#define LSM9DS1_M 0x1E
#define LSM9DS1_AG 0x6B

LSM9DS1 imu; // création de l'objet imu

// Configuration du module
#define PRINT_CALCULATED
#define PRINT_SPEED 250
static unsigned long lastPrint = 0;

// Le champ magnétique terrestre varie en fonction de sa localisation.
// Il faut ajouter ou soustraire une constante pour obtenir la bonne valeur
// du champ magnétique à l'aide du site suivant
// http://www.ngdc.noaa.gov/geomag-web/#declination
#define DECLINATION -0.33 // déclinaison (en degrés) pour Paris.

void setup(void)
{
Serial.begin(115200); // initialisation de la liaison série
imu.settings.device.commInterface = IMU_MODE_I2C; // initialisation du module
imu.settings.device.mAddress = LSM9DS1_M;
imu.settings.device.agAddress = LSM9DS1_AG;
if (!imu.begin())
{
Serial.println("Probleme de communication avec le LSM9DS1.");
while (1);
}
}

void loop()
{

if ( imu.gyroAvailable() )
{
imu.readGyro(); // acquisition des données du gyroscope
}
if ( imu.accelAvailable() )
{
imu.readAccel(); // acquisition des données de l'accéléromètre
}
if ( imu.magAvailable() )
{
imu.readMag(); // acquisition du magnétomètre
}

if ((lastPrint + PRINT_SPEED) < millis())
{
printGyro(); // Print "G: gx, gy, gz"
printAccel(); // Print "A: ax, ay, az"
printMag(); // Print "M: mx, my, mz"
printAttitude(imu.ax, imu.ay, imu.az,-imu.my, -imu.mx, imu.mz);
Serial.println();

lastPrint = millis();
}
}

void printGyro()
{
Serial.print("G: ");
#ifdef PRINT_CALCULATED
Serial.print(imu.calcGyro(imu.gx), 2);
Serial.print(", ");
Serial.print(imu.calcGyro(imu.gy), 2);
Serial.print(", ");
Serial.print(imu.calcGyro(imu.gz), 2);
Serial.println(" deg/s");
#elif defined PRINT_RAW
Serial.print(imu.gx);
Serial.print(", ");
Serial.print(imu.gy);
Serial.print(", ");
Serial.println(imu.gz);
#endif
}

void printAccel()
{
Serial.print("A: ");
#ifdef PRINT_CALCULATED
Serial.print(imu.calcAccel(imu.ax), 2);
Serial.print(", ");
Serial.print(imu.calcAccel(imu.ay), 2);
Serial.print(", ");
Serial.print(imu.calcAccel(imu.az), 2);
Serial.println(" g");
#elif defined PRINT_RAW
Serial.print(imu.ax);
Serial.print(", ");
Serial.print(imu.ay);
Serial.print(", ");
Serial.println(imu.az);
#endif

}

void printMag()
{
Serial.print("M: ");
#ifdef PRINT_CALCULATED
Serial.print(imu.calcMag(imu.mx), 2);
Serial.print(", ");
Serial.print(imu.calcMag(imu.my), 2);
Serial.print(", ");
Serial.print(imu.calcMag(imu.mz), 2);
Serial.println(" gauss");
#elif defined PRINT_RAW
Serial.print(imu.mx);
Serial.print(", ");
Serial.print(imu.my);
Serial.print(", ");
Serial.println(imu.mz);
#endif
}

void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
float roll = atan2(ay, az);
float pitch = atan2(-ax, sqrt(ay * ay + az * az));
float heading;
if (my == 0)
heading = (mx < 0) ? PI : 0;
else
heading = atan2(mx, my);
heading -= DECLINATION * PI / 180;
if (heading > PI) heading -= (2 * PI);
else if (heading < -PI) heading += (2 * PI);
else if (heading < 0) heading += 2 * PI;
heading *= 180.0 / PI;
pitch *= 180.0 / PI;
roll *= 180.0 / PI;
Serial.print("Pitch, Roll: ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);
Serial.print("Heading: "); Serial.println(heading, 2);
}