62 #include <condition_variable> 66 #define NUM_MSGS 40000 69 #define START_BYTE 0xA5 71 #define START_BYTE_LEN 1 72 #define PAYLOAD_LEN 12 74 #define PACKET_LEN (START_BYTE_LEN + PAYLOAD_LEN + CRC_LEN) 77 #define BAUD_RATE 921600 78 #define NUM_START_BITS 1 79 #define NUM_STOP_BITS 1 98 data = inCrc ^ inData;
100 for ( i = 0; i < 8; i++ )
102 if (( data & 0x80 ) != 0 )
128 memcpy(dst+1, &
id, 4);
129 memcpy(dst+5, &v1, 4);
130 memcpy(dst+9, &v2, 4);
133 for (
size_t i = 0; i < PACKET_LEN-1; i++)
137 dst[PACKET_LEN-1] = crc;
154 memcpy(v1, src+4, 4);
155 memcpy(v2, src+8, 4);
165 PARSE_STATE_GOT_START_BYTE,
166 PARSE_STATE_GOT_PAYLOAD
186 static size_t payload_count;
191 case PARSE_STATE_IDLE:
192 if (byte == START_BYTE)
201 case PARSE_STATE_GOT_START_BYTE:
204 if (++payload_count >= PAYLOAD_LEN)
209 case PARSE_STATE_GOT_PAYLOAD:
221 std::unique_lock<std::mutex> lock(
mutex);
243 for (
size_t i = 0; i < len; i++)
250 int main(
int argc,
char** argv)
256 std::printf(
"USAGE: %s PORT\n", argv[0]);
261 std::printf(
"Using port %s\n", argv[1]);
271 std::printf(
"Failed to initialize serial port\n");
278 auto start = std::chrono::high_resolution_clock::now();
281 uint8_t buffer[PACKET_LEN];
282 for (uint32_t i = 0; i < NUM_MSGS; i++)
287 auto finish_write = std::chrono::high_resolution_clock::now();
291 std::unique_lock<std::mutex> lock(
mutex);
295 auto finish_read = std::chrono::high_resolution_clock::now();
301 int num_received = 0;
302 for (
int i = 0; i < NUM_MSGS; i++)
310 std::printf(
"Missing message %d\n", i);
315 std::chrono::duration<double, std::milli> write_time = finish_write - start;
316 std::chrono::duration<double, std::milli> read_time = finish_read - start;
318 std::printf(
"Received %d of %d messages\n", num_received, NUM_MSGS);
319 std::printf(
"Elapsed write time: %fms\n", write_time.count());
320 std::printf(
"Elapsed read time: %fms\n", read_time.count());
322 int num_bytes = NUM_MSGS * PACKET_LEN;
323 double expected_time = num_bytes * (8 + NUM_START_BITS + NUM_STOP_BITS) / (
double) BAUD_RATE;
324 std::printf(
"Expected read time: %fms\n", expected_time*1e3);
325 std::printf(
"Total: %d bytes\n", num_bytes);
Asynchronous communication class for a serial port.
volatile int receive_count
Keeps track of how many valid messages have been received.
ParseState parse_state
Current state of the parser state machine.
uint8_t update_crc(uint8_t inCrc, uint8_t inData)
Recursively update the cyclic redundancy check (CRC)
volatile bool all_messages_received
flag for whether all messages have been received back
void send_bytes(const uint8_t *src, size_t len)
Send bytes from a buffer over the port.
bool init()
Initializes and opens the port.
void pack_message(uint8_t *dst, uint32_t id, uint32_t v1, uint32_t v2)
Pack message contents into a buffer.
void register_receive_callback(std::function< void(const uint8_t *, size_t)> fun)
Register a callback function for when bytes are received on the port.
void close()
Closes the port.
ParseState
States for the parser state machine.
void unpack_payload(uint8_t *src, uint32_t *id, uint32_t *v1, uint32_t *v2)
Unpack the contents of a message payload buffer.
std::mutex mutex
mutex for synchronization between the main thread and callback thread
void callback(const uint8_t *buf, size_t len)
Callback function for the async_comm library.
uint8_t receive_buffer[PAYLOAD_LEN]
Buffer for accumulating received payload.
void parse_byte(uint8_t byte)
Passes a received byte through the parser state machine.
std::condition_variable condition_variable
condition variable used to suspend main thread until all messages have been received back ...
bool received[NUM_MSGS]
Keeps track of which messages we've received back.