Replacing the Arduino setup/loop Mechanism

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();
}