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

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).

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

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?

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

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.

@mike thanks for sharing the code