Fixes long register packet handling and synchronization issues in the binary stream layer; makes reads non-blocking and robust to partial frames.

src/comms/SimpleFOCRegisters.cpp

@@ -94,6 +94,12 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
             else
                 comms << (uint32_t)0;
             break;
+        case SimpleFOCRegister::REG_TELEMETRY_MIN_ELAPSED:
+            if (Telemetry::num_telemetry > 0)
+                comms << (uint32_t)(Telemetry::telemetries[Telemetry::telemetry_ctrl]->min_elapsed_time);
+            else
+                comms << (uint32_t)0;
+            break;
         case SimpleFOCRegister::REG_ITERATIONS_SEC:
             if (Telemetry::num_telemetry > 0)
                 comms << (Telemetry::telemetries[0]->last_iterations);
@@ -359,6 +365,11 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
             if (Telemetry::telemetry_ctrl < Telemetry::num_telemetry)
                 Telemetry::telemetries[Telemetry::telemetry_ctrl]->downsample = (uint16_t)val32;
             return true;
+        case SimpleFOCRegister::REG_TELEMETRY_MIN_ELAPSED:
+            comms >> val32;
+            if (Telemetry::telemetry_ctrl < Telemetry::num_telemetry)
+                Telemetry::telemetries[Telemetry::telemetry_ctrl]->min_elapsed_time = val32;
+            return true;
         case SimpleFOCRegister::REG_TELEMETRY_CTRL:
             comms >> val8;
             if (val8 < Telemetry::num_telemetry)
@@ -654,6 +665,7 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
         case SimpleFOCRegister::REG_INDUCTANCE:
         case SimpleFOCRegister::REG_TELEMETRY_DOWNSAMPLE:
         case SimpleFOCRegister::REG_ITERATIONS_SEC:
+        case SimpleFOCRegister::REG_TELEMETRY_MIN_ELAPSED:
         case SimpleFOCRegister::REG_CURA_GAIN:
         case SimpleFOCRegister::REG_CURB_GAIN:
         case SimpleFOCRegister::REG_CURC_GAIN:

src/comms/SimpleFOCRegisters.h

@@ -37,6 +37,7 @@ typedef enum : uint8_t  {
     REG_TELEMETRY_CTRL = 0x1B,          // R/W - 1 byte 
     REG_TELEMETRY_DOWNSAMPLE = 0x1C,    // R/W - uint32_t
     REG_ITERATIONS_SEC = 0x1D,          // RO - uint32_t
+    REG_TELEMETRY_MIN_ELAPSED = 0x1E,   // R/W - uint32_t (microseconds)
 
     REG_VOLTAGE_Q = 0x20,       // RO - float
     REG_VOLTAGE_D = 0x21,       // RO - float

src/comms/streams/BinaryIO.cpp

@@ -2,6 +2,8 @@
 
 #include "./BinaryIO.h"
 
+#include <string.h>
+
 BinaryIO::BinaryIO(Stream& io) : _io(io) {
     // nothing to do here
 };
@@ -91,44 +93,116 @@ void BinaryIO::_flush() {
 
 
 BinaryIO& BinaryIO::operator>>(float &value) {
-    remaining -= _io.readBytes((uint8_t*)&value, 4);
+    uint8_t buf[4] = {0};
+    uint8_t read = 0;
+    _rx_fill();
+    while (read < 4 && _rx_available() > 0) {
+        int byte = _rx_read();
+        if (byte < 0) {
+            break;
+        }
+        buf[read++] = (uint8_t)byte;
+    }
+    memcpy(&value, buf, sizeof(buf));
+    if (read > 0) {
+        remaining -= read;
+    }
     return *this;
 };
 
 
 
 BinaryIO& BinaryIO::operator>>(uint32_t &value) {
-    remaining -= _io.readBytes((uint8_t*)&value, 4);
+    uint8_t buf[4] = {0};
+    uint8_t read = 0;
+    _rx_fill();
+    while (read < 4 && _rx_available() > 0) {
+        int byte = _rx_read();
+        if (byte < 0) {
+            break;
+        }
+        buf[read++] = (uint8_t)byte;
+    }
+    memcpy(&value, buf, sizeof(buf));
+    if (read > 0) {
+        remaining -= read;
+    }
     return *this;
 };
 
 
 
 BinaryIO& BinaryIO::operator>>(uint8_t &value) {
-    value = (uint8_t)_io.read();
-    remaining--;
+    _rx_fill();
+    int byte = _rx_read();
+    if (byte >= 0) {
+        value = (uint8_t)byte;
+        remaining--;
+    } else {
+        value = 0;
+    }
     return *this;
 };
 
 
 
 PacketIO& BinaryIO::operator>>(Packet& value) {
-    while (!in_sync && _io.available() > 0) {
-        if (_io.peek() == MARKER_BYTE)
-            in_sync = true;
-        else
-            _io.read();
+    _rx_fill();
+    if (_pending) {
+        if (_rx_available() < _pending_payload) {
+            value.type = 0x00;
+            value.payload_size = 0;
+            return *this;
+        }
+        value.type = _pending_type;
+        value.payload_size = _pending_payload;
+        remaining = value.payload_size;
+        _pending = false;
+        return *this;
+    }
+    // Always resync to marker, even if in_sync was true.
+    while (_rx_available() > 0 && _rx_peek() != MARKER_BYTE) {
+        in_sync = false;
+        _rx_read();
     }
-    if (_io.peek() == MARKER_BYTE) {
-        _io.read(); // discard the marker
+    if (_rx_available() == 0) {
+        value.type = 0x00;
+        value.payload_size = 0;
+        return *this;
     }
-    if (!in_sync || _io.available() < 3) {  // size, frame type, payload = 3 bytes minimum frame size
+    in_sync = true;
+    if (_rx_available() < 3) {  // size, frame type, payload = 3 bytes minimum frame size
         value.type = 0x00;
         value.payload_size = 0;
         return *this;
     }
-    value.payload_size = (uint8_t)_io.read() - 1;
-    value.type = (uint8_t)_io.read();
+    _rx_read(); // discard the marker
+    int size_byte = _rx_read();
+    int type_byte = _rx_read();
+    if (size_byte <= 0) {
+        in_sync = false;
+        value.type = 0x00;
+        value.payload_size = 0;
+        return *this;
+    }
+    uint8_t payload_size = (uint8_t)(size_byte - 1);
+    if (payload_size >= BINARYIO_RX_BUFFER_SIZE) {
+        // Payload cannot fit; drop out of sync and wait for next marker.
+        in_sync = false;
+        value.type = 0x00;
+        value.payload_size = 0;
+        return *this;
+    }
+    if (_rx_available() < payload_size) {
+        _pending = true;
+        _pending_type = (uint8_t)type_byte;
+        _pending_payload = payload_size;
+        value.type = 0x00;
+        value.payload_size = 0;
+        return *this;
+    }
+    value.type = (uint8_t)type_byte;
+    value.payload_size = payload_size;
     remaining = value.payload_size;
     return *this;
 };
@@ -139,3 +213,35 @@ bool BinaryIO::is_complete() {
     return (remaining <= 0);
 };
 
+void BinaryIO::_rx_fill() {
+    while (_io.available() > 0 && _rx_count < BINARYIO_RX_BUFFER_SIZE) {
+        int byte = _io.read();
+        if (byte < 0) {
+            break;
+        }
+        _rx_buffer[_rx_head] = (uint8_t)byte;
+        _rx_head = (uint8_t)((_rx_head + 1) % BINARYIO_RX_BUFFER_SIZE);
+        _rx_count++;
+    }
+}
+
+uint8_t BinaryIO::_rx_available() const {
+    return _rx_count;
+}
+
+int BinaryIO::_rx_peek() {
+    if (_rx_count == 0) {
+        return -1;
+    }
+    return _rx_buffer[_rx_tail];
+}
+
+int BinaryIO::_rx_read() {
+    if (_rx_count == 0) {
+        return -1;
+    }
+    uint8_t value = _rx_buffer[_rx_tail];
+    _rx_tail = (uint8_t)((_rx_tail + 1) % BINARYIO_RX_BUFFER_SIZE);
+    _rx_count--;
+    return value;
+}

src/comms/streams/BinaryIO.h

@@ -16,6 +16,10 @@
 #define BINARYIO_BUFFER_SIZE 58
 #endif
 
+#ifndef BINARYIO_RX_BUFFER_SIZE
+#define BINARYIO_RX_BUFFER_SIZE 256
+#endif
+
 class BinaryIO : public PacketIO {
     public:
         BinaryIO(Stream& io);
@@ -35,12 +39,21 @@ class BinaryIO : public PacketIO {
         void _buff(uint8_t* data, uint8_t size);
         void _buff(uint8_t data);
         void _flush();
+        void _rx_fill();
+        int _rx_peek();
+        int _rx_read();
+        uint8_t _rx_available() const;
         Stream& _io;
         uint8_t remaining = 0;
         uint8_t _pos = 0;
         uint8_t _buffer[BINARYIO_BUFFER_SIZE];
+        uint8_t _rx_buffer[BINARYIO_RX_BUFFER_SIZE];
+        uint8_t _rx_head = 0;
+        uint8_t _rx_tail = 0;
+        uint8_t _rx_count = 0;
+        bool _pending = false;
+        uint8_t _pending_type = 0;
+        uint8_t _pending_payload = 0;
 };
 
 
-
-

src/encoders/tle5012b/STM32TLE5012B.cpp

@@ -127,7 +127,9 @@ void TLE5012B::init() {
 
     if (HAL_SPI_Init(&_spi) != HAL_OK) {
         // setup error
+#ifdef DEBUG_SERIAL
         Serial.println("TLE5012B setup error");
+#endif
     }
 };