/*
  Using the BNO080 IMU
  By: Nathan Seidle
  SparkFun Electronics
  Date: December 21st, 2017
  License: This code is public domain but you buy me a beer if you use this and we meet someday (Beerware license).

  Feel like supporting our work? Buy a board from SparkFun!
  https://www.sparkfun.com/products/14586

  This example shows how to calibrate the sensor. See document 1000-4044.

  It takes about 1ms at 400kHz I2C to read a record from the sensor, but we are polling the sensor continually
  between updates from the sensor. Use the interrupt pin on the BNO080 breakout to avoid polling.

  Hardware Connections:
  Attach the Qwiic Shield to your Arduino/Photon/ESP32 or other
  Plug the sensor onto the shield
  Serial.print it out at 9600 baud to serial monitor.
*/

#include <Wire.h>

#include "SparkFun_BNO080_Arduino_Library.h"
BNO080 myIMU;

void setup()
{
  Serial.begin(9600);
  Serial.println();
  Serial.println("BNO080 Calibration Example");

  Wire.begin();
  Wire.setClock(400000); //Increase I2C data rate to 400kHz

  myIMU.begin(0x4b,Wire);

  //Enable dynamic calibration for accel, gyro, and mag
  myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag

  //Enable Game Rotation Vector output
  myIMU.enableGameRotationVector(100); //Send data update every 100ms

  //Enable Magnetic Field output
  myIMU.enableMagnetometer(100); //Send data update every 100ms

  //Once magnetic field is 2 or 3, run the Save DCD Now command
  Serial.println(F("Calibrating. Press 's' to save to flash"));
  Serial.println(F("Output in form x, y, z, in uTesla"));
}

void loop()
{
  if(Serial.available())
  {
    byte incoming = Serial.read();

    if(incoming == 's')
    {
      myIMU.saveCalibration(); //Saves the current dynamic calibration data (DCD) to memory
      myIMU.endCalibration(); //Turns off all calibration
      Serial.println("Calibration ended");
    }
  }

  //Look for reports from the IMU
  if (myIMU.dataAvailable() == true) {
    Serial.print(myIMU.getMagX(), 2);
    Serial.print(F(","));
    Serial.print(myIMU.getMagY(), 2);
    Serial.print(F(","));
    Serial.print(myIMU.getMagZ(), 2);
    Serial.print(F(","));
    Serial.print(myIMU.getMagAccuracy());
    Serial.println();
  }
}