I’m working with two ODrive Pro units connected to a Teensy 4.1 over CAN bus, trying to build a general-purpose motor control program. Communication is mostly working. I can send commands like position control and configuration updates without issue. However, I’m not receiving any feedback from either ODrive. This is a sudden issue, and this feature has worked in the past, so I’m hoping someone may have some insight on what would have caused this.
The CAN bus is running at 1 Mbps, and I’ve confirmed that the baud rate is correctly set on all devices. The problem is that the received_feedback flag for each ODrive never gets set. As a result, this section of my code never executes:
This function is called periodically, so timing isn’t the issue. I’ve linked the section of code that directly “sets up” the odrive here in case it’s helpful:
The full function that utilizes the feedback can be found here, but its worth noting that if I have a similar line in, say odrive.ino (the above file), the same issue occurs:
This is also the configuration that our odrives are based on. Its pretty standard but there could be something in there.
I’m trying to figure out whether the problem is with message reception, CAN filtering, or something in the ODrive configuration that I’ve missed. Any insight would be appreciated.
That did not seem to solve my issue, unfortunately. I’m fairly confident I have correctly diagnosed the issue, similar functions like while(odrv16_user_data.last_heartbeat.Axis_State != ODriveAxisState::AXIS_STATE_CLOSED_LOOP_CONTROL) will also loop forever.
@LucasHorsman I think you may be inadvertently replicating the issue we just fixed. Can you try removing the delay(10) here, and instead running pumpEvents() for at least 100ms? Maybe more like 150 to be safe:
I agree, I had read that commit and made the change you suggest there as well.
Based on some other testing it looks like the CAN bus on that specific teensy is damaged in some way. The teensy in question had the same issue on my test bench when running the basic sinewave code, but another teensy did perfectly fine. So I suspect that somehow the CAN rx line of the teensy was damaged. I will be able to test again with a new teensy tomorrow and will let you know if it was fixed by replacing the teensy.
Made the directed changes and also replaced the teensy at the same time, now it works. I was a little curious so I went back to the previous teensy with the changed code, and now that one works as well. I believe were exactly correct with removing the delays in that loop, as well as running pumpEvents for 150ms. I’m definitely confused as to why this issue just came up, when I had very similar code in the past with the same hardware that had no issue reading heartbeat messages. (Also, it bewilders me that it was not working on my testbench but works fine now)
I’m curious, how does running for 150ms pumpEvents help make this code work?
Thank you for your time and help, I greatly appreciate it.
I’ve moved past the initial blocking while loop, but now I’m seeing a new issue: after the first run, received_feedback never becomes true again. I get one or two valid-looking iterations in the timer_callback function (called every 250 ms), but afterwards it looks like the teensy is simply not receiving the feedback messages.
To verify message reception, I added LED indicators inside the onFeedback and onHeartbeat handlers. They do blink, so the teensy does appear to be receiving those messages. Like so:
void onFeedback(Get_Encoder_Estimates_msg_t &msg, void *user_data)
{
//blink_led(2, 100);
// blink the LED to indicate that feedback is received
ODriveUserData *odrv_user_data = static_cast<ODriveUserData *>(user_data);
odrv_user_data->last_feedback = msg;
odrv_user_data->received_feedback = true;
}
Furthermore, after the setup process, when I submit commands (i.e position commands) to the odrives, they receive and follow them fine, and I’ll sometimes see one or two datas from one of them after the command has been sent.
To be absolutely clear, I can verify that messages are being received right as power is turned on, by blinking the led when a message is received. Then, once I send a position control message down the line, the led stops blinking, indicating that messages are no longer being received.
I’m curious, how does running for 150ms pumpEvents help make this code work?
The Teensy has an internal buffer of 16 CAN messages – once more than 16 messages are received, any new ones are dropped. If you have two ODrives, each sending a handful of feedback messages at 100Hz, and then the heartbeat at 10Hz, the feedback messages will fill the buffer very rapidly, and the heartbeat messages will be dropped.
pumpEvents just processes all queued messages in the Teensy’s buffer, so by running it constantly for 150ms, you’re processing messages as they come in, and you’re guaranteed to have a heartbeat message within 150ms (technically it could be 101ms, but 150ms is safe in case there’s any messages that get dropped/retransmitted/etc).
While this is technically deterministic, if you’re running it with a delay, there’s the chance the teensy does catch the heartbeat messages in the first 16 messages since last running pumpEvents. Which is likely why it worked on your test bench. Sometimes even just the crystal oscillator frequencies being a little out of spec can be enough to nudge the clock between the ODrives and Teensy a bit out of phase so the Teensy will eventually catch a heartbeat message. Tricky bug for sure.
@LucasHorsman Regarding the received_feedback never going true – this is a little tricky. It seems like odrive.ino isn’t the complete file, would you mind sending (or uploading somewhere) the entire Arduino code you’re using, so I can take a look at the whole context?
here is a link to the full repository. Its a little messy but the idea is that setup calls setupOdrive(), then the loop eventually kicks off the timer_callback function which is found in micro_ros_callbacks.ino
I’m curious if this could be a busload issue. I see you’re using 250Kbaud, and each of your two ODrives is outputting heartbeat at 10Hz and 6x feedback messages at 100Hz each:
Usually you want to keep CAN busload under 50% to prevent any chance of dropped or retransmitted messages – so that would be 125Kbit/s here.
Each CAN message is about 14 bytes, so per-ODrive, that’s 14 bytes * (6*100Hz + 1*10Hz) * 8 bits/byte = 68320 Kbit, between the two ODrives it’s 136640 Kbit/s, which is greater than the 50% baudrate target.
I’d recommend seeing what happens if you bump the baudrate up to 1Mbit/s, and/or disable the feedback messages you’re not using (iq_msg, torques_msg, error_msg, temperature_msg, bus_voltage_msg) on the ODrive.
I’m also curious – if you have a USB-CAN adapter, could you connect that and see if the issue is that the ODrive is sending the CAN message but the Teensy isn’t receiving it (but you can see it on the USB-CAN), or if the ODrive isn’t sending the message at all? It could also be some sort of bus wiring issue – feel free to send pictures. I’d make sure you have 120ohm termination at the Teensy and on the furthest ODrive on the chain, and that the CAN_GND from the Pro is connected to DC- at the system star point.
The issue is that the odrives get setup in that function you point out perfectly fine, and then send feedback fine, until I request a position command. At this point, the odrives seem to stop sending feedback, verified by debugging the onFeedback and onHeartbeat callbacks. However I can continue sending commands to the odrives.
OK, so we have changed the baudrate already up to 1MB, I’m not sure how thats not reflected in the repo, but I am using 1M baud. I also tried decreasing message rates which didnt seem to do anything.
When using the USB-CAN adapter everything looks normal feedback wise, I just plugged it in where my transceiver was basically.
So when using the USB-CAN adapter, you can see that the ODrives keep sending feedback messages even after you send the position command, but the Teensy is no longer receiving any of the feedback messages?
As far as I can tell, yes that’s correct. It might be more accurate to say that the onReceive function stops being called. I haven’t hooked up an oscilloscope yet, so I can’t say certainly that the teensy is no longer receiving feedback messages.
I’ll try and send pictures the next chance I get, on Monday.
Ah - understood. Because the Teensy CAN implementation doesn’t use interrupts, the only time it’ll actually read/process incoming messages is when pumpEvents is called, and because of the 100Hz feedback messages, this needs to be called at least at 100Hz. One option would be to use an IntervalTimer, set up to call pumpEvents at 100Hz (or a little faster, maybe 200Hz), however the fact this would be calling it in an interrupt means that there’s some other things you would need to do to avoid race conditions (like using volatile, interrupt locking, __disable_irq() / __enable_irq(), etc).
Maybe instead you could make timer_callback run at 200Hz, start by calling pumpEvents(), and then only actually run the odometry/public code every e.g. 10 loop iterations == 20Hz odom publish? Just something like this, where you run pumpEvents every loop but only run the odometry code every 10th loop:
// in create_entities.ino
const unsigned int timer_timeout = 5; //in milliseconds
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// in micro_ros_callbacks.ino
unsigned int counter = 0;
void timer_callback(rcl_timer_t *timer, int64_t last_call_time) {
pumpEvents();
counter++;
if (counter < 10) return;
counter = 0;
// Rest of the loop after this -- calculating odometry, running rcl_publish(), etc, everything that's currently in the loop.
}
I’m a little suprised to hear the teensy can implementation doesnt use interrupts
I think interrupts are a better solution than polling. That being said, the ODriveArduino stuff is mostly just designed as a reference, and anything with interrupts can cause some really unexpected behavior, especially with users with less embedded experience (race conditions, issues with interrupt masking, etc), and we need to orient the library to be used by inexperienced users. You should be able to call can_intf.enableMBInterrupts(), but you’ll have to go back and do the required modifications (adding volatile keywords, mainly).
How do the onRecieve and onFeedback functions work then?
pumpEvents causes the Teensy FlexCAN impl to process messages, which are sent to onReceive, which dispatches to the corresponding onFeedback/onHeartbeat/etc function.