Teensy 4 + 6 odrives - optimal communication protocol for high frequency control

[mike]

As the topic suggests, I have a Teensy 4 talking to 6 odrives via UART (the teensy 4 has 7 hardware serial pairs!) and the ASCII protocol gets me an update rate of ~50hz on 24 floats (axis*.encoder.pos_estimate and axis*.motor.current_control.Iq_measured for both axes on 6 odrives). Basically, the teensy is running a busy loop that reads/writes 1 byte at a time to each odrive UART as needed in a non-blocking, round robin process.

I would like to increase this update frequency from 50hz to ~500hz (perhaps even more, if possible). Going by byte count, it seems that implementing the native protocol over UART might give me a 2-4x speedup (assuming the communications medium, and not the actual command processing time, is the primary cause of request/response latency). So perhaps 100hz-200hz, but certainly not 500hz. A similar speedup could be obtained by implementing a minimalist ASCII serial protocol with custom firmware.

In terms of increasing communications bitrate: I’ve looked at the CAN and I2C options but there is little to no clear information/examples about actual CAN/odrive implementations (it’s not even clear to me whether I need a CAN transceiver for the teensy, each odrive, etc, or not), and I2C requires irreversible hardware modifications. There are designated SPI pins on the odrive for encoder interfacing, and SPI is typically quite high performance, but I have not seen any mention of SPI communication for odrive commands.

USB serial seems like a very interesting option, considering the high data rates, but does this actually pan out in practice? I believe USB does some packetization that puts a ceiling on request/response frequencies, though that ceiling may be fairly high (~1000hz). I’m also not sure what’s involved in setting up the teensy 4 as a usb host to 6 odrives, but I believe it’s possible.

It’s also possible that algorithms requiring update rates of 500hz or more are better offloaded to the odrive itself as custom firmware and shuffling data at these frequencies is more trouble than it’s worth. Is this the case?

edit: I came across this discussion - UART Port baud rate - which indicates that very substantial (8x or more) UART baud rate speedup is possible by flashing custom firmware. Some posters even indicate that they had success with baud rates in the megabits. This seems to be the most fruitful way forward to get the speedup I want, especially if coupled with the native or minimal ASCII protocol I mentioned above. Would be nice if the UART baud rate could be updated via config parameter (and take effect upon reboot, I suppose).

[mike]

A quick update - I modified the baud rate to 921600 as per the discussion I mentioned above, reflashed the firmware, and have done some crude speed tests; I measured how many microseconds it takes to send and receive 10 motor status commands for each axis (“f 0” and “f 1”). There are two ways to do this - send the commands in series (send command 1, read response 1, send command 2, read response 2), or batch the commands and the responses (send command 1 and 2, read response 1 and 2). The batching approach is roughly 2x faster with the higher baud rate and slightly (1.2x) faster with the lower baud rate.

Test results (ascii protocol, “f 0\n” and “f 1\n” commands sent + responses read 10 times each):

921600 baud, series approach: ~20 milliseconds
921600 baud, batching approach: ~10 milliseconds
115200 baud, series approach: ~60 milliseconds
115200 baud, batching approach: ~50 milliseconds

[mike]

Are there any public benchmarks available on native vs ascii protocol performance? Specifically, when the ASCII commands are short (“f 0” and such), is there any speed benefit to using the native/binary protocol?

[mike]

Quick update, for those who are curious - I am now getting a 500hz update rate (~2ms from request start to response finish) for position, velocity, and current on all 12 axes! I did this with two steps:

1. Changed UART baud rate to 921600
2. Implemented a single-character ascii command that the odrive responds to with position, velocity, and current (measured). If an axis index is supplied, the response is for that axis only; if no axis is supplied, the response contains data for both axes.

The code, in case someone wants it (ascii_protocol.cpp):

    } else if (cmd[0] == 's') { // status; position, velocity, current
        unsigned motor_number;
        int numscan = sscanf(cmd, "s %u", &motor_number);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "%f %f %f %f %f %f",
                    (double)axes[0]->encoder_.pos_estimate_,
                    (double)axes[0]->encoder_.vel_estimate_,
                    (double)axes[0]->motor_.current_control_.Iq_measured,
                    (double)axes[1]->encoder_.pos_estimate_,
                    (double)axes[1]->encoder_.vel_estimate_,
                    (double)axes[1]->motor_.current_control_.Iq_measured);
            //respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            respond(response_channel, use_checksum, "%f %f %f",
                    (double)axes[motor_number]->encoder_.pos_estimate_,
                    (double)axes[motor_number]->encoder_.vel_estimate_,
                    (double)axes[motor_number]->motor_.current_control_.Iq_measured);
        }

[Riewert]

That seems similar to what I am getting in java over native-usb. About 0.3ms for one message, 0.6ms for request-response sequence. As much as possible into a single message is very important.

Odrive and Java

Performance So i am now reading values quite consistently. When I read the value “vbus_voltage” 1000 times I am seeing an average of 1.5ms ± 0.5ms for request and reply. I have no other devices connected to the USB port, and 95% of the time is taken up by the library reading or writing from the port. So this appears to the be fastest I can get it to work. I recorded one of my tests in Wireshark and measured the time between messages. In general the median time the PC takes send a new request a…

[alexisdal]

@mike thanks for sharing the code

[ealkhteeb]

Hi mike,

Thank you for this effort.

I added the above code to ascii_protocol.cpp and flashed the new firmware.
But, how can call from Teensy?

I wrote this function to send ‘s’ to the odrive, but the odrive doesn’t respond at all. Therefore, the function returns 0.0000

float ODriveArduino::GetDataFromOdrive(int motor_number){
      serial_<< "s " << motor_number;
      return ODriveArduino::readFloat();
}

How does this work? or did I understand this wrong?

Thanks

[mike]

Hello there,

There are two problems with your code:

1. The odrive ascii protocol expects a newline behind each command, so you should append a \n:

serial_ << "s " << motor_number << '\n';

2. I’m not familiar with readFloat(), but I presume it reads just one float from the serial response, whereas this ‘s’ command returns three floats (and if you don’t specify a motor number, it returns 6 since it gives data for both channels).

[ealkhteeb]

Hi mike,

Thank you for the prompt reply.

I tried this

serial_ << "s " << motor_number << '\n';

but nothing has changed. I checked ODriveArduino::readString() to see if odrive respond with any message. and found no response at all.

[ealkhteeb]

Here is the ascii_protocol.cpp

/*
* The ASCII protocol is a simpler, human readable alternative to the main native
* protocol.
* In the future this protocol might be extended to support selected GCode commands.
* For a list of supported commands see doc/ascii-protocol.md
*/

/* Includes ------------------------------------------------------------------*/

#include "odrive_main.h"
#include "../build/version.h" // autogenerated based on Git state
#include "communication.h"
#include "ascii_protocol.hpp"
#include <utils.h>
#include <fibre/cpp_utils.hpp>

/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/

#define MAX_LINE_LENGTH 256
#define TO_STR_INNER(s) #s
#define TO_STR(s) TO_STR_INNER(s)

/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/

// @brief Sends a line on the specified output.
template<typename ... TArgs>
void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) {
    char response[64];
    size_t len = snprintf(response, sizeof(response), fmt, std::forward<TArgs>(args)...);
    output.process_bytes((uint8_t*)response, len, nullptr); // TODO: use process_all instead
    if (include_checksum) {
        uint8_t checksum = 0;
        for (size_t i = 0; i < len; ++i)
            checksum ^= response[i];
        len = snprintf(response, sizeof(response), "*%u", checksum);
        output.process_bytes((uint8_t*)response, len, nullptr);
    }
    output.process_bytes((const uint8_t*)"\r\n", 2, nullptr);
}


// @brief Executes an ASCII protocol command
// @param buffer buffer of ASCII encoded characters
// @param len size of the buffer
void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
    static_assert(sizeof(char) == sizeof(uint8_t));

    // scan line to find beginning of checksum and prune comment
    uint8_t checksum = 0;
    size_t checksum_start = SIZE_MAX;
    for (size_t i = 0; i < len; ++i) {
        if (buffer[i] == ';') { // ';' is the comment start char
            len = i;
            break;
        }
        if (checksum_start > i) {
            if (buffer[i] == '*') {
                checksum_start = i + 1;
            } else {
                checksum ^= buffer[i];
            }
        }
    }

    // copy everything into a local buffer so we can insert null-termination
    char cmd[MAX_LINE_LENGTH + 1];
    if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH;
    memcpy(cmd, buffer, len);

    // optional checksum validation
    bool use_checksum = (checksum_start < len);
    if (use_checksum) {
        unsigned int received_checksum;
        sscanf((const char *)cmd + checksum_start, "%u", &received_checksum);
        if (received_checksum != checksum)
            return;
        len = checksum_start - 1; // prune checksum and asterisk
    }

    cmd[len] = 0; // null-terminate

    // check incoming packet type
    if (cmd[0] == 'p') { // position control
        unsigned motor_number;
        float pos_setpoint, vel_feed_forward, current_feed_forward;
        int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            if (numscan < 3)
                vel_feed_forward = 0.0f;
            if (numscan < 4)
                current_feed_forward = 0.0f;
            Axis* axis = axes[motor_number];
            axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'q') { // position control with limits
        unsigned motor_number;
        float pos_setpoint, vel_limit, current_lim;
        int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &current_lim);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            Axis* axis = axes[motor_number];
            axis->controller_.pos_setpoint_ = pos_setpoint;
            if (numscan >= 3)
                axis->controller_.config_.vel_limit = vel_limit;
            if (numscan >= 4)
                axis->motor_.config_.current_lim = current_lim;

            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'v') { // velocity control
        unsigned motor_number;
        float vel_setpoint, current_feed_forward;
        int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, &current_feed_forward);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            if (numscan < 3)
                current_feed_forward = 0.0f;
            Axis* axis = axes[motor_number];
            axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'c') { // current control
        unsigned motor_number;
        float current_setpoint;
        int numscan = sscanf(cmd, "c %u %f", &motor_number, &current_setpoint);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            Axis* axis = axes[motor_number];
            axis->controller_.set_current_setpoint(current_setpoint);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 't') { // trapezoidal trajectory
        unsigned motor_number;
        float goal_point;
        int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            Axis* axis = axes[motor_number];
            axis->controller_.move_to_pos(goal_point);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'f') { // feedback
        unsigned motor_number;
        int numscan = sscanf(cmd, "f %u", &motor_number);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            respond(response_channel, use_checksum, "%f %f",
                    (double)axes[motor_number]->encoder_.pos_estimate_,
                    (double)axes[motor_number]->encoder_.vel_estimate_);
        }

    } else if (cmd[0] == 'h') {  // Help
        respond(response_channel, use_checksum, "Please see documentation for more details");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Available commands syntax reference:");
        respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim");
        respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff");
        respond(response_channel, use_checksum, "Velocity: v axis vel I-ff");
        respond(response_channel, use_checksum, "Current: c axis I");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state");
        respond(response_channel, use_checksum, "Read: r property");
        respond(response_channel, use_checksum, "Write: w property value");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Save config: ss");
        respond(response_channel, use_checksum, "Erase config: se");
        respond(response_channel, use_checksum, "Reboot: sr");

    } else if (cmd[0] == 'i'){ // Dump device info
        // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature());
        // respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision());
        // respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize());
        respond(response_channel, use_checksum, "Hardware version: %d.%d-%dV", HW_VERSION_MAJOR, HW_VERSION_MINOR, HW_VERSION_VOLTAGE);
        respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION);
        respond(response_channel, use_checksum, "Serial number: %s", serial_number_str);

    } else if (cmd[0] == 's'){ // System
        if(cmd[1] == 's') { // Save config
            save_configuration();
        } else if (cmd[1] == 'e'){ // Erase config
            erase_configuration();
        } else if (cmd[1] == 'r'){ // Reboot
            NVIC_SystemReset();
        }

    } else if (cmd[0] == 'r') { // read property
        char name[MAX_LINE_LENGTH];
        int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "invalid command format");
        } else {
            Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name));
            if (!endpoint) {
                respond(response_channel, use_checksum, "invalid property");
            } else {
                char response[10];
                bool success = endpoint->get_string(response, sizeof(response));
                if (!success)
                    respond(response_channel, use_checksum, "not implemented");
                else
                    respond(response_channel, use_checksum, response);
            }
        }

    } else if (cmd[0] == 'w') { // write property
        char name[MAX_LINE_LENGTH];
        char value[MAX_LINE_LENGTH];
        int numscan = sscanf(cmd, "w %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "invalid command format");
        } else {
            Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name));
            if (!endpoint) {
                respond(response_channel, use_checksum, "invalid property");
            } else {
                bool success = endpoint->set_string(value, sizeof(value));
                if (!success)
                    respond(response_channel, use_checksum, "not implemented");
            }
        }

    } else if (cmd[0] == 's') { // status; position, velocity, current
        unsigned motor_number;
        int numscan = sscanf(cmd, "s %u", &motor_number);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "%f %f %f %f %f %f",
                    (double)axes[0]->encoder_.pos_estimate_,
                    (double)axes[0]->encoder_.vel_estimate_,
                    (double)axes[0]->motor_.current_control_.Iq_measured,
                    (double)axes[1]->encoder_.pos_estimate_,
                    (double)axes[1]->encoder_.vel_estimate_,
                    (double)axes[1]->motor_.current_control_.Iq_measured);
            //respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            respond(response_channel, use_checksum, "%f %f %f",
                    (double)axes[motor_number]->encoder_.pos_estimate_,
                    (double)axes[motor_number]->encoder_.vel_estimate_,
                    (double)axes[motor_number]->motor_.current_control_.Iq_measured);
        }

    }else if (cmd[0] == 'u') { // Update axis watchdog. 
        unsigned motor_number;
        int numscan = sscanf(cmd, "u %u", &motor_number);
        if(numscan < 1){
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        }else {
            axes[motor_number]->watchdog_feed();
        }

    } else if (cmd[0] != 0) {
        respond(response_channel, use_checksum, "unknown command");
    }
}

void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
    static uint8_t parse_buffer[MAX_LINE_LENGTH];
    static bool read_active = true;
    static uint32_t parse_buffer_idx = 0;

    while (len--) {
        // if the line becomes too long, reset buffer and wait for the next line
        if (parse_buffer_idx >= MAX_LINE_LENGTH) {
            read_active = false;
            parse_buffer_idx = 0;
        }

        // Fetch the next char
        uint8_t c = *(buffer++);
        bool is_end_of_line = (c == '\r' || c == '\n' || c == '!');
        if (is_end_of_line) {
            if (read_active)
                ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel);
            parse_buffer_idx = 0;
            read_active = true;
        } else {
            if (read_active) {
                parse_buffer[parse_buffer_idx++] = c;
            }
        }
    }
}

[mike]

Are you running UART at 921600 baud on the teensy?

[ealkhteeb]

I tried both 921600 and 115200, and I got the same thing.

[mike]

It seems there must be something you’re overlooking; what happens if you enter other commands, like ‘f 0’ etc?

[ealkhteeb]

I tried
serial_<<"f 0" << '\n';

I got results of position and speed I think.

[mike]

Hello there,

I looked at the code you posted and discovered the problem. There is already another command in the ascii protocol that looks for a leading ‘s’ character - more specifically, the ‘system’ commands. I didn’t notice this when I originally implemented this patch, and it worked fine for me because I put the new block above the existing ‘s’ conditional in the sequence of 'else if’s. You inserted the new block after the existing ‘system’ block that looks for a leading ‘s’ in the command, and thus the block never executes.

The way to fix this would be to assign a new character for this command instead of s; for instance, a or z. Of course, you’ll have to update the serial command sent by the teensy as well.

[ealkhteeb]

It works now, I just changed where I added the new code for ‘s’ I added after this section
} else if (cmd[0] == 'f') { // feedback

Thank you mike for the help, really appreciated.

[mike]

I’m glad you got it working - did you read my reply just prior to yours? The way you resolved it has the side effect of making the other system commands (save configuration, erase configuration, and reboot) inaccessible via the ascii protocol. A better solution would be to reassign this new ‘status’ command to ‘z’ instead of ‘s’ so that it does not conflict with the pre-existing system commands.

[ealkhteeb]

I modified it to cancel the side effect, I’m using the character ‘m’, since it is not used elsewhere in the code.

Thank you mike for the help

Here is the updated version, if anyone wants to use it.

/*
* The ASCII protocol is a simpler, human readable alternative to the main native
* protocol.
* In the future this protocol might be extended to support selected GCode commands.
* For a list of supported commands see doc/ascii-protocol.md
*/

/* Includes ------------------------------------------------------------------*/

#include "odrive_main.h"
#include "../build/version.h" // autogenerated based on Git state
#include "communication.h"
#include "ascii_protocol.hpp"
#include <utils.h>
#include <fibre/cpp_utils.hpp>

/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/

#define MAX_LINE_LENGTH 256
#define TO_STR_INNER(s) #s
#define TO_STR(s) TO_STR_INNER(s)

/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/

// @brief Sends a line on the specified output.
template<typename ... TArgs>
void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) {
    char response[64];
    size_t len = snprintf(response, sizeof(response), fmt, std::forward<TArgs>(args)...);
    output.process_bytes((uint8_t*)response, len, nullptr); // TODO: use process_all instead
    if (include_checksum) {
        uint8_t checksum = 0;
        for (size_t i = 0; i < len; ++i)
            checksum ^= response[i];
        len = snprintf(response, sizeof(response), "*%u", checksum);
        output.process_bytes((uint8_t*)response, len, nullptr);
    }
    output.process_bytes((const uint8_t*)"\r\n", 2, nullptr);
}


// @brief Executes an ASCII protocol command
// @param buffer buffer of ASCII encoded characters
// @param len size of the buffer
void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
    static_assert(sizeof(char) == sizeof(uint8_t));

    // scan line to find beginning of checksum and prune comment
    uint8_t checksum = 0;
    size_t checksum_start = SIZE_MAX;
    for (size_t i = 0; i < len; ++i) {
        if (buffer[i] == ';') { // ';' is the comment start char
            len = i;
            break;
        }
        if (checksum_start > i) {
            if (buffer[i] == '*') {
                checksum_start = i + 1;
            } else {
                checksum ^= buffer[i];
            }
        }
    }

    // copy everything into a local buffer so we can insert null-termination
    char cmd[MAX_LINE_LENGTH + 1];
    if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH;
    memcpy(cmd, buffer, len);

    // optional checksum validation
    bool use_checksum = (checksum_start < len);
    if (use_checksum) {
        unsigned int received_checksum;
        sscanf((const char *)cmd + checksum_start, "%u", &received_checksum);
        if (received_checksum != checksum)
            return;
        len = checksum_start - 1; // prune checksum and asterisk
    }

    cmd[len] = 0; // null-terminate

    // check incoming packet type
    if (cmd[0] == 'p') { // position control
        unsigned motor_number;
        float pos_setpoint, vel_feed_forward, current_feed_forward;
        int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            if (numscan < 3)
                vel_feed_forward = 0.0f;
            if (numscan < 4)
                current_feed_forward = 0.0f;
            Axis* axis = axes[motor_number];
            axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'q') { // position control with limits
        unsigned motor_number;
        float pos_setpoint, vel_limit, current_lim;
        int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &current_lim);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            Axis* axis = axes[motor_number];
            axis->controller_.pos_setpoint_ = pos_setpoint;
            if (numscan >= 3)
                axis->controller_.config_.vel_limit = vel_limit;
            if (numscan >= 4)
                axis->motor_.config_.current_lim = current_lim;

            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'v') { // velocity control
        unsigned motor_number;
        float vel_setpoint, current_feed_forward;
        int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, &current_feed_forward);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            if (numscan < 3)
                current_feed_forward = 0.0f;
            Axis* axis = axes[motor_number];
            axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'c') { // current control
        unsigned motor_number;
        float current_setpoint;
        int numscan = sscanf(cmd, "c %u %f", &motor_number, &current_setpoint);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            Axis* axis = axes[motor_number];
            axis->controller_.set_current_setpoint(current_setpoint);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 't') { // trapezoidal trajectory
        unsigned motor_number;
        float goal_point;
        int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point);
        if (numscan < 2) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            Axis* axis = axes[motor_number];
            axis->controller_.move_to_pos(goal_point);
            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'f') { // feedback
        unsigned motor_number;
        int numscan = sscanf(cmd, "f %u", &motor_number);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            respond(response_channel, use_checksum, "%f %f",
                    (double)axes[motor_number]->encoder_.pos_estimate_,
                    (double)axes[motor_number]->encoder_.vel_estimate_);
        }

   } else if (cmd[0] == 'm') { // status; position, velocity, current
        unsigned motor_number;
        int numscan = sscanf(cmd, "m %u", &motor_number);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "%f %f %f %f %f %f",
                    (double)axes[0]->encoder_.pos_estimate_,
                    (double)axes[0]->encoder_.vel_estimate_,
                    (double)axes[0]->motor_.current_control_.Iq_measured,
                    (double)axes[1]->encoder_.pos_estimate_,
                    (double)axes[1]->encoder_.vel_estimate_,
                    (double)axes[1]->motor_.current_control_.Iq_measured);
            //respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
            respond(response_channel, use_checksum, "%f %f %f",
                    (double)axes[motor_number]->encoder_.pos_estimate_,
                    (double)axes[motor_number]->encoder_.vel_estimate_,
                    (double)axes[motor_number]->motor_.current_control_.Iq_measured);
        }

    } else if (cmd[0] == 'h') {  // Help
        respond(response_channel, use_checksum, "Please see documentation for more details");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Available commands syntax reference:");
        respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim");
        respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff");
        respond(response_channel, use_checksum, "Velocity: v axis vel I-ff");
        respond(response_channel, use_checksum, "Current: c axis I");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state");
        respond(response_channel, use_checksum, "Read: r property");
        respond(response_channel, use_checksum, "Write: w property value");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Save config: ss");
        respond(response_channel, use_checksum, "Erase config: se");
        respond(response_channel, use_checksum, "Reboot: sr");

    } else if (cmd[0] == 'i'){ // Dump device info
        // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature());
        // respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision());
        // respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize());
        respond(response_channel, use_checksum, "Hardware version: %d.%d-%dV", HW_VERSION_MAJOR, HW_VERSION_MINOR, HW_VERSION_VOLTAGE);
        respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION);
        respond(response_channel, use_checksum, "Serial number: %s", serial_number_str);

    } else if (cmd[0] == 's'){ // System
        if(cmd[1] == 's') { // Save config
            save_configuration();
        } else if (cmd[1] == 'e'){ // Erase config
            erase_configuration();
        } else if (cmd[1] == 'r'){ // Reboot
            NVIC_SystemReset();
        }

    } else if (cmd[0] == 'r') { // read property
        char name[MAX_LINE_LENGTH];
        int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "invalid command format");
        } else {
            Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name));
            if (!endpoint) {
                respond(response_channel, use_checksum, "invalid property");
            } else {
                char response[10];
                bool success = endpoint->get_string(response, sizeof(response));
                if (!success)
                    respond(response_channel, use_checksum, "not implemented");
                else
                    respond(response_channel, use_checksum, response);
            }
        }

    } else if (cmd[0] == 'w') { // write property
        char name[MAX_LINE_LENGTH];
        char value[MAX_LINE_LENGTH];
        int numscan = sscanf(cmd, "w %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value);
        if (numscan < 1) {
            respond(response_channel, use_checksum, "invalid command format");
        } else {
            Endpoint* endpoint = application_endpoints_->get_by_name(name, sizeof(name));
            if (!endpoint) {
                respond(response_channel, use_checksum, "invalid property");
            } else {
                bool success = endpoint->set_string(value, sizeof(value));
                if (!success)
                    respond(response_channel, use_checksum, "not implemented");
            }
        }

     }else if (cmd[0] == 'u') { // Update axis watchdog. 
        unsigned motor_number;
        int numscan = sscanf(cmd, "u %u", &motor_number);
        if(numscan < 1){
            respond(response_channel, use_checksum, "invalid command format");
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        }else {
            axes[motor_number]->watchdog_feed();
        }

    } else if (cmd[0] != 0) {
        respond(response_channel, use_checksum, "unknown command");
    }
}

void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) {
    static uint8_t parse_buffer[MAX_LINE_LENGTH];
    static bool read_active = true;
    static uint32_t parse_buffer_idx = 0;

    while (len--) {
        // if the line becomes too long, reset buffer and wait for the next line
        if (parse_buffer_idx >= MAX_LINE_LENGTH) {
            read_active = false;
            parse_buffer_idx = 0;
        }

        // Fetch the next char
        uint8_t c = *(buffer++);
        bool is_end_of_line = (c == '\r' || c == '\n' || c == '!');
        if (is_end_of_line) {
            if (read_active)
                ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel);
            parse_buffer_idx = 0;
            read_active = true;
        } else {
            if (read_active) {
                parse_buffer[parse_buffer_idx++] = c;
            }
        }
    }
}

[Boon_Chiang_Ng]

Hi Mike,

On the teensy side, how did your codes look like?

Is it something as followings?

Serial3 << “s 0\n”;
float pos= odrive.readFloat();
float vel= odrive.readFloat();
float current= odrive.readFloat();

I only receive position value, but zero for vel and current.

[mike]

I’m not using any C++ odrive object, but rather reading and writing the serial directly.

I declare a serial buffer to store bytes read from serial:
char serial_buffer[128];

I then read bytes from serial into serial_buffer until we read a newline character, then null-terminate the buffer

    char* vel_start;
    char* cur_start;
    float jump_motor_position = strtof(serial_buffer, &vel_start);
    float jump_motor_velocity = strtof(vel_start, &cur_start);
    float jump_motor_current = strtof(cur_start, NULL);

It’s also probably advisable to use something like Serial3.println(“s 0”) rather than the << operator, but that may just be a style preference.