fixing bugs in new IO code

Author: Richard Unger

src/comms/SimpleFOCRegisters.cpp

@@ -34,7 +34,7 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
             break;
         case SimpleFOCRegister::REG_POSITION:
             if (motor->sensor) {
-                comms << (uint32_t)motor->sensor->getFullRotations();
+                comms << (uint32_t)motor->sensor->getFullRotations(); // TODO fix me!
                 comms << motor->sensor->getMechanicalAngle();
             }
             else {
@@ -51,6 +51,24 @@ bool SimpleFOCRegisters::registerToComms(RegisterIO& comms, uint8_t reg, FOCMoto
             else
                 comms << motor->shaft_angle;
             break;
+        case SimpleFOCRegister::REG_SENSOR_MECHANICAL_ANGLE:
+            if (motor->sensor)
+                comms << motor->sensor->getMechanicalAngle(); // stored angle
+            else
+                comms << motor->shaft_angle;
+            break;
+        case SimpleFOCRegister::REG_SENSOR_VELOCITY:
+            if (motor->sensor)
+                comms << motor->sensor->getVelocity(); // stored angle
+            else
+                comms << motor->shaft_velocity;
+            break;
+        case SimpleFOCRegister::REG_SENSOR_TIMESTAMP:
+            if (motor->sensor)
+                comms << (uint32_t)motor->sensor->angle_prev_ts; // stored angle
+            else
+                comms << (uint32_t)0;
+            break;
 
         case SimpleFOCRegister::REG_TELEMETRY_REG:
             if (Telemetry::num_telemetry > 0){
@@ -405,6 +423,9 @@ bool SimpleFOCRegisters::commsToRegister(RegisterIO& comms, uint8_t reg, FOCMoto
         case SimpleFOCRegister::REG_POSITION:
         case SimpleFOCRegister::REG_VELOCITY:
         case SimpleFOCRegister::REG_SENSOR_ANGLE:
+        case SimpleFOCRegister::REG_SENSOR_MECHANICAL_ANGLE:
+        case SimpleFOCRegister::REG_SENSOR_VELOCITY:
+        case SimpleFOCRegister::REG_SENSOR_TIMESTAMP:
         case SimpleFOCRegister::REG_VOLTAGE_Q:
         case SimpleFOCRegister::REG_VOLTAGE_D:
         case SimpleFOCRegister::REG_CURRENT_Q:
@@ -433,6 +454,9 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
         case SimpleFOCRegister::REG_ANGLE:
         case SimpleFOCRegister::REG_VELOCITY:
         case SimpleFOCRegister::REG_SENSOR_ANGLE:
+        case SimpleFOCRegister::REG_SENSOR_MECHANICAL_ANGLE:
+        case SimpleFOCRegister::REG_SENSOR_VELOCITY:
+        case SimpleFOCRegister::REG_SENSOR_TIMESTAMP:        
         case SimpleFOCRegister::REG_VOLTAGE_Q:
         case SimpleFOCRegister::REG_VOLTAGE_D:
         case SimpleFOCRegister::REG_CURRENT_Q:
@@ -494,7 +518,7 @@ uint8_t SimpleFOCRegisters::sizeOfRegister(uint8_t reg){
                 return 2*telemetry->numRegisters + 1;
             }
             else
-                return 0;
+                return 1;
         case SimpleFOCRegister::REG_DRIVER_ENABLE:
         case SimpleFOCRegister::REG_REPORT: // size can vary, handled in Commander if used - may discontinue this feature
         case SimpleFOCRegister::REG_ENABLE_ALL: // write-only

src/comms/SimpleFOCRegisters.h

@@ -27,6 +27,9 @@ typedef enum : uint8_t  {
     REG_POSITION = 0x10,        // RO - int32_t full rotations + float position (0-2PI, in radians) (4 bytes + 4 bytes)
     REG_VELOCITY = 0x11,        // RO - float
     REG_SENSOR_ANGLE = 0x12,    // RO - float
+    REG_SENSOR_MECHANICAL_ANGLE = 0x13,    // RO - float
+    REG_SENSOR_VELOCITY = 0x14, // RO - float
+    REG_SENSOR_TIMESTAMP = 0x15,// RO - uint32_t
 
     REG_PHASE_VOLTAGE = 0x16,  // R/W - 3xfloat = 12 bytes
     REG_PHASE_STATE = 0x17,     // R/W - 3 bytes (1 byte per phase)

src/comms/streams/BinaryIO.cpp

@@ -114,11 +114,13 @@ BinaryIO& BinaryIO::operator>>(uint8_t &value) {
 
 PacketIO& BinaryIO::operator>>(Packet& value) {
     while (!in_sync && _io.available() > 0) {
-        if (_io.read() == MARKER_BYTE)
+        if (_io.peek() == MARKER_BYTE)
             in_sync = true;
+        else
+            _io.read();
     }
     if (_io.peek() == MARKER_BYTE) {
-        _io.read(); // discard the \n
+        _io.read(); // discard the marker
     }
     if (!in_sync || _io.available() < 3) {  // size, frame type, payload = 3 bytes minimum frame size
         value.type = 0x00;

src/comms/streams/PacketCommander.cpp

@@ -76,7 +76,7 @@ void PacketCommander::handleRegisterPacket(bool write, uint8_t reg) {
 
 bool PacketCommander::commsToRegister(uint8_t reg){
     switch (reg) {
-        case REG_MOTOR_ADDRESS:
+        case SimpleFOCRegister::REG_MOTOR_ADDRESS:
             uint8_t val;
             *_io >> val;
             if (val >= numMotors)

src/comms/streams/TextIO.cpp

@@ -79,7 +79,7 @@ TextIO& TextIO::operator>>(float &value) {
     if (in_sep) {
         _io.read(); // discard the separator
     }
-    value = _io.parseFloat(LookaheadMode::SKIP_NONE);
+    value = _io.parseFloat(LookaheadMode::SKIP_NONE);  // TODO LookaheadMode is not defined on ESP32
     in_sep = true;
     return *this;
 };