Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix status response reading logic #4

Merged
merged 3 commits into from
Dec 7, 2023

Conversation

omelia-iliffe
Copy link
Contributor

Currently when read_status_response is called from sync_read_cb multiple packets can be read into the buffer but only one packet is parsed. This leads to the second response packet to be consumed without being parsed.

A sync read instruction packet is sent to 2 dynamixels, ID 1 and 2

Trace: sending instruction: [FF, FF, FD, 00, FE, 09, 00, 82, 7E, 00, 0A, 00, 02, 01, 32, 98]

The expected response data is read into the buffer on the first call to read_status_response. Note that 2 packets are read, not one.

Trace: read data length: 42
Trace: read data: [FF, FF, FD, 00, 02, 0E, 00, 55, 00, 00, 00, 00, 00, 00, 00, C2, 09, 00, 00, 03, DB, FF, FF, FD, 00, 01, 0E, 00, 55, 00, 00, 00, 00, 00, 00, 00, 9F, 0C, 00, 00, 55, 7C] 

however the current implementation doesn't check if more than one packet was read so it timeouts trying to read a response it has already read.

Trace: read packet: [FF, FF, FD, 00, 02, 0E, 00, 55, 00, 00, 00, 00, 00, 00, 00, C2, 09, 00, 00, 03, DB]
Error: std::io::ErrorKind::TimedOut as serial port didn't receive any more data

Moving the read buffer checks, that check if the read buffer has a valid packet, to before the self.serial_port.read call prevents this issue. Although the nested if statements may not be the most elegant solution.

when using sync_read at high baudrates multiple response packets are
read at the same time, and the tailing packets discarded.
By checking if a packet is in the buffer before trying to read new bytes
this is prevented.
@omelia-iliffe
Copy link
Contributor Author

Another implementation option is that only the correct and necessary bytes are read from the serial port. This is how Dynamixel's SDK works.
The serial2 crate has the read_exact function which we could use.

@de-vri-es
Copy link
Member

de-vri-es commented Dec 5, 2023

Hey! Thanks for reporting the issue and fixing it! :D

I like the approach of this PR. It should be more efficient than limiting the read with read_exact().

The only thing I'm thinking, remove_garbage() should be taken out of the if self.read_len > HEADER_PREFIX.len() { ... }. Otherwise we think we have enough data for a valid header, but there might still be some leading garbage in the buffer that gets removed only later. And then the self.read_buf[6] could panic.

@de-vri-es
Copy link
Member

The only thing I'm thinking, remove_garbage() should be taken out of the if self.read_len > HEADER_PREFIX.len() { ... }. Otherwise we think we have enough data for a valid header, but there might still be some leading garbage in the buffer that gets removed only later. And then the self.read_buf[6] could panic.

Ah, I see it couldn't panic. But we can remove the entire outer if if we just call remove_garbage() unconditionally :)

@de-vri-es de-vri-es self-assigned this Dec 5, 2023
@omelia-iliffe
Copy link
Contributor Author

Sweet, I made that change.
I also removed the && self.read_buffer.as_mut()[..self.read_len].starts_with(&HEADER_PREFIX) I believe its unnecessary. The remove garbage call will ensure that a header is always at the beginning so if we have > STATUS_PACKET_LEN in the buffer we must have a packet.

src/bus.rs Outdated Show resolved Hide resolved
@de-vri-es
Copy link
Member

I also removed the && self.read_buffer.as_mut()[..self.read_len].starts_with(&HEADER_PREFIX) I believe its unnecessary. The remove garbage call will ensure that a header is always at the beginning so if we have > STATUS_PACKET_LEN in the buffer we must have a packet.

Good point! The change looks good. For future readers of the code, I also added a code suggestion to explain this with a comment. If you agree with the wording we can apply it and get this merged :)

Co-authored-by: Maarten de Vries <maarten@de-vri.es>
@omelia-iliffe
Copy link
Contributor Author

Yup! Looks great!

@de-vri-es de-vri-es merged commit e9c6053 into robohouse-delft:main Dec 7, 2023
1 check passed
@de-vri-es
Copy link
Member

Released as v0.5.1. Thanks again for the PR!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants