samm_flynn
Active member
EDIT : There was a bug in my previous code, I found just after posting. Editing the question since I have a new question.
Hey everyone,
I'm working on a project involving two Teensy 4.1 boards, where I need to send structured binary data to the master over usb serial, then from the master to the slave over hardware UART (Serial1 → Serial8). The setup works flawlessly when I send data to the slave teensy over USB serial, Can almost send 512KB at 20 MB/s, sometimes faster.
But when I switch over to hardware Serial I don't get the same performance, I didn't measure it, but the delay is order of magnitude higher.
I was wondering is it because of the way coded it or hardwareSerial is significantly slower by design.
Master Teensy Code -
Slave Teensy Code -
Hey everyone,
I'm working on a project involving two Teensy 4.1 boards, where I need to send structured binary data to the master over usb serial, then from the master to the slave over hardware UART (Serial1 → Serial8). The setup works flawlessly when I send data to the slave teensy over USB serial, Can almost send 512KB at 20 MB/s, sometimes faster.
But when I switch over to hardware Serial I don't get the same performance, I didn't measure it, but the delay is order of magnitude higher.
I was wondering is it because of the way coded it or hardwareSerial is significantly slower by design.
Master Teensy Code -
C++:
int packetIndex = 0;
bool inPacket = false;
// Serial port initialization
void setup()
{
Serial.begin(921600);
while (!Serial) { delay(1); }
Serial1.begin(921600);
}
void loop()
{
if (Serial.available() >= 8)
{
byte startByte = Serial.read();
byte commandByte = Serial.read();
byte axisId = Serial.read();
byte headerByte = Serial.read();
uint32_t payloadSize;
Serial.readBytes(reinterpret_cast<char *>(&payloadSize), sizeof(payloadSize));
uint8_t payloadBuffer[payloadSize];
Serial.readBytes(reinterpret_cast<char *>(payloadBuffer), payloadSize); // Read payload
// Read the end byte
byte endByte = Serial.read();
Serial.println("\n--- Sending Packet Packet ---");
Serial.printf("Start Byte: 0x%02X\n", startByte);
Serial.printf("Command Type: 0x%02X\n", commandByte);
Serial.printf("Axis ID: %d\n", axisId);
Serial.printf("Header Byte: 0x%02X\n", headerByte);
Serial.printf("Payload Size: %u \n", payloadSize);
Serial.printf("End Byte: 0x%02X\n", endByte);
Serial.println("-----------------------\n");
// Allocate buffer dynamically for the full packet
uint32_t totalPacketSize = 8 + payloadSize + 1; // Start + command + axisId + header + size(4) + payload + end
uint8_t fullPacket[totalPacketSize];
int index = 0;
fullPacket[index++] = startByte;
fullPacket[index++] = commandByte;
fullPacket[index++] = axisId;
fullPacket[index++] = headerByte;
fullPacket[index++] = (payloadSize >> 0) & 0xFF;
fullPacket[index++] = (payloadSize >> 8) & 0xFF;
fullPacket[index++] = (payloadSize >> 16) & 0xFF;
fullPacket[index++] = (payloadSize >> 24) & 0xFF;
for (uint32_t i = 0; i < payloadSize; i++)
{
fullPacket[index++] = payloadBuffer[i];
}
fullPacket[index++] = endByte;
Serial.printf("packetSize:%d, Axis:%d\n", totalPacketSize, axisId);
Serial.print("Sending : ");
for (uint32_t i = 0; i < totalPacketSize; i++)
{
Serial.printf("0x%02X ", fullPacket[i]);
}
Serial.println();
if (axisId == 0)
{
int bytesWritten = Serial1.write(fullPacket, totalPacketSize);
Serial.printf("w:%d\n", bytesWritten);
}
}
}
void serialEvent1()
{
while (Serial1.available() > 0)
Serial.write(Serial1.read());
}Slave Teensy Code -
C++:
#define N 63488 // Define max trajectory size
#define IRQ_LINE 5
#define ENABLE_LINE 32
#define ESTOP 33
#include <math.h>
#include "motor_axis.h"
// #include "utils.h"
#include <IntervalTimer.h>
IntervalTimer motor_timer;
// Define the buffer using DMA memory
DMAMEM union
{
float thetas[N];
uint8_t rawBytes[sizeof(float) * N];
} thetaBuffer1;
DMAMEM union
{
float thetas[N];
uint8_t rawBytes[sizeof(float) * N];
} thetaBuffer2;
// Function prototypes
void processPacket();
void handlePayload(byte headerByte, int payloadSize);
template<typename T> T readPayload(int payloadSize);
const int PROX_INT1 = 38; // or 2
const int PROX_INT2 = 2; // Different proximity interrupt for motor2
const int8_t LED_PIN = 13;
const unsigned long CAN_ID1 = 0x01; // or 1
const unsigned long CAN_ID2 = 0x01; // Different CAN ID for motor2
const int dir1 = -1; // or -1
const int dir2 = 1; // Opposite direction for second motor
MotorAxis<FlexCAN_T4<CAN1, RX_SIZE_8, TX_SIZE_8>> motor1(CAN_ID1, PROX_INT1, dir1, 1);
MotorAxis<FlexCAN_T4<CAN2, RX_SIZE_8, TX_SIZE_8>> motor2(CAN_ID2, PROX_INT2, dir2, 2);
float position_setpoint1, position_setpoint2;
bool automatic = false, manual = false;
uint32_t trajectory_length = 0;
volatile int ArrayIndex;
// Setup function
void setup()
{
Serial8.begin(921600); // From Master Teensy
Serial8.setTimeout(5000);
Serial2.begin(921600);//CP2102
Serial2.setTimeout(5000);
delay(3500);
if (CrashReport)
Serial8.println(CrashReport);
// Initialize both motors
motor1.init();
motor1.setVelocity(0);
motor1.disable();
motor2.init();
motor2.setVelocity(0);
motor2.disable();
pinMode(IRQ_LINE, INPUT); // IRQ Line
pinMode(ENABLE_LINE, INPUT); // ENABLE LINE
// attachInterrupt(digitalPinToInterrupt(IRQ_LINE), sendCmd, RISING);
// motor_timer.begin(sendCmd, 1000);
Serial8.println("<READY>");
Serial2.println("<READY>");
}
void sendCmd()
{
// if (digitalReadFast(ENABLE_LINE)) // USE A Static Inline function defination here.
if (true) // USE A Static Inline function defination here.
{
if (automatic && !manual) // USE A Static Inline function defination here.
{
ArrayIndex = (ArrayIndex + 1) % (trajectory_length); // Wraps back to 0 when exceeding maxValue
if (ArrayIndex % 100 == 0)
{
Serial8.print("Idx:");
Serial8.println(ArrayIndex);
}
}
}
if (motor1.armed && motor1.calibrated)
{
if (manual && !automatic)
{
motor1.setPosition(position_setpoint1);
}
else if (automatic && !manual)
{
motor1.setPosition(thetaBuffer1.thetas[ArrayIndex]);
}
}
if (motor2.armed && motor2.calibrated)
{
if (manual && !automatic)
{
motor2.setPosition(position_setpoint2);
}
else if (automatic && !manual)
{
motor2.setPosition(thetaBuffer2.thetas[ArrayIndex]);
}
}
}
// Main loop
void loop()
{
if (Serial8.available() >= 10)
processPacket();
}
// Function to process an incoming Serial8 packet
void processPacket()
{
uint32_t payloadSize;
Serial8.println("\n--- Received Packet ---");
Serial2.println("\n--- Received Packet ---");
byte startByte = Serial8.read();
Serial8.printf("Start Byte: 0x%02X\n", startByte);
Serial2.printf("Start Byte: 0x%02X\n", startByte);
byte commandByte = Serial8.read();
Serial8.printf("Command Type: 0x%02X\n", commandByte);
Serial2.printf("Command Type: 0x%02X\n", commandByte);
byte axisId = Serial8.read();
Serial8.printf("Axis ID: %d\n", axisId);
Serial2.printf("Axis ID: %d\n", axisId);
byte headerByte = Serial8.read();
Serial8.printf("Header Byte: 0x%02X\n", headerByte);
Serial2.printf("Header Byte: 0x%02X\n", headerByte);
Serial8.readBytes(reinterpret_cast<char *>(&payloadSize), sizeof(payloadSize));
Serial8.printf("Payload Size: %u \n", payloadSize);
Serial2.printf("Payload Size: %u \n", payloadSize);
handlePayload(headerByte, payloadSize, axisId);
byte endByte = Serial8.read();
Serial8.printf("End Byte: 0x%02X\n", endByte);
Serial2.printf("End Byte: 0x%02X\n", endByte);
Serial8.println("-----------------------\n");
Serial2.println("-----------------------\n");
}
// Handles different payload commands based on the header byte
void handlePayload(byte headerByte, int payloadSize, byte axisId)
{
String commandType;
switch (headerByte)
{
case 0x87: // Trajectory
{
if (axisId == 0)
{
// Allocate temporary buffer in stack memory
uint8_t tempBuffer[payloadSize];
// Read data once into tempBuffer
if (Serial8.readBytes(reinterpret_cast<char *>(tempBuffer), payloadSize) != static_cast<size_t>(payloadSize))
{
Serial8.println("Error: Array Size Mismatch");
}
else
{
// Copy the same data into both thetaBuffer1 and thetaBuffer2
memcpy(thetaBuffer1.rawBytes, tempBuffer, payloadSize);
memcpy(thetaBuffer2.rawBytes, tempBuffer, payloadSize);
}
}
else if (axisId == 1)
{
if (Serial8.readBytes(reinterpret_cast<char *>(thetaBuffer1.rawBytes), payloadSize) != static_cast<size_t>(payloadSize))
Serial8.println("Error: Array Size Mismatch");
}
else if (axisId == 2)
{
if (Serial8.readBytes(reinterpret_cast<char *>(thetaBuffer2.rawBytes), payloadSize) != static_cast<size_t>(payloadSize))
Serial8.println("Error: Array Size Mismatch");
}
commandType = "Trajectory(" + String(payloadSize) + ")";
break;
}
case 0x86: // setPosition
{
float val = readPayload<float>(payloadSize);
commandType = "setPosition(" + String(val, 6) + ")";
if (axisId == 0 || axisId == 1)
{
position_setpoint1 = val * M_PI / 180;
}
if (axisId == 0 || axisId == 2)
{
position_setpoint2 = val * M_PI / 180;
}
break;
}
case 0x88: //TrajectoryLength
{
uint32_t trajLength = readPayload<uint32_t>(payloadSize);
trajectory_length = trajLength;
commandType = "TrajectoryLength(" + String(trajLength) + ")";
break;
}
case 0x89: //FeedRate
{
byte feedRate = readPayload<byte>(payloadSize);
commandType = "FeedRate(" + String(feedRate) + ")";
break;
}
case 0x80: //Reboot
{
byte rebootVal = readPayload<byte>(payloadSize);
commandType = "Reboot(" + String(rebootVal) + ")";
break;
}
case 0x81: //eStop
{
byte eStopVal = readPayload<byte>(payloadSize);
commandType = "eStop(" + String(eStopVal) + ")";
break;
}
case 0x82: //Enable
{
byte enableVal = readPayload<byte>(payloadSize);
commandType = "Enable(" + String(enableVal) + ")";
if (axisId == 0 || axisId == 1)
motor1.enable();
if (axisId == 0 || axisId == 2)
motor2.enable();
break;
}
case 0x83: //Disable
{
byte disableVal = readPayload<byte>(payloadSize);
commandType = "Disable(" + String(disableVal) + ")";
if (axisId == 0 || axisId == 1)
motor1.disable();
if (axisId == 0 || axisId == 2)
motor2.disable();
break;
}
case 0x84: // Calibrate
{
byte calibrateVal = readPayload<byte>(payloadSize);
commandType = "Calibrate(" + String(calibrateVal) + ")";
if (axisId == 0 || axisId == 1)
motor1.calibrate();
if (axisId == 0 || axisId == 2)
motor2.calibrate();
break;
}
case 0x85: //Mode
{
byte modeVal = readPayload<byte>(payloadSize);
if (modeVal == 0)
{
manual = true;
automatic = false;
}
else if (modeVal == 1)
{
automatic = true;
manual = false;
}
commandType = "Mode(" + String(modeVal) + ")";
break;
}
default:
commandType = "Unknown Command";
break;
}
Serial8.printf("axis ID : %d -> %s\n", axisId, commandType.c_str());
Serial2.printf("axis ID : %d -> %s\n", axisId, commandType.c_str());
}
// Generic function to read payload data
template<typename T>
T readPayload(int payloadSize)
{
T value;
if (static_cast<size_t>(payloadSize) < sizeof(T)) // Cast payloadSize to avoid signed/unsigned mismatch
{
Serial1.println("Error: Payload size mismatch!");
return 0;
}
Serial.readBytes(reinterpret_cast<char *>(&value), sizeof(T));
return value;
}
void ext_output1(const CAN_message_t &msg)
{
if (msg.bus == 1)
motor1.CanRxHandler(msg);
if (msg.bus == 2)
motor2.CanRxHandler(msg);
}
Last edited:
