Make I2C sources suitable for commutation or output encoders

Previously, I2C sources were only polled in the main thread during the
1 ms poll cycle.  This meant that if things were off-nominal, like
during startup when some things block the main thread for a long time,
the I2C devices would not be polled at all, and for some time after as
the ms loop caught back up.  It could also cause the PLL filter to get
mightily confused.

Either of those two effects rendered control unusable.

So, now we perform I2C operations from the control interrupt like
other encoder sources.  This hurts our "good case" ISR timing even
with no I2C devices enabled, but means the I2C devices keep working
even if the main loop is bogged down.

While we are at it, change the I2C configuration to permit microsecond
level period selection.  Doing so requires that we bump the ABI
version so that moteus_tool can upgrade the previous millisecond level
period selection.

fw/aux_common.h

@@ -238,13 +238,13 @@ struct I2C {
     };
     Type type = kNone;
     uint8_t address = 0x40;
-    int32_t poll_ms = 10;
+    int32_t poll_rate_us = 1000;
 
     template <typename Archive>
     void Serialize(Archive* a) {
       a->Visit(MJ_NVP(type));
       a->Visit(MJ_NVP(address));
-      a->Visit(MJ_NVP(poll_ms));
+      a->Visit(MJ_NVP(poll_rate_us));
     }
   };
 

fw/aux_port.h

@@ -209,6 +209,42 @@ class AuxPort {
     if (cui_amt21_) {
       cui_amt21_->ISR_Update(&status_.uart);
     }
+
+    if (i2c_) {
+      ISR_I2C_Update();
+    }
+  }
+
+  void ISR_I2C_Update() {
+    if (!i2c_startup_complete_) {
+      if (timer_->read_ms() > config_.i2c_startup_delay_ms) {
+        i2c_startup_complete_ = true;
+      }
+    }
+
+    ISR_PollI2c();
+
+    i2c_->Poll();
+
+    const auto read_status = i2c_->CheckRead();
+
+    if (read_status != Stm32I2c::ReadStatus::kNoStatus) {
+      for (size_t i = 0; i < status_.i2c.devices.size(); i++) {
+        auto& state = i2c_state_[i];
+        auto& status = status_.i2c.devices[i];
+
+        if (!state.pending) { continue; }
+
+        state.pending = false;
+        if (read_status == Stm32I2c::ReadStatus::kError) {
+          status.error_count++;
+          break;
+        }
+
+        ISR_ParseI2c(i);
+        break;
+      }
+    }
   }
 
   // Call this after AuxADC::ISR_EndSample() has completed.
@@ -246,16 +282,6 @@ class AuxPort {
   }
 
   void PollMillisecond() {
-    if (!i2c_startup_complete_) {
-      if (timer_->read_ms() > config_.i2c_startup_delay_ms) {
-        i2c_startup_complete_ = true;
-      }
-    }
-    if (i2c_) {
-      // We have I2C devices to potentially process.
-      PollI2c();
-    }
-
     if (!as5047_ && as5047_options_) {
       // We can only start sampling the as5047 after 10ms have passed
       // since boot.
@@ -323,29 +349,6 @@ class AuxPort {
         uart_->start_dma_read(current_tunnel_write_buf_);
       }
     }
-
-    if (i2c_) {
-      i2c_->Poll();
-      const auto read_status = i2c_->CheckRead();
-
-      if (read_status != Stm32I2c::ReadStatus::kNoStatus) {
-        for (size_t i = 0; i < status_.i2c.devices.size(); i++) {
-          auto& state = i2c_state_[i];
-          auto& status = status_.i2c.devices[i];
-
-          if (!state.pending) { continue; }
-
-          state.pending = false;
-          if (read_status == Stm32I2c::ReadStatus::kError) {
-            status.error_count++;
-            break;
-          }
-
-          ParseI2c(i);
-          break;
-        }
-      }
-    }
   }
 
 
@@ -537,18 +540,18 @@ class AuxPort {
   static constexpr uint8_t AS5600_REG_MAG_HIGH = 0x1B;
   static constexpr uint8_t AS5600_REG_MAG_LOW = 0x1C;
 
-  void ParseI2c(size_t index) {
+  void ISR_ParseI2c(size_t index) {
     const auto& config = config_.i2c.devices[index];
     auto& status = status_.i2c.devices[index];
 
     using DC = aux::I2C::DeviceConfig;
     switch (config.type) {
       case DC::kAs5048: {
-        ParseAs5048(&status);
+        ISR_ParseAs5048(&status);
         break;
       }
       case DC::kAs5600: {
-        ParseAs5600(&status);
+        ISR_ParseAs5600(&status);
         break;
       }
       case DC::kNone:
@@ -559,15 +562,13 @@ class AuxPort {
     }
   }
 
-  void ParseAs5048(aux::I2C::DeviceStatus* status) {
+  void ISR_ParseAs5048(aux::I2C::DeviceStatus* status) {
     status->active = i2c_startup_complete_;
 
-    __disable_irq();
     status->value =
         (encoder_raw_data_[4] << 8) |
         (encoder_raw_data_[5] << 2);
     status->nonce += 1;
-    __enable_irq();
 
     status->ams_agc = encoder_raw_data_[0];
     status->ams_diag = encoder_raw_data_[1];
@@ -576,30 +577,34 @@ class AuxPort {
         (encoder_raw_data_[3] << 2);
   }
 
-  void ParseAs5600(aux::I2C::DeviceStatus* status) {
+  void ISR_ParseAs5600(aux::I2C::DeviceStatus* status) {
     status->active = i2c_startup_complete_;
 
-    __disable_irq();
     status->value =
         ((encoder_raw_data_[1] << 8) |
          (encoder_raw_data_[2])) & 0x0fff;
     status->nonce += 1;
-    __enable_irq();
 
     status->ams_agc = 0;
     status->ams_diag = encoder_raw_data_[0];
     status->ams_mag = 0;
   }
 
-  void PollI2c() {
+  void ISR_PollI2c() {
     using DC = aux::I2C::DeviceConfig;
 
+    // If our periperhal is currently busy, nothing we can do.
+    if (i2c_->busy()) { return; }
+
+    const auto now_us = timer_->read_us();
+
     for (size_t i = 0; i < i2c_state_.size(); i++) {
       const auto& config = config_.i2c.devices[i];
-      auto& state = i2c_state_[i];
 
       if (config.type == DC::kNone) { continue; }
 
+      auto& state = i2c_state_[i];
+
       if (!state.initialized) {
         switch (config.type) {
           case DC::kAs5048: {
@@ -629,15 +634,16 @@ class AuxPort {
             break;
           }
         }
-        // We can initialize at most one device per ms poll cycle.
+        // We can initialize at most one device per poll cycle.
         state.initialized = true;
         return;
       }
 
-      state.ms_since_last_poll++;
+      const auto delta_us =
+          timer_->subtract_us(now_us, state.last_poll_us);
 
-      if (state.ms_since_last_poll >= config.poll_ms) {
-        state.ms_since_last_poll = 0;
+      if (delta_us >= config.poll_rate_us) {
+        state.last_poll_us = now_us;
         state.pending = true;
 
         switch (config.type) {
@@ -760,7 +766,7 @@ class AuxPort {
     cosine_pin_ = -1;
 
     for (auto& device : config_.i2c.devices) {
-      device.poll_ms = std::max<int32_t>(1, device.poll_ms);
+      device.poll_rate_us = std::max<int32_t>(100, device.poll_rate_us);
     }
 
     for (auto& pin : hw_config_.pins) {
@@ -1167,7 +1173,7 @@ class AuxPort {
 
   struct I2cState {
     bool initialized = false;
-    int32_t ms_since_last_poll = 0;
+    int32_t last_poll_us = 0;
     bool pending = false;
   };
 

fw/moteus_hw.h

@@ -123,8 +123,13 @@ MoteusHwPins FindHardwarePins(FamilyAndVersion);
 //
 // * Switched to a new encoder and position subsystem.
 
+// # 0x0106 #
+//
+// * Switched aux?.sources.x.i2c.poll_ms to poll_rate_us to match UART
+//   and give more resolution.
+
 #define MOTEUS_MODEL_NUMBER 0x0000
-#define MOTEUS_FIRMWARE_VERSION 0x000105
+#define MOTEUS_FIRMWARE_VERSION 0x000106
 
 extern MoteusHwPins g_hw_pins;
 

fw/motor_position.h

@@ -425,8 +425,8 @@ class MotorPosition {
           // filter bandwidth is no more than 1/10th of the expected
           // update rate.
           const float source_rate_hz =
-              1000.0f /
-              aux_config->i2c.devices[source_config.i2c_device].poll_ms;
+              1000000.0f /
+              aux_config->i2c.devices[source_config.i2c_device].poll_rate_us;
           const float max_pll_hz = source_rate_hz / 10.0f;
           source_config.pll_filter_hz =
               std::min(source_config.pll_filter_hz, max_pll_hz);

fw/stm32_i2c.h

@@ -222,6 +222,23 @@ class Stm32I2c {
     }
   }
 
+  bool busy() const {
+    switch (mode_) {
+      case Mode::kIdle:
+      case Mode::kComplete:
+      case Mode::kError: {
+        return false;
+      }
+      case Mode::kSentRegisterRead:
+      case Mode::kReadingData:
+      case Mode::kWritingData: {
+        return true;
+      }
+    }
+    MJ_ASSERT(false);
+    return false;
+  }
+
  private:
   const Options options_;
   bool valid_ = false;

lib/python/moteus/moteus_tool.py

@@ -54,13 +54,25 @@ def __init__(self, old, new):
         self.old = old
         self.new = new
 
-        if new > 0x0105:
+        if new > 0x0106:
             raise RuntimeError("Firmware to be flashed has a newer version than we support")
 
     def fix_config(self, old_config):
         lines = old_config.split(b'\n')
         items = dict([line.split(b' ') for line in lines if b' ' in line])
 
+        if self.new <= 0x0105 and self.old >= 0x0106:
+            # Downgrade the I2C polling rate.
+            for aux in [1, 2]:
+                for i2c_device in [0, 1, 2]:
+                    poll_rate_us_key = f'aux{aux}.i2c.devices.{i2c_device}.poll_rate_us'.encode('utf8')
+                    poll_ms_key = f'aux{aux}.i2c.devices.{i2c_device}.poll_ms'.encode('utf8')
+
+                    poll_rate_us = items[poll_rate_us_key]
+                    items.pop(poll_rate_us_key)
+                    items[poll_ms_key] = str(max(1, int(poll_rate_us) // 1000)).encode('utf8')
+            print("Reverting I2C rates for version 0x0105")
+
         if self.new <= 0x0104 and self.old >= 0x0105:
             # For now, we will only attempt to downgrade a config
             # where there is only an onboard encoder and nothing else.
@@ -250,6 +262,17 @@ def fix_config(self, old_config):
 
             print('Upgraded encoder configuration for version 0x0105')
 
+        if self.new >= 0x0106 and self.old <= 0x0105:
+            # We gave I2C encoders more timing resolution.
+            for aux in [1, 2]:
+                for i2c_device in [0, 1, 2]:
+                    poll_rate_us_key = f'aux{aux}.i2c.devices.{i2c_device}.poll_rate_us'.encode('utf8')
+                    poll_ms_key = f'aux{aux}.i2c.devices.{i2c_device}.poll_ms'.encode('utf8')
+                    poll_ms = items[poll_ms_key]
+                    items.pop(poll_ms_key)
+                    items[poll_rate_us_key] = str(int(poll_ms) * 1000).encode('utf8')
+            print("Upgrading I2C rates for version 0x0106")
+
 
         lines = [key + b' ' + value for key, value in items.items()]
         return b'\n'.join(lines)