Teensy 4.1 program compiles fine, but upload attempts fail

S

snelsond

New member
I posted on here with a similar question about a month ago, but one of my university's research labs has set up a Raspberry Pi 4 running headless Ubuntu 22.04 LTS connected by USB to a Teensy 4.1 as the control hardware on an underwater robot we're developing. We've been sshing into the RP4 and using Platform.io to do remote development and uploads at times when we can't access the hardware (e.g. sealed up for testing in a lake or pool).

Recently (when we compile and upload code from our programs) the compilation step runs fine, but the command-line programming tools we're using (we've tried both teensy_loader_cli and tycmd, with the same issue) fail at uploading to the teensy 80-90% percent of the time, crashing more than halfway into the upload. We've spent a lot of time isolating and testing our code, and we've tracked the problem to the GNSS and Serial7 initialization code below. Weirdly, the microROS framework (the code below without the GNSS object) compiles and uploads fine on its own, and the GNSS and Serial7 code works fine with no microROS additions. Since the compiler isn't throwing any errors, we're pretty confident it's a serial issue, but don't know enough about the serial interaction on the Teensy to debug and fix it (although we have most certainly tried). We're pretty confident it's not a power or cord length problem (we've tested that extensively), and even tried setting up on a new Raspberry Pi only with the same problems to result.

Any insights on what might be the cause?

Our microROS setup and integration might make the issue hard to replicate, but here's code for a simplified program with the same problem below (the code we're pretty sure is the issue is in the void setup() { } function in the first file):

C++: 
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/executor.h>
#include <rclc/rclc.h>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>

#include "gps_pub.h"

#define GPS_RATE 38400

#define RCCHECK(fn)                                                            \
  {                                                                            \
    rcl_ret_t temp_rc = fn;                                                    \
    if ((temp_rc != RCL_RET_OK)) {                                             \
      error_loop();                                                            \
    }                                                                          \
  }

#define RCSOFTCHECK(fn)                                                        \
  {                                                                            \
    rcl_ret_t temp_rc = fn;                                                    \
    if ((temp_rc != RCL_RET_OK)) {                                             \
    }                                                                          \
  }

#define EXECUTE_EVERY_N_MS(MS, X)                                              \
  do {                                                                         \
    static volatile int64_t init = -1;                                         \
    if (init == -1) {                                                          \
      init = uxr_millis();                                                     \
    }                                                                          \
    if (uxr_millis() - init > MS) {                                            \
      X;                                                                       \
      init = uxr_millis();                                                     \
    }                                                                          \
  } while (0)

#define BAUD_RATE 6000000
#define CALLBACK_TOTAL 1
#define TIMER_PERIOD 30000
#define SYNC_TIMEOUT 1000

// micro-ROS objects
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rclc_executor_t executor;
rcl_timer_t timer_pub;

// publisher objects
GPSPub gps_pub;

// sensor objects
SFE_UBLOX_GNSS GNSS;

// states for state machine in loop function
enum states {
  WAITING_AGENT,
  AGENT_AVAILABLE,
  AGENT_CONNECTED,
  AGENT_DISCONNECTED
} static state;

// responds to errors with micro-ROS functions
void error_loop() {
  while (1) {
    delay(100);
  }
}

// micro-ROS function that publishes all the data to their topics
void timer_pub_callback(rcl_timer_t *timer, int64_t last_call_time) {

  (void)last_call_time;
  if (timer != NULL) {

    // TO TEST: ADD HERE
    // gps_pub.update(GNSS);
    gps_pub.publish();
  }
}

bool create_entities() {

  // the allocator object wraps the dynamic memory allocation and deallocation
  // methods used in micro-ROS
  allocator = rcl_get_default_allocator();
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  RCCHECK(
      rclc_node_init_default(&node, "micro_ros_platformio_node", "", &support));

  // synchronize timestamps with the Raspberry Pi
  // after sync, timing should be able to be accessed with "rmw_uros_epoch"
  // functions
  RCCHECK(rmw_uros_sync_session(SYNC_TIMEOUT));

  // create timer (handles periodic publications)
  RCCHECK(rclc_timer_init_default(
      &timer_pub, &support, RCL_MS_TO_NS(TIMER_PERIOD), timer_pub_callback));

  // create executor
  RCSOFTCHECK(rclc_executor_init(&executor, &support.context, CALLBACK_TOTAL,
                                 &allocator));

  // add callbacks to executor
  RCSOFTCHECK(rclc_executor_add_timer(&executor, &timer_pub));

  // TO TEST: ADD HERE
  gps_pub.setup(node);

  return true;
}

void destroy_entities() {
  rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context);
  (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0);

  // TO TEST: ADD HERE
  gps_pub.destroy(node);

  // destroy everything else
  rcl_timer_fini(&timer_pub);
  rclc_executor_fini(&executor);
  rcl_node_fini(&node);
  rclc_support_fini(&support);
}

void setup() {

  Serial.begin(BAUD_RATE);
  set_microros_serial_transports(Serial);

  do {

    Serial7.begin(GPS_RATE);
    if (GNSS.begin(Serial7) == true) break;

    delay(100);
    Serial7.begin(9600);
    if (GNSS.begin(Serial7) == true) {
        GNSS.setSerialRate(GPS_RATE);
        delay(100);
    } else {
        // GNSS.factoryReset();
        delay(2000);
    }
  } while(1);

  GNSS.setUART1Output(COM_TYPE_UBX); // Set the UART port to output UBX only
  GNSS.setI2COutput(COM_TYPE_UBX); // Set the I2C port to output UBX only (turn off NMEA noise)
  GNSS.saveConfiguration(); // Save the current settings to flash and BBR
 
  state = WAITING_AGENT;
}

void loop() {

  // state machine to manage connecting and disconnecting the micro-ROS agent
  switch (state) {
  case WAITING_AGENT:
    EXECUTE_EVERY_N_MS(500, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1))
                                        ? AGENT_AVAILABLE
                                        : WAITING_AGENT;);
    break;

  case AGENT_AVAILABLE:
    state = (true == create_entities()) ? AGENT_CONNECTED : WAITING_AGENT;
    if (state == WAITING_AGENT) {
      destroy_entities();
    };
    break;

  case AGENT_CONNECTED:
    EXECUTE_EVERY_N_MS(200, state = (RMW_RET_OK == rmw_uros_ping_agent(100, 1))
                                        ? AGENT_CONNECTED
                                        : AGENT_DISCONNECTED;);

    if (state == AGENT_CONNECTED) {
      rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
    }
    break;

  case AGENT_DISCONNECTED:
    destroy_entities();
    state = WAITING_AGENT;
    break;

  default:
    break;
  }
}

C++: 
#ifndef PUBLISHER
#define PUBLISHER

#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/executor.h>
#include <rclc/rclc.h>

#define RCCHECK(fn)                                                            \
  {                                                                            \
    rcl_ret_t temp_rc = fn;                                                    \
    if ((temp_rc != RCL_RET_OK)) {                                             \
      error_loop();                                                            \
    }                                                                          \
  }
#define RCSOFTCHECK(fn)                                                        \
  {                                                                            \
    rcl_ret_t temp_rc = fn;                                                    \
    if ((temp_rc != RCL_RET_OK)) {                                             \
    }                                                                          \
  }

class Publisher {

public:
  // these need to be defined by each publisher
  virtual void setup(rcl_node_t node) = 0;
  virtual void publish() = 0;

  void destroy(rcl_node_t node) {
    RCCHECK(rcl_publisher_fini(&publisher, &node));
  }

protected:
  rcl_publisher_t publisher;

  void error_loop() {
    while (1) {
      delay(100);
    }
  }
};

#endif // PUBLISHER

Code: 
#ifndef GPS_PUB
#define GPS_PUB

#include "publisher.h"
#include <frost_interfaces/msg/gps.h>
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>

class GPSPub : Publisher {

public:
  void setup(rcl_node_t node);
  void update(SFE_UBLOX_GNSS GNSS);
  void publish();
  using Publisher::destroy;

private:
  frost_interfaces__msg__GPS msg;
};

#endif // GPS_PUB

Code: 
#include "gps_pub.h"

void GPSPub::setup(rcl_node_t node) {

  RCCHECK(rclc_publisher_init_best_effort(
      &publisher, &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(frost_interfaces, msg, GPS), "gps_data"));
}

// void GPSPub::update(SFE_UBLOX_GNSS GNSS) {

//   msg.longitude = GNSS.getLongitude();
//   msg.latitude = GNSS.getLatitude();
//   msg.altitude = GNSS.getAltitude();
//   msg.siv = GNSS.getSIV();
//   msg.header.stamp.nanosec = rmw_uros_epoch_nanos();
// }

void GPSPub::publish() {

  RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
}
 
Is it possible in ANY way for successfully compiled code to cause a failed upload like you are saying? I don't think so.
Is it possible running code could cause an electronic issue say pulling a supply low and cause uploads to fail? I think so.

I don't know the answers to these but hoping someone will answer them while helping you out.
Gavin.
 
Crashing during the upload might be indicative of a power supply issue (erasure of flash memory triggering a brown-out) which is being avoided when the code is slimmed down due to less flash pages being occupied...
 
You must log in or register to reply here.
Back
Top