test.cpp

#include <Arduino.h>
#include <Wire.h>
#include <EEPROM.h>
#include <SoftwareSerial.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
// #include <Adafruit_MPL3115A2.h>
#include <utility/imumaths.h>



#include "SdFat.h"

// 32 KiB buffer.
const size_t BUF_DIM = 32768;

// 32 MiB file.
const uint32_t FILE_SIZE = 1024UL * BUF_DIM;

SdFatSdioEX sdEx;
File file;
#define FILE_TRUNC (O_READ | O_WRITE | O_CREAT | O_TRUNC)

uint8_t buf[BUF_DIM];

// buffer as uint32_t
uint32_t *buf32 = (uint32_t *)buf;



#define BNO055_SAMPLERATE_DELAY_MS (10)
int calib_data_addr = 0x0000;
const int imu_id = 56;
adafruit_bno055_offsets_t calib_data;
bool saveCal = true;
bool resetCal = false;
bool magCal = false;

Adafruit_BNO055 bno = Adafruit_BNO055(imu_id);
sensor_t imu_sensor;
uint8_t sys, gyro, accel, mag;

unsigned long now = 0;
unsigned long now_long = 0;
unsigned long dt = 0;
uint16_t niter = 0;

float mag_offsets[3] = {29.81F, 24.31F, -29.66F};
// Soft iron error compensation matrix
float mag_softiron_matrix[3][3] = {{0.896, -0.043, -0.017},
                                   {0.043, 1.157, 0.067},
                                   {0.017, 0.067, 0.970}};
float mag_field_strength = 63.80F;

// float mag_offsets[3]            = { 0.0F, 0.0F, 0.0F };
// // Soft iron error compensation matrix
// float mag_softiron_matrix[3][3] = { {  1.0,  0.0,  0.0 },
//                                     {  0.0,  1.0,  0.0 },
//                                     {  0.0,  0.0,  1.0 } };
// float mag_field_strength        = 0.0F;

#include <madgwick.h>
Madgwick filter;
// #include <mahony.h>
// Mahony filter;

struct float_offset_data
{
  float acc_x;
  float acc_y;
  float acc_z;
  float gyro_x;
  float gyro_y;
  float gyro_z;
  float mag_x;
  float mag_y;
  float mag_z;
};

float_offset_data offset_data;

// Adafruit_MPL3115A2 mpl = Adafruit_MPL3115A2();
// float pres_pa, temp_C, alt_m;

void displayCalStatus(void)
{
  sys = gyro = accel = mag = 0;
  bno.getCalibration(&sys, &gyro, &accel, &mag);

  if (!sys)
  {
    Serial.print("! ");
  }

  Serial.print("Sys=");
  Serial.print(sys, DEC);
  Serial.print(" Gyro=");
  Serial.print(gyro, DEC);
  Serial.print(" Accel=");
  Serial.print(accel, DEC);
  Serial.print(" Mag=");
  Serial.println(mag, DEC);
}

void displaySensorOffsets(adafruit_bno055_offsets_t &calib_data)
{
  Serial.print("Accelerometer: ");
  Serial.print(calib_data.accel_offset_x);
  Serial.print(" ");
  Serial.print(calib_data.accel_offset_y);
  Serial.print(" ");
  Serial.print(calib_data.accel_offset_z);
  Serial.print(" ");

  Serial.print("\nGyro: ");
  Serial.print(calib_data.gyro_offset_x);
  Serial.print(" ");
  Serial.print(calib_data.gyro_offset_y);
  Serial.print(" ");
  Serial.print(calib_data.gyro_offset_z);
  Serial.print(" ");

  Serial.print("\nMag: ");
  Serial.print(calib_data.mag_offset_x);
  Serial.print(" ");
  Serial.print(calib_data.mag_offset_y);
  Serial.print(" ");
  Serial.print(calib_data.mag_offset_z);
  Serial.print(" ");

  Serial.print("\nAccel Radius: ");
  Serial.print(calib_data.accel_radius);

  Serial.print("\nMag Radius: ");
  Serial.print(calib_data.mag_radius);
}

void bnoStartup(void)
{
  long bno_id;
  bool found_calib = false;
  bno.getSensor(&imu_sensor);

  if (saveCal)
  {
    EEPROM.get(calib_data_addr, bno_id);

    if (bno_id != imu_sensor.sensor_id or resetCal)
    {
      Serial.println("\nNo Calibration Data for this sensor exists in EEPROM");
      delay(500);
    }
    else
    {
      Serial.println("\nFound Calibration for this sensor in EEPROM.");
      calib_data_addr += sizeof(long);
      EEPROM.get(calib_data_addr, calib_data);

      displaySensorOffsets(calib_data);

      Serial.println("\n\nRestoring Calibration data to the BNO055...");
      bno.setSensorOffsets(calib_data);

      Serial.println("\n\nCalibration data loaded into BNO055");
      found_calib = true;
    }
  }

  delay(1000);
  bno.setExtCrystalUse(true); // Crystal must be configured AFTER calibration

  if (found_calib and magCal)
  {
    Serial.println("Move sensor slightly to calibrate magnetometers");

    while (!bno.isFullyCalibrated())
    {
      delay(BNO055_SAMPLERATE_DELAY_MS);
    }
  }
  else if (found_calib and !magCal)
  {
    sys = gyro = accel = mag = 0;

    while (!(gyro == 3) and !(accel == 3))
    {
      bno.getCalibration(&sys, &gyro, &accel, &mag);
      delay(BNO055_SAMPLERATE_DELAY_MS);
    }
  }
  else
  {
    Serial.println("Please Calibrate Sensor: ");
    while (!bno.isFullyCalibrated())
    {
      imu::Vector<3> ang = bno.getVector(Adafruit_BNO055::VECTOR_EULER);

      Serial.print("X: ");
      Serial.print(ang.x(), 4);
      Serial.print("\tY: ");
      Serial.print(ang.y(), 4);
      Serial.print("\tZ: ");
      Serial.print(ang.z(), 4);

      Serial.print("\t");
      displayCalStatus();
      delay(BNO055_SAMPLERATE_DELAY_MS);
    }
  }

  Serial.println("\nFully calibrated!");
  delay(1000);
  Serial.println("--------------------------------");
  Serial.println("Calibration Results: ");
  bno.getSensorOffsets(calib_data);

  offset_data.acc_x = (float)calib_data.accel_offset_x / 1000;
  offset_data.acc_y = (float)calib_data.accel_offset_z / 1000;
  offset_data.acc_z = (float)calib_data.accel_offset_y / 1000;
  offset_data.gyro_x = (float)(calib_data.gyro_offset_x >> 4);
  offset_data.gyro_y = (float)(calib_data.gyro_offset_y >> 4);
  offset_data.gyro_z = (float)(calib_data.gyro_offset_z >> 4);
  offset_data.mag_x = (float)(calib_data.mag_offset_x >> 4);
  offset_data.mag_y = (float)(calib_data.mag_offset_y >> 4);
  offset_data.mag_z = (float)(calib_data.mag_offset_z >> 4);

  Serial.println(offset_data.gyro_x);
  Serial.println(offset_data.gyro_y);
  Serial.println(offset_data.gyro_z);

  displaySensorOffsets(calib_data);

  if (saveCal)
  {
    Serial.println("\n\nStoring calibration data to EEPROM...");

    calib_data_addr = 0x0000;
    bno.getSensor(&imu_sensor);
    bno_id = imu_sensor.sensor_id;
    EEPROM.put(calib_data_addr, bno_id);

    calib_data_addr += sizeof(long);
    EEPROM.put(calib_data_addr, calib_data);
    Serial.println("Data stored to EEPROM.");
  }

  bno.configMaxPerfAMG();
}

void writeData(File f,
               imu::Vector<3> acc)
{
  f.println(acc.x());
  // f.write(',');

  // f.print(acc.x(), 3);
  // f.write(',');
  // f.print(acc.y(), 3);
  // f.write(',');
  // f.print(acc.z(), 3);
  // f.write(',');

  // f.print(pitch, 2);
  // f.write(',');
  // f.print(yaw, 2);
  // f.write(',');;
  // f.print(roll, 2);

  // f.println();
}


void setup()
{
  Serial.begin(115200);

  while (!Serial)
  {
    ;
  }

  Serial.println("##############################################");
  Serial.println("                IMU SENS FUSE                 ");
  Serial.println("##############################################");

  if (!bno.begin())
  {
    Serial.println("No IMU detected");
    while (1)
      ;
  }

  delay(1000);

  bnoStartup();

  // if (!mpl.begin())
  // {
  //     Serial.println("No altimeter detected");
  //     return;
  // }
  // delay(1000);

  Serial.println("Calibration values:\n0=uncalibrated, 3=fully calibrated");
  Serial.println("");
  // Serial.println("----------------------------------------------");

  filter.begin(260.0f);

  if (!sdEx.begin())
  {
    sdEx.initErrorHalt("SdFatSdioEX begin() failed");
  }
  // make sdEx the current volume.
  sdEx.chvol();

  file = sdEx.open("test.csv", FILE_TRUNC);
  if (!file)
  {
    Serial.println("File failed to open.");
  }
  file.truncate(0);
  file.rewind();
  file.println("micros,acc_x,acc_y,acc_z,pitch,yaw,roll");
  file.println("hello");
  file.close();

  file = sdEx.open("test.csv", FILE_WRITE);
  if (!file)
  {
    Serial.println("File failed to open.");
  }
  file.flush();
  delay(100);
  now = micros();
  now_long = micros();
}

float max_acc = 0.0;
imu::Vector<3> acc_avg;
imu::Vector<3> angvel_avg;
float exp_gain = 0.1;
unsigned long t = 0;

void loop()
{

  // Possible vector values can be:
  // - VECTOR_ACCELEROMETER - m/s^2
  // - VECTOR_MAGNETOMETER  - uT
  // - VECTOR_GYROSCOPE     - rad/s
  // - VECTOR_EULER         - degrees
  // - VECTOR_LINEARACCEL   - m/s^2
  // - VECTOR_GRAVITY       - m/s^2

  // Serial.print("CALIBRATION: ");
  // displayCalStatus();

  // uint8_t sys_stat, self_test, sys_err;
  // bno.getSystemStatus(&sys_stat, &self_test, &sys_err);
  // Serial.println(sys_stat);
  // Serial.println(self_test);
  // Serial.println(sys_err);
  // Serial.println(bno._mode);

  imu::Vector<3> acc = bno.getVector(Adafruit_BNO055::VECTOR_ACCELEROMETER);
  imu::Vector<3> angvel = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);

  acc_avg = acc_avg * (1 - exp_gain) + acc * exp_gain;
  angvel_avg = angvel_avg * (1 - exp_gain) + angvel * exp_gain;

  // imu::Vector<3> mag = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);

  float max_acc_new = max(abs(acc.x()), max(abs(acc.y()), abs(acc.z())));
  max_acc = max(max_acc, max_acc_new);

  // filter.updateIMU(angvel.x() + offset_data.gyro_x,
  //                  angvel.y() + offset_data.gyro_y,
  //                  angvel.z() + offset_data.gyro_z,
  //                  acc.x() + offset_data.acc_x,
  //                  acc.y() + offset_data.acc_y,
  //                  acc.z() + offset_data.acc_z);

  filter.updateIMU(angvel_avg.x() + offset_data.gyro_x,
                   angvel_avg.y() + offset_data.gyro_y,
                   angvel_avg.z() + offset_data.gyro_z,
                   acc_avg.x() + offset_data.acc_x,
                   acc_avg.y() + offset_data.acc_y,
                   acc_avg.z() + offset_data.acc_z);

  niter++;
  dt = micros() - now;
  now = micros();
  t = t + dt;

  if (niter % 100 == 0)
  {
    float freq = niter * 1e+6;
    freq /= (float)(micros() - now_long);
    Serial.print("  FREQ: ");
    Serial.println(freq);

    niter = 0;
    now_long = micros();
  }

  // pres_pa = mpl.getPressure();
  // Serial.print("AMBIENT PRESSURE:    ");
  // Serial.print(pres_pa / 101325, 4);
  // Serial.println(" atm");

  // // Altitude can be calculated directly through the following equation
  // alt_m = 44330.77 * (1 - pow(pres_pa / 101326.0, 0.1902632));
  // // alt_m = mpl.getAltitude();
  // Serial.print("BAROMETRIC ALTITUDE: ");
  // Serial.print(alt_m, 2);
  // Serial.println(" m");

  // temp_C = mpl.getTemperature();
  // Serial.print("AMBIENT TEMPERATURE:  ");
  // Serial.print(temp_C, 2);
  // Serial.println(" *C");

  Serial.print("ROT:  ");
  Serial.print(filter.getPitch());
  Serial.print(",\t");
  Serial.print(filter.getYaw());
  Serial.print(",\t ");
  Serial.print(filter.getRoll());
  Serial.println();
  Serial.print("MAX ACC:  ");
  Serial.print(max_acc);
  Serial.println();

  if (file)
  {
    file.println("test");
    writeData(file, acc);
    // writeData(file, acc, filter.getPitch(), filter.getYaw(), filter.getRoll(), micros());
    file.flush();
    Serial.println("here");
    // file.sync();
  }
  
  if ((t > 3e+6) and (file))
  {
    Serial.println("  CLOSED\n\n\n\n\n\n\n\n\n\n\n\n");
    file.close();
  }
  // Serial.println("----------------------------------------------");

  // delay(BNO055_SAMPLERATE_DELAY_MS);
}