ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Threading in micro-ROS on Teensy?

asked 2022-10-23 01:38:30 -0500

PointCloud gravatar image

Hi all, I've been wondering if any of the experts here familiar with the matter would be able to provide some insights on threading in micro-ROS when used on Teensy?

As known micro-ROS with teensy does not utilize RTOS.

The task is continuously reading of values of a TFMini-Plus and then publish these on a topic. Previously this has been accomplished with threading. But since moving to micro-ROS I am uncertain this would be the same approach.

So the questions are basically boiled down to these two: 1. ok to use threading as before moving to micro-ROS? (see example source below, how it was done before) 2. ok to use time-slicing / state-machine style programming in micro-ROS, where spinOnce or spinSome is used? (Also seen in main loop of example below)

#include <TeensyThreads.h>
#include <Wire.h>


#ifdef __AVR__
  #include <avr/power.h>
#endif

volatile int TFMiniVal = 0;
int TFMini_FloorDistance = 30;
int TFMini_FloorDistanceTolerance = 12;

bool bFirstScan = true;
static uint32_t tTime[11];

#define SR00_DEBUG_EN 0
#define SR01_TFMINI_EN 1

namespace sensor {
    // --------------------------------------------------------
    // TF-MINI PLUS
    // --------------------------------------------------------
    #define TFMiniPlus Serial7  // 28=RX7, 29=TX7
    #define TFMini_FREQ 10      // TFMini-Plus service interval    

} // namespace sensor

// --------------------------------------------------------
// CLASS OBJECTS
// --------------------------------------------------------


// --------------------------------------------------------
// FUNCTION PROTOTYPES
// --------------------------------------------------------


// ========================================================
// SETUP
// ========================================================
void setup() {
  // TFMini Plus
  TFMiniPlus.begin(115200);  // HW Serial for TFmini
  delay (200);            // Give a little time for things to start
  // Set TFMini Plus to Standard Output mode
  TFMiniPlus.write(0x42);
  TFMiniPlus.write(0x57);
  TFMiniPlus.write(0x02);
  TFMiniPlus.write(0x00);
  TFMiniPlus.write(0x00);
  TFMiniPlus.write(0x00);
  TFMiniPlus.write(0x01);
  TFMiniPlus.write(0x06);
  // Setup thread for reading serial input from TFmini
  threads.addThread(readTFMiniPlus);

  delay(500);
}

// ========================================================
// MAIN LOOP
// ========================================================
void loop() {
  uint32_t t = millis();

  if (bFirstScan) {
    bFirstScan = false;
  }

  // ------------------------------------------------------
  // Service Routine[0]: DEBUG / INA260
  #if SR00_DEBUG_EN == 1
  if ((t-tTime[0]) >= (1000 / DEBUG_LED_PULSE))
  {
    // some debugging stuff happened here, like serial output, etc
    tTime[0] = t;
  }
  #endif

  // ------------------------------------------------------
  // Service Routine[1]: TFMini-Plus
  #if SR01_TFMINI_EN
  if ((t-tTime[1]) >= (1000 / TFMini_FREQ))
  {
    //TFMini Plus - read distance
    // readTFMiniPlus();    // not needed, as we are reading from the thread

    //if TFMini looses sight of floor STOP!
    if (TFMiniVal >=  (TFMini_FloorDistance + TFMini_FloorDistanceTolerance)) {
      // abyss detected
    } else {
      // all good, floor in sight
    }
    tTime[1] = t;
  }
  #endif

  // ------------------------------------------------------
  // Service Routine[10]: OLED
  #if SR10_OLED_EN
  if ((t-tTime[10]) >= (1000 / OLED_FREQ))
  {
    // update OLED
    tTime[10] = t;
  }
  #endif
} // End of MAIN LOOP

// --------------------------------------------------------
// FUNCTIONS
// --------------------------------------------------------
// readTFMiniPlus
void readTFMiniPlus(){
  // Data Format for Benewake TFmini
  // ===============================
  // 9 bytes total per message:
  // 1) 0x59
  // 2) 0x59
  // 3) Dist_L (low 8bit)
  // 4) Dist_H (high 8bit)
  // 5) Strength_L (low 8bit)
  // 6) Strength_H (high 8bit)
  // 7) Reserved bytes
  // 8) Original signal quality degree
  // 9) Checksum parity bit (low 8bit), Checksum = Byte1 + Byte2 +…+Byte8. This is only a low 8bit though
  while(TFMiniPlus.available()>=9) // When at least 9 bytes of data available (expected number of bytes for 1 signal), then read
  {
    if((0x59 == TFMiniPlus.read()) && (0x59 == TFMiniPlus.read())) // byte 1 and byte 2
    {
      unsigned int t1 = TFMiniPlus.read(); // byte 3 = Dist_L
      unsigned int t2 = TFMiniPlus.read(); // byte 4 = Dist_H
      t2 <<= 8;
      t2 += t1;
      TFMiniVal = t2;
      t1 = TFMiniPlus.read(); // byte 5 = Strength_L
      t2 = TFMiniPlus.read(); // byte 6 = Strength_H
      t2 <<= 8;
      t2 += t1 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2022-10-25 00:52:50 -0500

Pablogs gravatar image

micro-ROS do not provide a thread concept. It only provides support for being thread-safe using the system directives provided by the underlying RTOS.

So, I guess that you need to use a threading approach with an RTOS (or other solution) and then use micro-ROS accordingly.

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-10-23 01:38:30 -0500

Seen: 323 times

Last updated: Oct 25 '22