From d3105cfcaf22f63d03aa19b5633a3bd31c4aeb65 Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Tue, 15 May 2018 14:53:43 -0700
Subject: [PATCH 1/2] expose PWM mappings as endpoint_ref

---
 Firmware/MotorControl/low_level.cpp      |  8 +---
 Firmware/MotorControl/main.cpp           |  5 ++-
 Firmware/MotorControl/odrive_main.h      |  4 +-
 Firmware/communication/communication.cpp |  5 +++
 Firmware/communication/protocol.cpp      | 12 +++++
 Firmware/communication/protocol.hpp      | 56 +++++++++++++++++++++---
 6 files changed, 75 insertions(+), 15 deletions(-)

diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp
index 2a870a01..8fe52c57 100644
--- a/Firmware/MotorControl/low_level.cpp
+++ b/Firmware/MotorControl/low_level.cpp
@@ -486,7 +486,7 @@ void pwm_in_init() {
     GPIO_InitStruct.Alternate = GPIO_AF2_TIM5;
 
     for (int i = 1; i <= 4; ++i) {
-        if (board_config.pwm_mappings[i].endpoint) {
+        if (is_endpoint_ref_valid(board_config.pwm_mappings[i].endpoint)) {
             GPIO_InitStruct.Pin = get_gpio_pin_by_pin(i);
             HAL_GPIO_Init(get_gpio_port_by_pin(i), &GPIO_InitStruct);
             HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(i));
@@ -513,11 +513,7 @@ void handle_pulse(int gpio_num, uint32_t high_time) {
     float value = board_config.pwm_mappings[gpio_num].min +
                   (fraction * (board_config.pwm_mappings[gpio_num].max - board_config.pwm_mappings[gpio_num].min));
 
-    uint32_t endpoint_id = board_config.pwm_mappings[gpio_num].endpoint;
-    if (endpoint_id >= n_endpoints_)
-        return;
-
-    Endpoint* endpoint = endpoints_[endpoint_id];
+    Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num].endpoint);
     if (!endpoint)
         return;
 
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index e26cadfd..28ab8ce4 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -116,8 +116,6 @@ int odrive_main(void) {
         axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
                 *encoder, *sensorless_estimator, *controller, *motor);
     }
-    
-    pwm_in_init();
 
     // TODO: make dynamically reconfigurable
 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
@@ -131,6 +129,9 @@ int odrive_main(void) {
     // Init communications (this requires the axis objects to be constructed)
     init_communication();
 
+    // must happen after communication is initialized
+    pwm_in_init();
+
     // Setup hardware for all components
     for (size_t i = 0; i < AXIS_COUNT; ++i) {
         axes[i]->setup();
diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h
index b53249b1..3f210983 100644
--- a/Firmware/MotorControl/odrive_main.h
+++ b/Firmware/MotorControl/odrive_main.h
@@ -2,6 +2,7 @@
 #define __ODRIVE_MAIN_H
 
 #ifdef __cplusplus
+#include <communication/protocol.hpp>
 extern "C" {
 #endif
 
@@ -52,7 +53,7 @@ extern SystemStats_t system_stats_;
 }
 
 struct PWMMapping_t {
-    uint32_t endpoint = 0;
+    endpoint_ref_t endpoint = { 0 };
     float min = 0;
     float max = 0;
 };
@@ -95,7 +96,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
 
 
 // ODrive specific includes
-#include <communication/protocol.hpp>
 #include <utils.h>
 #include <low_level.h>
 #include <encoder.hpp>
diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp
index 3b74a3e4..811db943 100644
--- a/Firmware/communication/communication.cpp
+++ b/Firmware/communication/communication.cpp
@@ -63,6 +63,7 @@ const uint8_t fw_version_revision = FW_VERSION_REVISION;
 const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise
 
 osThreadId comm_thread;
+volatile bool endpoint_list_valid = false;
 
 static uint32_t test_property = 0;
 
@@ -84,6 +85,9 @@ void init_communication(void) {
     // Start command handling thread
     osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues
     comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL);
+
+    while (!endpoint_list_valid)
+        osDelay(1);
 }
 
 
@@ -179,6 +183,7 @@ void communication_task(void * ctx) {
     auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree());
     auto endpoint_provider = EndpointProvider_from_MemberList<tree_type>(*tree_ptr);
     set_application_endpoints(&endpoint_provider);
+    endpoint_list_valid = true;
     
     serve_on_uart();
     serve_on_usb();
diff --git a/Firmware/communication/protocol.cpp b/Firmware/communication/protocol.cpp
index 50e38fab..078f4290 100644
--- a/Firmware/communication/protocol.cpp
+++ b/Firmware/communication/protocol.cpp
@@ -237,3 +237,15 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_
 
     return 0;
 }
+
+bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref) {
+    return (endpoint_ref.json_crc == json_crc_)
+        && (endpoint_ref.endpoint_id < n_endpoints_);
+}
+
+Endpoint* get_endpoint(endpoint_ref_t endpoint_ref) {
+    if (is_endpoint_ref_valid(endpoint_ref))
+        return endpoints_[endpoint_ref.endpoint_id];
+    else
+        return nullptr;
+}
diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp
index cad17d35..01d5af13 100644
--- a/Firmware/communication/protocol.hpp
+++ b/Firmware/communication/protocol.hpp
@@ -36,7 +36,15 @@ constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently n
 // Maximum time we allocate for processing and responding to a request
 constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10;
 
-template<typename T>
+
+typedef struct {
+    uint16_t json_crc;
+    uint16_t node_id;
+    uint16_t endpoint_id;
+} endpoint_ref_t;
+
+
+template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
 inline size_t write_le(T value, uint8_t* buffer);
 
 template<typename T>
@@ -100,6 +108,12 @@ inline size_t write_le<float>(float value, uint8_t* buffer) {
     return write_le<uint32_t>(*value_as_uint32, buffer);
 }
 
+template<typename T>
+typename std::enable_if_t<std::is_const<T>::value, size_t>
+write_le(T value, uint8_t* buffer) {
+    return write_le<std::remove_const_t<T>>(value, buffer);
+}
+
 template<>
 inline size_t read_le<bool>(bool* value, const uint8_t* buffer) {
     *value = buffer[0];
@@ -320,7 +334,29 @@ typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length,
 
 
 template<typename T>
-void default_readwrite_endpoint_handler(const T* value, const uint8_t* input, size_t input_length, StreamSink* output) {
+void default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* input, size_t input_length, StreamSink* output) {
+    constexpr size_t size = sizeof(value->endpoint_id) + sizeof(value->json_crc);
+    if (output) {
+        // TODO: make buffer size dependent on the type
+        uint8_t buffer[size];
+        size_t cnt = write_le<decltype(value->endpoint_id)>(value->endpoint_id, buffer);
+        cnt += write_le<decltype(value->json_crc)>(value->json_crc, buffer + cnt);
+        if (cnt <= output->get_free_space())
+            output->process_bytes(buffer, cnt);
+    }
+    
+    // If a new value was passed, call the corresponding little endian deserialization function
+    if (input_length >= size) {
+        read_le<decltype(value->endpoint_id)>(&value->endpoint_id, input);
+        read_le<decltype(value->json_crc)>(&value->json_crc, input + 2);
+    }
+}
+
+
+// @brief Default endpoint handler for const types
+template<typename T>
+std::enable_if_t<!std::is_same<T, endpoint_ref_t>::value && std::is_const<T>::value>
+default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) {
     // If the old value was requested, call the corresponding little endian serialization function
     if (output) {
         // TODO: make buffer size dependent on the type
@@ -331,10 +367,12 @@ void default_readwrite_endpoint_handler(const T* value, const uint8_t* input, si
     }
 }
 
+// @brief Default endpoint handler for non-const types
 template<typename T>
-void default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) {
+std::enable_if_t<!std::is_same<T, endpoint_ref_t>::value && !std::is_const<T>::value>
+default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) {
     // Read the endpoint value into output
-    default_readwrite_endpoint_handler<T>(const_cast<const T*>(value), input, input_length, output);
+    default_readwrite_endpoint_handler<const T>(const_cast<const T*>(value), input, input_length, output);
     
     // If a new value was passed, call the corresponding little endian deserialization function
     uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type
@@ -403,6 +441,10 @@ template<>
 inline constexpr const char* get_default_json_modifier<bool>() {
     return "\"type\":\"bool\",\"access\":\"rw\"";
 }
+template<>
+inline constexpr const char* get_default_json_modifier<endpoint_ref_t>() {
+    return "\"type\":\"endpoint_ref\",\"access\":\"rw\"";
+}
 
 class Endpoint {
 public:
@@ -720,7 +762,7 @@ public:
             list[id] = this;
     }
     void handle(const uint8_t* input, size_t input_length, StreamSink* output) {
-        default_readwrite_endpoint_handler(property_, input, input_length, output);
+        default_readwrite_endpoint_handler<TProperty>(property_, input, input_length, output);
     }
     /*void handle(const uint8_t* input, size_t input_length, StreamSink* output) {
         handle(input, input_length, output);
@@ -975,5 +1017,9 @@ extern Endpoint* endpoints_[];
 extern size_t n_endpoints_;
 extern const size_t max_endpoints_;
 extern EndpointProvider* application_endpoints;
+extern uint16_t json_crc_;
+
+bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref);
+Endpoint* get_endpoint(endpoint_ref_t endpoint_ref);
 
 #endif

From 5159aca1bd719efe69b295deba3079dbe00da91a Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Tue, 15 May 2018 16:15:11 -0700
Subject: [PATCH 2/2] python support for sending/receiving endpoint refs

---
 tools/odrive/remote_object.py | 116 ++++++++++++++++++++++------------
 1 file changed, 75 insertions(+), 41 deletions(-)

diff --git a/tools/odrive/remote_object.py b/tools/odrive/remote_object.py
index 91869642..0d87dd63 100644
--- a/tools/odrive/remote_object.py
+++ b/tools/odrive/remote_object.py
@@ -8,14 +8,29 @@ import struct
 import threading
 import odrive.protocol
 
-#class ObjectDisappearedError(Exception):
-#    def __init__(self, channel):
-#        self._obj = obj
-#    pass
 
 class ObjectDefinitionError(Exception):
     pass
 
+codecs = {}
+
+class StructCodec():
+    """
+    Generic serializer/deserializer based on struct pack
+    """
+    def __init__(self, struct_format, target_type):
+        self._struct_format = struct_format
+        self._target_type = target_type
+    def get_length(self):
+        return struct.calcsize(self._struct_format)
+    def serialize(self, value):
+        value = self._target_type(value)
+        return struct.pack(self._struct_format, value)
+    def deserialize(self, buffer):
+        value = struct.unpack(self._struct_format, buffer)
+        value = value[0] if len(value) == 1 else value
+        return self._target_type(value)
+
 class RemoteProperty():
     """
     Used internally by dynamically created objects to translate
@@ -24,6 +39,7 @@ class RemoteProperty():
     """
     def __init__(self, json_data, parent):
         self._parent = parent
+        self.__channel__ = parent.__channel__
         id_str = json_data.get("id", None)
         if id_str is None:
             raise ObjectDefinitionError("unspecified endpoint ID")
@@ -37,51 +53,28 @@ class RemoteProperty():
         if type_str is None:
             raise ObjectDefinitionError("unspecified type")
 
-        if type_str == "float":
-            self._property_type = float
-            self._struct_format = "<f"
-        elif type_str == "bool":
-            self._property_type = bool
-            self._struct_format = "<?"
-        elif type_str == "int8":
-            self._property_type = int
-            self._struct_format = "<b"
-        elif type_str == "uint8":
-            self._property_type = int
-            self._struct_format = "<B"
-        elif type_str == "int16":
-            self._property_type = int
-            self._struct_format = "<h"
-        elif type_str == "uint16":
-            self._property_type = int
-            self._struct_format = "<H"
-        elif type_str == "int32":
-            self._property_type = int
-            self._struct_format = "<i"
-        elif type_str == "uint32":
-            self._property_type = int
-            self._struct_format = "<I"
-        elif type_str == "int64":
-            self._property_type = int
-            self._struct_format = "<q"
-        elif type_str == "uint64":
-            self._property_type = int
-            self._struct_format = "<Q"
-        else:
-            raise ObjectDefinitionError("unsupported type {}".format(type_str))
+        # Find all codecs that match the type_str and build a dictionary
+        # of the form {type1: codec1, type2: codec2}
+        eligible_types = {k: v[type_str] for (k,v) in codecs.items() if type_str in v}
+        
+        if not eligible_types:
+            raise ObjectDefinitionError("unsupported codec {}".format(type_str))
+
+        # TODO: better heuristics to select a matching type (i.e. prefer non lossless)
+        eligible_types = list(eligible_types.items())
+        self._property_type = eligible_types[0][0]
+        self._codec = eligible_types[0][1]
 
         access_mode = json_data.get("access", "r")
         self._can_read = 'r' in access_mode
         self._can_write = 'w' in access_mode
 
     def get_value(self):
-        size = struct.calcsize(self._struct_format)
-        buffer = self._parent.__channel__.remote_endpoint_operation(self._id, None, True, size)
-        return struct.unpack(self._struct_format, buffer)[0]
+        buffer = self._parent.__channel__.remote_endpoint_operation(self._id, None, True, self._codec.get_length())
+        return self._codec.deserialize(buffer)
 
     def set_value(self, value):
-        value = self._property_type(value)
-        buffer = struct.pack(self._struct_format, value)
+        buffer = self._codec.serialize(value)
         # TODO: Currenly we wait for an ack here. Settle on the default guarantee.
         self._parent.__channel__.remote_endpoint_operation(self._id, buffer, True, 0)
 
@@ -96,6 +89,47 @@ class RemoteProperty():
             val_str = str(self.get_value())
         return "{} = {} ({})".format(self._name, val_str, self._property_type.__name__)
 
+class EndpointRefCodec():
+    """
+    Serializer/deserializer for an endpoint reference
+    """
+    def get_length(self):
+        return struct.calcsize("<HH")
+    def serialize(self, value):
+        if value is None:
+            (ep_id, ep_crc) = (0, 0)
+        elif isinstance(value, RemoteProperty):
+            (ep_id, ep_crc) = (value._id, value.__channel__._interface_definition_crc)
+        else:
+            raise TypeError("Expected value of type RemoteProperty or None but got '{}'. En example for a RemoteProperty is this expression: odrv0.axis0.controller._remote_attributes['pos_setpoint']".format(type(value).__name__))
+        return struct.pack("<HH", ep_id, ep_crc)
+    def deserialize(self, buffer):
+        return struct.unpack("<HH", buffer)
+
+codecs[int] = {
+    'int8': StructCodec("<b", int),
+    'uint8': StructCodec("<B", int),
+    'int16': StructCodec("<h", int),
+    'uint16': StructCodec("<H", int),
+    'int32': StructCodec("<i", int),
+    'uint32': StructCodec("<I", int),
+    'int64': StructCodec("<q", int),
+    'uint64': StructCodec("<Q", int)
+}
+
+codecs[bool] = {
+    'bool': StructCodec("<?", bool)
+}
+
+codecs[float] = {
+    'float': StructCodec("<f", float)
+}
+
+codecs[RemoteProperty] = {
+    'endpoint_ref': EndpointRefCodec()
+}
+
+
 class RemoteFunction(object):
     """
     Represents a callable function that maps to a function call on a remote object