While my robot “Raven” uses MicroRos to communicate between my TeensyMonitor board, which is based on a Teensy 4.1 device, there are times when software faults happen and the information I need to debug the fault doesn’t get transmitted as a ROS message. To overcome that, I have a software module on the TeensyMonitor board that logs a lot of information to an SD memory card. After a fault is detected, I can pull that memory card off from the TeensyMonitor board and plug it into another computer and look at the text log files for diagnostic information.

A problem with the built-in SD card on the Teensy 4.1, using the Arduino SD library is that the availableForWrite function always returns zero, so the example code for non-blocking I/O doesn’t work. This means that writes to the card necessarily block until complete. In a robot, any operation that takes a long time can be hazardous to the safety of the robot. In my library, while not eliminating the problem I attempt to reduce the problem by writing data in 4K byte chunks to the SD card. The result is that my average loop calls per second reduces from around 200 loop calls per second to about 175. This, of course, is specific to the work I do per loop call, which involves reading over a dozen sensors and sending as well as receiving messages via MicroRos.

A previous article explained my use of TModule in my software stack, which injects software module performance monitoring into each of my modules, so I won’t explain that framework again here. To use this logging module, you initialize it simply by including one line of code in you main “.ino” file, namely

TSd& sd = TSd::singleton();

Then you add lines to the log file via a call like:

TSd::singleton().log("INFO [MyModule::myFunction] Some message here");

Which will write to the log file a line similar to:

[0000111.428] INFO MyModule::myFunction] Some message here

Where the number in brackets is the number of milliseconds that have elapsed since the Arduino started up.

Here is the header file, “tsd.h” for the module:

#pragma once

#include <Regexp.h>
#include <SD.h>

#include "tmodule.h"

// A class used to write logging information to an SD memory card.
//
// On setup, the SD card is examined for any existing files with names like
// "LOG12345.TXT", where "12345" is a 5-digit serial number. A new log file
// will be created on the card with a name having a serial number of one higher
// than any existing log file name,or LOG00001.TXT if no existing log files
// were found.
//
// Every time TSd::singleton().log() is called, the message is added to a
// string buffer. When the buffer becomes full enough, it is written as one
// large chunck of text to the log file. This is done so that lots of little
// writes are done, which would slow down the outer loop of the Arduino device.
//
// The downside is that if the card is pulled from the Arduino device, the last
// chunk of text won't have been written to the device. Currently, there is a fair 
// number of things written to the log file so if you just wait a few seconds before
// pulling the card, the interesting thing you were looking for in the log file may
// have been actually successfully written to the file before you pulled the card.
//
// This is intended to be used the the TModule software module as part of the
// TeensyMonitor stack. So all you need to do to instantiate and get this module
// going is to call TSd::singleton() in your ".ino" file before calling TModule::setup().
//
// If a write to the card fails, perhaps because the card is full or the card has been
// pulled from the Arduino device, further writes are not attempted until the system
// is restarted.

class TSd : TModule {
 public:
  // Write message to log file.
  void log(const char* message);

  // Singleton constructor.
  static TSd& singleton();

 protected:
  // From TModule.
  void loop();

  // From TModule.
  virtual const char* name() { return "TSd"; }

  void setup();

 private:
  // Private constructor.
  TSd();

  static void regexpMatchCallback(const char* match, const unsigned int length,
                                  const MatchState& matchState);

  // Used to hold a big chunk of data so writes are fewer.
  String data_buffer_;

  // The SD card device.
  static SDClass g_sd_;

  // Used to find to highest log file number already on the SD card.
  static int g_highestExistingLogFileNumber_;

  // Has SD device been properly initialized?
  static bool g_initialized_;

  // The file handle for the log file on the SD card device.
  static File g_logFile_;

  // Singleton instance.
  static TSd* g_singleton_;
};

Here the the body file, “tsd.cpp”

#include "tsd.h"

#include <Arduino.h>
#include <Regexp.h>
#include <SD.h>
#include <stdint.h>

// #include "tmicro_ros.h"

void TSd::log(const char* message) {
  static const unsigned int kChunkSize = 4096;
  if (g_initialized_) {
    char log_message[256];
    uint32_t now = millis();
    snprintf(log_message, sizeof(log_message), "[%07ld.%03ld] %s\n", now / 1000,
             now % 1000, message);
    data_buffer_ += log_message;
    if (data_buffer_.length() >= kChunkSize) {
      size_t bytes_written = g_logFile_.write(data_buffer_.c_str(), kChunkSize);
      if (bytes_written > 0) {
        g_logFile_.flush();
      } else {
        // Assume the card has been removed or is failing.
        g_initialized_ = false;
      }

      data_buffer_.remove(0, kChunkSize);
    }
  }
}

void TSd::loop() {}

void TSd::regexpMatchCallback(const char* match, const unsigned int length,
                              const MatchState& matchState) {
  char regexMatchString[10];  // Big enough to hold 5-digit file serial number.
  matchState.GetCapture(regexMatchString, 0);  // Get 0-th match from regexp.
  int logSerialNumberAsInt = atoi(regexMatchString);
  if (logSerialNumberAsInt > TSd::g_highestExistingLogFileNumber_) {
    TSd::g_highestExistingLogFileNumber_ = logSerialNumberAsInt;
  }
}

void TSd::setup() {
  data_buffer_.reserve(8192);
  g_highestExistingLogFileNumber_ = 0;
  g_initialized_ = false;
  if (!g_sd_.begin(BUILTIN_SDCARD)) {
    // ERROR Unable to access builtin SD card.
  } else {
    File rootDirectory = g_sd_.open("/");
    while (true) {
      File nextFileInDirectory = rootDirectory.openNextFile();
      if (!nextFileInDirectory) break;
      char fileName[256];
      strncpy(fileName, nextFileInDirectory.name(), sizeof(fileName));
      MatchState matchState;
      matchState.Target(fileName);
      matchState.GlobalMatch("LOG(%d+).TXT", regexpMatchCallback);
    }

    char
        newLogFileName[20];  // Big enough to hold file name like: LOG12345.TXT.
    sprintf(newLogFileName, "LOG%05d.TXT", ++g_highestExistingLogFileNumber_);
    g_logFile_ = g_sd_.open(newLogFileName, FILE_WRITE);
    if (!g_logFile_) {
      char diagnosic_message[128];
      snprintf(diagnosic_message, sizeof(diagnosic_message),
               "ERROR [TSd::setup] Unable to create new log file: '%s'",
               newLogFileName);
    } else {
      g_initialized_ = true;
      ;
    }
  }
}

TSd::TSd() : TModule(TModule::kSd) {}

TSd& TSd::singleton() {
  if (!g_singleton_) {
    g_singleton_ = new TSd();
  }

  return *g_singleton_;
}

SDClass TSd::g_sd_;

int TSd::g_highestExistingLogFileNumber_ = 0;

bool TSd::g_initialized_ = false;

File TSd::g_logFile_;

TSd* TSd::g_singleton_ = nullptr;

Arduino-like processors are programmed with a setup and loop mechanism. The function setup is called once by the underlying firmware, and then loop is called repeatedly after that. I created a TModule class that I use to extend/replace/wrap those functions (choose how you want to think about it). This primarily gives me the ability to keep track of the performance of each piece of software that I write.

My robot uses a Teensy 4.1 microprocessor, and a custom PC board that I designed, into which that Teensy 4.1 is plugged in and which then extends all the I/O of the Teensy in a useful way. For instance, I use one of the Teensy 4.1 I2C channels along with an I2C multiplexer to talk to the eight time-of-flight sensors on the robot. The HC-SR04 SONAR sensors require level converters to translate the 3.3 volt I/O pins of the Teensy to the 5 volt pins of the SONAR sensors. And so on. My robot uses nearly every I/O pin on the Teensy.

The software on the Teensy, which I call the Teensy Monitor, is organized around modules. For instance, there is a SONAR module, a time-of-flight module, a module to talk to the RoboClaw motor controller, a module to communicate with the other processors on the robot using micro-ROS, and so on. For each of these modules, I need to see if there are any hardware failures and I need to see if they are performing as expected.

For example, it’s possible that one of the time-of-flight sensors isn’t working correctly. It’s possible that one of the motor current sensors isn’t responding. It’s possible that some module is taking too long to operate, which might then effect how often the main processor of the robot gets SONAR data, which might make the robot operate in an unsafe manner.

My TModule class handles the performance monitoring part of all of this.

The way you use TModule is that each software module is a class that derives from TModule. As each software module class is instantiated, the instance is added to a list of all instantiated modules. TModule has a DoSetup method that, when called, will call setup for each instantiated module. And TModule has a DoLoop method that, when called, will call the loop method for each instantiated module, but it wraps that loop call with code that will measure the duration of the loop call.

For each software module, statistics are capture for the minimum, maximum and average execution duration of the loop call. There is a method in TModule that returns a JSON string showing the statistics for every instantiated module. My module that uses micro-ROS has a method that periodically gets the statistics from TModule and sends that JSON string to the ROS system on the robot. Asking for the statistics from TModule also resets the statistics. So each JSON message shows the performance of every instantiated module since the last reported performance.

Here is what a JSON statistics report looked like at one point in my software development:

{
  "Stats": {
    "loops": 211,
    "Ms": 999.8,
    "mdls": [
      { "n": "Alrm", "MnMxAv": [0.0, 0.0, 0.0] },
      { "n": "uRos", "MnMxAv": [0.2, 1.6, 1.3] },
      { "n": "Amps", "MnMxAv": [1.0, 1.0, 1.0] },
      { "n": "Rlay", "MnMxAv": [0.0, 0.0, 0.0] },
      { "n": "Robo", "MnMxAv": [0.6, 1.2, 0.8] },
      { "n": "Sonr", "MnMxAv": [0.0, 0.0, 0.0] },
      { "n": "Temp", "MnMxAv": [0.0, 0.0, 0.0] },
      { "n": "ToFl", "MnMxAv": [1.6, 1.7, 1.6] }
    ]
  }
}

This says that since the last reported statistics, the Arduino loop method was called 211 times and it took 999.8 milliseconds to execute those 211 calls. The 999.8 milliseconds period is because the micro-ROS module is set up to report statistics every second. This message lets me know that in the last second, the SONAR module was called 211 times, the time-of-flight module was called 211 times, and so on.

Each module subclass has a name method that returns a short string naming the module. That value shows after the “n” keyword above. So, the module that talks to the RoboClaw is named Robo, and over the last 211 times the RoboClaw module had its loop method called, the minimum execution time of a loop call was 0.6 milliseconds. The maximum loop call duration was 1.2 milliseconds, and the average call duration was 0.8 milliseconds. If I wanted to increase the overall number of loop calls per second, I should first look at improving the modules with the largest maximum or average duration, such as the micro-ROS loop method, called uROS in the report, as its average execution duration is 1.3 milliseconds, or the time-of-flight module, called ToFl in the report, as its average execution duration is 1.6 milliseconds.

The header file, tmodule.h is:

#pragma once

#include <stdint.h>

#include "Arduino.h"

/**
 * Base class for a kind of module which:
 *  - Needs to be setup once via a call to DoSetup().
 *  - Needs to be invoked once per "processing cycle" via a call to DoLoop().
 *  - Generates performance statistics gathered about performance during loop
 * execution.
 *
 * Usage:
 * Each module exists as a singleton. Somewhere, modules are
 * brought into existence by calling, typically, their
 * singleton() method. As each module comes into existence,
 * This TModule class keeps a list of them.
 *
 * After all modules are brought into existence, you call the
 * DoSetup() method to invoke the setup() method for all registered
 * modules. Then in the main processing loop, you invoke the
 * DoLoop() method to invoked the loop() method for all registered
 * modules.
 * 
 * Periodically, some agent calls GetStatistics() to get a string showing
 * execution performance of all the registered modules.
 */

class TModule {
 public:
  // A list of all possible modules.
  typedef enum Module {
    kMicroRos, // Make this the first module as it is used for diagnostic reporting.
    kMotorCurrent,
    kPanelSelector,
    kRelay,
    kRoboClaw,
    kSonar,
    kTemperature,
    kTimeOfFlight,
    kNumberModules  // The number of all possible modules.
  } Module;

  // Call loop() for all registered modules.
  static void DoLoop();

  // Call setup() for all registered modules.
  static void DoSetup();

  // Get a JSON string showing module performance since the
  // last call to GetStatistics.
  static void GetStatistics(char* outString, size_t outStringSize);

 protected:
  TModule(Module moduleKind);

  // Perform regular, cyclic work for the module.
  virtual void loop() = 0;

  // Return module name.
  virtual const char* name() = 0;

  // Perform one-time setup for the module.
  virtual void setup() = 0;

 private:
  TModule();

  // Define slots for gathering statistics for the module.
  typedef enum Slot {
    kMin,
    kMax,
    kSum,
    kNumberSlots            // Number of slots to reserve for statistics.
  } Slot;

  // Number of times DoLoop() was called.
  static uint32_t total_do_loop_count_;

  // A list of all registered modules.
  static TModule* all_modules_[];

  // Index to the next statistic reading for the module.
  int loop_calls_between_get_statistics_calls_;

  // Statistics gathered per registered modules.
  float duration_stats_[kNumberSlots];
};

The body code file tmodule.cpp

#include "tmodule.h"

#include <Arduino.h>
#include <stdint.h>

#include "tmicro_ros.h"

TModule::TModule(TModule::Module moduleKind) {
  all_modules_[moduleKind] = this;
  loop_calls_between_get_statistics_calls_ = 0;
  for (size_t i = 0; i < kNumberSlots; i++) {
    duration_stats_[i] = 0.0;
  }
}

void TModule::GetStatistics(char* outString, size_t outStringSize) {
  static uint32_t statTimingStart = micros();
  char statList[2048];

  statList[0] = '\0';

  for (size_t i = 0; i < kNumberModules; i++) {
    if (all_modules_[i] != nullptr) {
      TModule* module = all_modules_[i];
      char temp[512];
      temp[0] = '\0';
      snprintf(temp, sizeof(temp),
               "{\"n\":\"%-s\",\"MnMxAv\":[%-2.1f,%-2.1f,%-2.1f]},",
               module->name(), module->duration_stats_[kMin],
               module->duration_stats_[kMax],
               module->duration_stats_[kSum] / total_do_loop_count_);
      strcat(statList, temp);
      module->loop_calls_between_get_statistics_calls_ = 0;
      module->duration_stats_[kMin] = 10'000'000.0;
      module->duration_stats_[kMax] = -10'000'000.0;
      module->duration_stats_[kSum] = 0.0;
    }
  }

  // Remove trailing comma from previous list.
  if (strlen(statList) > 0) {
    statList[strlen(statList) - 1] = '\0';
  }

  snprintf(outString, outStringSize,
           "{\"loops\":%-ld,\"Ms\":%-2.1f,\"mdls\":[%-s]}",
           total_do_loop_count_, ((micros() * 1.0) - statTimingStart) / 1000.0,
           statList);
  statTimingStart = micros();
  total_do_loop_count_ = 0;
}

void TModule::DoLoop() {
  for (size_t i = 0; i < kNumberModules; i++) {
    if (all_modules_[i] != nullptr) {
      TModule* module = all_modules_[i];
      uint32_t start = micros();

      all_modules_[i]->loop();

      float duration = (micros() - start) / 1000.0;
      module->duration_stats_[kSum] += duration;
      if (duration < module->duration_stats_[kMin]) {
        module->duration_stats_[kMin] = duration;
      }

      if (duration > module->duration_stats_[kMax]) {
        module->duration_stats_[kMax] = duration;
      }

      module->loop_calls_between_get_statistics_calls_++;
    }
  }

  total_do_loop_count_++;
}

void TModule::DoSetup() {
  for (int i = 0; i < kNumberModules; i++) {
    if (all_modules_[i] != nullptr) {
      all_modules_[i]->setup();
    }
  }
}

TModule* TModule::all_modules_[TModule::kNumberModules + 1] = {
    nullptr, nullptr, nullptr, nullptr, nullptr,
    nullptr, nullptr, nullptr};

uint32_t TModule::total_do_loop_count_ = 0;

This is all driven by code in the main ino file for the Arduino-like system, where each module of interest is instantiated, then DoSetup and DoLoop are called as shown.

#include <stdint.h>

#include "tmicro_ros.h"
#include "tmodule.h"
#include "trelay.h"
#include "troboclaw.h"
#include "tsonar.h"
#include "ttemperature.h"
#include "ttime_of_flight.h"

// Initialize all TModule instances in any required order.
TMicroRos& micro_ros = TMicroRos::singleton();
TRelay& relay = TRelay::singleton();
TRoboClaw& roboclaw = TRoboClaw::singleton();
TSonar& sonar = TSonar::singleton();
TTemperature& temperature = TTemperature::singleton();
TTimeOfFlight& time_of_flight = TTimeOfFlight::singleton();

void setup() {
  TModule::DoSetup();
}

void loop() {
  TModule::DoLoop();
}