diff --git a/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h
index 0028db5d..3a55d727 100644
--- a/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h
+++ b/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h
@@ -102,7 +102,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 7 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)15360)
+#define configTOTAL_HEAP_SIZE                    ((size_t)15360*3)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configUSE_16_BIT_TICKS                   0
 #define configUSE_MUTEXES                        1
diff --git a/Firmware/Board/v3.3/Src/freertos.c b/Firmware/Board/v3.3/Src/freertos.c
index 06caaf74..a6ccbf25 100644
--- a/Firmware/Board/v3.3/Src/freertos.c
+++ b/Firmware/Board/v3.3/Src/freertos.c
@@ -121,7 +121,7 @@ void MX_FREERTOS_Init(void) {
 
   /* Create the thread(s) */
   /* definition and creation of defaultTask */
-  osThreadDef(defaultTask, StartDefaultTask, osPriorityIdle, 0, 256);
+  osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 256);
   defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
 
   /* USER CODE BEGIN RTOS_THREADS */
diff --git a/Firmware/Board/v3.3/Src/syscalls.c b/Firmware/Board/v3.3/Src/syscalls.c
index d546266c..3ddb5701 100644
--- a/Firmware/Board/v3.3/Src/syscalls.c
+++ b/Firmware/Board/v3.3/Src/syscalls.c
@@ -23,7 +23,6 @@
 static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE];
 
 int _write(int file, char* data, int len) {
-#if 0 // TODO: revert!
     //number of bytes written
     int written = 0;
     switch (serial_printf_select) {
@@ -58,8 +57,6 @@ int _write(int file, char* data, int len) {
     }
 
     return written;
-#endif
-    return len;
 }
 
 void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
diff --git a/Firmware/Board/v3.3/Src/usbd_cdc_if.c b/Firmware/Board/v3.3/Src/usbd_cdc_if.c
index ff11152b..b38385f7 100644
--- a/Firmware/Board/v3.3/Src/usbd_cdc_if.c
+++ b/Firmware/Board/v3.3/Src/usbd_cdc_if.c
@@ -274,7 +274,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len)
 {
   /* USER CODE BEGIN 6 */
 
-  //set_cmd_buffer(Buf, *Len); TODO: revert!
+  set_cmd_buffer(Buf, *Len);
   osSemaphoreRelease(sem_usb_rx);
 
   return (USBD_OK);
diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp
index 72931eeb..ff52a51b 100644
--- a/Firmware/MotorControl/axis.hpp
+++ b/Firmware/MotorControl/axis.hpp
@@ -143,10 +143,12 @@ public:
     Controller& controller;
     Motor& motor;
 
-    Error_t error = ERROR_NO_ERROR;
     osThreadId thread_id;
     volatile bool thread_id_valid = false;
-    bool enable_step_dir = false; // auto enabled after calibration, based on enable_step_dir_after_calibration
+
+    // variables exposed on protocol
+    Error_t error = ERROR_NO_ERROR;
+    bool enable_step_dir = false; // auto enabled after calibration, based on config.enable_step_dir
     AxisState_t current_state = AXIS_STATE_STARTUP;
     AxisState_t requested_state = AXIS_STATE_DONT_CARE;
     uint32_t loop_counter = 0;
diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h
index 0d9d7c87..2c82ad88 100644
--- a/Firmware/MotorControl/commands.h
+++ b/Firmware/MotorControl/commands.h
@@ -35,9 +35,9 @@ extern "C" {
 #endif
 
 void init_communication(void);
-void communication_task(void const * argument);
+void communication_task(void * ctx);
 void set_cmd_buffer(uint8_t *buf, uint32_t len);
-void usb_update_thread();
+void usb_update_thread(void * ctx);
 void USB_receive_packet(const uint8_t *buffer, size_t length);
 
 #ifdef __cplusplus
diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp
index 8b168083..df6e22e9 100644
--- a/Firmware/MotorControl/controller.hpp
+++ b/Firmware/MotorControl/controller.hpp
@@ -18,6 +18,7 @@ struct ControllerConfig_t {
     Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL;  //see: Motor_control_mode_t
     float pos_gain = 20.0f;  // [(counts/s) / counts]
     float vel_gain = 5.0f / 10000.0f;  // [A/(counts/s)]
+    // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
     float vel_integrator_gain = 10.0f / 10000.0f;  // [A/(counts/s * s)]
     float vel_limit = 20000.0f;           // [counts/s]
 };
@@ -38,12 +39,11 @@ public:
     ControllerConfig_t& config;
     Axis* axis = nullptr; // set by Axis constructor
 
-    float pos_setpoint = 0.0f;
-    float vel_setpoint = 0.0f;
-    // float vel_setpoint = 800.0f; <sensorless example>
-    // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
-    float vel_integrator_current = 0.0f;  // [A]
-    float current_setpoint = 0.0f;        // [A]
+    // TODO: anticogging overhaul:
+    // - expose selected (all?) variables on protocol
+    // - make calibration user experience similar to motor & encoder calibration
+    // - use python tools to Fourier transform and write back the smoothed map or Fourier coefficients
+    // - make the calibration persistent
 
     typedef struct {
         int index;
@@ -62,6 +62,13 @@ public:
         .calib_vel_threshold = 1.0f,
     };
 
+    // variables exposed on protocol
+    float pos_setpoint = 0.0f;
+    float vel_setpoint = 0.0f;
+    // float vel_setpoint = 800.0f; <sensorless example>
+    float vel_integrator_current = 0.0f;  // [A]
+    float current_setpoint = 0.0f;        // [A]
+
     // Cache for remote procedure calls arguments TODO: remove
     struct {
         float pos_setpoint; 
diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp
index 1ad2b258..f4d81da7 100644
--- a/Firmware/MotorControl/encoder.cpp
+++ b/Firmware/MotorControl/encoder.cpp
@@ -57,9 +57,9 @@ void Encoder::set_count(uint32_t count) {
 // TODO: add check_timing
 bool Encoder::calib_enc_offset(float voltage_magnitude) {
     static const float start_lock_duration = 1.0f;
-    static const float scan_duration = 1.0f;
-    static const float scan_range = 16.0f * M_PI;
-    static const size_t num_steps = scan_duration * current_meas_hz;
+    static const float scan_omega = 4.0f * M_PI;
+    static const float scan_distance = 16.0f * M_PI;
+    static const size_t num_steps = scan_distance / scan_omega * current_meas_hz;
 
     // go to motor zero phase for start_lock_duration to get ready to scan
     size_t i = 0;
@@ -76,7 +76,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) {
     // scan forward
     i = 0;
     axis->run_control_loop([&](){
-        float phase = wrap_pm_pi(scan_range * (float)i / (float)num_steps - scan_range / 2.0f);
+        float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
         float v_alpha = voltage_magnitude * arm_cos_f32(phase);
         float v_beta = voltage_magnitude * arm_sin_f32(phase);
         axis->motor.enqueue_voltage_timings(v_alpha, v_beta);
@@ -90,7 +90,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) {
 
     //TODO avoid recomputing elec_rad_per_enc every time
     float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr));
-    float expected_encoder_delta = scan_range / elec_rad_per_enc;
+    float expected_encoder_delta = scan_distance / elec_rad_per_enc;
     float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val);
     if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range)
     {
@@ -113,7 +113,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) {
     // scan backwards
     i = 0;
     axis->run_control_loop([&](){
-        float phase = wrap_pm_pi(-scan_range * (float)i / (float)num_steps + scan_range / 2.0f);
+        float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
         float v_alpha = voltage_magnitude * arm_cos_f32(phase);
         float v_beta = voltage_magnitude * arm_sin_f32(phase);
         axis->motor.enqueue_voltage_timings(v_alpha, v_beta);
diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c
index 87df7140..29a6c33a 100644
--- a/Firmware/MotorControl/legacy_commands.c
+++ b/Firmware/MotorControl/legacy_commands.c
@@ -1,7 +1,7 @@
 /* Includes ------------------------------------------------------------------*/
 #include "legacy_commands.h"
 #include <utils.h>
-#if 0
+
 /* Private macros ------------------------------------------------------------*/
 /* Private typedef -----------------------------------------------------------*/
 /* Global constant data ------------------------------------------------------*/
@@ -10,7 +10,7 @@
 // recently recieved a command. In the future we may want to separate
 // debug printf and the main serial comms.
 SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART;
-
+#if 0
 /* Private constant data -----------------------------------------------------*/
 
 // variables exposed to usb/serial interface via set/get/monitor
diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp
index b6a10061..6dde3afd 100644
--- a/Firmware/MotorControl/main.cpp
+++ b/Firmware/MotorControl/main.cpp
@@ -23,7 +23,7 @@ void save_configuration(void) {
     }
 }
 
-void load_configuration() {
+void load_configuration(void) {
     if (NVM_init() ||
         ConfigFormat::safe_load_config(
                 &axis_configs,
@@ -65,24 +65,17 @@ int odrive_main(void) {
     }
     
     // TODO: make dynamically reconfigurable
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
     if (enable_uart) {
         axes[0]->config.enable_step_dir = false;
         axes[0]->set_step_dir_enabled(false);
         SetGPIO12toUART();
     }
-/*
+#endif
+    //osDelay(100);
     // Init communications (this requires the axis objects to be constructed)
     init_communication();
 
-    // Start command handling thread
-    osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512);
-    thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL);
-
-    // Start USB interrupt handler thread
-    osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512);
-    thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL);
-    */
-
     // Setup hardware for all components
     for (size_t i = 0; i < AXIS_COUNT; ++i) {
         axes[i]->setup();
diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp
index b2029271..877921f9 100644
--- a/Firmware/MotorControl/motor.hpp
+++ b/Firmware/MotorControl/motor.hpp
@@ -94,21 +94,20 @@ public:
 //private:
 
     DRV8301_Obj gate_driver; // initialized in constructor
-
-    Error_t error = ERROR_NO_ERROR;
-    // bool enable_control = true; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true
-    // bool do_calibration = true; //  trigger motor calibration. will be reset to false after self test
-    // bool calibration_ok = false;
     uint16_t next_timings[3] = {
         TIM_1_8_PERIOD_CLOCKS / 2,
         TIM_1_8_PERIOD_CLOCKS / 2,
         TIM_1_8_PERIOD_CLOCKS / 2
     };
     uint16_t last_cpu_time = 0;
+    int timing_log_index = 0;
+    uint16_t timing_log[TIMING_LOG_SIZE] = { 0 };
+
+    // variables exposed on protocol
+    Error_t error = ERROR_NO_ERROR;
     Iph_BC_t current_meas = {0.0f, 0.0f};
     Iph_BC_t DC_calib = {0.0f, 0.0f};
-    DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup)
-    float shunt_conductance = 1.0f / SHUNT_RESISTANCE;  //[S]
+    const float shunt_conductance = 1.0f / SHUNT_RESISTANCE;  //[S]
     float phase_current_rev_gain = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
     Current_control_t current_control = {
         .p_gain = 0.0f,        // [V/A] should be auto set after resistance and inductance measurement
@@ -122,9 +121,50 @@ public:
         .Iq_measured = 0.0f,
         .max_allowed_current = 0.0f,
     };
-    int timing_log_index = 0;
-    uint16_t timing_log[TIMING_LOG_SIZE] = { 0 };
     DRV8301_FaultType_e drv_fault = DRV8301_FaultType_NoFault;
+    DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup)
+
+    // Communication protocol definitions
+    auto make_protocol_definitions() {
+        return make_protocol_member_list(
+            make_protocol_property("error", reinterpret_cast<int32_t*>(&this->error)),
+            make_protocol_ro_property("current_meas.phB", &this->current_meas.phB),
+            make_protocol_ro_property("current_meas.phC", &this->current_meas.phC),
+            make_protocol_property("DC_calib.phB", &this->DC_calib.phB),
+            make_protocol_property("DC_calib.phC", &this->DC_calib.phC),
+            make_protocol_property("shunt_conductance", &this->shunt_conductance),
+            make_protocol_property("phase_current_rev_gain", &this->phase_current_rev_gain),
+            make_protocol_object("current_control",
+                make_protocol_property("p_gain", &this->current_control.p_gain),
+                make_protocol_property("i_gain", &this->current_control.i_gain),
+                make_protocol_property("v_current_control_integral_d", &this->current_control.v_current_control_integral_d),
+                make_protocol_property("v_current_control_integral_q", &this->current_control.v_current_control_integral_q),
+                make_protocol_property("Ibus", &this->current_control.Ibus),
+                make_protocol_property("final_v_alpha", &this->current_control.final_v_alpha),
+                make_protocol_property("final_v_beta", &this->current_control.final_v_beta),
+                make_protocol_property("Iq_setpoint", &this->current_control.Iq_setpoint),
+                make_protocol_property("Iq_measured", &this->current_control.Iq_measured),
+                make_protocol_property("max_allowed_current", &this->current_control.max_allowed_current)
+            ),
+            make_protocol_object("gate_driver",
+                make_protocol_ro_property("drv_fault", reinterpret_cast<int32_t*>(&this->drv_fault)),
+                make_protocol_ro_property("status_reg_1", &this->gate_driver_regs.Stat_Reg_1_Value),
+                make_protocol_ro_property("status_reg_2", &this->gate_driver_regs.Stat_Reg_2_Value),
+                make_protocol_ro_property("ctrl_reg_1", &this->gate_driver_regs.Ctrl_Reg_1_Value),
+                make_protocol_ro_property("ctrl_reg_2", &this->gate_driver_regs.Ctrl_Reg_2_Value)
+            ),
+            make_protocol_object("config",
+                make_protocol_property("pole_pairs", &this->config.pole_pairs),
+                make_protocol_property("calibration_current", &this->config.calibration_current),
+                make_protocol_property("resistance_calib_max_voltage", &this->config.resistance_calib_max_voltage),
+                make_protocol_property("phase_inductance", &this->config.phase_inductance),
+                make_protocol_property("phase_resistance", &this->config.phase_resistance),
+                make_protocol_property("direction", &this->config.direction),
+                make_protocol_property("motor_type", reinterpret_cast<int32_t*>(&this->config.motor_type)),
+                make_protocol_property("current_lim", &this->config.current_lim)
+            )
+        );
+    }
 };
 
 #endif // __MOTOR_HPP
diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp
index 0f927d01..e9bb285d 100644
--- a/Firmware/MotorControl/odrive_main.hpp
+++ b/Firmware/MotorControl/odrive_main.hpp
@@ -35,6 +35,7 @@ extern Axis *axes[AXIS_COUNT];
 
 
 // ODrive specific includes
+#include <protocol.hpp>
 #include <utils.h>
 #include <low_level.h>
 #include <encoder.hpp>
@@ -43,4 +44,10 @@ extern Axis *axes[AXIS_COUNT];
 #include <motor.hpp>
 #include <axis.hpp>
 
+#include <commands.h> // TODO: remove
+
+// defined in main.cpp
+void save_configuration(void);
+void erase_configuration(void);
+
 #endif /* __ODRIVE_MAIN_HPP */
diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp
index 56f9afb5..3e0336fa 100644
--- a/Firmware/MotorControl/protocol.cpp
+++ b/Firmware/MotorControl/protocol.cpp
@@ -8,16 +8,7 @@
 #include <stdlib.h>
 
 /* Private defines -----------------------------------------------------------*/
-// Note that this option cannot be used to debug UART because it prints on UART
-//#define DEGUG_PROTOCOL
 /* Private macros ------------------------------------------------------------*/
-
-#ifdef DEGUG_PROTOCOL
-#define LOG_PROTO(...)  do { printf(__VA_ARGS__); osDelay(10); } while (0)
-#else
-#define LOG_PROTO(...)  ((void) 0)
-#endif
-
 /* Private typedef -----------------------------------------------------------*/
 /* Global constant data ------------------------------------------------------*/
 /* Global variables ----------------------------------------------------------*/
@@ -46,48 +37,6 @@ void hexdump(const uint8_t* buf, size_t len) {
 }
 #endif
 
-static inline int write_string(const char* str, StreamSink* output) {
-    return output->process_bytes(reinterpret_cast<const uint8_t*>(str), strlen(str));
-}
-
-void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const {
-    if (type_ == CLOSE_TREE) {
-        write_string("]}", output);
-        *need_comma = true;
-    } else {
-        if (*need_comma)
-            write_string(",", output);
-
-        // write name
-        write_string("{\"name\":\"", output);
-        if (name_)
-            write_string(name_, output);
-
-        // write endpoint ID
-        write_string("\",\"id\":", output);
-        char id_buf[10];
-        snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
-        write_string(id_buf, output);
-
-        // write additional JSON data
-        if (json_modifier_ && json_modifier_[0]) {
-            write_string(",", output);
-            write_string(json_modifier_, output);
-        }
-
-        if (type_ == BEGIN_OBJECT) {
-            write_string(",\"members\":[", output);
-            *need_comma = false;
-        } else if (type_ == BEGIN_FUNCTION) {
-            write_string(",\"arguments\":[", output);
-            *need_comma = false;
-        } else if (type_ == PROPERTY) {
-            write_string("}", output);
-            *need_comma = true;
-        }
-    }
-}
-
 
 
 int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) {
@@ -157,36 +106,80 @@ int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length
 }
 
 
-// Calculates the CRC16 of the JSON interface descriptor.
-// The init value is the protocol version.
-uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) {
-    CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
+class JSONDescriptorEndpoint : Endpoint {
+public:
+    static constexpr size_t endpoint_count = 1;
+    void write_json(size_t id, StreamSink* output);
+    void register_endpoints(Endpoint** list, size_t id, size_t length);
+    void handle(const uint8_t* input, size_t input_length, StreamSink* output);
+};
 
-    uint8_t offset[4] = { 0 };
-    interface_query(offset, sizeof(offset), &crc16_calculator);
+JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint();
+EndpointProvider* application_endpoints;
+uint16_t json_crc_;
 
-    return crc16_calculator.get_crc16();
+Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 };
+size_t n_endpoints_ = 0;
+EndpointProvider* endpoint_provider_ = nullptr;
+
+void JSONDescriptorEndpoint::write_json(size_t id, StreamSink* output) {
+    write_string("{\"name\":\"\",", output);
+
+    // write endpoint ID
+    write_string("\"id\":", output);
+    char id_buf[10];
+    snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
+    write_string(id_buf, output);
+
+    write_string(",\"type\":\"json\",\"access\":\"r\"}", output);
 }
 
+void JSONDescriptorEndpoint::register_endpoints(Endpoint** list, size_t id, size_t length) {
+    if (id < length)
+        list[id] = this;
+    
+};
+
 // Returns part of the JSON interface definition.
-void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) {
+void JSONDescriptorEndpoint::handle(const uint8_t* input, size_t input_length, StreamSink* output) {
     // The request must contain a 32 bit integer to specify an offset
     if (input_length < 4)
         return;
     uint32_t offset = 0;
     read_le<uint32_t>(&offset, input);
     NullStreamSink output_with_offset = NullStreamSink(offset, *output);
-    
-    bool need_comma = false;
+
+    size_t id = 0;
     write_string("[", &output_with_offset);
-    for (size_t i = 0; i < n_endpoints_; ++i) {
-        get_endpoint(i)->write_json(i, &need_comma, &output_with_offset);
-        if (!output->get_free_space())
-            return; // return early if the output cannot take more bytes
-    }
+    json_file_endpoint.write_json(id, &output_with_offset);
+    id += decltype(json_file_endpoint)::endpoint_count;
+    write_string(",", &output_with_offset);
+    application_endpoints->write_json(id, &output_with_offset);
     write_string("]", &output_with_offset);
 }
 
+void set_application_endpoints(EndpointProvider* endpoints) {
+    application_endpoints = endpoints;
+
+    n_endpoints_ = 0;
+    json_file_endpoint.register_endpoints(endpoints_, 0, MAX_ENDPOINTS);
+    n_endpoints_ += decltype(json_file_endpoint)::endpoint_count;
+    application_endpoints->register_endpoints(endpoints_, n_endpoints_, MAX_ENDPOINTS);
+    n_endpoints_ += application_endpoints->get_endpoint_count();
+    
+    // Calculates the CRC16 of the JSON file.
+    // The init value is the protocol version.
+    CRC16Calculator crc16_calculator(PROTOCOL_VERSION);
+    uint8_t offset[4] = { 0 };
+    json_file_endpoint.handle(offset, sizeof(offset), &crc16_calculator);
+    json_crc_ = crc16_calculator.get_crc16();
+
+
+    CRC16Calculator crc16_calculator2(PROTOCOL_VERSION);
+    endpoints_[0]->handle(offset, sizeof(offset), &crc16_calculator2);
+    json_crc_ = crc16_calculator2.get_crc16();
+}
+
 int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) {
     LOG_PROTO("got packet of length %d: \r\n", length);
     hexdump(buffer, length);
@@ -205,10 +198,15 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_
         bool expect_response = endpoint_id & 0x8000;
         endpoint_id &= 0x7fff;
 
-        const Endpoint* endpoint = get_endpoint(endpoint_id);
-        if (!endpoint)
+        if (endpoint_id >= n_endpoints_)
             return -1;
 
+        Endpoint* endpoint = endpoints_[endpoint_id];
+        if (!endpoint) {
+            LOG_PROTO("critical: no endpoint at %d", endpoint_id);
+            return -1;
+        }
+
         // Verify packet trailer. The expected trailer value depends on the selected endpoint.
         // For endpoint 0 this is just the protocol version, for all other endpoints it's a
         // CRC over the entire JSON descriptor tree (this may change in future versions).
@@ -218,7 +216,7 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_
             LOG_PROTO("trailer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_trailer, actual_trailer);
             return -1;
         }
-        LOG_PROTO("trailer ok\r\n");
+        LOG_PROTO("trailer ok for endpoint %d\r\n", endpoint_id);
 
         // TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible?
 
diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp
index 8b2e1edd..c4e1652f 100644
--- a/Firmware/MotorControl/protocol.hpp
+++ b/Firmware/MotorControl/protocol.hpp
@@ -13,6 +13,14 @@ see protocol.md for the protocol specification
 #include <cstring>
 #include "crc.hpp"
 
+// Note that this option cannot be used to debug UART because it prints on UART
+//#define DEBUG_PROTOCOL
+#ifdef DEBUG_PROTOCOL
+#define LOG_PROTO(...)  do { printf(__VA_ARGS__); osDelay(10); } while (0)
+#else
+#define LOG_PROTO(...)  ((void) 0)
+#endif
+
 
 constexpr uint8_t SYNC_BYTE = 0xAA;
 constexpr uint8_t CRC8_INIT = 0x42;
@@ -74,7 +82,8 @@ template<>
 inline size_t write_le<float>(float value, uint8_t* buffer) {
     static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
     static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
-    return write_le<uint32_t>(*reinterpret_cast<const uint32_t*>(&value), buffer);
+    const uint32_t * value_as_uint32 = reinterpret_cast<const uint32_t*>(&value);
+    return write_le<uint32_t>(*value_as_uint32, buffer);
 }
 
 template<>
@@ -268,15 +277,6 @@ private:
 };
 
 
-
-typedef enum {
-    PROPERTY,
-    BEGIN_OBJECT,
-    BEGIN_FUNCTION,
-    CLOSE_TREE
-} EndpointType_t;
-
-
 // @brief Endpoint request handler
 //
 // When passed a valid endpoint context, implementing functions shall handle an
@@ -293,8 +293,7 @@ typedef std::function<void(void* ctx, const uint8_t* input, size_t input_length,
 
 
 template<typename T>
-void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
-    const T* value = reinterpret_cast<const T*>(ctx);
+void default_readwrite_endpoint_handler(const 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
@@ -306,11 +305,9 @@ void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input
 }
 
 template<typename T>
-void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
-    T* value = reinterpret_cast<T*>(ctx);
-    
+void default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) {
     // Read the endpoint value into output
-    default_read_endpoint_handler<T>(ctx, input, input_length, output);
+    default_readwrite_endpoint_handler<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
@@ -318,124 +315,80 @@ void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t
         read_le<T>(value, input);
 }
 
-static void trigger_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
-    (void) input;
-    (void) input_length;
-    (void) output;
-    std::function<void(void)> function = reinterpret_cast<void(*)()>(ctx);
-    function();
-}
 
 
 template<typename T>
 static inline const char* get_default_json_modifier();
 
 template<>
-inline const char* get_default_json_modifier<const float>() {
+inline constexpr const char* get_default_json_modifier<const float>() {
     return "\"type\":\"float\",\"access\":\"r\"";
 }
 template<>
-inline const char* get_default_json_modifier<float>() {
+inline constexpr const char* get_default_json_modifier<float>() {
     return "\"type\":\"float\",\"access\":\"rw\"";
 }
 template<>
-inline const char* get_default_json_modifier<const int32_t>() {
+inline constexpr const char* get_default_json_modifier<const int32_t>() {
     return "\"type\":\"int32\",\"access\":\"r\"";
 }
 template<>
-inline const char* get_default_json_modifier<int32_t>() {
+inline constexpr const char* get_default_json_modifier<int32_t>() {
     return "\"type\":\"int32\",\"access\":\"rw\"";
 }
 template<>
-inline const char* get_default_json_modifier<const uint32_t>() {
+inline constexpr const char* get_default_json_modifier<const uint32_t>() {
     return "\"type\":\"uint32\",\"access\":\"r\"";
 }
 template<>
-inline const char* get_default_json_modifier<uint32_t>() {
+inline constexpr const char* get_default_json_modifier<uint32_t>() {
     return "\"type\":\"uint32\",\"access\":\"rw\"";
 }
 template<>
-inline const char* get_default_json_modifier<const uint16_t>() {
+inline constexpr const char* get_default_json_modifier<const uint16_t>() {
     return "\"type\":\"uint16\",\"access\":\"r\"";
 }
 template<>
-inline const char* get_default_json_modifier<uint16_t>() {
+inline constexpr const char* get_default_json_modifier<uint16_t>() {
     return "\"type\":\"uint16\",\"access\":\"rw\"";
 }
 template<>
-inline const char* get_default_json_modifier<const uint8_t>() {
+inline constexpr const char* get_default_json_modifier<const uint8_t>() {
     return "\"type\":\"uint8\",\"access\":\"r\"";
 }
 template<>
-inline const char* get_default_json_modifier<uint8_t>() {
+inline constexpr const char* get_default_json_modifier<uint8_t>() {
     return "\"type\":\"uint8\",\"access\":\"rw\"";
 }
 template<>
-inline const char* get_default_json_modifier<const bool>() {
+inline constexpr const char* get_default_json_modifier<const bool>() {
     return "\"type\":\"bool\",\"access\":\"r\"";
 }
 template<>
-inline const char* get_default_json_modifier<bool>() {
+inline constexpr const char* get_default_json_modifier<bool>() {
     return "\"type\":\"bool\",\"access\":\"rw\"";
 }
 
+constexpr size_t MAX_ENDPOINTS = 100;
+
 class Endpoint {
 public:
-    const char* const name_;
-
-    Endpoint(const char* name, EndpointType_t type, EndpointHandler handler, const char* json_modifier, void *ctx) :
-        name_(name),
-        type_(type),
-        handler_(handler),
-        json_modifier_(json_modifier),
-        ctx_(ctx)
-    {
-    }
-
-    template<typename T>
-    static Endpoint make_property(const char* name, const T* ctx) {
-        return Endpoint(name, PROPERTY,
-            default_read_endpoint_handler<T>,
-            get_default_json_modifier<const T>(),
-            const_cast<T*>(ctx) /* it's safe to cast the const away here because we
-            know that the default_read_endpoint_handler immediately adds it back */);
-    }
-
-    template<typename T>
-    static Endpoint make_property(const char* name, T* ctx) {
-        return Endpoint(name, PROPERTY,
-            default_readwrite_endpoint_handler<T>,
-            get_default_json_modifier<T>(), ctx);
-    }
-    
-    static Endpoint make_object(const char* name) {
-        return Endpoint(name, BEGIN_OBJECT, nullptr,
-            "\"type\":\"object\"", nullptr);
-    }
-
-    static Endpoint make_function(const char* name, void(*function)(void)) {
-        return Endpoint(name, BEGIN_FUNCTION, trigger_endpoint_handler,
-            "\"type\":\"function\"", reinterpret_cast<void*>(function));
-    }
-
-    static Endpoint close_tree() {
-        return Endpoint(nullptr, CLOSE_TREE, nullptr, nullptr, nullptr);
-    }
-
-    void write_json(size_t id, bool* need_comma, StreamSink* output) const;
-
-    void handle(const uint8_t* input, size_t input_length, StreamSink* output) const {
-        if (handler_)
-            return handler_(ctx_, input, input_length, output);
-    }
-
-private:
-    const EndpointType_t type_;
-    const EndpointHandler handler_;
-    const char* json_modifier_;
-    void* const ctx_;
+    //const char* const name_;
+    virtual void handle(const uint8_t* input, size_t input_length, StreamSink* output) = 0;
 };
 
+class EndpointProvider {
+public:
+    virtual size_t get_endpoint_count() = 0;
+    virtual void write_json(size_t id, StreamSink* output) = 0;
+    virtual void register_endpoints(Endpoint** list, size_t id, size_t length) = 0;
+};
+
+
+static inline int write_string(const char* str, StreamSink* output) {
+    return output->process_bytes(reinterpret_cast<const uint8_t*>(str), strlen(str));
+}
+
 
 /* @brief Handles the communication protocol on one channel.
 *
@@ -446,55 +399,333 @@ private:
 */
 class BidirectionalPacketBasedChannel : public PacketSink {
 public:
-    BidirectionalPacketBasedChannel(const Endpoint* endpoints, size_t n_endpoints, PacketSink& output) :
-        global_endpoints_(endpoints),
-        n_endpoints_(NUM_CHANNEL_SPECIFIC_ENDPOINTS + n_endpoints),
-        output_(output),
-        json_crc_(calculate_json_crc16())
-    {
-    }
+    BidirectionalPacketBasedChannel(PacketSink& output) :
+        output_(output)
+    { }
 
     int process_packet(const uint8_t* buffer, size_t length);
-
 private:
-    
-    uint16_t calculate_json_crc16(void);
-    void interface_query(const uint8_t* input, size_t input_length, StreamSink* output);
-
-    static void interface_query_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
-        reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->interface_query(input, input_length, output);
-    }
-    
-    static void subscription_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) {
-        reinterpret_cast<BidirectionalPacketBasedChannel*>(ctx)->subscription(input, input_length, output);
-    }
-
-    const Endpoint channel_specific_endpoints_[1] = {
-        Endpoint("", PROPERTY, BidirectionalPacketBasedChannel::interface_query_handler, "\"type\":\"json\",\"access\":\"rw\"", this),
-        //Endpoint("subscriptions", PROPERTY, BidirectionalPacketBasedChannel::subscription_handler, nullptr, this)
-    };
-    static constexpr size_t NUM_CHANNEL_SPECIFIC_ENDPOINTS = sizeof(channel_specific_endpoints_) / sizeof(channel_specific_endpoints_[0]);
-    
-    const Endpoint* get_endpoint(size_t index) {
-        if (index < NUM_CHANNEL_SPECIFIC_ENDPOINTS){
-            return &channel_specific_endpoints_[index];
-        } else if (index < n_endpoints_) {
-            return &global_endpoints_[index - NUM_CHANNEL_SPECIFIC_ENDPOINTS];
-        } else {
-            return nullptr;
-        }
-    }
-
-    void subscription(const uint8_t* input, size_t input_length, StreamSink* output) {
-        // TODO: handle
-        return;
-    }
-
-    const Endpoint * const global_endpoints_;
-    size_t n_endpoints_;
     PacketSink& output_;
     uint8_t tx_buf_[TX_BUF_SIZE];
-    const uint16_t json_crc_;
 };
 
+
+template<typename ... TMembers>
+struct MemberList;
+
+template<>
+struct MemberList<> {
+public:
+    static constexpr size_t endpoint_count = 0;
+    size_t get_endpoint_count() { return endpoint_count; }
+    static constexpr bool is_empty = true;
+    void write_json(size_t id, StreamSink* output) {
+        // no action
+    }
+    void register_endpoints(Endpoint** list, size_t id, size_t length) {
+        // no action
+    }
+    std::tuple<> get_names_as_tuple() const { return std::tuple<>(); }
+};
+
+template<typename TMember, typename ... TMembers>
+struct MemberList<TMember, TMembers...> {
+public:
+    static constexpr size_t endpoint_count = TMember::endpoint_count + MemberList<TMembers...>::endpoint_count;
+    size_t get_endpoint_count() { return endpoint_count; }
+    static constexpr bool is_empty = false;
+
+    MemberList(TMember&& this_member, TMembers&&... subsequent_members) :
+        this_member_(std::forward<TMember>(this_member)),
+        subsequent_members_(std::forward<TMembers>(subsequent_members)...) {}
+
+    MemberList(TMember&& this_member, MemberList<TMembers...>&& subsequent_members) :
+        this_member_(std::forward<TMember>(this_member)),
+        subsequent_members_(std::forward(subsequent_members)) {}
+
+    // @brief Move constructor
+/*    MemberList(MemberList&& other) :
+        this_member_(std::move(other.this_member_)),
+        subsequent_members_(std::move(other.subsequent_members_)) {}*/
+
+    void write_json(size_t id, StreamSink* output) /*final*/ {
+        this_member_.write_json(id, output);
+        if (!MemberList<TMembers...>::is_empty)
+            write_string(",", output);
+        subsequent_members_.write_json(id + TMember::endpoint_count, output);
+    }
+
+    void register_endpoints(Endpoint** list, size_t id, size_t length) /*final*/ {
+        this_member_.register_endpoints(list, id, length);
+        subsequent_members_.register_endpoints(list, id + TMember::endpoint_count, length);
+    }
+
+    TMember this_member_;
+    MemberList<TMembers...> subsequent_members_;
+};
+
+template<typename ... TMembers>
+MemberList<TMembers...> make_protocol_member_list(TMembers&&... member_list) {
+    return MemberList<TMembers...>(std::forward<TMembers>(member_list)...);
+}
+
+template<typename ... TMembers>
+class ProtocolObject {
+public:
+    ProtocolObject(const char * name, TMembers&&... member_list) :
+        name_(name),
+        member_list_(std::forward<TMembers>(member_list)...) {}
+
+    static constexpr size_t endpoint_count = MemberList<TMembers...>::endpoint_count;
+
+    void write_json(size_t id, StreamSink* output) {
+        write_string("{\"name\":\"", output);
+        write_string(name_, output);
+        write_string("\",\"type\":\"object\",\"members\":[", output);
+        member_list_.write_json(id, output),
+        write_string("]}", output);
+    }
+
+    void register_endpoints(Endpoint** list, size_t id, size_t length) {
+        member_list_.register_endpoints(list, id, length);
+    }
+    
+    const char * name_;
+    MemberList<TMembers...> member_list_;
+};
+
+template<typename ... TMembers>
+ProtocolObject<TMembers...> make_protocol_object(const char * name, TMembers&&... member_list) {
+    return ProtocolObject<TMembers...>(name, std::forward<TMembers>(member_list)...);
+}
+
+template<typename TProperty>
+class ProtocolProperty : Endpoint {
+public:
+    static constexpr const char * json_modifier = get_default_json_modifier<TProperty>();
+    static constexpr size_t endpoint_count = 1;
+
+    ProtocolProperty(const char * name, TProperty* property)
+        : name_(name), property_(property)
+    {}
+
+//    ProtocolProperty(const ProtocolProperty&) = delete;
+
+    // @brief Move constructor
+    ProtocolProperty(ProtocolProperty&& other) :
+        Endpoint(std::move(other)),
+        name_(std::move(other.name_)),
+        property_(other.property_)
+    {}
+
+    //constexpr ProtocolProperty& operator=(const ProtocolProperty& other) = delete;
+    /*constexpr ProtocolProperty& operator=(const ProtocolProperty& other) {
+        //Endpoint(std::move(other)),
+        //name_(std::move(other.name_)),
+        //property_(other.property_)
+        name_ = other.name_;
+        property_ = other.property_;
+        return *this;
+    }*/
+
+    /*ProtocolProperty& operator=(ProtocolProperty&& other)
+        : name_(other.name_), property_(other.property_)
+    {}
+    ProtocolProperty& operator=(const ProtocolProperty& other)
+        : name_(other.name_), property_(other.property_)
+    {}*/
+
+    void write_json(size_t id, StreamSink* output) {
+        // write name
+        write_string("{\"name\":\"", output);
+        LOG_PROTO("json: this at %x, name at %x is s\r\n", (uintptr_t)this, (uintptr_t)name_);
+        //LOG_PROTO("json\r\n");
+        write_string(name_, output);
+
+        // write endpoint ID
+        write_string("\",\"id\":", output);
+        char id_buf[10];
+        snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
+        write_string(id_buf, output);
+
+        // write additional JSON data
+        if (json_modifier && json_modifier[0]) {
+            write_string(",", output);
+            write_string(json_modifier, output);
+        }
+
+        write_string("}", output);
+    }
+
+    void register_endpoints(Endpoint** list, size_t id, size_t length) {
+        if (id < length)
+            list[id] = this;
+    }
+    void handle(const uint8_t* input, size_t input_length, StreamSink* output) {
+        default_readwrite_endpoint_handler(property_, input, input_length, output);
+    }
+    /*void handle(const uint8_t* input, size_t input_length, StreamSink* output) {
+        handle(input, input_length, output);
+    }*/
+
+    const char * name_;
+    TProperty* property_;
+};
+
+template<typename TProperty>
+ProtocolProperty<TProperty> make_protocol_property(const char * name, TProperty* property) {
+    return ProtocolProperty<TProperty>(name, property);
+};
+
+template<typename TProperty>
+ProtocolProperty<const TProperty> make_protocol_ro_property(const char * name, const TProperty* property) {
+    return ProtocolProperty<const TProperty>(name, property);
+};
+
+
+
+template<typename TObj, typename TRet, typename ... TArgs>
+class FunctionTraits {
+public:
+    template<unsigned IUnpacked, typename ... TUnpackedArgs, typename = std::enable_if_t<IUnpacked != sizeof...(TArgs)>>
+    static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple<TArgs...> packed_args, TUnpackedArgs ... args) {
+        return invoke<IUnpacked+1>(obj, func_ptr, packed_args, args..., std::get<IUnpacked>(packed_args));
+    }
+
+    template<unsigned IUnpacked>
+    static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple<TArgs...> packed_args, TArgs ... args) {
+        return (obj.*func_ptr)(args...);
+    }
+};
+
+/* @brief Invoke a class member function with a variable number of arguments that are supplied as a tuple
+
+Example usage:
+
+class MyClass {
+public:
+    int MyFunction(int a, int b) {
+        return 0;
+    }
+};
+
+MyClass my_object;
+std::tuple<int, int> my_args(3, 4); // arguments are supplied as a tuple
+int result = invoke_function_with_tuple(my_object, &MyClass::MyFunction, my_args);
+*/
+template<typename TObj, typename TRet, typename ... TArgs>
+TRet invoke_function_with_tuple(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple<TArgs...> packed_args) {
+    return FunctionTraits<TObj, TRet, TArgs...>::template invoke<0>(obj, func_ptr, packed_args);
+}
+
+
+template<typename ... TArgs>
+struct PropertyListFactory;
+
+template<>
+struct PropertyListFactory<> {
+    template<unsigned IPos, typename ... TAllProperties>
+    static MemberList<> make_property_list(std::array<const char *, sizeof...(TAllProperties)> names, std::tuple<TAllProperties...>& values) {
+        return MemberList<>();
+    }
+};
+
+template<typename TProperty, typename ... TProperties>
+struct PropertyListFactory<TProperty, TProperties...> {
+    template<unsigned IPos, typename ... TAllProperties>
+    static MemberList<ProtocolProperty<TProperty>, ProtocolProperty<TProperties>...>
+    make_property_list(std::array<const char *, sizeof...(TAllProperties)> names, std::tuple<TAllProperties...>& values) {
+        return MemberList<ProtocolProperty<TProperty>, ProtocolProperty<TProperties>...>(
+            make_protocol_property(std::get<IPos>(names), std::get<IPos>(values)),
+            PropertyListFactory<TProperties...>::template make_property_list<IPos+1>(names, values)
+        );
+    }
+};
+
+
+template<typename TObj, typename TRet, typename ... TArgs>
+class ProtocolFunction : Endpoint {
+public:
+    static constexpr size_t endpoint_count = 1 + MemberList<ProtocolProperty<TArgs>...>::endpoint_count;
+    template<typename ... TNames>
+    ProtocolFunction(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) :
+        name_(name), all_arg_names_{names...}, obj_(obj), func_ptr_(func_ptr),
+        input_properties_(PropertyListFactory<TArgs...>::template make_property_list<0>(all_arg_names_, in_args_))
+    {
+        LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_));
+    }
+
+    ProtocolFunction(const ProtocolFunction& other) :
+        name_(other.name_), all_arg_names_(other.all_arg_names_), obj_(other.obj_), func_ptr_(other.func_ptr_),
+        input_properties_(PropertyListFactory<TArgs...>::template make_property_list<0>(
+            all_arg_names_, in_args_))
+    {
+        LOG_PROTO("COPIED! my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_));
+    }
+
+    void write_json(size_t id, StreamSink* output) {
+        // write name
+        write_string("{\"name\":\"", output);
+        write_string(name_, output);
+
+        // write endpoint ID
+        write_string("\",\"id\":", output);
+        char id_buf[10];
+        snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf
+        write_string(id_buf, output);
+        
+        // write arguments
+        write_string(",\"type\":\"function\",\"arguments\":[", output);
+        input_properties_.write_json(id + 1, output),
+        write_string("]}", output);
+    }
+
+    void register_endpoints(Endpoint** list, size_t id, size_t length) {
+        if (id < length)
+            list[id] = this;
+        input_properties_.register_endpoints(list, id + 1, length);
+    }
+
+    void handle(const uint8_t* input, size_t input_length, StreamSink* output) {
+        (void) input;
+        (void) input_length;
+        (void) output;
+        LOG_PROTO("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_));
+        LOG_PROTO("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_));
+        invoke_function_with_tuple(obj_, func_ptr_, in_args_);
+    }
+
+    const char * name_;
+    std::array<const char *, sizeof...(TArgs)> all_arg_names_; // TODO: remove
+    TObj& obj_;
+    TRet(TObj::*func_ptr_)(TArgs...);
+    std::tuple<TArgs...> in_args_;
+    MemberList<ProtocolProperty<TArgs>...> input_properties_;
+};
+
+template<typename TObj, typename TRet, typename ... TArgs, typename ... TNames, typename = std::enable_if_t<sizeof...(TArgs) == sizeof...(TNames)>>
+ProtocolFunction<TObj, TRet, TArgs...> make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) {
+    return ProtocolFunction<TObj, TRet, TArgs...>(name, obj, func_ptr, names...);
+}
+
+
+
+template<typename T>
+class EndpointProvider_from_MemberList : public EndpointProvider {
+public:
+    EndpointProvider_from_MemberList(T& member_list) : member_list_(member_list) {}
+    size_t get_endpoint_count() final {
+        return member_list_.get_endpoint_count();
+    }
+    void write_json(size_t id, StreamSink* output) final {
+        return member_list_.write_json(id, output);
+    }
+    void register_endpoints(Endpoint** list, size_t id, size_t length) final {
+        return member_list_.register_endpoints(list, id, length);
+    }
+    T& member_list_;
+};
+
+void set_application_endpoints(EndpointProvider* endpoints);
+
 #endif
diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp
index b256e24c..18b291ca 100644
--- a/Firmware/MotorControl/sensorless_estimator.hpp
+++ b/Firmware/MotorControl/sensorless_estimator.hpp
@@ -14,6 +14,7 @@ public:
 
     Axis* axis = nullptr; // set by Axis constructor
 
+    // TODO: expose on protocol
     Error_t error = ERROR_NONE;
     float phase = 0.0f;                        // [rad]
     float pll_pos = 0.0f;                      // [rad]