What I suggest is to put things back into the error state, plug the camera in, etc. Then modify the code to time how long the Tele is taking. The easiest way to do this is to drop a Milliseconds block, a feedback node, and a subtract. wire this to an indicator, or perhaps even better a chart as shown below.
This will give you an indication of how long the Tele is taking. It is not really a good idea for it to take the full 20ms period or it will miss control packets. My assumption is that at least some of the time, the Tele takes too long. If that is the case, play with the parameters or look for how to make it better and worse.
I don't know how much time it takes to send out the CAN commands, but since the serial is 115kbaud or something like that, the limit will not be that hard to reach.
As for the camera stuff, this is hopefully not in any way in your Tele loop. Ideally it runs in parallel, sort of as a background task and the results are harvested when available to be used in the next Tele iteration. I suspect your usage is fine, but I've seen many teams who naively put it in the Tele, so I thought it was worth mentioning.
Anyway, with the BG camera task, taking a certain amount of CPU away, it makes it easier to overstep the watchdog time in the Tele loop, but it isn't directly responsible for it. The camera actually exports one of two jpeg formats, and by definition, color jpeg is 24 bits per pixel. IMAQ decodes this to a color format that uses 32 bits, primarily for architectural alignment. It is actually more expensive to access three bytes on random address alignments than to pick up one four byte long that is well aligned. So this is the classic space for time tradeoff and the 33% space waste is done to speed up the pixel access times. The camera does support a different form of JPG without color, and at least last year, that didn't decode properly. I'm not sure if it has been fixed. Because it didn't work, I didn't profile it, and perhaps that is a way to lighten the load and speed up the vision. Since you are so adept at this, maybe you can beat me to it
.
Post back once you have some measurements.
Greg McKaksle