Real low latency logging for Teensy 3.5/3.6 SDIO SD

Status
Not open for further replies.
I'm not send the float over serial. I'm logging data to an SD card, then plugging the SD card into the computer and parsing the file using Matlab. Everything is fine until one of the numbers has a decimal point.
 
serial or SD, floats are handled differently when streaming to either.
What is the output?
What do you expect to see?
What does notepad see?
What does Matlab see?

If you want the float written as text (im not sure what format your looking for in matlab) then you will need to convert it to char before writing to SD
 
Below is the code as I've modified it. I'm reading a string from Serial1 of the form $PRY,±PP.P,±RRR.R,+YYY.Y. I parse that into three floats, and write "counter, time, P, R, Y" (all floats) to the SD card. I remove the SD card from the teensy and insert it into the SD reader on my computer. With Matlab, I then open the file. If there are no decimal points, I get the correct data; if there are any decimal points, I get junk.

Code:
/*
   Distributed under the 2-clause BSD licence (See LICENCE.TXT file at the
   repository root).

   Copyright (c) 2017 Tilo Nitzsche.  All rights reserved.

   https://github.com/tni/teensy-samples
*/

// Sample code for data logging. It uses a pre-allocated, pre-erased
// file to minimize latency from SD card writes. This can reduce
// the worst case write latency from 850ms down to 40ms, drastically
// reducing the required buffer size.
//
// It is intented for usage with SdFat beta:
// https://github.com/greiman/SdFat-beta
//
// with the Teensy 3.5 or 3.6 SDIO slot.
//
// The log data is captured in an ISR, triggered by an 'IntervalTimer'.
// An interrupt-safe FIFO is used for interrupt-safe communication
// with the main loop which is responsible for writing the captured data
// to the SD card.

#include <array>
#include <atomic>
#include <SdFat.h>

// Content for debugging purposes.
// Replace with something useful.
struct LogEntry {
  float counter;
  float time;
  float pitch;
  float roll;
  float yaw;
};

String SSAHRSBuffer;

// Buffer that holds our log entries. When a buffer is full,
// it is written to SD.
template<class entry_t, size_t entry_count>
class LogBuffer {
  public:
    void addEntry(const entry_t& entry) {
      if (size < capacity) log_entries[size++] = entry;
    }
    bool full() {
      return size >= capacity;
    }

    static_assert(std::is_same<uint8_t, unsigned char>::value, "Aliasing rule violation.");
    uint8_t* rawData() {
      return (uint8_t*) log_entries.data();
    }

    size_t rawSize() {
      return size * sizeof(entry_t);
    }
    void reset() {
      *this = {};
    }
  private:
    static constexpr size_t capacity = entry_count;
    std::array<entry_t, capacity> log_entries = {};
    size_t size = 0;
};

// Simple lock free single-producer, single-consumer FIFO. Interrupt-safe.
// It is intended for small entries.
// Note: atomic_signal_fence is a compiler barrier only. It requires a CPU
//       with memory order guarantees (like ARM Cortex M) to be sufficient.
template<class entry_t, size_t capacity_>
class FifoSpSc {
  public:
    // Add entry to end of FIFO. FIFO must not be full.
    void push(const entry_t& elem) {
      atomic_signal_fence(std::memory_order_acq_rel);
      const size_t write_pos_ = write_pos;
      buffer[write_pos_] = elem;
      atomic_signal_fence(std::memory_order_acq_rel);
      write_pos = nextPos(write_pos_);
      atomic_signal_fence(std::memory_order_acq_rel);
    }
    // Remove and return first FIFO entry. FIFO must not be empty.
    entry_t pop() {
      atomic_signal_fence(std::memory_order_acq_rel);
      const size_t read_pos_ = read_pos;
      entry_t ret = buffer[read_pos_];
      atomic_signal_fence(std::memory_order_acq_rel);
      read_pos = nextPos(read_pos_);
      atomic_signal_fence(std::memory_order_acq_rel);
      return ret;
    }
    bool empty() const {
      atomic_signal_fence(std::memory_order_acq_rel);
      return read_pos == write_pos;
    }
    bool full() const {
      atomic_signal_fence(std::memory_order_acq_rel);
      return read_pos == nextPos(write_pos);
    }
    size_t size() const {
      atomic_signal_fence(std::memory_order_acq_rel);
      const size_t read_pos_ = read_pos;
      const size_t write_pos_ = write_pos;
      return write_pos_ - read_pos_ + (write_pos_ >= read_pos_ ? 0 : buffer_size);
    }
    static size_t capacity() {
      return capacity_;
    }
  private:
    static size_t nextPos(size_t pos) {
      return pos + 1 < buffer_size ? pos + 1 : 0;
    }

    static constexpr size_t buffer_size = capacity_ + 1;
    std::array<entry_t, buffer_size> buffer;
    volatile size_t write_pos;
    volatile size_t read_pos;
};

// Class for pre-allocated, pre-erased file.
// It has an internal buffer that holds partial sector writes. This allows efficient writes
// of any size.
template<class sd_fat_t, class debuglog_t = void>
class PreallocatedFile {
  public:
    enum Error {
      E_ok = 0,              // no error
      E_eof,                 // end of file reached
      E_create_contiguous,   // error allocating contiguous file
      E_erase,               // error erasing allocated block
      E_write_block,         // error writing data block
    };

    // Use SdFatSdio or SdFatSdioEX for sd_fat_t.
    // 'debuglog' is optionally used for printing debug messages. Use one of the Arduino Stream classes,
    // e.g. 'Serial'.
    PreallocatedFile(sd_fat_t& sd_fat, debuglog_t* debuglog = nullptr) : sd_fat(sd_fat), debuglog(debuglog) {}
    ~PreallocatedFile() {
      if (next_sector) close();
    }

    // Create a pre-allocated, pre-erased contiguous file. Any existing file
    // with the same name, is erased.
    // Return true on success.
    bool create(const char* name, uint32_t size) {
      log("Creating file: %s\n", name);
      if (next_sector) close();
      last_error = E_ok;
      sd_fat.remove(name);
      if (!file.createContiguous(sd_fat.vwd(), name, size)) {
        log("ContigFile: createContiguous() failed");
        last_error = E_create_contiguous;
        return false;
      }
      uint32_t first_sector, last_sector;
      if (!file.contiguousRange(&first_sector, &last_sector)) {
        log("PreallocatedFile: contiguousRange() failed");
        last_error = E_create_contiguous;
        return false;
      }
      uint32_t first_erase_sector = first_sector;
      const size_t erase_count = 64 * 1024; // number of sectors to erase at once
      while (first_erase_sector <= last_sector) {
        if (!sd_fat.card()->erase(first_erase_sector, std::min(first_erase_sector + erase_count, last_sector))) {
          log("PreallocatedFile: erase() failed");
          last_error = E_erase;
          return false;
        }
        first_erase_sector += erase_count;
      }
      log("First sector: %u     last sector: %u\n", first_sector, last_sector);
      this->first_sector = first_sector;
      this->last_sector = last_sector;
      next_sector = first_sector;

      file.flush();
      return true;
    }

    // Only full sectors can be written to disk. Partial sectors are automatically
    // buffered. Writes of any size are allowed.
    // sync_blocks == true closes the current multi-block write and allows the SD card to go
    // to sleep. Performance is much lower.
    bool write(const uint8_t* buffer, size_t size, bool sync_blocks = false) {
      last_error = E_ok;
      if (!next_sector || next_sector > last_sector) {
        last_error = E_eof;
        return false;
      }
      if (partial_sector_len) {
        size_t copy_len = std::min(512 - partial_sector_len, size);
        memcpy(partial_sector_buffer + partial_sector_len, buffer, copy_len);
        partial_sector_len += copy_len;
        if (partial_sector_len < 512) return true;
        if (!sd_fat.card()->writeBlocks(next_sector, partial_sector_buffer, 1)) {
          last_error = E_write_block;
          log("Error writing log file. Current sector: %u\n", next_sector);
        }
        next_sector++;
        buffer += copy_len;
        size -= copy_len;
        partial_sector_len = 0;
      }
      const size_t sector_count = size / 512;
      if (sector_count) {
        if (!sd_fat.card()->writeBlocks(next_sector, buffer, sector_count)) {
          last_error = E_write_block;
          log("Error writing log file. Current sector: %u\n", next_sector);
        }
        next_sector += sector_count;
        size -= sector_count * 512;
        buffer += sector_count * 512;
      }
      if (size) {
        memcpy(partial_sector_buffer, buffer, size);
        partial_sector_len = size;
      }
      if (sync_blocks) sd_fat.card()->syncBlocks();
      return last_error == E_ok;
    }

    // Buffered data is written to disk, the file is truncated to the amount of data
    // that was written and then closed.
    void close() {
      last_error = E_ok;
      if (next_sector && partial_sector_len) {
        if (!sd_fat.card()->writeBlocks(next_sector, partial_sector_buffer, 1)) {
          last_error = E_write_block;
          log("Error writing log file. Current sector: %u\n", next_sector);
        }
      }
      if (next_sector) file.truncate((next_sector - first_sector - 1) * 512 + partial_sector_len);
      file.close();
      partial_sector_len = 0;
      first_sector = 0;
      last_sector = 0;
      next_sector = 0;
    }

    size_t getWriteSector() {
      return next_sector;
    }
    Error getLastError() {
      return last_error;
    }

  private:
    void log(const char* msg) {
      if (debuglog) debuglog->println(msg);
    }

    template<class... args_t>
    void log(args_t... args) {
      if (debuglog) debuglog->printf(args...);
    }

    File file;
    sd_fat_t& sd_fat;
    debuglog_t* debuglog = nullptr;
    uint32_t first_sector = 0;
    uint32_t last_sector = 0;
    uint32_t next_sector = 0;
    uint8_t partial_sector_buffer[512] __attribute__ ((aligned (4))) = {};
    size_t partial_sector_len = 0;
    Error last_error = E_ok;
};

// Store 'LogEntry's in 'LogBuffer'. Each buffer holds around 4200 bytes.
using MyLogBuffer = LogBuffer < LogEntry, 4200 / sizeof(LogEntry) >;

constexpr size_t log_buffer_count = 40;
char log_file_name[] = "log 00.bin";
uint8_t sensor_pin = A0;
uint32_t log_file_size = 2ull * 1024 * 1024 * 1024;  // 2GB
// PIT timer interval for invoking our data capture ISR.
uint32_t capture_interval = 1000;  // in microseconds

// Allocate storage for log buffers.
std::array<MyLogBuffer, log_buffer_count> log_buffers;
// FIFO that holds pointers our empty log_buffers.
FifoSpSc<MyLogBuffer*, log_buffer_count> empty_log_buffers;
// Put all allocated log buffers into 'empty_log_buffers' FIFO.
char empty_log_buffers_init = []() {
  for (size_t i = 0; i < log_buffer_count; i++) empty_log_buffers.push(&log_buffers[i]);
  return 0;
}();
// Filled log buffer FIFO.
FifoSpSc<MyLogBuffer*, log_buffer_count> filled_log_buffers;

auto& serial = Serial;
// Count the number of times our log buffers overflow.
std::atomic<size_t> buffer_overflow_counter;


SdFatSdioEX sd_fat;
bool logging_finished = false;
size_t written_bytes = 0;
size_t max_buffers_used = 0;


// Add log entry to log buffers.
void logEntry(const LogEntry& log_entry) {
  static MyLogBuffer* log_buffer = nullptr;
  if (!log_buffer) {
    if (empty_log_buffers.empty()) {
      buffer_overflow_counter++;
      return;
    } else {
      log_buffer = empty_log_buffers.pop();
    }
  }
  log_buffer->addEntry(log_entry);
  if (log_buffer->full()) {
    filled_log_buffers.push(log_buffer);
    log_buffer = nullptr;
  }
}

// ISR, periodically triggered by PIT timer. It creates some dummy
// log entries. Put your data capturing here.
void captureData() {
  float time = micros();
  static float counter = 0;
  
  if (Serial1.available()) {
    char inputSerial1 = Serial1.read();

    if (inputSerial1 =='\n') {
      char * strtokIndx;
      char temp_buffer[25];
      SSAHRSBuffer.toCharArray(temp_buffer,25);
      
      strtokIndx = strtok(temp_buffer, ",");
      strtokIndx = strtok(NULL, ",");
      float pitch = atof(strtokIndx);
      strtokIndx = strtok(NULL, ",");
      float roll = atof(strtokIndx);
      strtokIndx = strtok(NULL, ",");
      float yaw = atof(strtokIndx);
      SSAHRSBuffer = "";

      logEntry( { counter, time, pitch, roll, yaw } );
      counter++;
    } else {
      SSAHRSBuffer += inputSerial1;
    }
  }
  
}

void setup() {
  pinMode(LED_BUILTIN, OUTPUT);

  SSAHRSBuffer.reserve(128);
  serial.begin(115200);
  Serial1.begin(115200);
  delay(2000);
  serial.println("Initializing SD.");

  if (!sd_fat.begin()) {
    serial.println("SD init failed.");
    return;
  }
  serial.println("SD init done.");

  serial.println("Press b to begin logging data");
  while (!serial.available()) {}

  while (sd_fat.exists(log_file_name)) {
    if (log_file_name[5] != '9') {
      log_file_name[5]++;
    } else {
      log_file_name[5] = '0';
      log_file_name[4]++;
    }
  }
    
  using file_t = PreallocatedFile<decltype(sd_fat), std::remove_reference<decltype(serial)>::type>;
  file_t file(sd_fat, &serial);
  file.create(log_file_name, log_file_size);
  serial.println("Log file allocated and pre-erased.");
  serial.println("\nPress 'c' to terminate logging.\n");

  elapsedMillis report_timer;
  IntervalTimer data_capture_timer;
  data_capture_timer.begin(captureData, capture_interval);

  while (true) {
    // If we have a filled log buffer, write it to SD.
    if (!filled_log_buffers.empty()) {
      max_buffers_used = std::max(max_buffers_used, empty_log_buffers.capacity() - empty_log_buffers.size());
      MyLogBuffer* log_buffer = filled_log_buffers.pop();
      if (!file.write(log_buffer->rawData(), log_buffer->rawSize())) {
        if (file.getLastError() == file.E_eof) {
          file.close();
          serial.println("Log file full. Logging finished.");
          return;
        }
        // ignore other write errors, hopefully subsequent writes will succeed
      }
      written_bytes += log_buffer->rawSize();
      *log_buffer = {};
      empty_log_buffers.push(log_buffer);
    }
    if (report_timer > 5000) {
      serial.printf("elapsed: %us     bytes written: %u     buffer overflow count: %u\n",
                    millis() / 1000, written_bytes, buffer_overflow_counter.load());
      serial.printf("next write sector: %u    max buffers used during last 5s: %u\n",
                    file.getWriteSector(), max_buffers_used);
      report_timer = 0;
      max_buffers_used = 0;
    }
    if (serial.available()) {
      if (serial.read() == 'c') {
        file.close();
        serial.println("Logging finished.");
        return;
      }
    }
  }
}

// Running when an error is encountered or when logging is finished.
void loop() {
  digitalWrite(LED_BUILTIN, HIGH);
  delay(200);
  digitalWrite(LED_BUILTIN, LOW);
  delay(200);
}
 
I'm not send the float over serial. I'm logging data to an SD card, then plugging the SD card into the computer and parsing the file using Matlab. Everything is fine until one of the numbers has a decimal point.

Do you see, that is why we have a forum rule
please provide program you use on Teensy to write the data and the program on Matlab you use to read the data.
So we can reproduce your problem.

If you say you changed the type in matlab to float then the data are still binary data and there is no decimal point in binary. there are only 32 bits.
Your description of the problem makes no sense, or at least I cannot understand it.

Edit see you posted teensy program, what about matlab?
 
In Matlab:

fileID=fopen(data 00.bin);
data=transpose(fread(fileID,[columns, length],'float'))

where columns in this case = 5 and length varies with the number of entries in the file.
 
yes. I have added "serial.println(pitch)" and it prints the correct value to the serial port. I've also tested using dummy values for pitch, roll and yaw, to the same effect, if one of the dummy values has a decimal point, reading the data in Matlab fails.
 
could you try to read in Matlab a couple of lines (2 or 3) with 'uint32' instead of 'float' and print the data using dec2hex(data) and post the result, maybe one can see what is wrong?
 
I have solved the problem. Matlab was displaying the data in a way I wasn't understanding, but it is all there and correct.
 
And generally, if you need to convert float to either double or ascii text, it will take many cycles. If you need real low latency, you might want to rethink sending floating point.
 
Status
Not open for further replies.
Back
Top