Support secondary encoders

mjbots · Sep 9, 2021 · 23d8544 · 23d8544
1 parent f03b6fc
commit 23d8544
Show file tree

Hide file tree

Showing 5 changed files with 110 additions and 17 deletions.
diff --git a/docs/reference.md b/docs/reference.md
@@ -1111,6 +1111,14 @@ The reported position is calculated from the raw value as follows:
 position = (raw + offset) / 65536 * scale
 ```
 
+## `encoder.mode` ##
+
+Selects whether the onboard magnetic encoder or an external magnetic
+encoder is used.
+
+* 0 - onboard
+* 1 - external AS5047 compatible SPI encoder
+
 # D. Maintenance #
 
 ## tview usage ##

diff --git a/fw/as5047.h b/fw/as5047.h
@@ -18,23 +18,48 @@
 
 #include "hal/spi_api.h"
 
+#include "mjlib/micro/persistent_config.h"
+
 #include "fw/moteus_hw.h"
 #include "fw/stm32_spi.h"
 
 namespace moteus {
 
 class AS5047 {
  public:
-  using Options = Stm32Spi::Options;
+  struct Options : Stm32Spi::Options {
+    PinName external_cs = NC;
+  };
+
+  enum Mode {
+    kIntegrated = 0,
+    kExternalSpi = 1,
+    kNumModes = 2,
+  };
+
+  struct Config {
+    Mode mode = kIntegrated;
 
-  AS5047(const Options& options)
-      : spi_([options]() {
+    template <typename Archive>
+    void Serialize(Archive* a) {
+      a->Visit(MJ_NVP(mode));
+    }
+  };
+
+  AS5047(mjlib::micro::PersistentConfig* persistent_config,
+         const Options& options)
+      : options_(options),
+        spi_([options]() {
           // The next frequency down is only 6MHz, so we run a bit out
           // of tolerance to save a fair amount of time.
           auto copy = options;
           copy.frequency = 12000000;
           return copy;
-        }()) {}
+        }()) {
+    persistent_config->Register(
+        "encoder", &config_,
+        std::bind(&AS5047::HandleConfigUpdate, this));
+  }
 
   uint16_t Sample() MOTEUS_CCM_ATTRIBUTE {
     return (spi_.write(0xffff) & 0x3fff) << 2;
@@ -49,7 +74,49 @@ class AS5047 {
   }
 
  private:
+  void HandleConfigUpdate() {
+    switch (config_.mode) {
+      case kIntegrated: {
+        spi_.set_cs(options_.cs);
+        return;
+      }
+      case kExternalSpi: {
+        spi_.set_cs(options_.external_cs);
+        return;
+      }
+      case kNumModes:{
+        break;
+      }
+    }
+
+    // Hmmm, guess we'll stick with integrated.
+    spi_.set_cs(options_.cs);
+  }
+
+  const Options options_;
+  Config config_;
   Stm32Spi spi_;
 };
 
 }
+
+
+namespace mjlib {
+namespace base {
+
+template <>
+struct IsEnum<moteus::AS5047::Mode> {
+  static constexpr bool value = true;
+
+  using M = moteus::AS5047::Mode;
+  static std::array<std::pair<M, const char*>,
+                    static_cast<int>(M::kNumModes)> map() {
+    return { {
+        { M::kIntegrated, "integrated" },
+        { M::kExternalSpi, "external_spi" },
+      } };
+  }
+};
+
+}
+}
diff --git a/fw/moteus_controller.cc b/fw/moteus_controller.cc
@@ -248,14 +248,17 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
        MillisecondTimer* timer,
        FirmwareInfo* firmware,
        AbsPort* abs_port)
-      : as5047_([]() {
-          AS5047::Options options;
-          options.mosi = MOTEUS_AS5047_MOSI;
-          options.miso = MOTEUS_AS5047_MISO;
-          options.sck = MOTEUS_AS5047_SCK;
-          options.cs = MOTEUS_AS5047_CS;
-          return options;
-        }()),
+      : as5047_(
+          persistent_config,
+          []() {
+            AS5047::Options options;
+            options.mosi = MOTEUS_AS5047_MOSI;
+            options.miso = MOTEUS_AS5047_MISO;
+            options.sck = MOTEUS_AS5047_SCK;
+            options.cs = MOTEUS_AS5047_CS;
+            options.external_cs = MOTEUS_EXTERNAL_ENCODER_CS;
+            return options;
+          }()),
         drv8323_(pool, persistent_config, telemetry_manager, timer, []() {
             Drv8323::Options options;
             options.mosi = DRV8323_MOSI;

diff --git a/fw/moteus_hw.h b/fw/moteus_hw.h
@@ -122,7 +122,9 @@ constexpr int kCompatibleHwRev[] = {
 #elif MOTEUS_HW_REV >= 3
 #define HWREV_PIN0 PC_6
 #define HWREV_PIN1 PA_15
-#define HWREV_PIN2 PC_13
+// Previously this was documented as PC_13, however we never pulled it
+// down, and decided to use PC_13 for something else.
+#define HWREV_PIN2 PA_10
 #endif
 
 #if MOTEUS_HW_REV <= 2
@@ -197,6 +199,8 @@ constexpr int kCompatibleHwRev[] = {
 #define MOTEUS_AS5047_CS PB_11
 #endif
 
+#define MOTEUS_EXTERNAL_ENCODER_CS PC_13
+
 #if MOTEUS_HW_REV >= 3
 #define MOTEUS_CAN_TD PA_12
 #define MOTEUS_CAN_RD PA_11

diff --git a/fw/stm32_spi.h b/fw/stm32_spi.h
@@ -14,6 +14,8 @@
 
 #pragma once
 
+#include <optional>
+
 #include "mbed.h"
 
 #include "hal/spi_api.h"
@@ -33,21 +35,30 @@ class Stm32Spi {
   };
 
   Stm32Spi(const Options& options)
-      : cs_(options.cs, 1) {
+      : cs_(std::in_place_t(), options.cs, 1) {
 
     spi_init(&spi_, options.mosi, options.miso, options.sck, NC);
     spi_format(&spi_, options.width, options.mode, 0);
     spi_frequency(&spi_, options.frequency);
   }
 
+  void set_cs(PinName cs) {
+    // The SPI class may be used from an interrupt.  If we want to
+    // change the CS line, we have to make sure no one can use it
+    // while it is being changed.
+    __disable_irq();
+    cs_.emplace(cs, 1);
+    __enable_irq();
+  }
+
   uint16_t write(uint16_t value) MOTEUS_CCM_ATTRIBUTE {
     start_write(value);
     return finish_write();
   }
 
   void start_write(uint16_t value) MOTEUS_CCM_ATTRIBUTE {
     auto* const spi = spi_.spi.handle.Instance;
-    cs_ = 0;
+    *cs_ = 0;
 
     // This doesn't seem to be a whole lot faster than the HAL in
     // practice, but it doesn't hurt to do it ourselves and not have
@@ -66,15 +77,15 @@ class Stm32Spi {
     while ((spi->SR & SPI_SR_BSY) != 0);
     spi->CR1 &= ~(SPI_CR1_SPE);
 
-    cs_ = 1;
+    *cs_ = 1;
     return result;
   }
 
  private:
   // We don't use the mbed SPI class because we want to be invokable
   // from an ISR.
   spi_t spi_;
-  DigitalOut cs_;
+  std::optional<DigitalOut> cs_;
 };
 
 }