Adafruit Feather M0 with LSM9DS1 using timer interrupts and SD card

lsm9ds1_interrupt_SD.ino

#include <SD.h>
#include <Wire.h>
#include <SPI.h>
#include <Adafruit_LSM9DS1.h>
#include <Adafruit_Sensor.h>

// I need to undef the max and min macros defined by the Arduino
// stuff, because <tuple> included in Ring.cpp requires the std::max
// and min from C++.
#undef max
#undef min
#include "Ring.cpp"

//#define DEBUG

// Digital pin to synchronize with other microcontroller
#define syncPin 19
volatile unsigned long sync_count = 0;
volatile unsigned long sync_time = 0;
volatile bool sync_flag = false;

// My SD card as a Device Block Size of 512 bytes (found with diskutil info or newfs_msdos -N)
// and a Cluster Size of 32kb.  I might want to buffer so that writes are a multiple of 512 bytes?
constexpr unsigned long LSM9DS1_BUFFER_SIZE = 1000;
#define LSM9DS1_DEBUG true
#define LSM9DS1_WIDTH 6
// sensors_vec_t is defined in Adafruit_Sensor.h
// it is float[3], int8_t, and uint8_t[3] wide.  sizeof(sensors_vec_t) gives 16 bytes.
// The Feather M0 has 32kb of RAM, so I can store ~1300 acceleration+time pairs.
struct measurement
{
  sensors_vec_t mval;
  unsigned long mtime;
  unsigned long timer_overflows;
};
Ring::Ring<measurement, LSM9DS1_BUFFER_SIZE> measurements;//_1, measurements_2;
//auto reading = &measurements_1;
//decltype(reading) writing = nullptr;
unsigned long timer_overflows = 0;
unsigned long last_mtime = 0;

// LSM9DS1 object
Adafruit_LSM9DS1 lsm = Adafruit_LSM9DS1();

// SD card object
File logfile;
#define SD_chipSelect 4
#define ledPin 13

#include <Arduino.h>
#include "Adafruit_ZeroTimer.h"
// Timer object
Adafruit_ZeroTimer zt3 = Adafruit_ZeroTimer(3);

// Timer callback function called when Compare0 is hit.
void Timer3Callback0(struct tc_module *const module_inst)
{
  // Flash the LED while reading the LSM9DS1.
  //digitalWrite(ledPin, HIGH);

  // The data is actually read during the read() call, so get the time now.
  // NOTE: micros overflows after about 70 minutes.
  auto mtime = micros();
  if(mtime < last_mtime) timer_overflows++;

  // This way to read works, but it reads all the sensors (twice, I think).
  //lsm.read(); 
  // Extract the acceleration value.
  // TODO: See if I can turn off reading the m, g and temp.
  // TODO: See if I can read more raw data, e.g. int instead of float.
  sensors_event_t a;//, mag, gyr, temp;
  lsm.getEvent(&a, nullptr, nullptr, nullptr);//&mag, &gyr, &temp); 

  // Add accelerations and measurement time to ring buffer.
  measurement m;
  m.mval = a.acceleration;
  m.mtime = mtime;
  m.timer_overflows = timer_overflows;
  measurements.add(m);

  last_mtime = mtime;
  //digitalWrite(ledPin, LOW);
}


void setup() {
  pinMode(ledPin, OUTPUT);
  #ifdef DEBUG
    Serial.begin(115200);
    while (!Serial);
    Serial.println("LSM9DS1 Interrupt-driven FIFO'd Thing");
  #endif
  
  // Timer #3, 16 bit, two PWM outs, period = 65535
  // DIV8 seems to give the best results, with a sample rate of ~4.9ms
  // Going too fast prevents proper writing to the SD card (buffer fills faster than we can empty it)
  zt3.configure(TC_CLOCK_PRESCALER_DIV8, // prescaler DIV can be 1, 2, 4, 8, 16, 64, 256, or 1024
                TC_COUNTER_SIZE_16BIT,   // bit width of timer/counter
                TC_WAVE_GENERATION_NORMAL_PWM // frequency or PWM mode 
                //TC_WAVE_GENERATION_NORMAL_FREQ
                );

  zt3.setCompare(0, 0xFFFF/4); 
  zt3.setCallback(true, TC_CALLBACK_CC_CHANNEL0, Timer3Callback0); 
  //zt3.enable(true);

  #ifdef DEBUG
    Serial.println("Done setting up timer interrupt.");
  #endif
  
  // LSM9DS1 Setup
  if (!lsm.begin())
  { // In case of failure to initialize...
    Serial.println("Unable to initialize the LSM9DS1.");
    while (1);
  }
  // Set the accelerometer, magnetometer, and gyroscope ranges
  lsm.setupAccel(lsm.LSM9DS1_ACCELRANGE_2G); // Can be _2G, _4G, _8G, or _16G
  lsm.setupMag(lsm.LSM9DS1_MAGGAIN_4GAUSS); // Can be _4GAUSS, _8, _12 or _16
  lsm.setupGyro(lsm.LSM9DS1_GYROSCALE_245DPS); // Can be _245DPS, _500 or _2000
  #ifdef DEBUG
    Serial.println("Done setting up LSM9DS1");
  #endif
  
  // SD card setup.
  if(!SD.begin(SD_chipSelect))
  {
    Serial.println("Unable to initialize SD card.");
    while (1);
  }
    // This section auto-increments the filename ACC*****.LOG.
  bool SD_name_ok = false;
  char filename[15];
  strcpy(filename, "ACC00000.LOG");
  for (uint32_t i = 0; i < 99999; i++) {
    filename[3] = '0' + i/10000;
    filename[4] = '0' + (i % 10000)/1000;
    filename[5] = '0' + (i % 1000)/100;
    filename[6] = '0' + (i % 100)/10;
    filename[7] = '0' + i%10;
    // create if does not exist, do not open existing, write, sync after write
    if (! SD.exists(filename)) {
      SD_name_ok = true;
      break;
    }
  }
  if(!SD_name_ok)
  {
    Serial.println("Error coming up with a filename on the SD card.");
    while (1);
  }
  logfile = SD.open(filename,FILE_WRITE);
  if(!logfile)
  {
    Serial.print("Error opening file ");Serial.print(filename);Serial.println();
    while (1);    
  }

  zt3.enable(true);
  pinMode(syncPin, INPUT);
  attachInterrupt(digitalPinToInterrupt(syncPin), sync_ISR, RISING);
  
  #ifdef DEBUG
    Serial.print("Writing to ");Serial.println(filename);
    Serial.print("Done setting up in "); 
    Serial.print(micros());
    Serial.println(" usec.");
    delay(5000);
    Serial.print("Finished 5000 ms delay at "); 
    Serial.print(micros());
    Serial.println(" usec.");
  #endif
}

void loop() {
  // Every DELAY, check if we got some measurements done.  If so, print'em.
  //delay(1000);
  if(sync_flag)
  {
    logfile.print("# sync ");logfile.print(sync_count);logfile.print(",");logfile.print(sync_time);logfile.print(",");logfile.println(timer_overflows);
    #ifdef DEBUG
    Serial.print("# sync ");Serial.print(sync_count);Serial.print(",");Serial.print(sync_time);Serial.print(",");Serial.println(timer_overflows);
    #endif
    sync_flag = false;
  }

  if(measurements.count > LSM9DS1_BUFFER_SIZE/2)
  {
    digitalWrite(ledPin, HIGH);
    bool wrote = false;
    while(measurements.count > 0)
    {
      #ifdef DEBUG
        if(!wrote)
        {
          Serial.print("Writing to SD card ");Serial.print(measurements.count);Serial.println(" measurements.");
        }
      #endif
      // Get the accelerations & times from the ring buffer.
      //sensors_vec_t a;
      //unsigned int mtime;
      
      // Disable interrupts during remove call, to avoid clashing with .add() call.
      zt3.enable(false);
      auto m_pair = measurements.remove();
      zt3.enable(true);
      
      //measurement m;
      //int m_ok;
      //std::tie(m,m_ok) = m_pair;
      //a = m.mval;
      sensors_vec_t a = m_pair.first.mval;
      unsigned int mtime = m_pair.first.mtime; //m.mtime.;
      unsigned int overflows = m_pair.first.timer_overflows;

      #ifdef DEBUG
        Serial.print(mtime); Serial.print(","); Serial.print(overflows);
        Serial.print(","); Serial.print(a.x); Serial.print(",");
        Serial.print(a.y); Serial.print(",");Serial.print(a.z); Serial.println();
      #endif
  
      // TODO: Replace logfile.print with logfile.write and binary data.
      // Write a python script to convert binary data to ascii-csv.
      logfile.print(mtime); logfile.print(","); logfile.print(overflows);
      logfile.print(","); logfile.print(a.x); logfile.print(",");
      logfile.print(a.y); logfile.print(",");logfile.print(a.z); logfile.println();
  
      wrote = true;
    }
    if(wrote)
    {
      logfile.flush();
    }
    digitalWrite(ledPin, LOW);
  }
}

void sync_ISR()
{
  sync_count++;
  sync_time = micros();
  sync_flag = true;
}

Ring.cpp

//#include <iostream>
//#include <algorithm>
//#include <cassert>
//#include <iomanip>
#include <utility>
#include <tuple>

namespace Ring
{

template<class T, unsigned long BUFF_SIZE>
class Ring
{
  public:
  T buff[BUFF_SIZE];
  unsigned long front = 0;
  unsigned long end = 0;
  unsigned long count = 0;
  
  Ring()
  {
    for(unsigned long i = 0; i<BUFF_SIZE; i++) buff[i] = T();
  }
  
  bool isEmpty()
  {
    if(count == 0)
    {
//      assert(front==end);
    }
    return not count;
  }

  bool isFull()
  {
    if(count == BUFF_SIZE)
    {
//      assert(front == end);
      return true;
    }
    return false;
  }

  void add(T val)
  {
    buff[front] = val;
    if(isFull())
    {
      end = (end + 1) % BUFF_SIZE;
    }
    else
    {
      count = (count + 1) < BUFF_SIZE ? (count + 1) : BUFF_SIZE;//std::min(count + 1,BUFF_SIZE);
    }
    front = (front + 1) % BUFF_SIZE;
  }

  std::pair<T,int> remove()
  {
    if(isEmpty())
    {
      return std::make_pair(T(),1);
    }
    //std::cout << "Removing index " << end << ", value " << buff[end] << std::endl;
    auto val = buff[end];
    buff[end] = T();

    end = (end + 1) % BUFF_SIZE;
    count = (count - 1) > static_cast<unsigned long>(0) ? (count - 1) : static_cast<unsigned long>(0);//std::max(count - 1, static_cast<unsigned long>(0));
    return std::make_pair(val,0);
  }
  
  // void print()
  // {
  // for(unsigned long i = 0; i < BUFF_SIZE; i++)
  // {
  //   std::cout << std::setfill('0') << std::setw(2) << buff[i] << ",";
  // }
  // std::cout << "count : " << count << "\n ";
  
  // for(unsigned long i = 0; i < BUFF_SIZE; i++)
  // {
  //   if((i == front) and (i == end)) std::cout << "&";
  //   else if(i == front) std::cout << "^";
  //   else if(i == end) std::cout << "*";
  //   else std::cout << " ";
  //   std::cout << "  ";
  // }
  // std::cout << std::endl;
  // }
};

} // End namespace Ring.


// int main() {
//   Ring::Ring<int,10> r;
//   unsigned int N = 15;
//   for(unsigned int i = 0; i < N; i++)
//   {
//     r.print();
//     r.add(i);
//   }
//   r.print();
//   std::cout << "Done reading." << std::endl;
//   for(unsigned int i = 0; i < N; i++)
//   {
//     r.print();
//     r.remove();
//   }
//   std::cout << "Done writing." << std::endl;
//   r.print();
//   return 0;
// }