From 603e0b1bf2e9952121f127336bc73637fc627b7c Mon Sep 17 00:00:00 2001
From: Paul Guenette <pfguenette@gmail.com>
Date: Thu, 14 Mar 2019 21:28:58 +0100
Subject: [PATCH] Throw a CAN error if we have duplicate node IDs

---
 Firmware/communication/can_simple.cpp    |  9 ++++++++-
 Firmware/communication/interface_can.cpp |  3 +++
 Firmware/communication/interface_can.hpp | 12 ++++++++++++
 3 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp
index 093cff0c..538e8d9e 100644
--- a/Firmware/communication/can_simple.cpp
+++ b/Firmware/communication/can_simple.cpp
@@ -28,7 +28,14 @@ void CANSimple::handle_can_message(CAN_message_t& msg) {
     for (uint8_t i = 0; i < AXIS_COUNT; i++) {
         if (axes[i]->config_.can_node_id == nodeID) {
             axis = axes[i];
-            validAxis = true;
+            if (!validAxis) {
+                validAxis = true;
+            } else {
+                // Duplicate can IDs, don't assign to any axis
+                odCAN->set_error(ODriveCAN::ERROR_DUPLICATE_CAN_IDS);
+                validAxis = false;
+                break;
+            }
         }
     }
 
diff --git a/Firmware/communication/interface_can.cpp b/Firmware/communication/interface_can.cpp
index af1af08a..cc8a3745 100644
--- a/Firmware/communication/interface_can.cpp
+++ b/Firmware/communication/interface_can.cpp
@@ -171,6 +171,9 @@ void ODriveCAN::reinit_can() {
         status = HAL_CAN_ActivateNotification(handle_, CAN_IT_RX_FIFO0_MSG_PENDING);
 }
 
+void ODriveCAN::set_error(Error_t error){
+    error_ |= error;
+}
 // This function is called by each axis.
 // It provides an abstraction from the specific CAN protocol in use
 void ODriveCAN::send_heartbeat(Axis *axis) {
diff --git a/Firmware/communication/interface_can.hpp b/Firmware/communication/interface_can.hpp
index 4105592b..2690559b 100644
--- a/Firmware/communication/interface_can.hpp
+++ b/Firmware/communication/interface_can.hpp
@@ -37,16 +37,25 @@ class ODriveCAN {
         CAN_Protocol_t protocol = CAN_PROTOCOL_SIMPLE;
     };
 
+    enum Error_t {
+        ERROR_NONE = 0x00,
+        ERROR_DUPLICATE_CAN_IDS = 0x01
+    };
+
     ODriveCAN(CAN_HandleTypeDef *handle, ODriveCAN::Config_t &config);
 
     // Thread Relevant Data
     osThreadId thread_id_;
+    Error_t error_ = ERROR_NONE;
+
     volatile bool thread_id_valid_ = false;
     bool start_can_server();
     void can_server_thread();
     void send_heartbeat(Axis *axis);
     void reinit_can();
 
+    void set_error(Error_t error);
+
     // I/O Functions
     uint32_t available();
     uint32_t write(CAN_message_t &txmsg);
@@ -55,6 +64,7 @@ class ODriveCAN {
     // Communication Protocol Handling
     auto make_protocol_definitions() {
         return make_protocol_member_list(
+            make_protocol_property("error", &error_),
             make_protocol_object("config",
                                  make_protocol_ro_property("baud_rate", &config_.baud)),
             make_protocol_property("can_protocol", &config_.protocol),
@@ -68,4 +78,6 @@ class ODriveCAN {
     void set_baud_rate(uint32_t baudRate);
 };
 
+DEFINE_ENUM_FLAG_OPERATORS(ODriveCAN::Error_t)
+
 #endif  // __INTERFACE_CAN_HPP