From 38a5cf59a1d3f437fdc62b4695ce161b4f56fad4 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jakub=20Van=C4=9Bk?= <linuxtardis@gmail.com>
Date: Wed, 6 Nov 2024 00:38:44 +0100
Subject: [PATCH] Hopefully fix EV3 IR sensor communication breakdown
---
micropython/modules/ev3_sensors/ev3_sensor.c | 1 +
micropython/modules/ev3_sensors/messages.h | 4 ++++
.../modules/ev3_sensors/stream_packetizer.c | 19 ++++++++++++++++---
.../modules/ev3_sensors/stream_packetizer.h | 1 +
4 files changed, 22 insertions(+), 3 deletions(-)
diff --git a/micropython/modules/ev3_sensors/ev3_sensor.c b/micropython/modules/ev3_sensors/ev3_sensor.c
index e586a6e..99bb220 100644
--- a/micropython/modules/ev3_sensors/ev3_sensor.c
+++ b/micropython/modules/ev3_sensors/ev3_sensor.c
@@ -101,6 +101,7 @@ bool ev3_sensor_refresh(repeating_timer_t *rt) {
if (sensor_id_or_error >= 0) { // got sync!
link->sensor_metadata.id = sensor_id_or_error;
link->state = EV3_PROCESSING_HANDSHAKE;
+ link->packetizer.ir_sensor_workaround = sensor_id_or_error == EV3_INFRA_SENSOR_ID;
}
}
if (link->state == EV3_PROCESSING_HANDSHAKE) {
diff --git a/micropython/modules/ev3_sensors/messages.h b/micropython/modules/ev3_sensors/messages.h
index f854c58..ee6467f 100644
--- a/micropython/modules/ev3_sensors/messages.h
+++ b/micropython/modules/ev3_sensors/messages.h
@@ -151,6 +151,10 @@ enum {
// Confirmation that the brick received all sensor information
EV3_MSG_ACK = (MTYPE_SYSTEM << EV3_MESSAGE_TYPE_SHIFT) |
(MTYPE_SYSTEM_ACK << EV3_MESSAGE_SUBTYPE_SHIFT),
+ // Obsolete message sent by the EV3 IR sensor. Early in the EV3
+ // development process it might've been used for checking baud rate sync
+ EV3_MSG_SYNC = (MTYPE_SYSTEM << EV3_MESSAGE_TYPE_SHIFT) |
+ (MTYPE_SYSTEM_SYNC << EV3_MESSAGE_SUBTYPE_SHIFT),
// First byte of the sensor handshake
EV3_INITIAL_MSG = (MTYPE_COMMAND << EV3_MESSAGE_TYPE_SHIFT) |
(MTYPE_COMMAND_TYPE << EV3_MESSAGE_SUBTYPE_SHIFT) |
diff --git a/micropython/modules/ev3_sensors/stream_packetizer.c b/micropython/modules/ev3_sensors/stream_packetizer.c
index a4f2e7d..c0a42de 100644
--- a/micropython/modules/ev3_sensors/stream_packetizer.c
+++ b/micropython/modules/ev3_sensors/stream_packetizer.c
@@ -16,6 +16,7 @@ void packetizer_reset(packetizer_state *st) {
st->message.total_size = 0;
st->message.buffer = st->buffer;
st->waiting_for_header = true;
+ st->ir_sensor_workaround = false;
}
void packetizer_poll(packetizer_state *st, opencube_uart_if *uart) {
@@ -26,6 +27,10 @@ void packetizer_poll(packetizer_state *st, opencube_uart_if *uart) {
if (try_complete_message(st, uart) == DONE) {
st->waiting_for_header = true;
+ if (st->ir_sensor_workaround && st->message.buffer[0] == EV3_MSG_SYNC) {
+ // skip broken SYNC packets from EV3 IR sensor
+ continue;
+ }
if (st->callback(st->callback_context, st->message.buffer, st->message.total_size) == PROC_STOP) {
break;
}
@@ -45,10 +50,18 @@ completion try_read_header(packetizer_state *st, opencube_uart_if *uart) {
}
st->message.buffer[0] = header;
- st->message.total_size = full_length_of_message(header);
st->message.done_size = 1;
- if (st->message.total_size > EV3_MSG_BUFFER_SIZE) {
- st->message.total_size = EV3_MSG_BUFFER_SIZE;
+
+ if (st->ir_sensor_workaround && header == EV3_MSG_SYNC) {
+ // The IR sensor sometimes sends 0x00 0xFF packets during the
+ // handshake - they look like broken SYNC packages. They are useless,
+ // so just detect them here and later skip them
+ st->message.total_size = 2;
+ } else {
+ st->message.total_size = full_length_of_message(header);
+ if (st->message.total_size > EV3_MSG_BUFFER_SIZE) {
+ st->message.total_size = EV3_MSG_BUFFER_SIZE;
+ }
}
return DONE;
}
diff --git a/micropython/modules/ev3_sensors/stream_packetizer.h b/micropython/modules/ev3_sensors/stream_packetizer.h
index c0fad3b..8fa8647 100644
--- a/micropython/modules/ev3_sensors/stream_packetizer.h
+++ b/micropython/modules/ev3_sensors/stream_packetizer.h
@@ -26,6 +26,7 @@ typedef struct {
partial_message message;
uint8_t buffer[EV3_MSG_BUFFER_SIZE];
bool waiting_for_header;
+ bool ir_sensor_workaround;
} packetizer_state;
/**
--
GitLab