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

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;
            }
        }
    }
}
2 Likes

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.

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.

Thanks, Mike! It works!

1 Like

@ealkhteeb @Boon_Chiang_Ng and anyone else interested - I made a small change to the conditional that processes the ‘s’ command described above, so that it does not interfere with the existing ‘s’ commands like ‘ss’ and ‘sr’. In my opinion, it may be advisable to adopt this pattern (or something similar) for the other single-letter commands as well (in the master branch @madcowswe ), so that they do not preclude the implementation/access of other commands that start with that character.

    } else if (cmd[0] == 's' && (len == 1 || cmd[1] == ' ')) { // status; position, velocity, current

Hi Mike,

Which branch are you working from? Most branches have a delay of around 1ms between UART polls that can give you a headache.

Cheers
Simon

IIRC, I just cloned the master around the time of my initial post (mid/late October). I don’t think I’ve encountered any UART polling issues, but I did discover a buffer overflow issue recently - when the status command responds with data for both axes, the 6 values can end up overflowing the UART TX buffer (which is only 64 bytes by default). Thus, I increased UART_TX_BUFFER_SIZE (defined in interface_uart.cpp) from 64 to 128. I also changed the formatting of the floats in the response from %f to %.1f (a floating point with 1 digit after decimal point) to reduce the length of the response (and the extra digits are largely meaningless anyway).

A quick follow-up - having spent more time in the odrive codebase recently, I think that parsing serial commands & parameters is probably best done the way it’s done in ascii_protocol.cpp - using scanf. In this case, that would be something like:

        float axis_position;
        float axis_velocity;
        float axis_current;
        int numscan = sscanf(cmd, "%f %f %f", &axis_position, &axis_velocity, &axis_current);
        if (numscan < 3) {
           // parameter parsing failed
        }else{
           // parameters parsed successfully
        }

Hi Mike,

I recently rewrote the UART code to try and improve the performance at any BAUD rate by using a quirk with the DMA interrupts to reduce the latency and jitter on serial commands. If you’re not having issues I wouldn’t worry about changing branches but the pull request can be found here:

I also tidied up and did a basic refactor of the parser so you can better see what letters have already been assigned routines. My next step was to reduce the overhead as the command letter is currently checked twice.

Cheers
Simon

Thank you for this work @mike
Have you figured out how to call a function in ascii, like how you modified the .cpp file to allow you to call a function?

I am trying to reset the encoder count.In native USB i use this…
axis*.encoder.set_linear_count(0)

Now i am trying to incorporate this into my modified ascii file so i can do it.

Jamil

If you want to call a native function via the ascii protocol, you have to implement it as I implemented the ‘status’ command above. There is no way to call a function RPC-style by simply specifying a text function label. All the information about human-readable function names is lost at compile time anyway.

Yeah that what i am asking. I still cannot figure out how to implement it like you did above.

    } else if (cmd[0] == 'e') { // encoder count
        unsigned motor_number;
        int32_t new_encoder_count;
        int numscan = sscanf(cmd, "e %u %i", &motor_number, &new_encoder_count);
        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 {
            axes[motor_number]->encoder_.set_linear_count(new_encoder_count);
        }
}

hhhmmmm, let me give it a try quick.
Why is there “e %u %i”?
I get the e has to match the variable you are using, but about the %u and %i?
Also why if (numscan < 3) rather than if (numscan < 1)?
Sorry I am not a coder.

This solution did not work :frowning:
Thank you for trying though

I got it to work simply like this…

else if (cmd[0] == 'e') { // encoder count
    axes[0]->encoder_.set_linear_count(0);
} 

But I know it is missing checksums and other crucial stuff.

e %u %i
tells sscanf to look for the letter e, followed by a space, an unsigned integer, another space, and then a signed integer. the ascii command to set the linear encoder value of motor 1 would look like:
e 1 25

numscan < 3 is there because a successfully parsed ascii command (as specified above) will yield 3 “scanned” values; the prepended ‘e’, the axis/motor ID (the ‘1’) and the desired linear encoder value (‘25’)

With respect, I believe my solution does work. Please be more specific about your process and the error(s) or erroneous behavior you are seeing.

There is nothing wrong with the code you posted from a “checksum” standpoint; it’s essentially what I posted but with all the parameterization removed, so your ‘e’ command can only be used to set to encoder value of motor 0 to 0. The code I posted allows the ‘e’ command to set the encoder value of either motor to whatever value is specified by the ascii command received by the odrive.

Mike,
First thank you for the explanations, this helps a lot.
When I used your code, and compiled it using TUP, the build files that it created did not have the ODriveFirmware.Hex
This tells me that there is an issue in the code?
When i removed the code you gave me, and re compiled, I got the .Hex file back.
Not sure my ignorance in coding is making any sense.

Jamil

Yes, you are correct, this limited it to only encoder 0 value being reset.