make stuff working again, improve state machine design, add underscore to member names

Firmware/Board/v3.3/Src/usbd_cdc_if.c

@@ -52,7 +52,7 @@
 #include "cmsis_os.h"
 #include "freertos_vars.h"
 #include "utils.h"
-#include "commands.h"
+#include "communication.h"
 #include <freertos_vars.h>
 /* USER CODE END INCLUDE */
 

Firmware/MotorControl/axis.cpp

@@ -12,24 +12,24 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
            SensorlessEstimator& sensorless_estimator,
            Controller& controller,
            Motor& motor)
-    : hw_config(hw_config),
-      config(config),
-      encoder(encoder),
-      sensorless_estimator(sensorless_estimator),
-      controller(controller),
-      motor(motor)
+    : hw_config_(hw_config),
+      config_(config),
+      encoder_(encoder),
+      sensorless_estimator_(sensorless_estimator),
+      controller_(controller),
+      motor_(motor)
 {
-    encoder.axis = this;
-    sensorless_estimator.axis = this;
-    controller.axis = this;
-    motor.axis = this;
+    encoder_.axis_ = this;
+    sensorless_estimator_.axis_ = this;
+    controller_.axis_ = this;
+    motor_.axis_ = this;
 }
 
 // @brief Sets up all components of the axis,
 // such as gate driver and encoder hardware.
 void Axis::setup() {
-    encoder.setup();
-    motor.setup();
+    encoder_.setup();
+    motor_.setup();
 }
 
 static void run_state_machine_loop_wrapper(void* ctx) {
@@ -38,16 +38,24 @@ static void run_state_machine_loop_wrapper(void* ctx) {
 
 // @brief Starts run_state_machine_loop in a new thread
 void Axis::start_thread() {
-    osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config.thread_priority, 0, 512);
-    thread_id = osThreadCreate(osThread(thread_def), this);
-    thread_id_valid = true;
+    osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 512);
+    thread_id_ = osThreadCreate(osThread(thread_def), this);
+    thread_id_valid_ = true;
 }
 
 // @brief Unblocks the control loop thread.
 // This is called from the current sense interrupt handler.
-void Axis::signal_thread(thread_signals sig) {
-    if (thread_id_valid)
-        osSignalSet(thread_id, sig);
+void Axis::signal_current_meas() {
+    if (thread_id_valid_)
+        osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS);
+}
+
+// @brief Blocks until a current measurement is completed
+// @returns True on success, false otherwise
+bool Axis::wait_for_current_meas() {
+    if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal)
+        return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
+    return true;
 }
 
 static void step_cb_wrapper(void* ctx) {
@@ -56,10 +64,10 @@ static void step_cb_wrapper(void* ctx) {
 
 // step/direction interface
 void Axis::step_cb() {
-    if (enable_step_dir) {
-        GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config.dir_port, hw_config.dir_pin);
+    if (enable_step_dir_) {
+        GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config_.dir_port, hw_config_.dir_pin);
         float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
-        controller.pos_setpoint += dir * config.counts_per_step;
+        controller_.pos_setpoint_ += dir * config_.counts_per_step;
     }
 };
 
@@ -68,38 +76,38 @@ void Axis::set_step_dir_enabled(bool enable) {
     if (enable) {
         // Set up the direction GPIO as input
         GPIO_InitTypeDef GPIO_InitStruct;
-        GPIO_InitStruct.Pin = hw_config.dir_pin;
+        GPIO_InitStruct.Pin = hw_config_.dir_pin;
         GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
         GPIO_InitStruct.Pull = GPIO_NOPULL;
-        HAL_GPIO_Init(hw_config.dir_port, &GPIO_InitStruct);
+        HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct);
 
         // Subscribe to rising edges of the step GPIO
-        GPIO_subscribe(hw_config.step_port, hw_config.step_pin, GPIO_PULLDOWN,
+        GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN,
                 step_cb_wrapper, this);
 
-        enable_step_dir = true;
+        enable_step_dir_ = true;
     } else {
-        enable_step_dir = false;
+        enable_step_dir_ = false;
 
         // Unsubscribe from step GPIO
-        GPIO_unsubscribe(hw_config.step_port, hw_config.step_pin);
+        GPIO_unsubscribe(hw_config_.step_port, hw_config_.step_pin);
     }
 }
 
 // @brief Returns true if the power supply is within range
 bool Axis::check_PSU_brownout() {
-    if(vbus_voltage < config.dc_bus_brownout_trip_level)
-        return error = ERROR_BAD_VOLTAGE, false;
+    if(vbus_voltage < config_.dc_bus_brownout_trip_level)
+        return error_ = ERROR_BAD_VOLTAGE, false;
     return true;
 }
 
 // @brief Returns true if everything is ok.
 // Sets error and returns false otherwise.
 bool Axis::do_checks() {
-    if (!motor.do_checks())
-        return error = ERROR_MOTOR_FAILED, false;
+    if (!motor_.do_checks())
+        return error_ = ERROR_MOTOR_FAILED, false;
     if (!check_PSU_brownout())
-        return error = ERROR_BAD_VOLTAGE, false;
+        return error_ = ERROR_BAD_VOLTAGE, false;
     return true;
 }
 
@@ -107,74 +115,81 @@ bool Axis::run_sensorless_spin_up() {
     // Early Spin-up: spiral up current
     float x = 0.0f;
     run_control_loop([&](){
-        float phase = wrap_pm_pi(config.ramp_up_distance * x);
-        float I_mag = config.spin_up_current * x;
-        x += current_meas_period / config.ramp_up_time;
-        if (!motor.update(I_mag, phase))
-            return error = ERROR_MOTOR_FAILED, false;
+        float phase = wrap_pm_pi(config_.ramp_up_distance * x);
+        float I_mag = config_.spin_up_current * x;
+        x += current_meas_period / config_.ramp_up_time;
+        if (!motor_.update(I_mag, phase))
+            return error_ = ERROR_MOTOR_FAILED, false;
         return x < 1.0f;
     });
-    if (error != ERROR_NO_ERROR)
+    if (error_ != ERROR_NO_ERROR)
         return false;
     
     // Late Spin-up: accelerate
-    float vel = config.ramp_up_distance / config.ramp_up_time;
-    float phase = wrap_pm_pi(config.ramp_up_distance);
+    float vel = config_.ramp_up_distance / config_.ramp_up_time;
+    float phase = wrap_pm_pi(config_.ramp_up_distance);
     run_control_loop([&](){
-        vel += config.spin_up_acceleration * current_meas_period;
+        vel += config_.spin_up_acceleration * current_meas_period;
         phase = wrap_pm_pi(phase + vel * current_meas_period);
-        float I_mag = config.spin_up_current;
-        if (!motor.update(I_mag, phase))
-            return error = ERROR_MOTOR_FAILED, false;
-        return vel < config.spin_up_target_vel;
+        float I_mag = config_.spin_up_current;
+        if (!motor_.update(I_mag, phase))
+            return error_ = ERROR_MOTOR_FAILED, false;
+        return vel < config_.spin_up_target_vel;
     });
-    return error == ERROR_NO_ERROR;
+    return error_ == ERROR_NO_ERROR;
 }
 
 // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
 bool Axis::run_sensorless_control_loop() {
+    set_step_dir_enabled(config_.enable_step_dir);
     run_control_loop([this](){
         float pos_estimate, vel_estimate, phase, current_setpoint;
 
-        if (controller.config.control_mode >= CTRL_MODE_POSITION_CONTROL)
-            return error = ERROR_POS_CTRL_DURING_SENSORLESS, false;
+        if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL)
+            return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false;
 
         // We update the encoder just in case someone needs the output for testing
-        encoder.update(nullptr, nullptr, nullptr);
-        if (!sensorless_estimator.update(&pos_estimate, &vel_estimate, &phase))
-            return error = ERROR_SENSORLESS_ESTIMATOR_FAILED, false;
-        if (!controller.update(pos_estimate, vel_estimate, &current_setpoint))
-            return error = ERROR_CONTROLLER_FAILED, false;
-        if (!motor.update(current_setpoint, phase))
-            return error = ERROR_MOTOR_FAILED, false;
+        encoder_.update(nullptr, nullptr, nullptr);
+        if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase))
+            return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false;
+        if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
+            return error_ = ERROR_CONTROLLER_FAILED, false;
+        if (!motor_.update(current_setpoint, phase))
+            return error_ = ERROR_MOTOR_FAILED, false;
         return true;
     });
-    return error == ERROR_NO_ERROR;
+    set_step_dir_enabled(false);
+    return error_ == ERROR_NO_ERROR;
 }
 
 bool Axis::run_closed_loop_control_loop() {
+    set_step_dir_enabled(config_.enable_step_dir);
     run_control_loop([this](){
         float pos_estimate, vel_estimate, phase, current_setpoint;
 
         // We update the sensorless estimator just in case someone needs the output for testing
-        sensorless_estimator.update(nullptr, nullptr, nullptr);
-        if (!encoder.update(&pos_estimate, &vel_estimate, &phase))
-            return error = ERROR_ENCODER_FAILED, false;
-        if (!controller.update(pos_estimate, vel_estimate, &current_setpoint))
-            return error = ERROR_CONTROLLER_FAILED, false;
-        if (!motor.update(current_setpoint, phase))
-            return error = ERROR_MOTOR_FAILED, false;
+        sensorless_estimator_.update(nullptr, nullptr, nullptr);
+        if (!encoder_.update(&pos_estimate, &vel_estimate, &phase))
+            return error_ = ERROR_ENCODER_FAILED, false;
+        if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
+            return error_ = ERROR_CONTROLLER_FAILED, false;
+        if (!motor_.update(current_setpoint, phase))
+            return error_ = ERROR_MOTOR_FAILED, false;
         return true;
     });
-    return error == ERROR_NO_ERROR;
+    set_step_dir_enabled(false);
+    return error_ == ERROR_NO_ERROR;
 }
 
 bool Axis::run_idle_loop() {
-    while (requested_state == AXIS_STATE_DONT_CARE) {
-        if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal)
-            return error = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
-    }
-    return error == ERROR_NO_ERROR;
+    // run_control_loop ignores missed modulation timing updates
+    // if and only if we're in AXIS_STATE_IDLE
+    run_control_loop([this](){
+        sensorless_estimator_.update(nullptr, nullptr, nullptr);
+        encoder_.update(nullptr, nullptr, nullptr);
+        return true;
+    });
+    return error_ == ERROR_NO_ERROR;
 }
 
 // Infinite loop that does calibration and enters main control loop as appropriate
@@ -183,84 +198,91 @@ void Axis::run_state_machine_loop() {
     // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
     // TODO: Move this somewhere else
     // TODO: respect changes of CPR
-    int encoder_cpr = encoder.config.cpr;
-    controller.anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
-    if (controller.anticogging.cogging_map != NULL) {
+    int encoder_cpr = encoder_.config_.cpr;
+    controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
+    if (controller_.anticogging_.cogging_map != NULL) {
         for (int i = 0; i < encoder_cpr; i++) {
-            controller.anticogging.cogging_map[i] = 0.0f;
+            controller_.anticogging_.cogging_map[i] = 0.0f;
         }
     }
 
-    current_state = AXIS_STATE_MOTOR_CALIBRATION;
-    bool force_state = false;
+    // arm!
+    motor_.arm();
     
     for (;;) {
-        AxisState_t next_state = AXIS_STATE_DONT_CARE;
-
-        switch (current_state) {
-
-        case AXIS_STATE_MOTOR_CALIBRATION:
-            {
-                bool skip = !force_state && !config.enable_motor_calibration;
-                if (skip || motor.run_calibration()) {
-                    next_state = AXIS_STATE_ENCODER_CALIBRATION;
-                } else {
-                    next_state = AXIS_STATE_IDLE;
-                }
+        // Load the task chain if a specific request is pending
+        if (requested_state_ != AXIS_STATE_UNDEFINED) {
+            size_t pos = 0;
+            if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) {
+                if (config_.startup_motor_calibration)
+                    task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
+                if (config_.startup_encoder_calibration)
+                    task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION;
+                if (config_.startup_closed_loop_control)
+                    task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
+                else if (config_.startup_sensorless_control)
+                    task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
+                task_chain_[pos++] = AXIS_STATE_IDLE;
+            } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
+                task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
+                task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION;
+                task_chain_[pos++] = AXIS_STATE_IDLE;
+            } else if (requested_state_ != AXIS_STATE_UNDEFINED) {
+                task_chain_[pos++] = requested_state_;
+                task_chain_[pos++] = AXIS_STATE_IDLE;
             }
+            task_chain_[pos++] = AXIS_STATE_UNDEFINED;
+            // TODO: bounds checking
+            requested_state_ = AXIS_STATE_UNDEFINED;
+        }
+
+        // Note that current_state is a reference to task_chain_[0]
+
+        // Validate the state before running it
+        if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_)
+            current_state_ = AXIS_STATE_UNDEFINED;
+        if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_)
+            current_state_ = AXIS_STATE_UNDEFINED;
+
+        // Run the specified state
+        // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
+        bool status;
+        switch (current_state_) {
+        case AXIS_STATE_MOTOR_CALIBRATION:
+            status = motor_.run_calibration();
             break;
 
         case AXIS_STATE_ENCODER_CALIBRATION:
-            {
-                bool skip = !force_state && !config.enable_encoder_calibration;
-                if (skip || encoder.run_calibration()) {
-                    next_state = config.enable_closed_loop_control ?
-                                AXIS_STATE_CLOSED_LOOP_CONTROL :
-                                config.enable_sensorless_control ?
-                                AXIS_STATE_SENSORLESS_SPINUP :
-                                AXIS_STATE_IDLE;
-                    if (next_state != AXIS_STATE_IDLE)
-                        set_step_dir_enabled(config.enable_step_dir);
-                } else {
-                    next_state = AXIS_STATE_IDLE;
-                }
-            }
-            break;
-
-        case AXIS_STATE_SENSORLESS_SPINUP:
-            if (run_sensorless_spin_up()) {
-                next_state = AXIS_STATE_SENSORLESS_CONTROL;
-            } else {
-                next_state = AXIS_STATE_IDLE;
-            }
+            status = encoder_.run_calibration();
             break;
 
         case AXIS_STATE_SENSORLESS_CONTROL:
-            run_sensorless_control_loop();
-            next_state = AXIS_STATE_IDLE; // TODO: restart if desired
+            status = run_sensorless_spin_up(); // TODO: restart if desired
+            if (status)
+                status = run_sensorless_control_loop();
             break;
 
         case AXIS_STATE_CLOSED_LOOP_CONTROL:
-            run_closed_loop_control_loop();
-            next_state = AXIS_STATE_IDLE;
+            status = run_closed_loop_control_loop();
             break;
 
         case AXIS_STATE_IDLE:
-        default:
-            current_state = AXIS_STATE_IDLE;
             run_idle_loop();
+            status = motor_.arm(); // done with idling - try to arm the motor
+            break;
+
+        default:
+            error_ = ERROR_INVALID_STATE;
+            status = false; // this will set the state to idle
             break;
         }
 
-        if (requested_state != AXIS_STATE_DONT_CARE) {
-            current_state = requested_state;
-            requested_state = AXIS_STATE_DONT_CARE;
-            force_state = true;
-        } else {
-            current_state = next_state;
-            force_state = false;
-        }
+        // If the state failed, go to idle, else advance task chain
+        if (!status)
+            current_state_ = AXIS_STATE_IDLE;
+        else
+            memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
     }
 
-    thread_id_valid = false;
+    thread_id_valid_ = false;
 }

Firmware/MotorControl/axis.hpp

@@ -5,23 +5,24 @@
 #error "This file should not be included directly. Include odrive_main.hpp instead."
 #endif
 
-
+// Warning: Do not reorder these enum values.
+// The state machine uses ">" comparision on them.
 enum AxisState_t {
-    AXIS_STATE_STARTUP,
-    AXIS_STATE_MOTOR_CALIBRATION,
-    AXIS_STATE_ENCODER_CALIBRATION,
-    AXIS_STATE_SENSORLESS_SPINUP,
-    AXIS_STATE_SENSORLESS_CONTROL,
-    AXIS_STATE_CLOSED_LOOP_CONTROL,
-    AXIS_STATE_IDLE,
-    AXIS_STATE_DONT_CARE // used to indicate that no state request is pending
+    AXIS_STATE_UNDEFINED,           //<! will fall through to idle
+    AXIS_STATE_IDLE,                //<! disable PWM and do nothing
+    AXIS_STATE_STARTUP_SEQUENCE, //<! the actual sequence is defined by the config.startup_... flags
+    AXIS_STATE_FULL_CALIBRATION_SEQUENCE,   //<! run all calibration procedures, then idle
+    AXIS_STATE_MOTOR_CALIBRATION,   //<! run motor calibration
+    AXIS_STATE_SENSORLESS_CONTROL,  //<! run sensorless calibration
+    AXIS_STATE_ENCODER_CALIBRATION, //<! run encoder calibration
+    AXIS_STATE_CLOSED_LOOP_CONTROL  //<! run closed loop control
 };
 
 struct AxisConfig_t {
-    bool enable_motor_calibration = true;   //<! run motor calibration at startup, skip otherwise
-    bool enable_encoder_calibration = true; //<! run encoder calibration after startup, skip otherwise
-    bool enable_closed_loop_control = true; //<! enable closed loop control after calibration/startup
-    bool enable_sensorless_control = false; //<! enable sensorless control after calibration/startup
+    bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
+    bool startup_encoder_calibration = false; //<! run encoder calibration after startup, skip otherwise
+    bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
+    bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
     bool enable_step_dir = true; //<! enable step/dir input after calibration
                                  //   For M0 this has no effect if enable_uart is true
 
@@ -40,6 +41,7 @@ class Axis {
 public:
     enum Error_t {
         ERROR_NO_ERROR,
+        ERROR_INVALID_STATE, //<! an invalid state was requested
         ERROR_BAD_VOLTAGE,
         ERROR_CURRENT_MEASUREMENT_TIMEOUT,
         ERROR_CONTROL_LOOP_TIMEOUT,
@@ -63,7 +65,8 @@ public:
 
     void setup();
     void start_thread();
-    void signal_thread(thread_signals sig);
+    void signal_current_meas();
+    bool wait_for_current_meas();
 
     void step_cb();
     void set_step_dir_enabled(bool enable);
@@ -75,16 +78,17 @@ public:
     // @brief Runs the specified update handler at the frequency of the current measurements.
     //
     // The loop runs until one of the following conditions:
-    //  - the update handler returns false
+    //  - update_handler returns false
     //  - the current measurement times out
     //  - the health checks fail (brownout, driver fault line)
-    //  - update_handler doesn't finish in time
+    //  - update_handler doesn't update the modulation timings in time
+    //    This criterion is ignored if current_state is AXIS_STATE_IDLE
     //
-    // The function arms the motor at the beginning of the control loop and disarms it at
-    // the end of the control loop.
+    // If update_handler is going to update the motor timings, you must call motor.arm()
+    // shortly before this function.
     //
-    // If the function returns, it is guaranteed that error is non-zero, except if the cause for the exit
-    // reason for the loop termination was a negative return value of update_handler or an external
+    // If the function returns, it is guaranteed that error is non-zero, except if the cause
+    // for the exit was a negative return value of update_handler or an external
     // state change request (requested_state != AXIS_STATE_DONT_CARE).
     // Under all exit conditions the motor is disarmed and the brake current set to zero.
     // Furthermore, if the update_handler does not set the phase voltages in time, they will
@@ -93,17 +97,15 @@ public:
     // @tparam T Must be a callable type that takes no arguments and returns a bool
     template<typename T>
     void run_control_loop(const T& update_handler) {
-        motor.arm();
-        while (requested_state == AXIS_STATE_DONT_CARE
-            && error == ERROR_NO_ERROR /* error may be set by interrupt handler */ ) {
-            if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) {
-                error = ERROR_CURRENT_MEASUREMENT_TIMEOUT;
+        while (requested_state_ == AXIS_STATE_UNDEFINED) {
+            if (motor_.error_ != Motor::ERROR_NO_ERROR) {
+                error_ = ERROR_MOTOR_FAILED;
+                break;
+            }
+            if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) {
+                error_ = ERROR_CONTROL_LOOP_TIMEOUT;
                 break;
             }
-            
-            // Proactively set phase voltages to 0. If the control deadline is missed,
-            // the voltages will go to zero.
-            motor.enqueue_voltage_timings(0.0f, 0.0f);
 
             if (!do_checks()) // error set during function call
                 break;
@@ -111,21 +113,15 @@ public:
             if (!update_handler()) // error set during function call
                 break;
 
-            update_brake_current();
-
             // Check we meet deadlines after queueing
-            motor.last_cpu_time = motor.check_timing();
-            if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) {
-                error = ERROR_CONTROL_LOOP_TIMEOUT;
+            ++loop_counter_;
+
+            // Wait until the current measurement interrupt fires
+            if (!wait_for_current_meas()) { // error set by function call
+                motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases
                 break;
             }
-            ++loop_counter;
         }
-
-        // We are exiting control: disarm motor, reset Ibus, and update brake current
-        motor.disarm();
-        motor.current_control.Ibus = 0.0f;
-        update_brake_current();
     }
 
     bool run_sensorless_spin_up();
@@ -135,23 +131,57 @@ public:
 
     void run_state_machine_loop();
 
-    const AxisHardwareConfig_t& hw_config;
-    AxisConfig_t& config;
+    const AxisHardwareConfig_t& hw_config_;
+    AxisConfig_t& config_;
 
-    Encoder& encoder;
-    SensorlessEstimator& sensorless_estimator;
-    Controller& controller;
-    Motor& motor;
+    Encoder& encoder_;
+    SensorlessEstimator& sensorless_estimator_;
+    Controller& controller_;
+    Motor& motor_;
 
-    osThreadId thread_id;
-    volatile bool thread_id_valid = false;
+    osThreadId thread_id_;
+    volatile bool thread_id_valid_ = false;
 
     // 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;
+    Error_t error_ = ERROR_NO_ERROR;
+    bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler
+                                         // whenever there's no active control loop that
+                                         // sets the timings. The flag must be explicitly
+                                         // cleared by a call to motors.arm().
+    bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
+    AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
+    AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
+    AxisState_t& current_state_ = task_chain_[0];
+    uint32_t loop_counter_ = 0;
+
+    // Communication protocol definitions
+    auto make_protocol_definitions() {
+        return make_protocol_member_list(
+            make_protocol_ro_property("error", &error_),
+            make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_),
+            make_protocol_property("enable_step_dir", &enable_step_dir_),
+            make_protocol_ro_property("current_state", &current_state_),
+            make_protocol_property("requested_state", &requested_state_),
+            make_protocol_ro_property("loop_counter", &loop_counter_),
+            make_protocol_object("config",
+                make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
+                make_protocol_property("startup_encoder_calibration", &config_.startup_encoder_calibration),
+                make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
+                make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
+                make_protocol_property("enable_step_dir", &config_.enable_step_dir),
+                make_protocol_property("counts_per_step", &config_.counts_per_step),
+                make_protocol_property("dc_bus_brownout_trip_level", &config_.dc_bus_brownout_trip_level),
+                make_protocol_property("ramp_up_time", &config_.ramp_up_time),
+                make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
+                make_protocol_property("spin_up_current", &config_.spin_up_current),
+                make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
+                make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel)
+            ),
+            make_protocol_object("motor", motor_.make_protocol_definitions()),
+            make_protocol_object("controller", controller_.make_protocol_definitions()),
+            make_protocol_object("encoder", encoder_.make_protocol_definitions())
+        );
+    }
 };
 
 #endif /* __AXIS_HPP */

Firmware/MotorControl/communication.cpp

@@ -0,0 +1,293 @@
+
+/* Includes ------------------------------------------------------------------*/
+
+// TODO: remove this option
+// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py
+// todo: make clean switches for protocol
+#define ENABLE_LEGACY_PROTOCOL
+
+#include "communication.h"
+//#include "low_level.h"
+#include "odrive_main.hpp"
+#include "protocol.hpp"
+#include "freertos_vars.h"
+#include "utils.h"
+
+#ifdef ENABLE_LEGACY_PROTOCOL
+#include "legacy_commands.h"
+#endif
+
+#include <cmsis_os.h>
+#include <memory>
+#include <usbd_cdc_if.h>
+#include <usb_device.h>
+#include <usart.h>
+#include <gpio.h>
+
+#define UART_TX_BUFFER_SIZE 64
+
+/* Private defines -----------------------------------------------------------*/
+/* Private macros ------------------------------------------------------------*/
+/* Private typedef -----------------------------------------------------------*/
+/* Global constant data ------------------------------------------------------*/
+/* Global variables ----------------------------------------------------------*/
+
+extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
+extern USBD_HandleTypeDef hUsbDeviceFS;
+
+/* Private constant data -----------------------------------------------------*/
+/* Private variables ---------------------------------------------------------*/
+
+static uint8_t* usb_buf;
+static uint32_t usb_len;
+
+// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable
+static thread_local uint32_t deadline_ms = 0;
+
+
+#if defined(USB_PROTOCOL_NATIVE)
+
+class USBSender : public PacketSink {
+public:
+    int process_packet(const uint8_t* buffer, size_t length) {
+        // cannot send partial packets
+        if (length > USB_TX_DATA_SIZE)
+            return -1;
+        // wait for USB interface to become ready
+        if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
+            return -1;
+        // transmit packet
+        uint8_t status = CDC_Transmit_FS(
+                const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
+                well... it's not actually. Stupid STM. */, length);
+        return (status == USBD_OK) ? 0 : -1;
+    }
+} usb_sender;
+
+BidirectionalPacketBasedChannel usb_channel(usb_sender);
+
+#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
+
+class USBSender : public StreamSink {
+public:
+    int process_bytes(const uint8_t* buffer, size_t length) {
+        // Loop to ensure all bytes get sent
+        while (length) {
+            size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE;
+            // wait for USB interface to become ready
+            if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK)
+                return -1;
+            // transmit chunk
+            if (CDC_Transmit_FS(
+                    const_cast<uint8_t*>(buffer) /* casting this const away is safe because...
+                    well... it's not actually. Stupid STM. */, chunk) != USBD_OK)
+                return -1;
+            buffer += chunk;
+            length -= chunk;
+        }
+        return 0;
+    }
+
+    size_t get_free_space() { return SIZE_MAX; }
+} usb_sender;
+
+PacketToStreamConverter usb_packet_sender(usb_sender);
+BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender);
+StreamToPacketConverter usb_stream_sink(usb_channel);
+
+#endif
+
+#if defined(UART_PROTOCOL_NATIVE)
+class UART4Sender : public StreamSink {
+public:
+    int process_bytes(const uint8_t* buffer, size_t length) {
+        // Loop to ensure all bytes get sent
+        while (length) {
+            size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE;
+            // wait for USB interface to become ready
+            // TODO: implement ring buffer to get a more continuous stream of data
+            if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK)
+                return -1;
+            // transmit chunk
+            memcpy(tx_buf_, buffer, chunk);
+            if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK)
+                return -1;
+            buffer += chunk;
+            length -= chunk;
+        }
+        return 0;
+    }
+
+    size_t get_free_space() { return SIZE_MAX; }
+private:
+    uint8_t tx_buf_[UART_TX_BUFFER_SIZE];
+} uart4_sender;
+
+PacketToStreamConverter uart4_packet_sender(uart4_sender);
+BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender);
+StreamToPacketConverter UART4_stream_sink(uart4_channel);
+#endif
+
+
+class test_class {
+public:
+    uint32_t property1;
+    float property2;
+
+    float set_both(uint32_t arg1, float arg2) {
+        printf("set_both called with %u and %.3f\n", (unsigned int)arg1, arg2);
+        property1 = arg1;
+        property2 = arg2;
+        return arg1 + arg2;
+    }
+};
+
+float bla;
+
+/* Private function prototypes -----------------------------------------------*/
+/* Function implementations --------------------------------------------------*/
+
+void init_communication(void) {
+    printf("hi!\r\n");
+
+    // Start command handling thread
+    osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 4*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);
+}
+
+
+
+static auto make_obj_tree() {
+    return make_protocol_member_list(
+        make_protocol_property("bla2", &bla),
+        make_protocol_object("axis0", axes[0]->make_protocol_definitions()),
+        make_protocol_object("axis1", axes[1]->make_protocol_definitions())
+    );
+}
+
+using tree_type = decltype(make_obj_tree());
+uint8_t tree_buffer[sizeof(tree_type)];
+
+// the protocol has one additional built-in endpoint
+constexpr size_t MAX_ENDPOINTS = decltype(make_obj_tree())::endpoint_count + 1;
+Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 };
+const size_t max_endpoints_ = MAX_ENDPOINTS;
+size_t n_endpoints_ = 0;
+
+// Thread to handle deffered processing of USB interrupt, and
+// read commands out of the UART DMA circular buffer
+void communication_task(void * ctx) {
+    (void) ctx; // unused parameter
+
+    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);
+    
+#if !defined(UART_PROTOCOL_NONE)
+    //DMA open loop continous circular buffer
+    //1ms delay periodic, chase DMA ptr around
+
+    #define UART_RX_BUFFER_SIZE 64
+    static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE];
+
+    // DMA is set up to recieve in a circular buffer forever.
+    // We dont use interrupts to fetch the data, instead we periodically read
+    // data out of the circular buffer into a parse buffer, controlled by a state machine
+    HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
+    uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
+#endif
+
+    // Re-run state-machine forever
+    for (;;) {
+#if !defined(UART_PROTOCOL_NONE)
+        // Check for UART errors and restart recieve DMA transfer if required
+        if (huart4.ErrorCode != HAL_UART_ERROR_NONE) {
+            HAL_UART_AbortReceive(&huart4);
+            HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer));
+        }
+        // Fetch the circular buffer "write pointer", where it would write next
+        uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR;
+
+        deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
+#if defined(UART_PROTOCOL_NATIVE)
+        // Process bytes in one or two chunks (two in case there was a wrap)
+        if (new_rcv_idx < last_rcv_idx) {
+            UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
+                    UART_RX_BUFFER_SIZE - last_rcv_idx);
+            last_rcv_idx = 0;
+        }
+        if (new_rcv_idx > last_rcv_idx) {
+            UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx,
+                    new_rcv_idx - last_rcv_idx);
+            last_rcv_idx = new_rcv_idx;
+        }
+#elif defined(UART_PROTOCOL_LEGACY)
+        // Process bytes in one or two chunks (two in case there was a wrap)
+        if (new_rcv_idx < last_rcv_idx) {
+            legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
+                    UART_RX_BUFFER_SIZE - last_rcv_idx);
+            last_rcv_idx = 0;
+        }
+        if (new_rcv_idx > last_rcv_idx) {
+            legacy_parse_stream(dma_circ_buffer + last_rcv_idx,
+                    new_rcv_idx - last_rcv_idx);
+            last_rcv_idx = new_rcv_idx;
+        }
+#endif
+#endif
+
+#if !defined(USB_PROTOCOL_NONE)
+        // When we reach here, we are out of immediate characters to fetch out of UART buffer
+        // Now we check if there is any USB processing to do: we wait for up to 1 ms,
+        // before going back to checking UART again.
+        const uint32_t usb_check_timeout = 1; // ms
+        osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout);
+        if (sem_stat == osOK) {
+            deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS);
+#if defined(USB_PROTOCOL_NATIVE)
+            usb_channel.process_packet(usb_buf, usb_len);
+#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED)
+            usb_stream_sink.process_bytes(usb_buf, usb_len);
+#elif defined(USB_PROTOCOL_LEGACY)
+            legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB);
+#endif
+            USBD_CDC_ReceivePacket(&hUsbDeviceFS);  // Allow next packet
+        }
+#endif
+
+#if defined(USB_PROTOCOL_NONE) && defined(UART_PROTOCOL_NONE)
+        osDelay(1); // don't starve other threads
+#endif
+    }
+
+    // If we get here, then this task is done
+    vTaskDelete(osThreadGetId());
+}
+
+// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the
+// incoming USB data
+void set_cmd_buffer(uint8_t *buf, uint32_t len) {
+    usb_buf = buf;
+    usb_len = len;
+}
+
+void usb_update_thread(void * ctx) {
+    (void) ctx; // unused parameter
+
+    for (;;) {
+        // Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
+        osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever);
+        if (semaphore_status == osOK) {
+            // We have a new incoming USB transmission: handle it
+            HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
+            // Let the irq (OTG_FS_IRQHandler) fire again.
+            HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
+        }
+    }
+
+    vTaskDelete(osThreadGetId());
+}

Firmware/MotorControl/communication.h

@@ -21,16 +21,6 @@
 // #define UART_PROTOCOL_LEGACY
 #define UART_PROTOCOL_NONE
 
-// Use GPIO 1/2 for step/dir input instead of UART
-// #define USE_GPIO_MODE_STEP_DIR
-
-
-typedef enum {
-    GPIO_MODE_NONE,
-    GPIO_MODE_UART,
-    GPIO_MODE_STEP_DIR,
-} GpioMode_t;
-
 extern "C" {
 #endif
 

Firmware/MotorControl/controller.cpp

@@ -3,7 +3,7 @@
 
 
 Controller::Controller(ControllerConfig_t& config) :
-    config(config)
+    config_(config)
 {}
 
 //--------------------------------
@@ -11,32 +11,39 @@ Controller::Controller(ControllerConfig_t& config) :
 //--------------------------------
 
 void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
-    pos_setpoint = pos_setpoint;
-    vel_setpoint = vel_feed_forward;
-    current_setpoint = current_feed_forward;
-    config.control_mode = CTRL_MODE_POSITION_CONTROL;
+    pos_setpoint_ = pos_setpoint;
+    vel_setpoint_ = vel_feed_forward;
+    current_setpoint_ = current_feed_forward;
+    config_.control_mode = CTRL_MODE_POSITION_CONTROL;
 #ifdef DEBUG_PRINT
-    printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint);
+    printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_);
 #endif
 }
 
 void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) {
-    vel_setpoint = vel_setpoint;
-    current_setpoint = current_feed_forward;
-    config.control_mode = CTRL_MODE_VELOCITY_CONTROL;
+    vel_setpoint_ = vel_setpoint;
+    current_setpoint_ = current_feed_forward;
+    config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
 #ifdef DEBUG_PRINT
-    printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint);
+    printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_);
 #endif
 }
 
 void Controller::set_current_setpoint(float current_setpoint) {
-    current_setpoint = current_setpoint;
-    config.control_mode = CTRL_MODE_CURRENT_CONTROL;
+    current_setpoint_ = current_setpoint;
+    config_.control_mode = CTRL_MODE_CURRENT_CONTROL;
 #ifdef DEBUG_PRINT
-    printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint);
+    printf("CURRENT_CONTROL %3.3f\n", current_setpoint_);
 #endif
 }
 
+void Controller::start_anticogging_calibration() {
+    // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
+    if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) {
+        anticogging_.calib_anticogging = true;
+    }
+}
+
 /*
  * This anti-cogging implementation iterates through each encoder position,
  * waits for zero velocity & position error,
@@ -44,21 +51,21 @@ void Controller::set_current_setpoint(float current_setpoint) {
  * 
  * This holding current is added as a feedforward term in the control loop.
  */
-bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate) {
-    if (anticogging.calib_anticogging && anticogging.cogging_map != NULL) {
-        float pos_err = anticogging.index - pos_estimate;
-        if (fabsf(pos_err) <= anticogging.calib_pos_threshold &&
-            fabsf(vel_estimate) < anticogging.calib_vel_threshold) {
-            anticogging.cogging_map[anticogging.index++] = vel_integrator_current;
+bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
+    if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) {
+        float pos_err = anticogging_.index - pos_estimate;
+        if (fabsf(pos_err) <= anticogging_.calib_pos_threshold &&
+            fabsf(vel_estimate) < anticogging_.calib_vel_threshold) {
+            anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_;
         }
-        if (anticogging.index < axis->encoder.config.cpr) { // TODO: remove the dependency on encoder CPR
-            set_pos_setpoint(anticogging.index, 0.0f, 0.0f);
+        if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR
+            set_pos_setpoint(anticogging_.index, 0.0f, 0.0f);
             return false;
         } else {
-            anticogging.index = 0;
+            anticogging_.index = 0;
             set_pos_setpoint(0.0f, 0.0f, 0.0f);  // Send the motor home
-            anticogging.use_anticogging = true;  // We're good to go, enable anti-cogging
-            anticogging.calib_anticogging = false;
+            anticogging_.use_anticogging = true;  // We're good to go, enable anti-cogging
+            anticogging_.calib_anticogging = false;
             return true;
         }
     }
@@ -66,42 +73,42 @@ bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate
 }
 
 bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
-    // Only runs if anticogging.calib_anticogging is true; non-blocking
-    anti_cogging_calibration(pos_estimate, vel_estimate);
+    // Only runs if anticogging_.calib_anticogging is true; non-blocking
+    anticogging_calibration(pos_estimate, vel_estimate);
     
     // Position control
     // TODO Decide if we want to use encoder or pll position here
-    float vel_des = vel_setpoint;
-    if (config.control_mode >= CTRL_MODE_POSITION_CONTROL) {
-        float pos_err = pos_setpoint - pos_estimate;
-        vel_des += config.pos_gain * pos_err;
+    float vel_des = vel_setpoint_;
+    if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
+        float pos_err = pos_setpoint_ - pos_estimate;
+        vel_des += config_.pos_gain * pos_err;
     }
 
     // Velocity limiting
-    float vel_lim = config.vel_limit;
+    float vel_lim = config_.vel_limit;
     if (vel_des > vel_lim) vel_des = vel_lim;
     if (vel_des < -vel_lim) vel_des = -vel_lim;
 
     // Velocity control
-    float Iq = current_setpoint;
+    float Iq = current_setpoint_;
 
     // Anti-cogging is enabled after calibration
     // We get the current position and apply a current feed-forward
     // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
-    if (anticogging.use_anticogging) {
-        Iq += anticogging.cogging_map[mod(pos_estimate, axis->encoder.config.cpr)];
+    if (anticogging_.use_anticogging) {
+        Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)];
     }
 
     float v_err = vel_des - vel_estimate;
-    if (config.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
-        Iq += config.vel_gain * v_err;
+    if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
+        Iq += config_.vel_gain * v_err;
     }
 
     // Velocity integral action before limiting
-    Iq += vel_integrator_current;
+    Iq += vel_integrator_current_;
 
     // Current limiting
-    float Ilim = std::min(axis->motor.config.current_lim, axis->motor.current_control.max_allowed_current);
+    float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
     bool limited = false;
     if (Iq > Ilim) {
         limited = true;
@@ -113,15 +120,15 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
     }
 
     // Velocity integrator (behaviour dependent on limiting)
-    if (config.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
+    if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
         // reset integral if not in use
-        vel_integrator_current = 0.0f;
+        vel_integrator_current_ = 0.0f;
     } else {
         if (limited) {
             // TODO make decayfactor configurable
-            vel_integrator_current *= 0.99f;
+            vel_integrator_current_ *= 0.99f;
         } else {
-            vel_integrator_current += (config.vel_integrator_gain * current_meas_period) * v_err;
+            vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err;
         }
     }
 

Firmware/MotorControl/controller.hpp

@@ -30,14 +30,15 @@ public:
     void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward);
     void set_vel_setpoint(float vel_setpoint, float current_feed_forward);
     void set_current_setpoint(float current_setpoint);
-
+    
     // TODO: make this more similar to other calibration loops
-    bool anti_cogging_calibration(float pos_estimate, float vel_estimate);
+    void start_anticogging_calibration();
+    bool anticogging_calibration(float pos_estimate, float vel_estimate);
 
     bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
 
-    ControllerConfig_t& config;
-    Axis* axis = nullptr; // set by Axis constructor
+    ControllerConfig_t& config_;
+    Axis* axis_ = nullptr; // set by Axis constructor
 
     // TODO: anticogging overhaul:
     // - expose selected (all?) variables on protocol
@@ -53,7 +54,7 @@ public:
         float calib_pos_threshold;
         float calib_vel_threshold;
     } Anticogging_t;
-    Anticogging_t anticogging = {
+    Anticogging_t anticogging_ = {
         .index = 0,
         .cogging_map = nullptr,
         .use_anticogging = false,
@@ -63,25 +64,38 @@ public:
     };
 
     // variables exposed on protocol
-    float pos_setpoint = 0.0f;
-    float vel_setpoint = 0.0f;
+    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]
+    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; 
-        float vel_feed_forward;
-        float current_feed_forward;
-    } set_pos_setpoint_args;
-    struct {
-        float vel_setpoint;
-        float current_feed_forward;
-    } set_vel_setpoint_args;
-    struct {
-        float current_setpoint;
-    } set_current_setpoint_args;
+    // Communication protocol definitions
+    auto make_protocol_definitions() {
+        return make_protocol_member_list(
+            make_protocol_property("pos_setpoint", &pos_setpoint_),
+            make_protocol_property("vel_setpoint", &vel_setpoint_),
+            make_protocol_property("vel_integrator_current", &vel_integrator_current_),
+            make_protocol_property("current_setpoint", &current_setpoint_),
+            make_protocol_object("config",
+                make_protocol_property("control_mode", &config_.control_mode),
+                make_protocol_property("pos_gain", &config_.pos_gain),
+                make_protocol_property("vel_gain", &config_.vel_gain),
+                make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain),
+                make_protocol_property("vel_limit", &config_.vel_limit)
+            ),
+            make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint,
+                "pos_setpoint",
+                "vel_feed_forward",
+                "current_feed_forward"),
+            make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
+                "vel_setpoint",
+                "current_feed_forward"),
+            make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
+                "current_setpoint"),
+            make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
+        );
+    }
 };
 
 #endif // __CONTROLLER_HPP

Firmware/MotorControl/encoder.cpp

@@ -5,16 +5,16 @@
 
 Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
                 EncoderConfig_t& config) :
-        hw_config(hw_config),
-        config(config)
+        hw_config_(hw_config),
+        config_(config)
 {
     // Calculate encoder pll gains
     // This calculation is currently identical to the PLL in SensorlessEstimator
     float pll_bandwidth = 1000.0f;  // [rad/s]
-    pll_kp = 2.0f * pll_bandwidth;
+    pll_kp_ = 2.0f * pll_bandwidth;
 
     // Critically damped
-    pll_ki = 0.25f * (pll_kp * pll_kp);
+    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
 }
 
 static void enc_index_cb_wrapper(void* ctx) {
@@ -22,8 +22,8 @@ static void enc_index_cb_wrapper(void* ctx) {
 }
 
 void Encoder::setup() {
-    HAL_TIM_Encoder_Start(hw_config.timer, TIM_CHANNEL_ALL);
-    GPIO_subscribe(hw_config.index_port, hw_config.index_pin, GPIO_NOPULL,
+    HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
+    GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
             enc_index_cb_wrapper, this);
 }
 
@@ -33,22 +33,22 @@ void Encoder::setup() {
 
 // Triggered when an encoder passes over the "Index" pin
 // TODO: only arm index edge interrupt when we know encoder has powered up
-// TODO: disarm interrupt once we found the index
+// TODO: disable interrupt once we found the index
 void Encoder::enc_index_cb() {
-    if (!index_found) {
+    if (!index_found_) {
         set_count(0);
-        index_found = true;
+        index_found_ = true;
     }
 }
 
 // Function that sets the current encoder count to a desired 32-bit value.
-void Encoder::set_count(uint32_t count) {
+void Encoder::set_count(int32_t count) {
     // Disable interrupts to make a critical section to avoid race condition
     uint32_t prim = __get_PRIMASK();
     __disable_irq();
-    state = count;
-    hw_config.timer->Instance->CNT = count;
-    pll_pos = (float)count;
+    state_ = count;
+    hw_config_.timer->Instance->CNT = count;
+    pll_pos_ = (float)count;
     __set_PRIMASK(prim);
 }
 
@@ -59,109 +59,109 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) {
     static const float start_lock_duration = 1.0f;
     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;
+    static const int 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;
-    axis->run_control_loop([&](){
-        axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f);
+    int i = 0;
+    axis_->run_control_loop([&](){
+        axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f);
         return ++i < start_lock_duration * current_meas_hz;
     });
-    if (axis->error != Axis::ERROR_NO_ERROR)
+    if (axis_->error_ != Axis::ERROR_NO_ERROR)
         return false;
 
-    int32_t init_enc_val = (int16_t)hw_config.timer->Instance->CNT;
+    int32_t init_enc_val = (int16_t)hw_config_.timer->Instance->CNT;
     int64_t encvaluesum = 0;
 
     // scan forward
     i = 0;
-    axis->run_control_loop([&](){
+    axis_->run_control_loop([&](){
         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);
+        axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
 
-        encvaluesum += (int64_t)hw_config.timer->Instance->CNT;
+        encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
         
         return ++i < num_steps;
     });
-    if (axis->error != Axis::ERROR_NO_ERROR)
+    if (axis_->error_ != Axis::ERROR_NO_ERROR)
         return false;
 
     //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 elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
     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)
+    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)
     {
-        error = ERROR_CPR_OUT_OF_RANGE;
+        error_ = ERROR_CPR_OUT_OF_RANGE;
         return false;
     }
     // check direction
-    if ((int16_t)hw_config.timer->Instance->CNT > init_enc_val + 8) {
+    if ((int16_t)hw_config_.timer->Instance->CNT > init_enc_val + 8) {
         // motor same dir as encoder
-        axis->motor.config.direction = 1;
-    } else if ((int16_t)hw_config.timer->Instance->CNT < init_enc_val - 8) {
+        axis_->motor_.config_.direction = 1;
+    } else if ((int16_t)hw_config_.timer->Instance->CNT < init_enc_val - 8) {
         // motor opposite dir as encoder
-        axis->motor.config.direction = -1;
+        axis_->motor_.config_.direction = -1;
     } else {
         // Encoder response error
-        error = ERROR_RESPONSE;
+        error_ = ERROR_RESPONSE;
         return false;
     }
 
     // scan backwards
     i = 0;
-    axis->run_control_loop([&](){
+    axis_->run_control_loop([&](){
         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);
+        axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
 
-        encvaluesum += (int64_t)hw_config.timer->Instance->CNT;
+        encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
         
         return ++i < num_steps;
     });
-    if (axis->error != Axis::ERROR_NO_ERROR)
+    if (axis_->error_ != Axis::ERROR_NO_ERROR)
         return false;
 
     int offset = encvaluesum / (num_steps * 2);
-    config.offset = offset;
-    config.calibrated = true;
+    config_.offset = offset;
+    is_calibrated_ = true;
     return true;
 }
 
 bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) {
-    index_found = false;
+    index_found_ = false;
     float phase = 0.0f;
-    axis->run_control_loop([&](){
+    axis_->run_control_loop([&](){
         phase = wrap_pm_pi(phase + omega * current_meas_period);
 
         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);
+        axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta);
         
         // continue until the index is found
-        return !index_found;
+        return !index_found_;
     });
-    return axis->error == Axis::ERROR_NO_ERROR;
+    return axis_->error_ == Axis::ERROR_NO_ERROR;
 }
 
 bool Encoder::run_calibration() {
     float enc_calibration_voltage;
-    if (axis->motor.config.motor_type == MOTOR_TYPE_HIGH_CURRENT)
-        enc_calibration_voltage = axis->motor.config.calibration_current * axis->motor.config.phase_resistance;
-    else if (axis->motor.config.motor_type == MOTOR_TYPE_GIMBAL)
-        enc_calibration_voltage = axis->motor.config.calibration_current;
+    if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
+        enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
+    else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
+        enc_calibration_voltage = axis_->motor_.config_.calibration_current;
     else
         return false;
 
-    if (config.use_index && !index_found)
+    if (config_.use_index && !index_found_)
         if (!scan_for_enc_idx(
-                /*(float)(axis->motor.config.direction) * */ config.idx_search_speed,
+                (float)(axis_->motor_.config_.direction) * config_.idx_search_speed,
                 enc_calibration_voltage))
             return false;
-    if (!config.calibrated)
+    if (!config_.hand_calibrated) // 
         if (!calib_enc_offset(enc_calibration_voltage))
             return false;
     return true;
@@ -169,38 +169,38 @@ bool Encoder::run_calibration() {
 
 bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
     // Check that we don't get problems with discrete time approximation
-    if (!(current_meas_period * pll_kp < 1.0f)) {
-        error = ERROR_NUMERICAL;
+    if (!(current_meas_period * pll_kp_ < 1.0f)) {
+        error_ = ERROR_NUMERICAL;
         return false;
     }
 
     // update internal encoder state
-    int16_t delta_enc = (int16_t)hw_config.timer->Instance->CNT - (int16_t)state;
-    state += (int32_t)delta_enc;
+    int16_t delta_enc = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)state_;
+    state_ += (int32_t)delta_enc;
 
     // compute electrical phase
-    int corrected_enc = state % config.cpr;
-    corrected_enc -= config.offset;
-    //corrected_enc *= axis->motor.config.direction; TODO: verify if this still works
+    int corrected_enc = state_ % config_.cpr;
+    corrected_enc -= config_.offset;
+    //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works
     //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 elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
     float ph = elec_rad_per_enc * (float)corrected_enc;
     // ph = fmodf(ph, 2*M_PI);
-    phase = wrap_pm_pi(ph);
+    phase_ = wrap_pm_pi(ph);
 
     // run pll (for now pll is in units of encoder counts)
     // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part?
     // Predict current pos
-    pll_pos += current_meas_period * pll_vel;
+    pll_pos_ += current_meas_period * pll_vel_;
     // discrete phase detector
-    float delta_pos = (float)(state - (int32_t)floorf(pll_pos));
+    float delta_pos = (float)(state_ - (int32_t)floorf(pll_pos_));
     // pll feedback
-    pll_pos += current_meas_period * pll_kp * delta_pos;
-    pll_vel += current_meas_period * pll_ki * delta_pos;
+    pll_pos_ += current_meas_period * pll_kp_ * delta_pos;
+    pll_vel_ += current_meas_period * pll_ki_ * delta_pos;
 
     // Assign output arguments
-    if (*pos_estimate) *pos_estimate = pll_pos;
-    if (*vel_estimate) *vel_estimate = pll_vel;
-    if (*phase_output) *phase_output = phase;
+    if (pos_estimate) *pos_estimate = pll_pos_;
+    if (vel_estimate) *vel_estimate = pll_vel_;
+    if (phase_output) *phase_output = phase_;
     return true;
 }

Firmware/MotorControl/encoder.hpp

@@ -7,7 +7,7 @@
 
 struct EncoderConfig_t {
     bool use_index = false;
-    bool calibrated = false;
+    bool hand_calibrated = false;
     float idx_search_speed = 10.0f; // [rad/s electrical]
     int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
     int32_t offset = 0;
@@ -30,25 +30,48 @@ public:
 
     void enc_index_cb();
 
-    void set_count(uint32_t count);
+    void set_count(int32_t count);
     bool calib_enc_offset(float voltage_magnitude);
     bool scan_for_enc_idx(float omega, float voltage_magnitude);
 
     bool update(float* pos_estimate, float* vel_estimate, float* phase);
     bool run_calibration();
 
-    const EncoderHardwareConfig_t& hw_config;
-    EncoderConfig_t& config;
-    Axis* axis = nullptr; // set by Axis constructor
+    const EncoderHardwareConfig_t& hw_config_;
+    EncoderConfig_t& config_;
+    Axis* axis_ = nullptr; // set by Axis constructor
 
-    Error_t error = ERROR_NONE;
-    volatile bool index_found = false;
-    int32_t state = 0;
-    float phase = 0.0f;    // [rad]
-    float pll_pos = 0.0f;  // [rad]
-    float pll_vel = 0.0f;  // [rad/s]
-    float pll_kp = 0.0f;   // [rad/s / rad]
-    float pll_ki = 0.0f;   // [(rad/s^2) / rad]
+    Error_t error_ = ERROR_NONE;
+    bool index_found_ = false;
+    bool is_calibrated_ = config_.hand_calibrated;
+    int32_t state_ = 0;
+    float phase_ = 0.0f;    // [rad]
+    float pll_pos_ = 0.0f;  // [rad]
+    float pll_vel_ = 0.0f;  // [rad/s]
+    float pll_kp_ = 0.0f;   // [rad/s / rad]
+    float pll_ki_ = 0.0f;   // [(rad/s^2) / rad]
+
+    // Communication protocol definitions
+    auto make_protocol_definitions() {
+        return make_protocol_member_list(
+            make_protocol_object("config",
+                make_protocol_property("use_index", &config_.use_index),
+                make_protocol_property("hand_calibrated", &config_.hand_calibrated),
+                make_protocol_property("idx_search_speed", &config_.idx_search_speed),
+                make_protocol_property("cpr", &config_.cpr),
+                make_protocol_property("offset", &config_.offset),
+                make_protocol_property("calib_range", &config_.calib_range)
+            ),
+            make_protocol_property("error", &error_),
+            make_protocol_ro_property("index_found", const_cast<bool*>(&index_found_)),
+            make_protocol_property("state", &state_),
+            make_protocol_property("phase", &phase_),
+            make_protocol_property("pll_pos", &pll_pos_),
+            make_protocol_property("pll_vel", &pll_vel_),
+            make_protocol_property("pll_kp", &pll_kp_),
+            make_protocol_property("pll_ki", &pll_ki_)
+        );
+    }
 };
 
 #endif // __ENCODER_HPP

Firmware/MotorControl/low_level.cpp

@@ -76,20 +76,6 @@ void start_adc_pwm() {
     HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
 }
 
-void halt_motors(Motor::Error_t error) {
-    // Disable motors NOW!
-    for (size_t i = 0; i < AXIS_COUNT; ++i) {
-        axes[i]->motor.disarm();
-    }
-    // Set fault codes, etc.
-    for (size_t i = 0; i < AXIS_COUNT; ++i) {
-        axes[i]->motor.error = error;
-        axes[i]->error = Axis::ERROR_MOTOR_FAILED;
-    }
-    // disable brake resistor
-    set_brake_current(0.0f);
-}
-
 void start_pwm(TIM_HandleTypeDef* htim) {
     // Init PWM
     int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
@@ -155,6 +141,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
     htim_b->Instance->BDTR |= MOE_store_b;
 }
 
+// @brief Floats ALL phases immediately and sets the brake current to 0.
+void disable_all_pwms(Motor::Error_t error) {
+    // Disable all motors NOW!
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        axes[i]->motor_.disarm();
+        axes[i]->motor_.error_ = error;
+    }
+}
+
 //--------------------------------
 // IRQ Callbacks
 //--------------------------------
@@ -175,7 +170,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
 
     // Ensure ADCs are expected ones to simplify the logic below
     if (!(hadc == &hadc2 || hadc == &hadc3)) {
-        halt_motors(Motor::ERROR_ADC_FAILED);
+        disable_all_pwms(Motor::ERROR_ADC_FAILED);
         return;
     };
 
@@ -185,27 +180,35 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
     // If we are counting down, we just sampled in SVM vector 7, with zero current
     Axis& axis = injected ? *axes[0] : *axes[1];
     Axis& other_axis = injected ? *axes[1] : *axes[0];
-    bool counting_down = axis.motor.hw_config.timer->Instance->CR1 & TIM_CR1_DIR;
+    bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
 
     bool current_meas_not_DC_CAL = !counting_down;
-    if (&axis == axes[1] && counting_down) {
-        // Load next timings for M0 (only once is sufficient)
-        if (hadc == &hadc2) {
-            other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0];
-            other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1];
-            other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2];
-        }
-    } else if (&axis == axes[0] && !counting_down) {
-        // Load next timings for M1 (only once is sufficient)
-        if (hadc == &hadc2) {
-            other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0];
-            other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1];
-            other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2];
+    bool update_timings = false;
+    if (hadc == &hadc2) {
+        if (&axis == axes[1] && counting_down)
+            update_timings = true; // update timings of M0
+        else if (&axis == axes[0] && !counting_down)
+            update_timings = true; // update timings of M1
+    }
+
+    // Load next timings for the motor that we're not currently sampling
+    if (update_timings) {
+        if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) {
+            other_axis.motor_.next_timings_valid_ = false;
+            other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0];
+            other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1];
+            other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2];
+            __HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer);  // enable pwm outputs
+            update_brake_current();
+        } else {
+            // the motor control loop failed to update the timings in time
+            // we must assume that it died and therefore float all phases
+            other_axis.motor_.disarm();
         }
     }
 
     // Check the timing of the sequencing
-    axis.motor.check_timing();
+    axis.motor_.log_timing();
 
     uint32_t ADCValue;
     if (injected) {
@@ -213,7 +216,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
     } else {
         ADCValue = HAL_ADC_GetValue(hadc);
     }
-    float current = axis.motor.phase_current_from_adcval(ADCValue);
+    float current = axis.motor_.phase_current_from_adcval(ADCValue);
 
     if (current_meas_not_DC_CAL) {
         // ADC2 and ADC3 record the phB and phC currents concurrently,
@@ -224,33 +227,32 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
 
         // return or continue
         if (hadc == &hadc2) {
-            axis.motor.current_meas.phB = current - axis.motor.DC_calib.phB;
+            axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB;
             return;
         } else {
-            axis.motor.current_meas.phC = current - axis.motor.DC_calib.phC;
+            axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
         }
         // Trigger axis thread
-        axis.signal_thread(Axis::thread_signals::M_SIGNAL_PH_CURRENT_MEAS);
+        axis.signal_current_meas();
     } else {
         // DC_CAL measurement
         if (hadc == &hadc2) {
-            axis.motor.DC_calib.phB += (current - axis.motor.DC_calib.phB) * calib_filter_k;
+            axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k;
         } else {
-            axis.motor.DC_calib.phC += (current - axis.motor.DC_calib.phC) * calib_filter_k;
+            axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
         }
     }
 }
 
+// @brief Sums up the Ibus contribution of each motor and updates the
+// brake resistor PWM accordingly.
 void update_brake_current() {
     float Ibus_sum = 0.0f;
     for (size_t i = 0; i < AXIS_COUNT; ++i) {
-        Ibus_sum += axes[i]->motor.current_control.Ibus;
+        Ibus_sum += axes[i]->motor_.current_control_.Ibus;
     }
-    // Note: set_brake_current will clip negative values to 0.0f
-    set_brake_current(-Ibus_sum);
-}
-
-void set_brake_current(float brake_current) {
+    float brake_current = -Ibus_sum;
+    // Clip negative values to 0.0f
     if (brake_current < 0.0f) brake_current = 0.0f;
     float brake_duty = brake_current * brake_resistance / vbus_voltage;
 

Firmware/MotorControl/low_level.h

@@ -12,12 +12,6 @@ extern "C" {
 #include <adc.h>
 
 /* Exported types ------------------------------------------------------------*/
-
-typedef struct{
-        int type;
-        int index;
-} monitoring_slot;
-
 /* Exported constants --------------------------------------------------------*/
 /* Exported variables --------------------------------------------------------*/
 /* Exported macro ------------------------------------------------------------*/
@@ -35,7 +29,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
         uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset);
 
 void update_brake_current();
-void set_brake_current(float brake_current);
 
 #ifdef __cplusplus
 }

Firmware/MotorControl/main.cpp

@@ -1,7 +1,7 @@
 
 #include "odrive_main.hpp"
-#include <nvm_config.hpp>
-
+#include "nvm_config.hpp"
+#include "communication.h"
 
 EncoderConfig_t encoder_configs[AXIS_COUNT];
 ControllerConfig_t controller_configs[AXIS_COUNT];
@@ -67,7 +67,7 @@ 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]->config_.enable_step_dir = false;
         axes[0]->set_step_dir_enabled(false);
         SetGPIO12toUART();
     }

Firmware/MotorControl/motor.cpp

@@ -9,33 +9,61 @@
 Motor::Motor(const MotorHardwareConfig_t& hw_config,
          const GateDriverHardwareConfig_t& gate_driver_config,
          MotorConfig_t& config) :
-        hw_config(hw_config),
-        gate_driver_config(gate_driver_config),
-        config(config),
-        gate_driver({
-            .spiHandle = gate_driver_config.spi,
-            .EngpioHandle = gate_driver_config.enable_port,
-            .EngpioNumber = gate_driver_config.enable_pin,
-            .nCSgpioHandle = gate_driver_config.nCS_port,
-            .nCSgpioNumber = gate_driver_config.nCS_pin,
+        hw_config_(hw_config),
+        gate_driver_config_(gate_driver_config),
+        config_(config),
+        gate_driver_({
+            .spiHandle = gate_driver_config_.spi,
+            .EngpioHandle = gate_driver_config_.enable_port,
+            .EngpioNumber = gate_driver_config_.enable_pin,
+            .nCSgpioHandle = gate_driver_config_.nCS_port,
+            .nCSgpioNumber = gate_driver_config_.nCS_pin,
         })
 {
 }
 
-void Motor::arm() {
-    __HAL_TIM_MOE_ENABLE(hw_config.timer);  // enable pwm outputs
+// @brief Arms the PWM outputs that belong to this motor.
+//
+// Note that this does not yet activate the PWM outputs, it just unlocks them.
+//
+// While the motor is armed, the control loop must set new modulation timings
+// between any two interrupts (that is, enqueue_modulation_timings must be executed).
+// If the control loop fails to do so, the next interrupt handler floats the
+// phases. Once this happens, missed_control_deadline is set to true and
+// the motor can be considered disarmed.
+//
+// @returns: True on success, false otherwise
+bool Motor::arm() {
+    // Wait until the interrupt handler triggers twice. After the first wait there is an
+    // undefined period until the next trigger. After the second wait we know for sure
+    // that we have exactly one full interrupt period until the third trigger. This gives
+    // the control loop the correct time quota to set up modulation timings.
+    if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas()))
+        return false;
+    next_timings_valid_ = false;
+    axis_->missed_control_deadline_ = false;
+    return true;
 }
 
+// @brief Floats the phases of this motor immediately and updates
+// the brake current accordingly.
 void Motor::disarm() {
-    __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config.timer);  // disables pwm outputs
+    // disable pwm
+    __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer);
+    // set this motor's contribution to 0
+    current_control_.Ibus = 0.0f;
+    update_brake_current();
+    // ensure the PWM is not re-enabled without the state machine explicitly
+    // calling motor.arm()
+    axis_->missed_control_deadline_ = true;
 }
 
-// Set up the gate drivers
+// @brief Set up the gate drivers
 void Motor::DRV8301_setup() {
-    DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs;
+    DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
 
-    DRV8301_enable(&gate_driver);
-    DRV8301_setupSpi(&gate_driver, local_regs);
+    DRV8301_enable(&gate_driver_);
+    DRV8301_setupSpi(&gate_driver_, local_regs);
 
     // TODO we can use reporting only if we actually wire up the nOCTW pin
     local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;
@@ -50,41 +78,42 @@ void Motor::DRV8301_setup() {
 
     switch (local_regs->Ctrl_Reg_2.GAIN) {
         case DRV8301_ShuntAmpGain_10VpV:
-            phase_current_rev_gain = 1.0f / 10.0f;
+            phase_current_rev_gain_ = 1.0f / 10.0f;
             break;
         case DRV8301_ShuntAmpGain_20VpV:
-            phase_current_rev_gain = 1.0f / 20.0f;
+            phase_current_rev_gain_ = 1.0f / 20.0f;
             break;
         case DRV8301_ShuntAmpGain_40VpV:
-            phase_current_rev_gain = 1.0f / 40.0f;
+            phase_current_rev_gain_ = 1.0f / 40.0f;
             break;
         case DRV8301_ShuntAmpGain_80VpV:
-            phase_current_rev_gain = 1.0f / 80.0f;
+            phase_current_rev_gain_ = 1.0f / 80.0f;
             break;
     }
 
     float margin = 0.90f;
-    float max_input = margin * 0.3f * hw_config.shunt_conductance;
-    float max_swing = margin * 1.6f * hw_config.shunt_conductance * phase_current_rev_gain;
-    current_control.max_allowed_current = std::min(max_input, max_swing);
+    float max_input = margin * 0.3f * hw_config_.shunt_conductance;
+    float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_;
+    current_control_.max_allowed_current = std::min(max_input, max_swing);
 
     local_regs->SndCmd = true;
-    DRV8301_writeData(&gate_driver, local_regs);
+    DRV8301_writeData(&gate_driver_, local_regs);
     local_regs->RcvCmd = true;
-    DRV8301_readData(&gate_driver, local_regs);
+    DRV8301_readData(&gate_driver_, local_regs);
 }
 
-//Returns true if everything is OK (no fault)
+// @brief Checks if the gate driver is in operational state.
+// @returns: true if the gate driver is OK (no fault), false otherwise
 bool Motor::check_DRV_fault() {
     //TODO: make this pin configurable per motor ch
-    GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config.nFAULT_port, gate_driver_config.nFAULT_pin);
+    GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin);
     if (nFAULT_state == GPIO_PIN_RESET) {
         // Update DRV Fault Code
-        drv_fault = DRV8301_getFaultType(&gate_driver);
+        drv_fault_ = DRV8301_getFaultType(&gate_driver_);
         // Update/Cache all SPI device registers
-        DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs;
+        DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
         local_regs->RcvCmd = true;
-        DRV8301_readData(&gate_driver, local_regs);
+        DRV8301_readData(&gate_driver_, local_regs);
         return false;
     };
     return true;
@@ -92,14 +121,14 @@ bool Motor::check_DRV_fault() {
 
 bool Motor::do_checks() {
     if (!check_DRV_fault()) {
-        error = ERROR_DRV_FAULT;
+        error_ = ERROR_DRV_FAULT;
         return false;
     }
     return true;
 }
 
-uint16_t Motor::check_timing() {
-    TIM_HandleTypeDef* htim = hw_config.timer;
+void Motor::log_timing() {
+    TIM_HandleTypeDef* htim = hw_config_.timer;
     uint16_t timing = htim->Instance->CNT;
     bool down = htim->Instance->CR1 & TIM_CR1_DIR;
     if (down) {
@@ -107,19 +136,17 @@ uint16_t Motor::check_timing() {
         timing = TIM_1_8_PERIOD_CLOCKS + delta;
     }
 
-    if (++(timing_log_index) == TIMING_LOG_SIZE) {
-        timing_log_index = 0;
+    if (++(timing_log_index_) == TIMING_LOG_SIZE) {
+        timing_log_index_ = 0;
     }
-    timing_log[timing_log_index] = timing;
-
-    return timing;
+    timing_log_[timing_log_index_] = timing;
 }
 
 float Motor::phase_current_from_adcval(uint32_t ADCValue) {
     int adcval_bal = (int)ADCValue - (1 << 11);
     float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
-    float shunt_volt = amp_out_volt * phase_current_rev_gain;
-    float current = shunt_volt * hw_config.shunt_conductance;
+    float shunt_volt = amp_out_volt * phase_current_rev_gain_;
+    float current = shunt_volt * hw_config_.shunt_conductance;
     return current;
 }
 
@@ -134,25 +161,25 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
     float test_voltage = 0.0f;
     
     size_t i = 0;
-    axis->run_control_loop([&](){
-        float Ialpha = -(current_meas.phB + current_meas.phC);
+    axis_->run_control_loop([&](){
+        float Ialpha = -(current_meas_.phB + current_meas_.phC);
         test_voltage += (kI * current_meas_period) * (test_current - Ialpha);
         if (test_voltage > max_voltage || test_voltage < -max_voltage)
-            return error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false;
+            return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false;
 
         // Test voltage along phase A
         enqueue_voltage_timings(test_voltage, 0.0f);
 
         return ++i < num_test_cycles;
     });
-    if (axis->error != Axis::ERROR_NO_ERROR)
+    if (axis_->error_ != Axis::ERROR_NO_ERROR)
         return false;
 
     //// De-energize motor
     //enqueue_voltage_timings(motor, 0.0f, 0.0f);
 
     float R = test_voltage / test_current;
-    config.phase_resistance = R;
+    config_.phase_resistance = R;
     return true; // if we ran to completion that means success
 }
 
@@ -162,16 +189,16 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
     static const int num_cycles = 5000;
 
     size_t t = 0;
-    axis->run_control_loop([&](){
+    axis_->run_control_loop([&](){
         int i = t & 1;
-        Ialphas[i] += -current_meas.phB - current_meas.phC;
+        Ialphas[i] += -current_meas_.phB - current_meas_.phC;
 
         // Test voltage along phase A
         enqueue_voltage_timings(test_voltages[i], 0.0f);
 
         return ++t < (num_cycles << 1);
     });
-    if (axis->error != Axis::ERROR_NO_ERROR)
+    if (axis_->error_ != Axis::ERROR_NO_ERROR)
         return false;
 
     //// De-energize motor
@@ -183,24 +210,24 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
     float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles);
     float L = v_L / dI_by_dt;
 
-    config.phase_inductance = L;
+    config_.phase_inductance = L;
     // TODO arbitrary values set for now
     if (L < 1e-6f || L > 500e-6f)
-        return error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false;
+        return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false;
     return true;
 }
 
 
 bool Motor::run_calibration() {
-    error = ERROR_NO_ERROR;
+    error_ = ERROR_NO_ERROR;
 
-    float R_calib_max_voltage = config.resistance_calib_max_voltage;
-    if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
-        if (!measure_phase_resistance(config.calibration_current, R_calib_max_voltage))
+    float R_calib_max_voltage = config_.resistance_calib_max_voltage;
+    if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
+        if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage))
             return false;
         if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage))
             return false;
-    } else if (config.motor_type == MOTOR_TYPE_GIMBAL) {
+    } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
         // no calibration needed
     } else {
         return false;
@@ -208,19 +235,21 @@ bool Motor::run_calibration() {
 
     // Calculate current control gains
     float current_control_bandwidth = 1000.0f;  // [rad/s]
-    current_control.p_gain = current_control_bandwidth * config.phase_inductance;
-    float plant_pole = config.phase_resistance / config.phase_inductance;
-    current_control.i_gain = plant_pole * current_control.p_gain;
-
+    current_control_.p_gain = current_control_bandwidth * config_.phase_inductance;
+    float plant_pole = config_.phase_resistance / config_.phase_inductance;
+    current_control_.i_gain = plant_pole * current_control_.p_gain;
+    
+    is_calibrated_ = true;
     return true;
 }
 
 void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
     float tA, tB, tC;
     SVM(mod_alpha, mod_beta, &tA, &tB, &tC);
-    next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
-    next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
-    next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
+    next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
+    next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
+    next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
+    next_timings_valid_ = true;
 }
 
 void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
@@ -242,14 +271,14 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
 }
 
 bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
-    Current_control_t* ictrl = &current_control;
+    Current_control_t* ictrl = &current_control_;
 
     // For Reporting
     ictrl->Iq_setpoint = Iq_des;
 
     // Clarke transform
-    float Ialpha = -current_meas.phB - current_meas.phC;
-    float Ibeta = one_by_sqrt3 * (current_meas.phB - current_meas.phC);
+    float Ialpha = -current_meas_.phB - current_meas_.phC;
+    float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
 
     // Park transform
     float c = arm_cos_f32(phase);
@@ -306,21 +335,21 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
 
 
 bool Motor::update(float current_setpoint, float phase) {
-    current_setpoint *= config.direction;
-    phase *= config.direction;
+    current_setpoint *= config_.direction;
+    phase *= config_.direction;
 
     // Execute current command
     // TODO: move this into the mot
-    if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
+    if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
         if(!FOC_current(0.0f, current_setpoint, phase)){
             return false;
         }
-    } else if (config.motor_type == MOTOR_TYPE_GIMBAL) {
+    } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
         //In gimbal motor mode, current is reinterptreted as voltage.
         if(!FOC_voltage(0.0f, current_setpoint, phase))
             return false;
     } else {
-        error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE;
+        error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE;
         return false;
     }
     return true;

Firmware/MotorControl/motor.hpp

@@ -36,6 +36,7 @@ typedef struct {
 // example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
 // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
 typedef struct {
+    bool hand_calibrated = true; // can be set to true to indicate that all values here are valid
     int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series.
     float calibration_current = 10.0f;    // [A]
     float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
@@ -67,7 +68,7 @@ public:
          const GateDriverHardwareConfig_t& gate_driver_config,
          MotorConfig_t& config);
 
-    void arm();
+    bool arm();
     void disarm();
     void setup() {
         DRV8301_setup();
@@ -75,7 +76,7 @@ public:
     void DRV8301_setup();
     bool check_DRV_fault();
     bool do_checks();
-    uint16_t check_timing();
+    void log_timing();
     float phase_current_from_adcval(uint32_t ADCValue);
     bool measure_phase_resistance(float test_current, float max_voltage);
     bool measure_phase_inductance(float voltage_low, float voltage_high);
@@ -86,30 +87,32 @@ public:
     bool FOC_current(float Id_des, float Iq_des, float phase);
     bool update(float current_setpoint, float phase);
 
-    const MotorHardwareConfig_t& hw_config;
-    const GateDriverHardwareConfig_t gate_driver_config;
-    MotorConfig_t& config;
-    Axis* axis = nullptr; // set by Axis constructor
+    const MotorHardwareConfig_t& hw_config_;
+    const GateDriverHardwareConfig_t gate_driver_config_;
+    MotorConfig_t& config_;
+    Axis* axis_ = nullptr; // set by Axis constructor
 
 //private:
 
-    DRV8301_Obj gate_driver; // initialized in constructor
-    uint16_t next_timings[3] = {
+    DRV8301_Obj gate_driver_; // initialized in constructor
+    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 };
+    bool next_timings_valid_ = false;
+    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};
-    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 = {
+    Error_t error_ = ERROR_NO_ERROR;
+    bool is_calibrated_ = config_.hand_calibrated;
+    Iph_BC_t current_meas_ = {0.0f, 0.0f};
+    Iph_BC_t DC_calib_ = {0.0f, 0.0f};
+    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
         .i_gain = 0.0f,        // [V/As] should be auto set after resistance and inductance measurement
         .v_current_control_integral_d = 0.0f,
@@ -121,47 +124,47 @@ public:
         .Iq_measured = 0.0f,
         .max_allowed_current = 0.0f,
     };
-    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)
+    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_property("error", &error_),
+            make_protocol_ro_property("current_meas_phB", &current_meas_.phB),
+            make_protocol_ro_property("current_meas_phC", &current_meas_.phC),
+            make_protocol_property("DC_calib_phB", &DC_calib_.phB),
+            make_protocol_property("DC_calib_phC", &DC_calib_.phC),
+            make_protocol_property("shunt_conductance", &shunt_conductance_),
+            make_protocol_property("phase_current_rev_gain", &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_property("p_gain", &current_control_.p_gain),
+                make_protocol_property("i_gain", &current_control_.i_gain),
+                make_protocol_property("v_current_control_integral_d", &current_control_.v_current_control_integral_d),
+                make_protocol_property("v_current_control_integral_q", &current_control_.v_current_control_integral_q),
+                make_protocol_property("Ibus", &current_control_.Ibus),
+                make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
+                make_protocol_property("final_v_beta", &current_control_.final_v_beta),
+                make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
+                make_protocol_property("Iq_measured", &current_control_.Iq_measured),
+                make_protocol_property("max_allowed_current", &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_ro_property("drv_fault", &drv_fault_),
+                make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value),
+                make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value),
+                make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value),
+                make_protocol_ro_property("ctrl_reg_2", &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)
+                make_protocol_property("pole_pairs", &config_.pole_pairs),
+                make_protocol_property("calibration_current", &config_.calibration_current),
+                make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
+                make_protocol_property("phase_inductance", &config_.phase_inductance),
+                make_protocol_property("phase_resistance", &config_.phase_resistance),
+                make_protocol_property("direction", &config_.direction),
+                make_protocol_property("motor_type", &config_.motor_type),
+                make_protocol_property("current_lim", &config_.current_lim)
             )
         );
     }

Firmware/MotorControl/odrive_main.hpp

@@ -44,8 +44,6 @@ 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);

Firmware/MotorControl/protocol.cpp

@@ -118,10 +118,6 @@ JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint();
 EndpointProvider* application_endpoints;
 uint16_t json_crc_;
 
-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);
 
@@ -162,9 +158,9 @@ void set_application_endpoints(EndpointProvider* endpoints) {
     application_endpoints = endpoints;
 
     n_endpoints_ = 0;
-    json_file_endpoint.register_endpoints(endpoints_, 0, MAX_ENDPOINTS);
+    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);
+    application_endpoints->register_endpoints(endpoints_, n_endpoints_, max_endpoints_);
     n_endpoints_ += application_endpoints->get_endpoint_count();
     
     // Calculates the CRC16 of the JSON file.
@@ -174,7 +170,6 @@ void set_application_endpoints(EndpointProvider* endpoints) {
     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();

Firmware/MotorControl/protocol.hpp

@@ -369,8 +369,6 @@ 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_;
@@ -417,7 +415,6 @@ 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
@@ -432,7 +429,6 @@ 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) :
@@ -441,7 +437,7 @@ public:
 
     MemberList(TMember&& this_member, MemberList<TMembers...>&& subsequent_members) :
         this_member_(std::forward<TMember>(this_member)),
-        subsequent_members_(std::forward(subsequent_members)) {}
+        subsequent_members_(std::forward<MemberList<TMembers...>>(subsequent_members)) {}
 
     // @brief Move constructor
 /*    MemberList(MemberList&& other) :
@@ -572,16 +568,30 @@ public:
     TProperty* property_;
 };
 
-template<typename TProperty>
+// Non-const non-enum types
+template<typename TProperty, typename = std::enable_if_t<!std::is_enum<TProperty>::value>>
 ProtocolProperty<TProperty> make_protocol_property(const char * name, TProperty* property) {
     return ProtocolProperty<TProperty>(name, property);
 };
 
-template<typename TProperty>
+// Const non-enum types
+template<typename TProperty, typename = std::enable_if_t<!std::is_enum<TProperty>::value>>
 ProtocolProperty<const TProperty> make_protocol_ro_property(const char * name, const TProperty* property) {
     return ProtocolProperty<const TProperty>(name, property);
 };
 
+// Non-const enum types
+template<typename TProperty, typename = std::enable_if_t<std::is_enum<TProperty>::value>>
+ProtocolProperty<std::underlying_type_t<TProperty>> make_protocol_property(const char * name, TProperty* property) {
+    return ProtocolProperty<std::underlying_type_t<TProperty>>(name, reinterpret_cast<std::underlying_type_t<TProperty>*>(property));
+};
+
+// Const enum types
+template<typename TProperty, typename = std::enable_if_t<std::is_enum<TProperty>::value>>
+ProtocolProperty<const std::underlying_type_t<TProperty>> make_protocol_ro_property(const char * name, const TProperty* property) {
+    return ProtocolProperty<const std::underlying_type_t<TProperty>>(name, reinterpret_cast<const std::underlying_type_t<TProperty>*>(property));
+};
+
 
 
 template<typename TObj, typename TRet, typename ... TArgs>
@@ -636,7 +646,7 @@ struct PropertyListFactory<TProperty, TProperties...> {
     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)),
+            make_protocol_property(std::get<IPos>(names), &std::get<IPos>(values)),
             PropertyListFactory<TProperties...>::template make_property_list<IPos+1>(names, values)
         );
     }
@@ -715,7 +725,7 @@ 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();
+        return T::endpoint_count;
     }
     void write_json(size_t id, StreamSink* output) final {
         return member_list_.write_json(id, output);
@@ -728,4 +738,10 @@ public:
 
 void set_application_endpoints(EndpointProvider* endpoints);
 
+
+// defined in communication.cpp
+extern Endpoint* endpoints_[];
+extern size_t n_endpoints_;
+extern const size_t max_endpoints_;
+
 #endif

Firmware/MotorControl/sensorless_estimator.cpp

@@ -7,10 +7,10 @@ SensorlessEstimator::SensorlessEstimator()
     // Calculate pll gains
     // This calculation is currently identical to the PLL in Encoder
     float pll_bandwidth = 1000.0f;  // [rad/s]
-    pll_kp = 2.0f * pll_bandwidth;
+    pll_kp_ = 2.0f * pll_bandwidth;
 
     // Critically damped
-    pll_ki = 0.25f * (pll_kp * pll_kp);
+    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
 }
 
 bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
@@ -23,35 +23,35 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float
     // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
 
     // Check that we don't get problems with discrete time approximation
-    if (!(current_meas_period * pll_kp < 1.0f)) {
-        error = ERROR_NUMERICAL;
+    if (!(current_meas_period * pll_kp_ < 1.0f)) {
+        error_ = ERROR_NUMERICAL;
         return false;
     }
 
     // Clarke transform
     float I_alpha_beta[2] = {
-        -axis->motor.current_meas.phB - axis->motor.current_meas.phC,
-        one_by_sqrt3 * (axis->motor.current_meas.phB - axis->motor.current_meas.phC)};
+        -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC,
+        one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)};
 
     // alpha-beta vector operations
     float eta[2];
     for (int i = 0; i <= 1; ++i) {
         // y is the total flux-driving voltage (see paper eqn 4)
-        float y = -axis->motor.config.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory[i];
+        float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
         // flux dynamics (prediction)
         float x_dot = y;
         // integrate prediction to current timestep
-        flux_state[i] += x_dot * current_meas_period;
+        flux_state_[i] += x_dot * current_meas_period;
 
         // eta is the estimated permanent magnet flux (see paper eqn 6)
-        eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i];
+        eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
     }
 
     // Non-linear observer (see paper eqn 8):
-    float pm_flux_sqr = pm_flux_linkage * pm_flux_linkage;
+    float pm_flux_sqr = pm_flux_linkage_ * pm_flux_linkage_;
     float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
-    float bandwidth_factor = 1.0f / (pm_flux_linkage * pm_flux_linkage);
-    float eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
+    float bandwidth_factor = 1.0f / pm_flux_sqr;
+    float eta_factor = 0.5f * (observer_gain_ * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
 
     static float eta_factor_avg_test = 0.0f;
     eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test);
@@ -61,25 +61,25 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float
         // add observer action to flux estimate dynamics
         float x_dot = eta_factor * eta[i];
         // convert action to discrete-time
-        flux_state[i] += x_dot * current_meas_period;
+        flux_state_[i] += x_dot * current_meas_period;
         // update new eta
-        eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i];
+        eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
     }
 
     // Flux state estimation done, store V_alpha_beta for next timestep
-    V_alpha_beta_memory[0] = axis->motor.current_control.final_v_alpha;
-    V_alpha_beta_memory[1] = axis->motor.current_control.final_v_beta;
+    V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha;
+    V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta;
 
     // PLL
     // TODO: the PLL part has some code duplication with the encoder PLL
     // predict PLL phase with velocity
-    pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_vel);
+    pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_vel_);
     // update PLL phase with observer permanent magnet phase
-    phase = fast_atan2(eta[1], eta[0]);
-    float delta_phase = wrap_pm_pi(phase - pll_pos);
-    pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_kp * delta_phase);
+    phase_ = fast_atan2(eta[1], eta[0]);
+    float delta_phase = wrap_pm_pi(phase_ - pll_pos_);
+    pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp_ * delta_phase);
     // update PLL velocity
-    pll_vel += current_meas_period * pll_ki * delta_phase;
+    pll_vel_ += current_meas_period * pll_ki_ * delta_phase;
 
     //TODO TEMP TEST HACK
     // static int trigger_ctr = 0;
@@ -94,8 +94,8 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float
     //     motor->rotor_mode = ROTOR_MODE_SENSORLESS;
     // }
 
-    if (pos_estimate) *pos_estimate = pll_pos;
-    if (vel_estimate) *vel_estimate = pll_vel;
-    if (phase_output) *phase_output = phase;
+    if (pos_estimate) *pos_estimate = pll_pos_;
+    if (vel_estimate) *vel_estimate = pll_vel_;
+    if (phase_output) *phase_output = phase_;
     return true;
 };

Firmware/MotorControl/sensorless_estimator.hpp

@@ -12,20 +12,20 @@ public:
 
     bool update(float* pos_estimate, float* vel_estimate, float* phase);
 
-    Axis* axis = nullptr; // set by Axis constructor
+    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]
-    float pll_vel = 0.0f;                      // [rad/s]
-    float pll_kp = 0.0f;                       // [rad/s / rad]
-    float pll_ki = 0.0f;                       // [(rad/s^2) / rad]
-    float observer_gain = 1000.0f;             // [rad/s]
-    float flux_state[2] = {0.0f, 0.0f};        // [Vs]
-    float V_alpha_beta_memory[2] = {0.0f, 0.0f}; // [V]
-    float pm_flux_linkage = 1.58e-3f;          // [V / (rad/s)]  { 5.51328895422 / (<pole pairs> * <rpm/v>) }
-    bool estimator_good = false;
+    Error_t error_ = ERROR_NONE;
+    float phase_ = 0.0f;                        // [rad]
+    float pll_pos_ = 0.0f;                      // [rad]
+    float pll_vel_ = 0.0f;                      // [rad/s]
+    float pll_kp_ = 0.0f;                       // [rad/s / rad]
+    float pll_ki_ = 0.0f;                       // [(rad/s^2) / rad]
+    float observer_gain_ = 1000.0f;             // [rad/s]
+    float flux_state_[2] = {0.0f, 0.0f};        // [Vs]
+    float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
+    float pm_flux_linkage_ = 1.58e-3f;          // [V / (rad/s)]  { 5.51328895422 / (<pole pairs> * <rpm/v>) }
+    bool estimator_good_ = false;
 };
 
 #endif /* __SENSORLESS_ESTIMATOR_HPP */

Firmware/Tupfile.lua

@@ -68,7 +68,7 @@ build{
         'MotorControl/low_level.cpp',
         'MotorControl/nvm.c',
         'MotorControl/axis.cpp',
-        'MotorControl/commands.cpp',
+        'MotorControl/communication.cpp',
         'MotorControl/protocol.cpp',
         'MotorControl/motor.cpp',
         'MotorControl/encoder.cpp',