register for sensor getAngle

Author: runger1101001

src/comms/i2c/I2CCommander.cpp

@@ -200,12 +200,12 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu
         case REG_ZERO_OFFSET:
             readBytes(&(motors[motorNum]->sensor_offset), 4);
             break;
-
-        // unknown register or RO register
+        // RO registers
         case REG_STATUS:
         case REG_ANGLE:
         case REG_POSITION:
         case REG_VELOCITY:
+        case REG_SENSOR_ANGLE:
         case REG_VOLTAGE_Q:
         case REG_VOLTAGE_D:
         case REG_CURRENT_Q:
@@ -220,7 +220,7 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu
         case REG_PHASE_RESISTANCE:
         case REG_NUM_MOTORS:
         case REG_SYS_TIME:
-        default:
+        default: // unknown register
             return false;   
     }
     return true;
@@ -271,6 +271,12 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) {
         case REG_VELOCITY:
             writeFloat(motors[motorNum]->shaft_velocity);
             break;
+        case REG_SENSOR_ANGLE:
+            if (motors[motorNum]->sensor)
+                writeFloat(motors[motorNum]->sensor->getAngle()); // stored angle
+            else
+                writeFloat(motors[motorNum]->shaft_angle);
+            break;
 
         case REG_VOLTAGE_Q:
             writeFloat(motors[motorNum]->voltage.q);

src/comms/i2c/I2CCommanderRegisters.h

@@ -17,6 +17,7 @@ typedef enum : uint8_t  {
     REG_ANGLE = 0x09,           // RO - float
     REG_POSITION = 0x10,        // RO - double
     REG_VELOCITY = 0x11,        // RO - float
+    REG_SENSOR_ANGLE = 0x12,    // RO - float
 
     REG_VOLTAGE_Q = 0x20,       // RO - float
     REG_VOLTAGE_D = 0x21,       // RO - float