I2CCommander supports new FOCMotorStatus

Author: runger1101001

src/comms/i2c/I2CCommander.cpp

@@ -23,21 +23,6 @@ void I2CCommander::addMotor(FOCMotor* motor){
 
 
 
-I2CCommander_Motor_Status I2CCommander::getMotorStatus(uint8_t motorNum){
-    if (motorNum<numMotors){
-        if (motors[motorNum]->zero_electric_angle==NOT_SET) // TODO detect open-loop uninitialized state!
-            return I2CCommander_Motor_Status::MOT_UNINITIALIZED;
-        if (motors[motorNum]->enabled==0)
-            return I2CCommander_Motor_Status::MOT_DISABLED;
-        if (motors[motorNum]->shaft_velocity >= I2CCOMMANDER_MIN_VELOCITY_FOR_MOTOR_MOVING)
-            return I2CCommander_Motor_Status::MOT_MOVING;
-        return I2CCommander_Motor_Status::MOT_IDLE;
-    }
-    return I2CCommander_Motor_Status::MOT_UNKNOWN;
-};
-
-
-
 
 bool I2CCommander::readBytes(void* valueToSet, uint8_t numBytes){
     if (_wire->available()>=numBytes){
@@ -85,7 +70,17 @@ void I2CCommander::onRequest(){
 
 
 
+/*
+Reads values from I2C bus and updates the motor's values.
+
+Currently this isn't really thread-safe, but works ok in practice on 32-bit MCUs.
 
+Do not use on 8-bit architectures where the 32 bit writes may not be atomic!
+
+Plan to make this safe: the writes should be buffered, and not actually executed 
+until in the main loop by calling commander->run();
+the run() method disables interrupts while the updates happen.
+*/
 bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int numBytes) {
     int val;
     float floatval;
@@ -230,7 +225,23 @@ bool I2CCommander::receiveRegister(uint8_t motorNum, uint8_t registerNum, int nu
 
 
 
+/*
+    Reads values from motor/sensor and writes them to I2C bus. Intended to be run
+    from the Wire.onRequest interrupt.
+
+    Assumes atomic 32 bit reads. On 8-bit arduino this assumption does not hold and this
+    code is not safe on those platforms. You might read "half-written" floats.
+    
+    A solution might be to maintain a complete set of shadow registers  in the commander
+    class, and update them in the run() method (which runs with interrupts off). Not sure
+    of the performance impact of all those 32 bit read/writes though. In any case, since
+    I use only 32 bit MCUs I'll leave it as an excercise to the one who needs it. ;-)
 
+    On 32 bit platforms the implication is that reads will occur atomically, so data will
+    be intact, but they can occur at any time during motor updates, so different values might
+    not be in a fully consistent state (i.e. phase A current might be from the current iteration
+    but phase B current from the previous iteration).
+*/
 bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) {
         // read the current register
     switch(registerNum) {
@@ -239,8 +250,7 @@ bool I2CCommander::sendRegister(uint8_t motorNum, uint8_t registerNum) {
             _wire->write((uint8_t)lastcommandregister);
             _wire->write((uint8_t)lastcommanderror+1);
             for (int i=0;(i<numMotors && i<28); i++) { // at most 28 motors, so we can fit in one packet
-                uint8_t status = (uint8_t)getMotorStatus(i);
-                _wire->write(status);
+                _wire->write(motors[motorNum]->motor_status);
             }
             break;
         case REG_MOTOR_ADDRESS:

src/comms/i2c/I2CCommander.h

@@ -26,8 +26,6 @@ class I2CCommander {
         void onReceive(int numBytes);
         void onRequest();
 
-        I2CCommander_Motor_Status getMotorStatus(uint8_t motorNum);
-
     protected:
         void writeFloat(float value);
         bool readBytes(void* valueToSet, uint8_t numBytes);

src/comms/i2c/I2CCommanderRegisters.h

@@ -62,23 +62,6 @@ typedef enum : uint8_t  {
 
 
 
-// TODO stati are work in progress. For example, it would be good to differentiate between not moving because stalled, 
-//  and not moving because target set-point has been reached. It would be good to be able to track initialization state
-// and error state.
-typedef enum : uint8_t {
-    MOT_UNINITIALIZED = 0x00,   // Motor is not initialized
-    MOT_INITIALIZING = 0x01,    // TODO - this status is not used at the moment, can't detect it
-    
-    MOT_DISABLED = 0x02,        // motor is disabled
-    MOT_IDLE = 0x03,            // motor is enabled, but not moving (current_velocity==target==0 in velocity mode, 
-                                //       or position==target in position mode)
-    MOT_MOVING = 0x04,          // motor is moving
-
-    MOT_ERROR = 0x05,           // TODO - motor is disabled because an error has occurred
-    MOT_INIT_FAILED = 0x0F,     // motor initialization failed
-    MOT_UNKNOWN = 0xFF          // incorrect motor number
-} I2CCommander_Motor_Status;
-
 
 
 #endif