9dof_orientation.ino

#include <Wire.h>
#include <SPI.h>
#include <SparkFunLSM9DS1.h>

LSM9DS1 imu;

#define LSM9DS1_M 0x1E
#define LSM9DS1_AG 0x6B
static unsigned long lastPrint = 0;

#define DECLINATION -8.58

void shockDetector();
bool calcTilted(float ax, float ay, float az, float mx, float my, float mz, float boundaryDegrees);

void setup()
{
  Serial.begin(9600);
  imu.settings.device.commInterface = IMU_MODE_I2C;
  imu.settings.device.mAddress = LSM9DS1_M;
  imu.settings.device.agAddress = LSM9DS1_AG;
  if (!imu.begin())
  {
    Serial.println("Failed to communicate with LSM9DS1.");
    Serial.println("Double-check wiring.");
    Serial.println("Default settings in this sketch will "
                   "work for an out of the box LSM9DS1 "
                   "Breakout, but may need to be modified "
                   "if the board jumpers are.");
    while (1)
      ;
  }
}

void loop()
{
  if (imu.gyroAvailable())
  {
    imu.readGyro();
  }
  if (imu.accelAvailable())
  {
    imu.readAccel();
  }
  if (imu.magAvailable())
  {
    imu.readMag();
  }

    //shockDetector();
    float boundaryDegrees = 45;
    calcTilted(imu.ax, imu.ay, imu.az,
                  -imu.my, -imu.mx, imu.mz, boundaryDegrees);
}

float shockDetector()
{
  float accel_z = imu.calcAccel(imu.az);
  if (accel_z > 1.5 || accel_z < 0.5)
  {
    Serial.print("Shock detected with a value of: ");
    return accel_z;
  }
  return -1000;
}

// Calculate pitch, roll, and heading.
// Pitch/roll calculations take from this app note:
bool calcTilted(float ax, float ay, float az, float mx, float my, float mz, float boundaryDegrees)
{
  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);

  // Convert everything from radians to degrees:
  heading *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll *= 180.0 / PI;

  bool outOfBoundsRoll = (roll < boundaryDegrees && roll > boundaryDegrees * -1);
  bool outOfBoundsPitch = (pitch < boundaryDegrees && pitch > boundaryDegrees * -1);
  if(outOfBoundsRoll && outOfBoundsPitch){
    Serial.println("Not tilted facing top.");
    return true;
  } 
  Serial.print("Tilted beyond boundary.");
  return false; 
}