UUID reporting and masking

Report the UUID over the register protocol. Implement a means to have devices only respond to a frame if a subset of the UUID matches what was provided. This will let client applications disambiguate devices that have the same ID set at the expense of larger CAN frames.
FutureInLogistics · Jul 24, 2024 · 13c171c · 13c171c
1 parent 1088fe2
commit 13c171c
Show file tree

Hide file tree

Showing 8 changed files with 125 additions and 11 deletions.
diff --git a/docs/reference.md b/docs/reference.md
@@ -1254,6 +1254,23 @@ for fault register 2.  Up to 16 bits may be set.  This will only be
 non-zero if the current mode is fault (1) and the fault code is 33
 (motor driver fault).
 
+#### 0x150 - 0x153 - UUID ####
+
+Name: UUID
+Mode: Read only, int32 only
+
+This returns a 128 bit UUID, this is the value printed on mjbots
+packaging and returned by `moteus_tool --info`
+
+#### 0x154 - 0x157 - UUID Mask ####
+
+Name: UUID
+Mode: Write only, int32 only
+
+If one or more of these fields are written, then the entire frame
+after this point will be discarded unless the devices corresponding
+UUID matches what was written.
+
 ## A.3 Example ##
 
 A single CAN-FD frame can be used to command the servo, and initiate a

diff --git a/fw/moteus.cc b/fw/moteus.cc
@@ -260,7 +260,8 @@ int main(void) {
       &clock,
       &system_info,
       &timer,
-      &firmware_info);
+      &firmware_info,
+      &uuid);
 
   BoardDebug board_debug(
       &pool, &command_manager, &telemetry_manager, &multiplex_protocol,

diff --git a/fw/moteus_controller.cc b/fw/moteus_controller.cc
@@ -128,8 +128,14 @@ Value ScalePower(float value, size_t type) {
 
 int8_t ReadIntMapping(Value value) {
   return std::visit([](auto a) {
-      return static_cast<int8_t>(a);
-    }, value);
+    return static_cast<int8_t>(a);
+  }, value);
+}
+
+int32_t ReadInt32Mapping(Value value) {
+  return std::visit([](auto a) {
+    return static_cast<int32_t>(a);
+  }, value);
 }
 
 struct ValueScaler {
@@ -326,6 +332,16 @@ enum class Register {
 
   kDriverFault1 = 0x140,
   kDriverFault2 = 0x141,
+
+  kUuid1 = 0x150,
+  kUuid2 = 0x151,
+  kUuid3 = 0x152,
+  kUuid4 = 0x153,
+
+  kUuidMask1 = 0x154,
+  kUuidMask2 = 0x155,
+  kUuidMask3 = 0x156,
+  kUuidMask4 = 0x157,
 };
 
 aux::AuxHardwareConfig GetAux1HardwareConfig() {
@@ -426,7 +442,8 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
        ClockManager* clock_manager,
        SystemInfo* system_info,
        MillisecondTimer* timer,
-       FirmwareInfo* firmware)
+       FirmwareInfo* firmware,
+       Uuid* uuid)
       : aux1_port_("aux1", "ic_pz1", GetAux1HardwareConfig(),
                    &aux_adc_.aux_info[0],
                    persistent_config, command_manager, telemetry_manager,
@@ -482,7 +499,8 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
           }()),
         clock_manager_(clock_manager),
         system_info_(system_info),
-        firmware_(firmware) {}
+        firmware_(firmware),
+        uuid_(uuid) {}
 
   void Start() {
     bldc_.Start();
@@ -506,9 +524,24 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
     motor_position_.PollMillisecond();
   }
 
+  void StartFrame() override {
+    command_valid_ = false;
+    discard_all_ = false;
+  }
+
+  Action CompleteFrame() override {
+    if (discard_all_) {
+      command_valid_ = false;
+      return kDiscard;
+    }
+    return kAccept;
+  }
+
   uint32_t Write(multiplex::MicroServer::Register reg,
                  const multiplex::MicroServer::Value& value) override
       __attribute__ ((optimize("O3"))){
+    if (discard_all_) { return 0; }
+
     switch (static_cast<Register>(reg)) {
       case Register::kMode: {
         const auto new_mode_int = ReadIntMapping(value);
@@ -670,6 +703,23 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
         return 0;
       }
 
+      case Register::kUuidMask1:
+      case Register::kUuidMask2:
+      case Register::kUuidMask3:
+      case Register::kUuidMask4: {
+        const auto uuid = uuid_->uuid();
+        const auto index =
+            (static_cast<int>(reg) -
+             static_cast<int>(Register::kUuidMask1)) * 4;
+
+        const auto expected = *(reinterpret_cast<const int32_t*>(&uuid[index]));
+        const auto written = ReadInt32Mapping(value);
+        if (expected != written) {
+          discard_all_ = true;
+        }
+        return 0;
+      }
+
       case Register::kPosition:
       case Register::kVelocity:
       case Register::kMotorTemperature:
@@ -718,6 +768,10 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
       case Register::kSerialNumber1:
       case Register::kSerialNumber2:
       case Register::kSerialNumber3:
+      case Register::kUuid1:
+      case Register::kUuid2:
+      case Register::kUuid3:
+      case Register::kUuid4:
       case Register::kRegisterMapVersion:
       case Register::kFirmwareVersion:
       case Register::kMultiplexId:
@@ -744,6 +798,12 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
       multiplex::MicroServer::Register reg,
       size_t type) const override
       __attribute__ ((optimize("O3"))) {
+
+    if (discard_all_) {
+      // report unknown register for anything else
+      return static_cast<uint32_t>(1);
+    }
+
     auto vi32 = [](auto v) { return Value(static_cast<int32_t>(v)); };
 
     switch (static_cast<Register>(reg)) {
@@ -1017,13 +1077,29 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
             static_cast<int>(Register::kSerialNumber1);
         return Value(vi32(serial_number.number[index]));
       }
+      case Register::kUuid1:
+      case Register::kUuid2:
+      case Register::kUuid3:
+      case Register::kUuid4: {
+        if (type != 2) { break; }
+
+        const auto uuid = uuid_->uuid();
+        const auto index =
+            (static_cast<int>(reg) -
+             static_cast<int>(Register::kUuid1)) * 4;
+        return Value(*(reinterpret_cast<const int32_t*>(&uuid[index])));
+      }
       case Register::kMultiplexId: {
         break;
       }
       case Register::kSetOutputNearest:
       case Register::kSetOutputExact:
       case Register::kRequireReindex:
-      case Register::kRecapturePositionVelocity: {
+      case Register::kRecapturePositionVelocity:
+      case Register::kUuidMask1:
+      case Register::kUuidMask2:
+      case Register::kUuidMask3:
+      case Register::kUuidMask4: {
         break;
       }
       case Register::kDriverFault1: {
@@ -1047,8 +1123,10 @@ class MoteusController::Impl : public multiplex::MicroServer::Server {
   ClockManager* const clock_manager_;
   SystemInfo* const system_info_;
   FirmwareInfo* const firmware_;
+  Uuid* const uuid_;
 
   bool command_valid_ = false;
+  bool discard_all_ = false;
   BldcServo::CommandData command_;
 };
 
@@ -1060,9 +1138,11 @@ MoteusController::MoteusController(micro::Pool* pool,
                                    ClockManager* clock_manager,
                                    SystemInfo* system_info,
                                    MillisecondTimer* timer,
-                                   FirmwareInfo* firmware)
+                                   FirmwareInfo* firmware,
+                                   Uuid* uuid)
     : impl_(pool, pool, persistent_config, command_manager, telemetry_manager,
-            multiplex_protocol, clock_manager, system_info, timer, firmware) {}
+            multiplex_protocol, clock_manager, system_info, timer, firmware,
+            uuid) {}
 
 MoteusController::~MoteusController() {}
 

diff --git a/fw/moteus_controller.h b/fw/moteus_controller.h
@@ -23,6 +23,7 @@
 #include "fw/firmware_info.h"
 #include "fw/millisecond_timer.h"
 #include "fw/system_info.h"
+#include "fw/uuid.h"
 
 namespace moteus {
 
@@ -38,7 +39,8 @@ class MoteusController {
                    ClockManager*,
                    SystemInfo*,
                    MillisecondTimer*,
-                   FirmwareInfo*);
+                   FirmwareInfo*,
+                   Uuid*);
   ~MoteusController();
 
   void Start();

diff --git a/fw/uuid.cc b/fw/uuid.cc
@@ -24,6 +24,8 @@ Uuid::Uuid(mjlib::micro::PersistentConfig& config) {
   Update();
 }
 
+const uint8_t* Uuid::uuid() const { return &data_.uuid[0]; }
+
 void Uuid::Update() {
   // If any of the UUID bytes are non- 0xff, then just force our UUID
   // to be that from OTP.

diff --git a/fw/uuid.h b/fw/uuid.h
@@ -35,6 +35,8 @@ class Uuid {
     }
   };
 
+  const uint8_t* uuid() const;
+
  private:
   void Update();
 

diff --git a/lib/python/moteus/moteus.py b/lib/python/moteus/moteus.py
@@ -267,6 +267,16 @@ class Register(enum.IntEnum):
     DRIVER_FAULT1 = 0x140
     DRIVER_FAULT2 = 0x141
 
+    UUID1 = 0x150
+    UUID2 = 0x151
+    UUID3 = 0x152
+    UUID4 = 0x153
+
+    UUID_MASK1 = 0x0154
+    UUID_MASK2 = 0x0155
+    UUID_MASK3 = 0x0156
+    UUID_MASK4 = 0x0157
+
 
 class Mode(enum.IntEnum):
     """Valid values for the Register.MODE register"""

diff --git a/tools/workspace/mjlib/repository.bzl b/tools/workspace/mjlib/repository.bzl
@@ -20,6 +20,6 @@ def mjlib_repository(name):
     github_archive(
         name = name,
         repo = "mjbots/mjlib",
-        commit = "9ce0aeeab9dad44da79515864854eff74a9e46b1",
-        sha256 = "f0007a5934bdfb7dead58fcf4b52d67d6166555379ae7505185ab9af5b22c7f2",
+        commit = "7311db634349eb29b981f78627cb9914a6284c42",
+        sha256 = "aa3e3357c9c164ad0bd273bd8d1e67a361b2d566032abc1e9f4d19c2523bc875",
     )
-Original file line number
+Diff line change
@@ Expand Up / @@ -35,6 +35,8 @@ class Uuid { @@
         }
       };
+      const uint8_t* uuid() const;
      private:
       void Update();
@@ Expand Down @@