*

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/arm_cos_f32.c

@@ -0,0 +1,122 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_cos_f32.c
+ * Description:  Fast cosine calculation for floating-point values
+ *
+ * $Date:        27. January 2017
+ * $Revision:    V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <stm32f4xx_hal.h>  // Sets up the correct chip specifc defines required by arm_math
+#define ARM_MATH_CM4 // TODO: might change in future board versions
+#include "arm_math.h"
+#include "arm_common_tables.h"
+/**
+ * @ingroup groupFastMath
+ */
+
+/**
+ * @defgroup cos Cosine
+ *
+ * Computes the trigonometric cosine function using a combination of table lookup
+ * and linear interpolation.  There are separate functions for
+ * Q15, Q31, and floating-point data types.
+ * The input to the floating-point version is in radians and in the range [0 2*pi) while the
+ * fixed-point Q15 and Q31 have a scaled input with the range
+ * [0 +0.9999] mapping to [0 2*pi).  The fixed-point range is chosen so that a
+ * value of 2*pi wraps around to 0.
+ *
+ * The implementation is based on table lookup using 256 values together with linear interpolation.
+ * The steps used are:
+ *  -# Calculation of the nearest integer table index
+ *  -# Compute the fractional portion (fract) of the table index.
+ *  -# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
+ *
+ * where
+ * <pre>
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ * </pre>
+ */
+
+ /**
+ * @addtogroup cos
+ * @{
+ */
+
+/**
+ * @brief  Fast approximation to the trigonometric cosine function for floating-point data.
+ * @param[in] x input value in radians.
+ * @return cos(x).
+ */
+
+float32_t our_arm_cos_f32(
+  float32_t x)
+{
+  float32_t cosVal, fract, in;                   /* Temporary variables for input, output */
+  uint16_t index;                                /* Index variable */
+  float32_t a, b;                                /* Two nearest output values */
+  int32_t n;
+  float32_t findex;
+
+  /* input x is in radians */
+  /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi, add 0.25 (pi/2) to read sine table */
+  in = x * 0.159154943092f + 0.25f;
+
+  /* Calculation of floor value of input */
+  n = (int32_t) in;
+
+  /* Make negative values towards -infinity */
+  if (in < 0.0f)
+  {
+    n--;
+  }
+
+  /* Map input value to [0 1] */
+  in = in - (float32_t) n;
+
+  /* Calculation of index of the table */
+  findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
+  index = (uint16_t)findex;
+
+  /* when "in" is exactly 1, we need to rotate the index down to 0 */
+  if (index >= FAST_MATH_TABLE_SIZE) {
+    index = 0;
+    findex -= (float32_t)FAST_MATH_TABLE_SIZE;
+  }
+
+  /* fractional value calculation */
+  fract = findex - (float32_t) index;
+
+  /* Read two nearest values of input value from the cos table */
+  a = sinTable_f32[index];
+  b = sinTable_f32[index+1];
+
+  /* Linear interpolation process */
+  cosVal = (1.0f-fract)*a + fract*b;
+
+  /* Return the output value */
+  return (cosVal);
+}
+
+/**
+ * @} end of cos group
+ */

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/arm_sin_f32.c

@@ -0,0 +1,124 @@
+/* ----------------------------------------------------------------------
+ * Project:      CMSIS DSP Library
+ * Title:        arm_sin_f32.c
+ * Description:  Fast sine calculation for floating-point values
+ *
+ * $Date:        27. January 2017
+ * $Revision:    V.1.5.1
+ *
+ * Target Processor: Cortex-M cores
+ * -------------------------------------------------------------------- */
+/*
+ * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the License); you may
+ * not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an AS IS BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <stm32f4xx_hal.h>  // Sets up the correct chip specifc defines required by arm_math
+#define ARM_MATH_CM4 // TODO: might change in future board versions
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+/**
+ * @ingroup groupFastMath
+ */
+
+/**
+ * @defgroup sin Sine
+ *
+ * Computes the trigonometric sine function using a combination of table lookup
+ * and linear interpolation.  There are separate functions for
+ * Q15, Q31, and floating-point data types.
+ * The input to the floating-point version is in radians and in the range [0 2*pi) while the
+ * fixed-point Q15 and Q31 have a scaled input with the range
+ * [0 +0.9999] mapping to [0 2*pi).  The fixed-point range is chosen so that a
+ * value of 2*pi wraps around to 0.
+ *
+ * The implementation is based on table lookup using 256 values together with linear interpolation.
+ * The steps used are:
+ *  -# Calculation of the nearest integer table index
+ *  -# Compute the fractional portion (fract) of the table index.
+ *  -# The final result equals <code>(1.0f-fract)*a + fract*b;</code>
+ *
+ * where
+ * <pre>
+ *    b=Table[index+0];
+ *    c=Table[index+1];
+ * </pre>
+ */
+
+/**
+ * @addtogroup sin
+ * @{
+ */
+
+/**
+ * @brief  Fast approximation to the trigonometric sine function for floating-point data.
+ * @param[in] x input value in radians.
+ * @return  sin(x).
+ */
+
+float32_t our_arm_sin_f32(
+  float32_t x)
+{
+  float32_t sinVal, fract, in;                           /* Temporary variables for input, output */
+  uint16_t index;                                        /* Index variable */
+  float32_t a, b;                                        /* Two nearest output values */
+  int32_t n;
+  float32_t findex;
+
+  /* input x is in radians */
+  /* Scale the input to [0 1] range from [0 2*PI] , divide input by 2*pi */
+  in = x * 0.159154943092f;
+
+  /* Calculation of floor value of input */
+  n = (int32_t) in;
+
+  /* Make negative values towards -infinity */
+  if (x < 0.0f)
+  {
+    n--;
+  }
+
+  /* Map input value to [0 1] */
+  in = in - (float32_t) n;
+
+  /* Calculation of index of the table */
+  findex = (float32_t)FAST_MATH_TABLE_SIZE * in;
+  index = (uint16_t)findex;
+
+  /* when "in" is exactly 1, we need to rotate the index down to 0 */
+  if (index >= FAST_MATH_TABLE_SIZE) {
+    index = 0;
+    findex -= (float32_t)FAST_MATH_TABLE_SIZE;
+  }
+
+  /* fractional value calculation */
+  fract = findex - (float32_t) index;
+
+  /* Read two nearest values of input value from the sin table */
+  a = sinTable_f32[index];
+  b = sinTable_f32[index+1];
+
+  /* Linear interpolation process */
+  sinVal = (1.0f-fract)*a + fract*b;
+
+  /* Return the output value */
+  return (sinVal);
+}
+
+/**
+ * @} end of sin group
+ */

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/axis.cpp

@@ -0,0 +1,593 @@
+
+#include <stdlib.h>
+#include <functional>
+#include "gpio.h"
+
+#include "odrive_main.h"
+#include "utils.hpp"
+#include "gpio_utils.hpp"
+#include "communication/interface_can.hpp"
+
+Axis::Axis(int axis_num,
+           const AxisHardwareConfig_t& hw_config,
+           Config_t& config,
+           Encoder& encoder,
+           SensorlessEstimator& sensorless_estimator,
+           Controller& controller,
+           OnboardThermistorCurrentLimiter& fet_thermistor,
+           OffboardThermistorCurrentLimiter& motor_thermistor,
+           Motor& motor,
+           TrapezoidalTrajectory& trap,
+           Endstop& min_endstop,
+           Endstop& max_endstop)
+    : axis_num_(axis_num),
+      hw_config_(hw_config),
+      config_(config),
+      encoder_(encoder),
+      sensorless_estimator_(sensorless_estimator),
+      controller_(controller),
+      fet_thermistor_(fet_thermistor),
+      motor_thermistor_(motor_thermistor),
+      motor_(motor),
+      trap_traj_(trap),
+      min_endstop_(min_endstop),
+      max_endstop_(max_endstop),
+      current_limiters_(make_array(
+          static_cast<CurrentLimiter*>(&fet_thermistor),
+          static_cast<CurrentLimiter*>(&motor_thermistor))),
+      thermistors_(make_array(
+          static_cast<ThermistorCurrentLimiter*>(&fet_thermistor),
+          static_cast<ThermistorCurrentLimiter*>(&motor_thermistor)))
+{
+    encoder_.axis_ = this;
+    sensorless_estimator_.axis_ = this;
+    controller_.axis_ = this;
+    fet_thermistor_.axis_ = this;
+    motor_thermistor.axis_ = this;
+    motor_.axis_ = this;
+    trap_traj_.axis_ = this;
+    min_endstop_.axis_ = this;
+    max_endstop_.axis_ = this;
+    decode_step_dir_pins();
+    watchdog_feed();
+}
+
+Axis::LockinConfig_t Axis::default_calibration() {
+    Axis::LockinConfig_t config;
+    config.current = 10.0f;           // [A]
+    config.ramp_time = 0.4f;          // [s]
+    config.ramp_distance = 1 * M_PI;  // [rad]
+    config.accel = 20.0f;     // [rad/s^2]
+    config.vel = 40.0f; // [rad/s]
+    config.finish_distance = 100.0f * 2.0f * M_PI;  // [rad]
+    config.finish_on_vel = false;
+    config.finish_on_distance = true;
+    config.finish_on_enc_idx = true;
+    return config;
+}
+
+Axis::LockinConfig_t Axis::default_sensorless() {
+    Axis::LockinConfig_t config;
+    config.current = 10.0f;           // [A]
+    config.ramp_time = 0.4f;          // [s]
+    config.ramp_distance = 1 * M_PI;  // [rad]
+    config.accel = 200.0f;     // [rad/s^2]
+    config.vel = 400.0f; // [rad/s]
+    config.finish_distance = 100.0f;  // [rad]
+    config.finish_on_vel = true;
+    config.finish_on_distance = false;
+    config.finish_on_enc_idx = false;
+    return config;
+}
+
+static void step_cb_wrapper(void* ctx) {
+    reinterpret_cast<Axis*>(ctx)->step_cb();
+}
+
+
+// @brief Does Nothing
+void Axis::setup() {
+    // Does nothing - Motor and encoder setup called separately.
+}
+
+static void run_state_machine_loop_wrapper(void* ctx) {
+    reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
+    reinterpret_cast<Axis*>(ctx)->thread_id_valid_ = false;
+}
+
+// @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, stack_size_ / sizeof(StackType_t));
+    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_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() {
+    return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal;
+}
+
+// step/direction interface
+void Axis::step_cb() {
+    const bool dir_pin = dir_port_->IDR & dir_pin_;
+    const int32_t dir = (-1 + 2 * dir_pin) * step_dir_active_;
+    controller_.input_pos_ += dir * config_.turns_per_step;
+    controller_.input_pos_updated();
+};
+
+void Axis::load_default_step_dir_pin_config(
+        const AxisHardwareConfig_t& hw_config, Config_t* config) {
+    config->step_gpio_pin = hw_config.step_gpio_pin;
+    config->dir_gpio_pin = hw_config.dir_gpio_pin;
+}
+
+void Axis::load_default_can_id(const int& id, Config_t& config){
+    config.can_node_id = id;
+}
+
+void Axis::decode_step_dir_pins() {
+    step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin);
+    step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
+    dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin);
+    dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
+}
+
+// @brief (de)activates step/dir input
+void Axis::set_step_dir_active(bool active) {
+    if (active) {
+        // Set up the direction GPIO as input
+        GPIO_InitTypeDef GPIO_InitStruct;
+        GPIO_InitStruct.Pin = dir_pin_;
+        GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+        GPIO_InitStruct.Pull = GPIO_NOPULL;
+        HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);
+
+        // Subscribe to rising edges of the step GPIO
+        GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, step_cb_wrapper, this);
+
+        step_dir_active_ = true;
+    } else {
+        step_dir_active_ = false;
+
+        // Unsubscribe from step GPIO
+        GPIO_unsubscribe(step_port_, step_pin_);
+    }
+}
+
+// @brief Do axis level checks and call subcomponent do_checks
+// Returns true if everything is ok.
+bool Axis::do_checks() {
+    if (!brake_resistor_armed)
+        error_ |= ERROR_BRAKE_RESISTOR_DISARMED;
+    if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED))
+        // motor got disarmed in something other than the idle loop
+        error_ |= ERROR_MOTOR_DISARMED;
+    if (!(vbus_voltage >= odrv.config_.dc_bus_undervoltage_trip_level))
+        error_ |= ERROR_DC_BUS_UNDER_VOLTAGE;
+    if (!(vbus_voltage <= odrv.config_.dc_bus_overvoltage_trip_level))
+        error_ |= ERROR_DC_BUS_OVER_VOLTAGE;
+
+    // Sub-components should use set_error which will propegate to this error_
+    for (ThermistorCurrentLimiter* thermistor : thermistors_) {
+        thermistor->do_checks();
+    }
+    motor_.do_checks();
+    // encoder_.do_checks();
+    // sensorless_estimator_.do_checks();
+    // controller_.do_checks();
+
+    // Check for endstop presses
+    if (min_endstop_.config_.enabled && min_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
+        error_ |= ERROR_MIN_ENDSTOP_PRESSED;
+    } else if (max_endstop_.config_.enabled && max_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
+        error_ |= ERROR_MAX_ENDSTOP_PRESSED;
+    }
+
+    return check_for_errors();
+}
+
+// @brief Update all esitmators
+bool Axis::do_updates() {
+    // Sub-components should use set_error which will propegate to this error_
+    for (ThermistorCurrentLimiter* thermistor : thermistors_) {
+        thermistor->update();
+    }
+    encoder_.update();
+    sensorless_estimator_.update();
+    min_endstop_.update();
+    max_endstop_.update();
+    bool ret = check_for_errors();
+    odCAN->send_heartbeat(this);
+    return ret;
+}
+
+// @brief Feed the watchdog to prevent watchdog timeouts.
+void Axis::watchdog_feed() {
+    watchdog_current_value_ = get_watchdog_reset();
+}
+
+// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
+bool Axis::watchdog_check() {
+    if (!config_.enable_watchdog) return true;
+
+    // explicit check here to ensure that we don't underflow back to UINT32_MAX
+    if (watchdog_current_value_ > 0) {
+        watchdog_current_value_--;
+        return true;
+    } else {
+        error_ |= ERROR_WATCHDOG_TIMER_EXPIRED;
+        return false;
+    }
+}
+
+bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
+    // Spiral up current for softer rotor lock-in
+    lockin_state_ = LOCKIN_STATE_RAMP;
+    float x = 0.0f;
+    run_control_loop([&]() {
+        float phase = wrap_pm_pi(lockin_config.ramp_distance * x);
+        float torque = lockin_config.current * motor_.config_.torque_constant * x;
+        x += current_meas_period / lockin_config.ramp_time;
+        if (!motor_.update(torque, phase, 0.0f))
+            return false;
+        return x < 1.0f;
+    });
+    
+    // Spin states
+    float distance = lockin_config.ramp_distance;
+    float phase = wrap_pm_pi(distance);
+    float vel = distance / lockin_config.ramp_time;
+
+    // Function of states to check if we are done
+    auto spin_done = [&](bool vel_override = false) -> bool {
+        bool done = false;
+        if (lockin_config.finish_on_vel || vel_override)
+            done = done || std::abs(vel) >= std::abs(lockin_config.vel);
+        if (lockin_config.finish_on_distance)
+            done = done || std::abs(distance) >= std::abs(lockin_config.finish_distance);
+        if (lockin_config.finish_on_enc_idx)
+            done = done || encoder_.index_found_;
+        return done;
+    };
+
+    // Accelerate
+    lockin_state_ = LOCKIN_STATE_ACCELERATE;
+    run_control_loop([&]() {
+        vel += lockin_config.accel * current_meas_period;
+        distance += vel * current_meas_period;
+        phase = wrap_pm_pi(phase + vel * current_meas_period);
+
+        if (!motor_.update(lockin_config.current * motor_.config_.torque_constant, phase, vel))
+            return false;
+        return !spin_done(true); //vel_override to go to next phase
+    });
+
+    if (!encoder_.index_found_)
+        encoder_.set_idx_subscribe(true);
+
+    // Constant speed
+    if (!spin_done()) {
+        lockin_state_ = LOCKIN_STATE_CONST_VEL;
+        vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
+        run_control_loop([&]() {
+            distance += vel * current_meas_period;
+            phase = wrap_pm_pi(phase + vel * current_meas_period);
+
+            if (!motor_.update(lockin_config.current * motor_.config_.torque_constant, phase, vel))
+                return false;
+            return !spin_done();
+        });
+    }
+
+    lockin_state_ = LOCKIN_STATE_INACTIVE;
+    return check_for_errors();
+}
+
+// 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() {
+    controller_.pos_estimate_linear_src_ = nullptr;
+    controller_.pos_estimate_circular_src_ = nullptr;
+    controller_.pos_estimate_valid_src_ = nullptr;
+    controller_.vel_estimate_src_ = &sensorless_estimator_.vel_estimate_;
+    controller_.vel_estimate_valid_src_ = &sensorless_estimator_.vel_estimate_valid_;
+
+    run_control_loop([this](){
+        // Note that all estimators are updated in the loop prefix in run_control_loop
+        float torque_setpoint;
+        if (!controller_.update(&torque_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+        if (!motor_.update(torque_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
+            return false; // set_error should update axis.error_
+        return true;
+    });
+    return check_for_errors();
+}
+
+bool Axis::run_closed_loop_control_loop() {
+    if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
+        return error_ |= ERROR_CONTROLLER_FAILED, false;
+    }
+
+    // To avoid any transient on startup, we intialize the setpoint to be the current position
+    if (controller_.config_.circular_setpoints) {
+        if (!controller_.pos_estimate_circular_src_) {
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+        }
+        else {
+            controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
+            controller_.input_pos_ = *controller_.pos_estimate_circular_src_;
+        }
+    }
+    else {
+        if (!controller_.pos_estimate_linear_src_) {
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+        }
+        else {
+            controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
+            controller_.input_pos_ = *controller_.pos_estimate_linear_src_;
+        }
+    }
+    controller_.input_pos_updated();
+
+    // Avoid integrator windup issues
+    controller_.vel_integrator_torque_ = 0.0f;
+
+    set_step_dir_active(config_.enable_step_dir);
+    run_control_loop([this](){
+        // Note that all estimators are updated in the loop prefix in run_control_loop
+        float torque_setpoint;
+        if (!controller_.update(&torque_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+
+        float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
+        if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
+            return false; // set_error should update axis.error_
+
+        return true;
+    });
+    set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
+    return check_for_errors();
+}
+
+
+// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
+// When pressed, set the linear count to the offset (default 0), and then go to position 0
+bool Axis::run_homing() {
+    Controller::ControlMode stored_control_mode = controller_.config_.control_mode;
+    Controller::InputMode stored_input_mode = controller_.config_.input_mode;
+
+    // TODO: theoretically this check should be inside the update loop,
+    // otherwise someone could disable the endstop while homing is in progress.
+    if (!min_endstop_.config_.enabled) {
+        return error_ |= ERROR_HOMING_WITHOUT_ENDSTOP, false;
+    }
+
+    controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL;
+    controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP;
+
+    controller_.input_pos_ = 0.0f;
+    controller_.input_pos_updated();
+    controller_.input_vel_ = -controller_.config_.homing_speed;
+    controller_.input_torque_ = 0.0f;
+
+    homing_.is_homed = false;
+
+    if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
+        return error_ |= ERROR_CONTROLLER_FAILED, false;
+    }
+    
+    // To avoid any transient on startup, we intialize the setpoint to be the current position
+    // note - input_pos_ is not set here. It is set to 0 earlier in this method and velocity control is used.
+    if (controller_.config_.circular_setpoints) {
+        if (!controller_.pos_estimate_circular_src_) {
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+        }
+        else {
+            controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
+        }
+    }
+    else {
+        if (!controller_.pos_estimate_linear_src_) {
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+        }
+        else {
+            controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
+        }
+    }
+
+    // Avoid integrator windup issues
+    controller_.vel_integrator_torque_ = 0.0f;
+
+    run_control_loop([this](){
+        // Note that all estimators are updated in the loop prefix in run_control_loop
+        float torque_setpoint;
+        if (!controller_.update(&torque_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+
+        float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
+        if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
+            return false; // set_error should update axis.error_
+
+        return !min_endstop_.get_state();
+    });
+    error_ &= ~ERROR_MIN_ENDSTOP_PRESSED; // clear this error since we deliberately drove into the endstop
+
+    // pos_setpoint is the starting position for the trap_traj so we need to set it.
+    controller_.pos_setpoint_ = min_endstop_.config_.offset;
+    controller_.vel_setpoint_ = 0.0f;  // Change directions without decelerating
+
+    // Set our current position in encoder counts to make control more logical
+    encoder_.set_linear_count((int32_t)(controller_.pos_setpoint_ * encoder_.config_.cpr));
+
+    controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
+    controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
+
+    controller_.input_pos_ = 0.0f;
+    controller_.input_pos_updated();
+    controller_.input_vel_ = 0.0f;
+    controller_.input_torque_ = 0.0f;
+
+    run_control_loop([this](){
+        // Note that all estimators are updated in the loop prefix in run_control_loop
+        float torque_setpoint;
+        if (!controller_.update(&torque_setpoint))
+            return error_ |= ERROR_CONTROLLER_FAILED, false;
+
+        float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
+        if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
+            return false; // set_error should update axis.error_
+
+        return !controller_.trajectory_done_;
+    });
+
+    controller_.config_.control_mode = stored_control_mode;
+    controller_.config_.input_mode = stored_input_mode;
+    homing_.is_homed = true;
+
+    return check_for_errors();
+}
+
+bool Axis::run_idle_loop() {
+    // run_control_loop ignores missed modulation timing updates
+    // if and only if we're in AXIS_STATE_IDLE
+    safety_critical_disarm_motor_pwm(motor_);
+    set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
+    run_control_loop([this]() {
+        return true;
+    });
+    return check_for_errors();
+}
+
+// Infinite loop that does calibration and enters main control loop as appropriate
+void Axis::run_state_machine_loop() {
+
+    // arm!
+    motor_.arm();
+
+    for (;;) {
+        // 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_index_search && encoder_.config_.use_index)
+                    task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
+                if (config_.startup_encoder_offset_calibration)
+                    task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
+                if (config_.startup_homing)
+                    task_chain_[pos++] = AXIS_STATE_HOMING;
+                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;
+                if (encoder_.config_.use_index)
+                    task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
+                task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_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;
+            // Auto-clear any invalid state error
+            error_ &= ~ERROR_INVALID_STATE;
+        }
+
+        // Note that current_state is a reference to task_chain_[0]
+
+        // 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_INDEX_SEARCH: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+                if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
+                    goto invalid_state_label;
+
+                status = encoder_.run_index_search();
+            } break;
+
+            case AXIS_STATE_ENCODER_DIR_FIND: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+
+                status = encoder_.run_direction_find();
+            } break;
+
+            case AXIS_STATE_HOMING: {
+                status = run_homing();
+            } break;
+
+            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+                status = encoder_.run_offset_calibration();
+            } break;
+
+            case AXIS_STATE_LOCKIN_SPIN: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                    goto invalid_state_label;
+                status = run_lockin_spin(config_.general_lockin);
+            } break;
+
+            case AXIS_STATE_SENSORLESS_CONTROL: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                        goto invalid_state_label;
+                status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
+                if (status) {
+                    // call to controller.reset() that happend when arming means that vel_setpoint
+                    // is zeroed. So we make the setpoint the spinup target for smooth transition.
+                    controller_.vel_setpoint_ = config_.sensorless_ramp.vel / (2.0f * M_PI * motor_.config_.pole_pairs);
+                    status = run_sensorless_control_loop();
+                }
+            } break;
+
+            case AXIS_STATE_CLOSED_LOOP_CONTROL: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                    goto invalid_state_label;
+                if (!encoder_.is_ready_)
+                    goto invalid_state_label;
+                watchdog_feed();
+                status = run_closed_loop_control_loop();
+            } break;
+
+            case AXIS_STATE_IDLE: {
+                run_idle_loop();
+                status = motor_.arm(); // done with idling - try to arm the motor
+            } break;
+
+            default:
+            invalid_state_label:
+                error_ |= ERROR_INVALID_STATE;
+                status = false;  // this will set the state to idle
+                break;
+        }
+
+        // If the state failed, go to idle, else advance task chain
+        if (!status) {
+            std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
+            current_state_ = AXIS_STATE_IDLE;
+        } else {
+            std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
+            task_chain_.back() = AXIS_STATE_UNDEFINED;
+        }
+    }
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/axis.hpp

@@ -0,0 +1,242 @@
+#ifndef __AXIS_HPP
+#define __AXIS_HPP
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+#include <array>
+
+class Axis : public ODriveIntf::AxisIntf {
+public:
+    struct LockinConfig_t {
+        float current = 10.0f;           // [A]
+        float ramp_time = 0.4f;          // [s]
+        float ramp_distance = 1 * M_PI;  // [rad]
+        float accel = 20.0f;     // [rad/s^2]
+        float vel = 40.0f; // [rad/s]
+        float finish_distance = 100.0f;  // [rad]
+        bool finish_on_vel = false;
+        bool finish_on_distance = false;
+        bool finish_on_enc_idx = false;
+    };
+
+    static LockinConfig_t default_calibration();
+    static LockinConfig_t default_sensorless();
+    static LockinConfig_t default_lockin();
+
+    struct Config_t {
+        bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
+        bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
+                                                // this only has an effect if encoder.config.use_index is also true
+        bool startup_encoder_offset_calibration = false; //<! run encoder offset 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 startup_homing = false; //<! enable homing after calibration/startup
+
+        bool enable_step_dir = false; //<! enable step/dir input after calibration
+                                    //   For M0 this has no effect if enable_uart is true
+        bool step_dir_always_on = false; //<! Keep step/dir enabled while the motor is disabled.
+                                         //<! This is ignored if enable_step_dir is false.
+                                         //<! This setting only takes effect on a state transition
+                                         //<! into idle or out of closed loop control.
+
+        float turns_per_step = 1.0f / 1024.0f;
+
+        float watchdog_timeout = 0.0f; // [s]
+        bool enable_watchdog = false;
+
+        // Defaults loaded from hw_config in load_configuration in main.cpp
+        uint16_t step_gpio_pin = 0;
+        uint16_t dir_gpio_pin = 0;
+
+        LockinConfig_t calibration_lockin = default_calibration();
+        LockinConfig_t sensorless_ramp = default_sensorless();
+        LockinConfig_t general_lockin;
+        uint32_t can_node_id = 0; // Both axes will have the same id to start
+        bool can_node_id_extended = false;
+        uint32_t can_heartbeat_rate_ms = 100;
+
+        // custom setters
+        Axis* parent = nullptr;
+        void set_step_gpio_pin(uint16_t value) { step_gpio_pin = value; parent->decode_step_dir_pins(); }
+        void set_dir_gpio_pin(uint16_t value) { dir_gpio_pin = value; parent->decode_step_dir_pins(); }
+    };
+
+    struct Homing_t {
+        bool is_homed = false;
+    };
+
+    enum thread_signals {
+        M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
+    };
+
+    Axis(int axis_num,
+            const AxisHardwareConfig_t& hw_config,
+            Config_t& config,
+            Encoder& encoder,
+            SensorlessEstimator& sensorless_estimator,
+            Controller& controller,
+            OnboardThermistorCurrentLimiter& fet_thermistor,
+            OffboardThermistorCurrentLimiter& motor_thermistor,
+            Motor& motor,
+            TrapezoidalTrajectory& trap,
+            Endstop& min_endstop,
+            Endstop& max_endstop);
+
+    void setup();
+    void start_thread();
+    void signal_current_meas();
+    bool wait_for_current_meas();
+
+    void step_cb();
+    void set_step_dir_active(bool enable);
+    void decode_step_dir_pins();
+
+    static void load_default_step_dir_pin_config(
+        const AxisHardwareConfig_t& hw_config, Config_t* config);
+    static void load_default_can_id(const int& id, Config_t& config);
+
+    bool check_DRV_fault();
+    bool check_PSU_brownout();
+    bool do_checks();
+    bool do_updates();
+
+    void watchdog_feed();
+    bool watchdog_check();
+
+    void clear_errors() {
+        motor_.error_ = Motor::ERROR_NONE;
+        controller_.error_ = Controller::ERROR_NONE;
+        sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
+        encoder_.error_ = Encoder::ERROR_NONE;
+        encoder_.spi_error_rate_ = 0.0f;
+
+        error_ = ERROR_NONE;
+    }
+
+    // True if there are no errors
+    bool inline check_for_errors() {
+        return error_ == ERROR_NONE;
+    }
+
+    // @brief Runs the specified update handler at the frequency of the current measurements.
+    //
+    // The loop runs until one of the following conditions:
+    //  - update_handler returns false
+    //  - the current measurement times out
+    //  - the health checks fail (brownout, driver fault line)
+    //  - update_handler doesn't update the modulation timings in time
+    //    This criterion is ignored if current_state is AXIS_STATE_IDLE
+    //
+    // 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 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
+    // go to zero.
+    //
+    // @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) {
+        while (requested_state_ == AXIS_STATE_UNDEFINED) {
+            // look for errors at axis level and also all subcomponents
+            bool checks_ok = do_checks();
+            // Update all estimators
+            // Note: updates run even if checks fail
+            bool updates_ok = do_updates(); 
+
+            // make sure the watchdog is being fed. 
+            bool watchdog_ok = watchdog_check();
+            
+            if (!checks_ok || !updates_ok || !watchdog_ok) {
+                // It's not useful to quit idle since that is the safe action
+                // Also leaving idle would rearm the motors
+                if (current_state_ != AXIS_STATE_IDLE)
+                    break;
+            }
+
+            // Run main loop function, defer quitting for after wait
+            // TODO: change arming logic to arm after waiting
+            bool main_continue = update_handler();
+
+            // Check we meet deadlines after queueing
+            ++loop_counter_;
+
+            // Wait until the current measurement interrupt fires
+            if (!wait_for_current_meas()) {
+                // maybe the interrupt handler is dead, let's be
+                // safe and float the phases
+                safety_critical_disarm_motor_pwm(motor_);
+                update_brake_current();
+                error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT;
+                break;
+            }
+
+            if (!main_continue)
+                break;
+        }
+    }
+
+    bool run_lockin_spin(const LockinConfig_t &lockin_config);
+    bool run_sensorless_control_loop();
+    bool run_closed_loop_control_loop();
+    bool run_homing();
+    bool run_idle_loop();
+
+    constexpr uint32_t get_watchdog_reset() {
+        return static_cast<uint32_t>(std::clamp<float>(config_.watchdog_timeout, 0, UINT32_MAX / (current_meas_hz + 1)) * current_meas_hz);
+    }
+
+    void run_state_machine_loop();
+
+    int axis_num_;
+    const AxisHardwareConfig_t& hw_config_;
+    Config_t& config_;
+
+    Encoder& encoder_;
+    SensorlessEstimator& sensorless_estimator_;
+    Controller& controller_;
+    OnboardThermistorCurrentLimiter& fet_thermistor_;
+    OffboardThermistorCurrentLimiter& motor_thermistor_;
+    Motor& motor_;
+    TrapezoidalTrajectory& trap_traj_;
+    Endstop& min_endstop_;
+    Endstop& max_endstop_;
+
+    // List of current_limiters and thermistors to
+    // provide easy iteration.
+    std::array<CurrentLimiter*, 2> current_limiters_;
+    std::array<ThermistorCurrentLimiter*, 2> thermistors_;
+
+    osThreadId thread_id_;
+    const uint32_t stack_size_ = 2048; // Bytes
+    volatile bool thread_id_valid_ = false;
+
+    // variables exposed on protocol
+    Error error_ = ERROR_NONE;
+    bool step_dir_active_ = false; // auto enabled after calibration, based on config.enable_step_dir
+
+    // updated from config in constructor, and on protocol hook
+    GPIO_TypeDef* step_port_;
+    uint16_t step_pin_;
+    GPIO_TypeDef* dir_port_;
+    uint16_t dir_pin_;
+
+    AxisState requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
+    std::array<AxisState, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
+    AxisState& current_state_ = task_chain_.front();
+    uint32_t loop_counter_ = 0;
+    LockinState lockin_state_ = LOCKIN_STATE_INACTIVE;
+    Homing_t homing_;
+    uint32_t last_heartbeat_ = 0;
+
+    // watchdog
+    uint32_t watchdog_current_value_= 0;
+};
+
+
+#endif /* __AXIS_HPP */

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/board_config_v3.h

@@ -0,0 +1,175 @@
+/*
+* @brief Contains board specific configuration for ODrive v3.x
+*/
+
+#ifndef __BOARD_CONFIG_H
+#define __BOARD_CONFIG_H
+
+// STM specific includes
+#include <gpio.h>
+#include <spi.h>
+#include <tim.h>
+#include <main.h>
+
+#if HW_VERSION_MAJOR == 3
+#if HW_VERSION_MINOR <= 3
+#define SHUNT_RESISTANCE (675e-6f)
+#else
+#define SHUNT_RESISTANCE (500e-6f)
+#endif
+#endif
+
+
+typedef struct {
+    uint16_t step_gpio_pin;
+    uint16_t dir_gpio_pin;
+    osPriority thread_priority;
+} AxisHardwareConfig_t;
+
+typedef struct {
+    TIM_HandleTypeDef* timer;
+    GPIO_TypeDef* index_port;
+    uint16_t index_pin;
+    GPIO_TypeDef* hallA_port;
+    uint16_t hallA_pin;
+    GPIO_TypeDef* hallB_port;
+    uint16_t hallB_pin;
+    GPIO_TypeDef* hallC_port;
+    uint16_t hallC_pin;
+    SPI_HandleTypeDef* spi;
+} EncoderHardwareConfig_t;
+typedef struct {
+    TIM_HandleTypeDef* timer;
+    uint16_t control_deadline;
+    float shunt_conductance;
+} MotorHardwareConfig_t;
+typedef struct {
+    const float* const coeffs;
+    size_t num_coeffs;
+    size_t adc_ch;
+} ThermistorHardwareConfig_t;
+typedef struct {
+    SPI_HandleTypeDef* spi;
+    GPIO_TypeDef* enable_port;
+    uint16_t enable_pin;
+    GPIO_TypeDef* nCS_port;
+    uint16_t nCS_pin;
+    GPIO_TypeDef* nFAULT_port;
+    uint16_t nFAULT_pin;
+} GateDriverHardwareConfig_t;
+typedef struct {
+    AxisHardwareConfig_t axis_config;
+    EncoderHardwareConfig_t encoder_config;
+    MotorHardwareConfig_t motor_config;
+    ThermistorHardwareConfig_t thermistor_config;
+    GateDriverHardwareConfig_t gate_driver_config;
+} BoardHardwareConfig_t;
+
+extern const BoardHardwareConfig_t hw_configs[2];
+
+//TODO stick this in a C file
+#ifdef __MAIN_CPP__
+const float fet_thermistor_poly_coeffs[] =
+    {363.93910201f, -462.15369634f, 307.55129571f, -27.72569531f};
+const size_t fet_thermistor_num_coeffs = sizeof(fet_thermistor_poly_coeffs)/sizeof(fet_thermistor_poly_coeffs[1]);
+
+const BoardHardwareConfig_t hw_configs[2] = { {
+    //M0
+    .axis_config = {
+        .step_gpio_pin = 1,
+        .dir_gpio_pin = 2,
+        .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1),
+    },
+    .encoder_config = {
+        .timer = &htim3,
+        .index_port = M0_ENC_Z_GPIO_Port,
+        .index_pin = M0_ENC_Z_Pin,
+        .hallA_port = M0_ENC_A_GPIO_Port,
+        .hallA_pin = M0_ENC_A_Pin,
+        .hallB_port = M0_ENC_B_GPIO_Port,
+        .hallB_pin = M0_ENC_B_Pin,
+        .hallC_port = M0_ENC_Z_GPIO_Port,
+        .hallC_pin = M0_ENC_Z_Pin,
+        .spi = &hspi3,
+    },
+    .motor_config = {
+        .timer = &htim1,
+        .control_deadline = TIM_1_8_PERIOD_CLOCKS,
+        .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
+    },
+    .thermistor_config = {
+        .coeffs = &fet_thermistor_poly_coeffs[0],
+        .num_coeffs = fet_thermistor_num_coeffs,
+        .adc_ch = 15,
+    },
+    .gate_driver_config = {
+        .spi = &hspi3,
+        // Note: this board has the EN_Gate pin shared!
+        .enable_port = EN_GATE_GPIO_Port,
+        .enable_pin = EN_GATE_Pin,
+        .nCS_port = M0_nCS_GPIO_Port,
+        .nCS_pin = M0_nCS_Pin,
+        .nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors
+        .nFAULT_pin = nFAULT_Pin,
+    }
+},{
+    //M1
+    .axis_config = {
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5
+        .step_gpio_pin = 7,
+        .dir_gpio_pin = 8,
+#else
+        .step_gpio_pin = 3,
+        .dir_gpio_pin = 4,
+#endif
+        .thread_priority = osPriorityHigh,
+    },
+    .encoder_config = {
+        .timer = &htim4,
+        .index_port = M1_ENC_Z_GPIO_Port,
+        .index_pin = M1_ENC_Z_Pin,
+        .hallA_port = M1_ENC_A_GPIO_Port,
+        .hallA_pin = M1_ENC_A_Pin,
+        .hallB_port = M1_ENC_B_GPIO_Port,
+        .hallB_pin = M1_ENC_B_Pin,
+        .hallC_port = M1_ENC_Z_GPIO_Port,
+        .hallC_pin = M1_ENC_Z_Pin,
+        .spi = &hspi3,
+    },
+    .motor_config = {
+        .timer = &htim8,
+        .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
+        .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
+    },
+    .thermistor_config = {
+        .coeffs = &fet_thermistor_poly_coeffs[0],
+        .num_coeffs = fet_thermistor_num_coeffs,
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+        .adc_ch = 4,
+#else
+        .adc_ch = 1,
+#endif
+    },
+    .gate_driver_config = {
+        .spi = &hspi3,
+        // Note: this board has the EN_Gate pin shared!
+        .enable_port = EN_GATE_GPIO_Port,
+        .enable_pin = EN_GATE_Pin,
+        .nCS_port = M1_nCS_GPIO_Port,
+        .nCS_pin = M1_nCS_Pin,
+        .nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors
+        .nFAULT_pin = nFAULT_Pin,
+    }
+} };
+#endif
+
+
+
+#define I2C_A0_PORT GPIO_3_GPIO_Port
+#define I2C_A0_PIN GPIO_3_Pin
+#define I2C_A1_PORT GPIO_4_GPIO_Port
+#define I2C_A1_PIN GPIO_4_Pin
+#define I2C_A2_PORT GPIO_5_GPIO_Port
+#define I2C_A2_PIN GPIO_5_Pin
+
+#endif // __BOARD_CONFIG_H

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/controller.cpp

@@ -0,0 +1,346 @@
+
+#include "odrive_main.h"
+#include <algorithm>
+
+#include <algorithm>
+
+Controller::Controller(Config_t& config) :
+    config_(config)
+{
+    update_filter_gains();
+}
+
+void Controller::reset() {
+    pos_setpoint_ = 0.0f;
+    vel_setpoint_ = 0.0f;
+    vel_integrator_torque_ = 0.0f;
+    torque_setpoint_ = 0.0f;
+}
+
+void Controller::set_error(Error error) {
+    error_ |= error;
+    axis_->error_ |= Axis::ERROR_CONTROLLER_FAILED;
+}
+
+//--------------------------------
+// Command Handling
+//--------------------------------
+
+
+bool Controller::select_encoder(size_t encoder_num) {
+    if (encoder_num < AXIS_COUNT) {
+        Axis* ax = axes[encoder_num];
+        pos_estimate_circular_src_ = &ax->encoder_.pos_circular_;
+        pos_wrap_src_ = &config_.circular_setpoint_range;
+        pos_estimate_linear_src_ = &ax->encoder_.pos_estimate_;
+        pos_estimate_valid_src_ = &ax->encoder_.pos_estimate_valid_;
+        vel_estimate_src_ = &ax->encoder_.vel_estimate_;
+        vel_estimate_valid_src_ = &ax->encoder_.vel_estimate_valid_;
+        return true;
+    } else {
+        return set_error(Controller::ERROR_INVALID_LOAD_ENCODER), false;
+    }
+}
+
+void Controller::move_to_pos(float goal_point) {
+    axis_->trap_traj_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
+                                 axis_->trap_traj_.config_.vel_limit,
+                                 axis_->trap_traj_.config_.accel_limit,
+                                 axis_->trap_traj_.config_.decel_limit);
+    axis_->trap_traj_.t_ = 0.0f;
+    trajectory_done_ = false;
+}
+
+void Controller::move_incremental(float displacement, bool from_input_pos = true){
+    if(from_input_pos){
+        input_pos_ += displacement;
+    } else{
+        input_pos_ = pos_setpoint_ + displacement;
+    }
+
+    input_pos_updated();
+}
+
+void Controller::start_anticogging_calibration() {
+    // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
+    if (axis_->error_ == Axis::ERROR_NONE) {
+        config_.anticogging.calib_anticogging = true;
+    }
+}
+
+
+/*
+ * This anti-cogging implementation iterates through each encoder position,
+ * waits for zero velocity & position error,
+ * then samples the current required to maintain that position.
+ * 
+ * This holding current is added as a feedforward term in the control loop.
+ */
+bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
+    float pos_err = input_pos_ - pos_estimate;
+    if (std::abs(pos_err) <= config_.anticogging.calib_pos_threshold / (float)axis_->encoder_.config_.cpr &&
+        std::abs(vel_estimate) < config_.anticogging.calib_vel_threshold / (float)axis_->encoder_.config_.cpr) {
+        config_.anticogging.cogging_map[std::clamp<uint32_t>(config_.anticogging.index++, 0, 3600)] = vel_integrator_torque_;
+    }
+    if (config_.anticogging.index < 3600) {
+        config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
+        input_pos_ = config_.anticogging.index * axis_->encoder_.getCoggingRatio();
+        input_vel_ = 0.0f;
+        input_torque_ = 0.0f;
+        input_pos_updated();
+        return false;
+    } else {
+        config_.anticogging.index = 0;
+        config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
+        input_pos_ = 0.0f;  // Send the motor home
+        input_vel_ = 0.0f;
+        input_torque_ = 0.0f;
+        input_pos_updated();
+        anticogging_valid_ = true;
+        config_.anticogging.calib_anticogging = false;
+        return true;
+    }
+}
+
+void Controller::update_filter_gains() {
+    float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz);
+    input_filter_ki_ = 2.0f * bandwidth;  // basic conversion to discrete time
+    input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped
+}
+
+static float limitVel(const float vel_limit, const float vel_estimate, const float vel_gain, const float torque) {
+    float Tmax = (vel_limit - vel_estimate) * vel_gain;
+    float Tmin = (-vel_limit - vel_estimate) * vel_gain;
+    return std::clamp(torque, Tmin, Tmax);
+}
+
+bool Controller::update(float* torque_setpoint_output) {
+    float* pos_estimate_linear = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
+            ? pos_estimate_linear_src_ : nullptr;
+    float* pos_estimate_circular = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
+            ? pos_estimate_circular_src_ : nullptr;
+    float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_)
+            ? vel_estimate_src_ : nullptr;
+
+    // Calib_anticogging is only true when calibration is occurring, so we can't block anticogging_pos
+    float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio();
+    if (config_.anticogging.calib_anticogging) {
+        if (!axis_->encoder_.pos_estimate_valid_ || !axis_->encoder_.vel_estimate_valid_) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+        // non-blocking
+        anticogging_calibration(axis_->encoder_.pos_estimate_, axis_->encoder_.vel_estimate_);
+    }
+
+    // TODO also enable circular deltas for 2nd order filter, etc.
+    if (config_.circular_setpoints) {
+        // Keep pos setpoint from drifting
+        input_pos_ = fmodf_pos(input_pos_, config_.circular_setpoint_range);
+    }
+
+    // Update inputs
+    switch (config_.input_mode) {
+        case INPUT_MODE_INACTIVE: {
+            // do nothing
+        } break;
+        case INPUT_MODE_PASSTHROUGH: {
+            pos_setpoint_ = input_pos_;
+            vel_setpoint_ = input_vel_;
+            torque_setpoint_ = input_torque_; 
+        } break;
+        case INPUT_MODE_VEL_RAMP: {
+            float max_step_size = std::abs(current_meas_period * config_.vel_ramp_rate);
+            float full_step = input_vel_ - vel_setpoint_;
+            float step = std::clamp(full_step, -max_step_size, max_step_size);
+
+            vel_setpoint_ += step;
+            torque_setpoint_ = (step / current_meas_period) * config_.inertia;
+        } break;
+        case INPUT_MODE_TORQUE_RAMP: {
+            float max_step_size = std::abs(current_meas_period * config_.torque_ramp_rate);
+            float full_step = input_torque_ - torque_setpoint_;
+            float step = std::clamp(full_step, -max_step_size, max_step_size);
+
+            torque_setpoint_ += step;
+        } break;
+        case INPUT_MODE_POS_FILTER: {
+            // 2nd order pos tracking filter
+            float delta_pos = input_pos_ - pos_setpoint_; // Pos error
+            float delta_vel = input_vel_ - vel_setpoint_; // Vel error
+            float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback
+            torque_setpoint_ = accel * config_.inertia; // Accel
+            vel_setpoint_ += current_meas_period * accel; // delta vel
+            pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos
+        } break;
+        case INPUT_MODE_MIRROR: {
+            if (config_.axis_to_mirror < AXIS_COUNT) {
+                pos_setpoint_ = axes[config_.axis_to_mirror]->encoder_.pos_estimate_ * config_.mirror_ratio;
+                vel_setpoint_ = axes[config_.axis_to_mirror]->encoder_.vel_estimate_ * config_.mirror_ratio;
+            } else {
+                set_error(ERROR_INVALID_MIRROR_AXIS);
+                return false;
+            }
+        } break;
+        // case INPUT_MODE_MIX_CHANNELS: {
+        //     // NOT YET IMPLEMENTED
+        // } break;
+        case INPUT_MODE_TRAP_TRAJ: {
+            if(input_pos_updated_){
+                move_to_pos(input_pos_);
+                input_pos_updated_ = false;
+            }
+            // Avoid updating uninitialized trajectory
+            if (trajectory_done_)
+                break;
+            
+            if (axis_->trap_traj_.t_ > axis_->trap_traj_.Tf_) {
+                // Drop into position control mode when done to avoid problems on loop counter delta overflow
+                config_.control_mode = CONTROL_MODE_POSITION_CONTROL;
+                pos_setpoint_ = input_pos_;
+                vel_setpoint_ = 0.0f;
+                torque_setpoint_ = 0.0f;
+                trajectory_done_ = true;
+            } else {
+                TrapezoidalTrajectory::Step_t traj_step = axis_->trap_traj_.eval(axis_->trap_traj_.t_);
+                pos_setpoint_ = traj_step.Y;
+                vel_setpoint_ = traj_step.Yd;
+                torque_setpoint_ = traj_step.Ydd * config_.inertia;
+                axis_->trap_traj_.t_ += current_meas_period;
+            }
+            anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate
+        } break;
+        default: {
+            set_error(ERROR_INVALID_INPUT_MODE);
+            return false;
+        }
+        
+    }
+
+    // Position control
+    // TODO Decide if we want to use encoder or pll position here
+    float gain_scheduling_multiplier = 1.0f;
+    float vel_des = vel_setpoint_;
+    if (config_.control_mode >= CONTROL_MODE_POSITION_CONTROL) {
+        float pos_err;
+
+        if (config_.circular_setpoints) {
+            if(!pos_estimate_circular) {
+                set_error(ERROR_INVALID_ESTIMATE);
+                return false;
+            }
+            // Keep pos setpoint from drifting
+            pos_setpoint_ = fmodf_pos(pos_setpoint_, *pos_wrap_src_);
+            // Circular delta
+            pos_err = pos_setpoint_ - *pos_estimate_circular;
+            pos_err = wrap_pm(pos_err, 0.5f * *pos_wrap_src_);
+        } else {
+            if(!pos_estimate_linear) {
+                set_error(ERROR_INVALID_ESTIMATE);
+                return false;
+            }
+            pos_err = pos_setpoint_ - *pos_estimate_linear;
+        }
+
+        vel_des += config_.pos_gain * pos_err;
+        // V-shaped gain shedule based on position error
+        float abs_pos_err = std::abs(pos_err);
+        if (config_.enable_gain_scheduling && abs_pos_err <= config_.gain_scheduling_width) {
+            gain_scheduling_multiplier = abs_pos_err / config_.gain_scheduling_width;
+        }
+    }
+
+    // Velocity limiting
+    float vel_lim = config_.vel_limit;
+    if (config_.enable_vel_limit) {
+        vel_des = std::clamp(vel_des, -vel_lim, vel_lim);
+    }
+
+    // Check for overspeed fault (done in this module (controller) for cohesion with vel_lim)
+    if (config_.enable_overspeed_error) {  // 0.0f to disable
+        if (!vel_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+        if (std::abs(*vel_estimate_src) > config_.vel_limit_tolerance * vel_lim) {
+            set_error(ERROR_OVERSPEED);
+            return false;
+        }
+    }
+
+    // TODO: Change to controller working in torque units
+    // Torque per amp gain scheduling (ACIM)
+    float vel_gain = config_.vel_gain;
+    float vel_integrator_gain = config_.vel_integrator_gain;
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
+        float effective_flux = axis_->motor_.current_control_.acim_rotor_flux;
+        float minflux = axis_->motor_.config_.acim_gain_min_flux;
+        if (fabsf(effective_flux) < minflux)
+            effective_flux = std::copysignf(minflux, effective_flux);
+        vel_gain /= effective_flux;
+        vel_integrator_gain /= effective_flux;
+        // TODO: also scale the integral value which is also changing units.
+        // (or again just do control in torque units)
+    }
+
+    // Velocity control
+    float torque = torque_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_valid_ && config_.anticogging.anticogging_enabled) {
+        torque += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)];
+    }
+
+    float v_err = 0.0f;
+    if (config_.control_mode >= CONTROL_MODE_VELOCITY_CONTROL) {
+        if (!vel_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+
+        v_err = vel_des - *vel_estimate_src;
+        torque += (vel_gain * gain_scheduling_multiplier) * v_err;
+
+        // Velocity integral action before limiting
+        torque += vel_integrator_torque_;
+    }
+
+    // Velocity limiting in current mode
+    if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL && config_.enable_current_mode_vel_limit) {
+        if (!vel_estimate_src) {
+            set_error(ERROR_INVALID_ESTIMATE);
+            return false;
+        }
+        torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque);
+    }
+
+    // Torque limiting
+    bool limited = false;
+    float Tlim = axis_->motor_.max_available_torque();
+    if (torque > Tlim) {
+        limited = true;
+        torque = Tlim;
+    }
+    if (torque < -Tlim) {
+        limited = true;
+        torque = -Tlim;
+    }
+
+    // Velocity integrator (behaviour dependent on limiting)
+    if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL) {
+        // reset integral if not in use
+        vel_integrator_torque_ = 0.0f;
+    } else {
+        if (limited) {
+            // TODO make decayfactor configurable
+            vel_integrator_torque_ *= 0.99f;
+        } else {
+            vel_integrator_torque_ += ((vel_integrator_gain * gain_scheduling_multiplier) * current_meas_period) * v_err;
+        }
+    }
+
+    if (torque_setpoint_output) *torque_setpoint_output = torque;
+    return true;
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/controller.hpp

@@ -0,0 +1,109 @@
+#ifndef __CONTROLLER_HPP
+#define __CONTROLLER_HPP
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+class Controller : public ODriveIntf::ControllerIntf {
+public:
+    typedef struct {
+        uint32_t index = 0;
+        float cogging_map[3600];
+        bool pre_calibrated = false;
+        bool calib_anticogging = false;
+        float calib_pos_threshold = 1.0f;
+        float calib_vel_threshold = 1.0f;
+        float cogging_ratio = 1.0f;
+        bool anticogging_enabled = true;
+    } Anticogging_t;
+
+    struct Config_t {
+        ControlMode control_mode = CONTROL_MODE_POSITION_CONTROL;  //see: ControlMode_t
+        InputMode input_mode = INPUT_MODE_PASSTHROUGH;             //see: InputMode_t
+        float pos_gain = 20.0f;                  // [(turn/s) / turn]
+        float vel_gain = 1.0f / 6.0f;            // [Nm/(turn/s)]
+        // float vel_gain = 0.2f / 200.0f,       // [Nm/(rad/s)] <sensorless example>
+        float vel_integrator_gain = 2.0f / 6.0f; // [Nm/(turn/s * s)]
+        float vel_limit = 2.0f;                  // [turn/s] Infinity to disable.
+        float vel_limit_tolerance = 1.2f;        // ratio to vel_lim. Infinity to disable.
+        float vel_ramp_rate = 1.0f;              // [(turn/s) / s]
+        float torque_ramp_rate = 0.01f;          // Nm / sec
+        bool circular_setpoints = false;
+        float circular_setpoint_range = 1.0f; // Circular range when circular_setpoints is true. [turn]
+        float inertia = 0.0f;                 // [Nm/(turn/s^2)]
+        float input_filter_bandwidth = 2.0f;  // [1/s]
+        float homing_speed = 0.25f;           // [turn/s]
+        Anticogging_t anticogging;
+        float gain_scheduling_width = 10.0f;
+        bool enable_gain_scheduling = false;
+        bool enable_vel_limit = true;
+        bool enable_overspeed_error = true;
+        bool enable_current_mode_vel_limit = true;  // enable velocity limit in current control mode (requires a valid velocity estimator)
+        uint8_t axis_to_mirror = -1;
+        float mirror_ratio = 1.0f;
+        uint8_t load_encoder_axis = -1;  // default depends on Axis number and is set in load_configuration()
+
+        // custom setters
+        Controller* parent;
+        void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); }
+    };
+
+    explicit Controller(Config_t& config);
+    void reset();
+    void set_error(Error error);
+
+    constexpr void input_pos_updated() {
+        input_pos_updated_ = true;
+    }
+
+    bool select_encoder(size_t encoder_num);
+
+    // Trajectory-Planned control
+    void move_to_pos(float goal_point);
+    void move_incremental(float displacement, bool from_goal_point);
+    
+    // TODO: make this more similar to other calibration loops
+    void start_anticogging_calibration();
+    bool anticogging_calibration(float pos_estimate, float vel_estimate);
+
+    void update_filter_gains();
+    bool update(float* torque_setpoint);
+
+    Config_t& config_;
+    Axis* axis_ = nullptr; // set by Axis constructor
+
+    Error error_ = ERROR_NONE;
+
+    float* pos_estimate_linear_src_ = nullptr;
+    float* pos_estimate_circular_src_ = nullptr;
+    bool* pos_estimate_valid_src_ = nullptr;
+    float* vel_estimate_src_ = nullptr;
+    bool* vel_estimate_valid_src_ = nullptr;
+    float* pos_wrap_src_ = nullptr; 
+
+
+    float pos_setpoint_ = 0.0f; // [turns]
+    float vel_setpoint_ = 0.0f; // [turn/s]
+    // float vel_setpoint = 800.0f; <sensorless example>
+    float vel_integrator_torque_ = 0.0f;    // [Nm]
+    float torque_setpoint_ = 0.0f;  // [Nm]
+
+    float input_pos_ = 0.0f;     // [turns]
+    float input_vel_ = 0.0f;     // [turn/s]
+    float input_torque_ = 0.0f;  // [Nm]
+    float input_filter_kp_ = 0.0f;
+    float input_filter_ki_ = 0.0f;
+
+    bool input_pos_updated_ = false;
+    
+    bool trajectory_done_ = true;
+
+    bool anticogging_valid_ = false;
+
+    // custom setters
+    void set_input_pos(float value) { input_pos_ = value; input_pos_updated(); }
+
+};
+
+#endif // __CONTROLLER_HPP

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/current_limiter.hpp

@@ -0,0 +1,14 @@
+#ifndef __CURRENT_LIMITER_HPP
+#define __CURRENT_LIMITER_HPP
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+class CurrentLimiter {
+public:
+    virtual ~CurrentLimiter() = default;
+    virtual float get_current_limit(float base_current_lim) const = 0;
+};
+
+#endif // __CURRENT_LIMITER_HPP

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/encoder.cpp

@@ -0,0 +1,572 @@
+
+#include "odrive_main.h"
+
+
+Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
+                Config_t& config, const Motor::Config_t& motor_config) :
+        hw_config_(hw_config),
+        config_(config)
+{
+    update_pll_gains();
+
+    if (config.pre_calibrated) {
+        if (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)
+            is_ready_ = true;
+        if (motor_config.motor_type == Motor::MOTOR_TYPE_ACIM)
+            is_ready_ = true;
+    }
+}
+
+static void enc_index_cb_wrapper(void* ctx) {
+    reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
+}
+
+void Encoder::setup() {
+    HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
+    set_idx_subscribe();
+
+    mode_ = config_.mode;
+    if(mode_ & MODE_FLAG_ABS){
+        abs_spi_cs_pin_init();
+        abs_spi_init();
+        if (axis_->controller_.config_.anticogging.pre_calibrated) {
+            axis_->controller_.anticogging_valid_ = true;
+        }
+    }
+}
+
+void Encoder::set_error(Error error) {
+    vel_estimate_valid_ = false;
+    pos_estimate_valid_ = false;
+    error_ |= error;
+    axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
+}
+
+bool Encoder::do_checks(){
+    return error_ == ERROR_NONE;
+}
+
+//--------------------
+// Hardware Dependent
+//--------------------
+
+// Triggered when an encoder passes over the "Index" pin
+// TODO: only arm index edge interrupt when we know encoder has powered up
+// (maybe by attaching the interrupt on start search, synergistic with following)
+void Encoder::enc_index_cb() {
+    if (config_.use_index) {
+        set_circular_count(0, false);
+        if (config_.zero_count_on_find_idx)
+            set_linear_count(0); // Avoid position control transient after search
+        if (config_.pre_calibrated) {
+            is_ready_ = true;
+            if(axis_->controller_.config_.anticogging.pre_calibrated){
+                axis_->controller_.anticogging_valid_ = true;
+            }
+        } else {
+            // We can't use the update_offset facility in set_circular_count because
+            // we also set the linear count before there is a chance to update. Therefore:
+            // Invalidate offset calibration that may have happened before idx search
+            is_ready_ = false;
+        }
+        index_found_ = true;
+    }
+
+    // Disable interrupt
+    GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
+}
+
+void Encoder::set_idx_subscribe(bool override_enable) {
+    if (config_.use_index && (override_enable || !config_.find_idx_on_lockin_only)) {
+        GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN,
+                enc_index_cb_wrapper, this);
+    } else if (!config_.use_index || config_.find_idx_on_lockin_only) {
+        GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
+    }
+}
+
+void Encoder::update_pll_gains() {
+    pll_kp_ = 2.0f * config_.bandwidth;  // basic conversion to discrete time
+    pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); // Critically damped
+
+    // Check that we don't get problems with discrete time approximation
+    if (!(current_meas_period * pll_kp_ < 1.0f)) {
+        set_error(ERROR_UNSTABLE_GAIN);
+    }
+}
+
+void Encoder::check_pre_calibrated() {
+    // TODO: restoring config from python backup is fragile here (ACIM motor type must be set first)
+    if (!is_ready_ && axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_ACIM)
+        config_.pre_calibrated = false;
+    if (mode_ == MODE_INCREMENTAL && !index_found_)
+        config_.pre_calibrated = false;
+}
+
+// Function that sets the current encoder count to a desired 32-bit value.
+void Encoder::set_linear_count(int32_t count) {
+    // Disable interrupts to make a critical section to avoid race condition
+    uint32_t prim = cpu_enter_critical();
+
+    // Update states
+    shadow_count_ = count;
+    pos_estimate_counts_ = (float)count;
+    tim_cnt_sample_ = count;
+
+    //Write hardware last
+    hw_config_.timer->Instance->CNT = count;
+
+    cpu_exit_critical(prim);
+}
+
+// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
+// Note that this will get mod'ed down to [0, cpr)
+void Encoder::set_circular_count(int32_t count, bool update_offset) {
+    // Disable interrupts to make a critical section to avoid race condition
+    uint32_t prim = cpu_enter_critical();
+
+    if (update_offset) {
+        config_.offset += count - count_in_cpr_;
+        config_.offset = mod(config_.offset, config_.cpr);
+    }
+
+    // Update states
+    count_in_cpr_ = mod(count, config_.cpr);
+    pos_cpr_counts_ = (float)count_in_cpr_;
+
+    cpu_exit_critical(prim);
+}
+
+bool Encoder::run_index_search() {
+    config_.use_index = true;
+    index_found_ = false;
+    if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
+        axis_->motor_.config_.direction = 1;
+    }
+    set_idx_subscribe();
+
+    bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
+    return status;
+}
+
+bool Encoder::run_direction_find() {
+    int32_t init_enc_val = shadow_count_;
+    axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
+
+    Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
+    lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
+    lockin_config.finish_on_distance = true;
+    lockin_config.finish_on_enc_idx = false;
+    lockin_config.finish_on_vel = false;
+    bool status = axis_->run_lockin_spin(lockin_config);
+
+    if (status) {
+        // Check response and direction
+        if (shadow_count_ > init_enc_val + 8) {
+            // motor same dir as encoder
+            axis_->motor_.config_.direction = 1;
+        } else if (shadow_count_ < init_enc_val - 8) {
+            // motor opposite dir as encoder
+            axis_->motor_.config_.direction = -1;
+        } else {
+            axis_->motor_.config_.direction = 0;
+        }
+    }
+
+    return status;
+}
+
+// @brief Turns the motor in one direction for a bit and then in the other
+// direction in order to find the offset between the electrical phase 0
+// and the encoder state 0.
+// TODO: Do the scan with current, not voltage!
+bool Encoder::run_offset_calibration() {
+    const float start_lock_duration = 1.0f;
+    const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
+
+    // Require index found if enabled
+    if (config_.use_index && !index_found_) {
+        set_error(ERROR_INDEX_NOT_FOUND_YET);
+        return false;
+    }
+
+    // We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_
+    // Therefore we have to sync them for calibration
+    shadow_count_ = count_in_cpr_;
+
+    float voltage_magnitude;
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
+        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
+    else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
+        voltage_magnitude = axis_->motor_.config_.calibration_current;
+    else
+        return false;
+
+    // go to motor zero phase for start_lock_duration to get ready to scan
+    int i = 0;
+    axis_->run_control_loop([&](){
+        if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
+            return false; // error set inside enqueue_voltage_timings
+        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
+        return ++i < start_lock_duration * current_meas_hz;
+    });
+    if (axis_->error_ != Axis::ERROR_NONE)
+        return false;
+
+    int32_t init_enc_val = shadow_count_;
+    int64_t encvaluesum = 0;
+
+    // scan forward
+    i = 0;
+    axis_->run_control_loop([&]() {
+        float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
+        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
+        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
+        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
+            return false; // error set inside enqueue_voltage_timings
+        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
+
+        encvaluesum += shadow_count_;
+        
+        return ++i < num_steps;
+    });
+    if (axis_->error_ != Axis::ERROR_NONE)
+        return false;
+
+    // Check response and direction
+    if (shadow_count_ > init_enc_val + 8) {
+        // motor same dir as encoder
+        axis_->motor_.config_.direction = 1;
+    } else if (shadow_count_ < init_enc_val - 8) {
+        // motor opposite dir as encoder
+        axis_->motor_.config_.direction = -1;
+    } else {
+        // Encoder response error
+        set_error(ERROR_NO_RESPONSE);
+        return false;
+    }
+
+    //TODO avoid recomputing elec_rad_per_enc every time
+    // Check CPR
+    float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
+    float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
+    calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
+    if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
+        set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
+        return false;
+    }
+
+    // scan backwards
+    i = 0;
+    axis_->run_control_loop([&]() {
+        float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
+        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
+        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
+        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
+            return false; // error set inside enqueue_voltage_timings
+        axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
+
+        encvaluesum += shadow_count_;
+        
+        return ++i < num_steps;
+    });
+    if (axis_->error_ != Axis::ERROR_NONE)
+        return false;
+
+    config_.offset = encvaluesum / (num_steps * 2);
+    int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
+    config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f;  // add 0.5 to center-align state to phase
+
+    is_ready_ = true;
+    return true;
+}
+
+static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
+    switch (hall_state) {
+        case 0b001: *hall_cnt = 0; return true;
+        case 0b011: *hall_cnt = 1; return true;
+        case 0b010: *hall_cnt = 2; return true;
+        case 0b110: *hall_cnt = 3; return true;
+        case 0b100: *hall_cnt = 4; return true;
+        case 0b101: *hall_cnt = 5; return true;
+        default: return false;
+    }
+}
+
+void Encoder::sample_now() {
+    switch (mode_) {
+        case MODE_INCREMENTAL: {
+            tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT;
+        } break;
+
+        case MODE_HALL: {
+            // do nothing: samples already captured in general GPIO capture
+        } break;
+
+        case MODE_SINCOS: {
+            sincos_sample_s_ = (get_adc_voltage(get_gpio_port_by_pin(config_.sincos_gpio_pin_sin), get_gpio_pin_by_pin(config_.sincos_gpio_pin_sin)) / 3.3f) - 0.5f;
+            sincos_sample_c_ = (get_adc_voltage(get_gpio_port_by_pin(config_.sincos_gpio_pin_cos), get_gpio_pin_by_pin(config_.sincos_gpio_pin_cos)) / 3.3f) - 0.5f;
+        } break;
+
+        case MODE_SPI_ABS_AMS:
+        case MODE_SPI_ABS_CUI:
+        case MODE_SPI_ABS_AEAT:
+        case MODE_SPI_ABS_RLS:
+        {
+            axis_->motor_.log_timing(TIMING_LOG_SAMPLE_NOW);
+            // Do nothing
+        } break;
+
+        default: {
+           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
+        } break;
+    }
+}
+
+bool Encoder::abs_spi_init(){
+    if ((mode_ & MODE_FLAG_ABS) == 0x0)
+        return false;
+
+    SPI_HandleTypeDef * spi = hw_config_.spi;
+    spi->Init.Mode = SPI_MODE_MASTER;
+    spi->Init.Direction = SPI_DIRECTION_2LINES;
+    spi->Init.DataSize = SPI_DATASIZE_16BIT;
+    spi->Init.CLKPolarity = SPI_POLARITY_LOW;
+    spi->Init.CLKPhase = SPI_PHASE_2EDGE;
+    spi->Init.NSS = SPI_NSS_SOFT;
+    spi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
+    spi->Init.FirstBit = SPI_FIRSTBIT_MSB;
+    spi->Init.TIMode = SPI_TIMODE_DISABLE;
+    spi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+    spi->Init.CRCPolynomial = 10;
+    if (mode_ == MODE_SPI_ABS_AEAT) {
+        spi->Init.CLKPolarity = SPI_POLARITY_HIGH;
+    }
+    HAL_SPI_DeInit(spi);
+    HAL_SPI_Init(spi);
+    return true;
+}
+
+bool Encoder::abs_spi_start_transaction(){
+    if (mode_ & MODE_FLAG_ABS){
+        axis_->motor_.log_timing(TIMING_LOG_SPI_START);
+        if(hw_config_.spi->State != HAL_SPI_STATE_READY){
+            set_error(ERROR_ABS_SPI_NOT_READY);
+            return false;
+        }
+        HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_RESET);
+        HAL_SPI_TransmitReceive_DMA(hw_config_.spi, (uint8_t*)abs_spi_dma_tx_, (uint8_t*)abs_spi_dma_rx_, 1);
+    }
+    return true;
+}
+
+uint8_t ams_parity(uint16_t v) {
+    v ^= v >> 8;
+    v ^= v >> 4;
+    v ^= v >> 2;
+    v ^= v >> 1;
+    return v & 1;
+}
+
+uint8_t cui_parity(uint16_t v) {
+    v ^= v >> 8;
+    v ^= v >> 4;
+    v ^= v >> 2;
+    return ~v & 3;
+}
+
+void Encoder::abs_spi_cb(){
+    HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
+
+    axis_->motor_.log_timing(TIMING_LOG_SPI_END);
+
+    uint16_t pos;
+
+    switch (mode_) {
+        case MODE_SPI_ABS_AMS: {
+            uint16_t rawVal = abs_spi_dma_rx_[0];
+            // check if parity is correct (even) and error flag clear
+            if (ams_parity(rawVal) || ((rawVal >> 14) & 1)) {
+                return;
+            }
+            pos = rawVal & 0x3fff;
+        } break;
+
+        case MODE_SPI_ABS_CUI: {
+            uint16_t rawVal = abs_spi_dma_rx_[0];
+            // check if parity is correct
+            if (cui_parity(rawVal)) {
+                return;
+            }
+            pos = rawVal & 0x3fff;
+        } break;
+
+        case MODE_SPI_ABS_RLS: {
+            uint16_t rawVal = abs_spi_dma_rx_[0];
+            pos = (rawVal >> 2) & 0x3fff;
+        } break;
+
+        default: {
+           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
+           return;
+        } break;
+    }
+
+    pos_abs_ = pos;
+    abs_spi_pos_updated_ = true;
+    if (config_.pre_calibrated) {
+        is_ready_ = true;
+    }
+}
+
+void Encoder::abs_spi_cs_pin_init(){
+    // Decode cs pin
+    abs_spi_cs_port_ = get_gpio_port_by_pin(config_.abs_spi_cs_gpio_pin);
+    abs_spi_cs_pin_ = get_gpio_pin_by_pin(config_.abs_spi_cs_gpio_pin);
+
+    // Init cs pin
+    HAL_GPIO_DeInit(abs_spi_cs_port_, abs_spi_cs_pin_);
+    GPIO_InitTypeDef GPIO_InitStruct;
+    GPIO_InitStruct.Pin = abs_spi_cs_pin_;
+    GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    HAL_GPIO_Init(abs_spi_cs_port_, &GPIO_InitStruct);
+
+    // Write pin high
+    HAL_GPIO_WritePin(abs_spi_cs_port_, abs_spi_cs_pin_, GPIO_PIN_SET);
+}
+
+bool Encoder::update() {
+    // update internal encoder state.
+    int32_t delta_enc = 0;
+    int32_t pos_abs_latched = pos_abs_; //LATCH
+
+    switch (mode_) {
+        case MODE_INCREMENTAL: {
+            //TODO: use count_in_cpr_ instead as shadow_count_ can overflow
+            //or use 64 bit
+            int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
+            delta_enc = (int32_t)delta_enc_16; //sign extend
+        } break;
+
+        case MODE_HALL: {
+            int32_t hall_cnt;
+            if (decode_hall(hall_state_, &hall_cnt)) {
+                delta_enc = hall_cnt - count_in_cpr_;
+                delta_enc = mod(delta_enc, 6);
+                if (delta_enc > 3)
+                    delta_enc -= 6;
+            } else {
+                if (!config_.ignore_illegal_hall_state) {
+                    set_error(ERROR_ILLEGAL_HALL_STATE);
+                    return false;
+                }
+            }
+        } break;
+
+        case MODE_SINCOS: {
+            float phase = fast_atan2(sincos_sample_s_, sincos_sample_c_);
+            int fake_count = (int)(1000.0f * phase);
+            //CPR = 6283 = 2pi * 1k
+
+            delta_enc = fake_count - count_in_cpr_;
+            delta_enc = mod(delta_enc, 6283);
+            if (delta_enc > 6283/2)
+                delta_enc -= 6283;
+        } break;
+        
+        case MODE_SPI_ABS_RLS:
+        case MODE_SPI_ABS_AMS:
+        case MODE_SPI_ABS_CUI: 
+        case MODE_SPI_ABS_AEAT: {
+            if (abs_spi_pos_updated_ == false) {
+                // Low pass filter the error
+                spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
+                if (spi_error_rate_ > 0.005f)
+                    set_error(ERROR_ABS_SPI_COM_FAIL);
+            } else {
+                // Low pass filter the error
+                spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
+            }
+
+            abs_spi_pos_updated_ = false;
+            delta_enc = pos_abs_latched - count_in_cpr_; //LATCH
+            delta_enc = mod(delta_enc, config_.cpr);
+            if (delta_enc > config_.cpr/2) {
+                delta_enc -= config_.cpr;
+            }
+
+        }break;
+        default: {
+           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
+           return false;
+        } break;
+    }
+
+    shadow_count_ += delta_enc;
+    count_in_cpr_ += delta_enc;
+    count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
+
+    if(mode_ & MODE_FLAG_ABS)
+        count_in_cpr_ = pos_abs_latched;
+
+    //// run pll (for now pll is in units of encoder counts)
+    // Predict current pos
+    pos_estimate_counts_ += current_meas_period * vel_estimate_counts_;
+    pos_cpr_counts_      += current_meas_period * vel_estimate_counts_;
+    // discrete phase detector
+    float delta_pos_counts = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_counts_));
+    float delta_pos_cpr_counts = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_counts_));
+    delta_pos_cpr_counts = wrap_pm(delta_pos_cpr_counts, 0.5f * (float)(config_.cpr));
+    // pll feedback
+    pos_estimate_counts_ += current_meas_period * pll_kp_ * delta_pos_counts;
+    pos_cpr_counts_ += current_meas_period * pll_kp_ * delta_pos_cpr_counts;
+    pos_cpr_counts_ = fmodf_pos(pos_cpr_counts_, (float)(config_.cpr));
+    vel_estimate_counts_ += current_meas_period * pll_ki_ * delta_pos_cpr_counts;
+    bool snap_to_zero_vel = false;
+    if (std::abs(vel_estimate_counts_) < 0.5f * current_meas_period * pll_ki_) {
+        vel_estimate_counts_ = 0.0f;  //align delta-sigma on zero to prevent jitter
+        snap_to_zero_vel = true;
+    }
+
+    // Outputs from Encoder for Controller
+    float pos_cpr_last = pos_cpr_;
+    pos_estimate_ = pos_estimate_counts_ / (float)config_.cpr;
+    vel_estimate_ = vel_estimate_counts_ / (float)config_.cpr;
+    pos_cpr_= pos_cpr_counts_ / (float)config_.cpr;
+    float delta_pos_cpr = wrap_pm(pos_cpr_ - pos_cpr_last, 0.5f);
+    pos_circular_ += delta_pos_cpr;
+    pos_circular_ = fmodf_pos(pos_circular_, axis_->controller_.config_.circular_setpoint_range);
+
+    //// run encoder count interpolation
+    int32_t corrected_enc = count_in_cpr_ - config_.offset;
+    // if we are stopped, make sure we don't randomly drift
+    if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
+        interpolation_ = 0.5f;
+    // reset interpolation if encoder edge comes
+    // TODO: This isn't correct. At high velocities the first phase in this count may very well not be at the edge.
+    } else if (delta_enc > 0) {
+        interpolation_ = 0.0f;
+    } else if (delta_enc < 0) {
+        interpolation_ = 1.0f;
+    } else {
+        // Interpolate (predict) between encoder counts using vel_estimate,
+        interpolation_ += current_meas_period * vel_estimate_counts_;
+        // don't allow interpolation indicated position outside of [enc, enc+1)
+        if (interpolation_ > 1.0f) interpolation_ = 1.0f;
+        if (interpolation_ < 0.0f) interpolation_ = 0.0f;
+    }
+    float interpolated_enc = corrected_enc + interpolation_;
+
+    //// compute electrical phase
+    //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 ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
+    // ph = fmodf(ph, 2*M_PI);
+    phase_ = wrap_pm_pi(ph);
+
+    vel_estimate_valid_ = true;
+    pos_estimate_valid_ = true;
+    return true;
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/encoder.hpp

@@ -0,0 +1,120 @@
+#ifndef __ENCODER_HPP
+#define __ENCODER_HPP
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+
+class Encoder : public ODriveIntf::EncoderIntf {
+public:
+    static constexpr uint32_t MODE_FLAG_ABS = 0x100;
+
+    struct Config_t {
+        Mode mode = MODE_INCREMENTAL;
+        bool use_index = false;
+        bool pre_calibrated = false; // If true, this means the offset stored in
+                                    // configuration is valid and does not need
+                                    // be determined by run_offset_calibration.
+                                    // In this case the encoder will enter ready
+                                    // state as soon as the index is found.
+        bool zero_count_on_find_idx = true;
+        int32_t cpr = (2048 * 4);   // Default resolution of CUI-AMT102 encoder,
+        int32_t offset = 0;        // Offset between encoder count and rotor electrical phase
+        float offset_float = 0.0f; // Sub-count phase alignment offset
+        bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
+        float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
+        float calib_scan_distance = 16.0f * M_PI; // rad electrical
+        float calib_scan_omega = 4.0f * M_PI; // rad/s electrical
+        float bandwidth = 1000.0f;
+        bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
+        bool idx_search_unidirectional = false; // Only allow index search in known direction
+        bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
+        uint16_t abs_spi_cs_gpio_pin = 1;
+        uint16_t sincos_gpio_pin_sin = 3;
+        uint16_t sincos_gpio_pin_cos = 4;
+
+        // custom setters
+        Encoder* parent = nullptr;
+        void set_use_index(bool value) { use_index = value; parent->set_idx_subscribe(); }
+        void set_find_idx_on_lockin_only(bool value) { find_idx_on_lockin_only = value; parent->set_idx_subscribe(); }
+        void set_abs_spi_cs_gpio_pin(uint16_t value) { abs_spi_cs_gpio_pin = value; parent->abs_spi_cs_pin_init(); }
+        void set_pre_calibrated(bool value) { pre_calibrated = value; parent->check_pre_calibrated(); }
+        void set_bandwidth(float value) { bandwidth = value; parent->update_pll_gains(); }
+    };
+
+    Encoder(const EncoderHardwareConfig_t& hw_config,
+            Config_t& config, const Motor::Config_t& motor_config);
+    
+    void setup();
+    void set_error(Error error);
+    bool do_checks();
+
+    void enc_index_cb();
+    void set_idx_subscribe(bool override_enable = false);
+    void update_pll_gains();
+    void check_pre_calibrated();
+
+    void set_linear_count(int32_t count);
+    void set_circular_count(int32_t count, bool update_offset);
+    bool calib_enc_offset(float voltage_magnitude);
+
+    bool run_index_search();
+    bool run_direction_find();
+    bool run_offset_calibration();
+    void sample_now();
+    bool update();
+
+    const EncoderHardwareConfig_t& hw_config_;
+    Config_t& config_;
+    Axis* axis_ = nullptr; // set by Axis constructor
+
+    Error error_ = ERROR_NONE;
+    bool index_found_ = false;
+    bool is_ready_ = false;
+    int32_t shadow_count_ = 0;
+    int32_t count_in_cpr_ = 0;
+    float interpolation_ = 0.0f;
+    float phase_ = 0.0f;        // [count]
+    float pos_estimate_counts_ = 0.0f;  // [count]
+    float pos_cpr_counts_ = 0.0f;  // [count]
+    float vel_estimate_counts_ = 0.0f;  // [count/s]
+    float pll_kp_ = 0.0f;   // [count/s / count]
+    float pll_ki_ = 0.0f;   // [(count/s^2) / count]
+    float calib_scan_response_ = 0.0f; // debug report from offset calib
+    int32_t pos_abs_ = 0;
+    float spi_error_rate_ = 0.0f;
+
+    float pos_estimate_ = 0.0f; // [turn]
+    float vel_estimate_ = 0.0f; // [turn/s]
+    float pos_cpr_ = 0.0f;      // [turn]
+    float pos_circular_ = 0.0f; // [turn]
+
+    bool pos_estimate_valid_ = false;
+    bool vel_estimate_valid_ = false;
+
+    int16_t tim_cnt_sample_ = 0; // 
+    // Updated by low_level pwm_adc_cb
+    uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC
+    float sincos_sample_s_ = 0.0f;
+    float sincos_sample_c_ = 0.0f;
+
+    bool abs_spi_init();
+    bool abs_spi_start_transaction();
+    void abs_spi_cb();
+    void abs_spi_cs_pin_init();
+    uint16_t abs_spi_dma_tx_[1] = {0xFFFF};
+    uint16_t abs_spi_dma_rx_[1];
+    bool abs_spi_pos_updated_ = false;
+    Mode mode_ = MODE_INCREMENTAL;
+    GPIO_TypeDef* abs_spi_cs_port_;
+    uint16_t abs_spi_cs_pin_;
+    uint32_t abs_spi_cr1;
+    uint32_t abs_spi_cr2;
+
+    constexpr float getCoggingRatio(){
+        return 1.0f / 3600.0f;
+    }
+};
+
+#endif // __ENCODER_HPP

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/endstop.cpp

@@ -0,0 +1,55 @@
+#include <odrive_main.h>
+
+Endstop::Endstop(Endstop::Config_t& config)
+    : config_(config) {
+    update_config();
+    debounceTimer_.setIncrement(current_meas_period);
+}
+
+
+void Endstop::update() {
+    debounceTimer_.update();
+    if (config_.enabled) {
+        bool last_pin_state = pin_state_;
+
+        uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
+        GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
+        pin_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
+
+        // If the pin state has changed, reset the timer
+        if (pin_state_ != last_pin_state)
+            debounceTimer_.reset();
+
+        if (debounceTimer_.expired())
+            endstop_state_ = config_.is_active_high ? pin_state_ : !pin_state_;  // endstop_state is the logical state
+    } else {
+        endstop_state_ = false;
+    }
+}
+
+bool Endstop::get_state() {
+    return endstop_state_;
+}
+
+void Endstop::update_config() {
+    set_enabled(config_.enabled);
+    debounceTimer_.setIncrement(config_.debounce_ms * 0.001f);
+}
+
+void Endstop::set_enabled(bool enable) {
+    debounceTimer_.reset();
+    if (config_.gpio_num != 0) {
+        uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
+        GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
+        if (enable) {
+            HAL_GPIO_DeInit(gpio_port, gpio_pin);
+            GPIO_InitTypeDef GPIO_InitStruct;
+            GPIO_InitStruct.Pin = gpio_pin;
+            GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+            GPIO_InitStruct.Pull = config_.pullup ? GPIO_PULLUP : GPIO_PULLDOWN;
+            HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
+            debounceTimer_.start();
+        } else
+            debounceTimer_.stop();
+    }
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/endstop.hpp

@@ -0,0 +1,40 @@
+#ifndef __ENDSTOP_HPP
+#define __ENDSTOP_HPP
+
+#include "timer.hpp"
+class Endstop {
+   public:
+    struct Config_t {
+        float offset = 0;
+        uint32_t debounce_ms = 50;
+        uint16_t gpio_num = 0;
+        bool enabled = false;
+        bool is_active_high = false;
+        bool pullup = true;
+
+        // custom setters
+        Endstop* parent = nullptr;
+        void set_gpio_num(uint16_t value) { gpio_num = value; parent->update_config(); }
+        void set_enabled(uint32_t value) { enabled = value; parent->update_config(); }
+        void set_debounce_ms(uint32_t value) { debounce_ms = value; parent->update_config(); }
+    };
+
+    explicit Endstop(Endstop::Config_t& config);
+
+    Endstop::Config_t& config_;
+    Axis* axis_ = nullptr;
+
+    void update_config();
+    void set_enabled(bool enabled);
+
+    void update();
+    bool get_state();
+
+    bool endstop_state_ = false;
+
+   private:
+    bool pin_state_ = false;
+    float pos_when_pressed_ = 0.0f;
+    Timer<float> debounceTimer_;
+};
+#endif

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/example.json

@@ -0,0 +1,37 @@
+[
+    {
+        "name": "",
+        "id": 0,
+        "type": "json"
+    },
+    {
+        "name": "subscriptions",
+        "id": 1,
+        "type": "int32[]"
+    },
+    {
+        "name": "motor0",
+        "id": 2,
+        "type": "tree",
+        "content": [
+            {
+                "name": "pos_setpoint",
+                "id": 3,
+                "type": "float",
+                "access": "rw"
+            },
+            {
+                "name": "pos_gain",
+                "id": 4,
+                "type": "float",
+                "access": "rw"
+            },
+            {
+                "name": "vel_setpoint",
+                "id": 5,
+                "type": "float",
+                "access": "rw"
+            }
+        ]
+    }
+]

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/gpio_utils.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "gpio.h"
+constexpr GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
+  switch(GPIO_pin){
+    case 1: return GPIO_1_GPIO_Port; break;
+    case 2: return GPIO_2_GPIO_Port; break;
+    case 3: return GPIO_3_GPIO_Port; break;
+    case 4: return GPIO_4_GPIO_Port; break;
+#ifdef GPIO_5_GPIO_Port
+    case 5: return GPIO_5_GPIO_Port; break;
+#endif
+#ifdef GPIO_6_GPIO_Port
+    case 6: return GPIO_6_GPIO_Port; break;
+#endif
+#ifdef GPIO_7_GPIO_Port
+    case 7: return GPIO_7_GPIO_Port; break;
+#endif
+#ifdef GPIO_8_GPIO_Port
+    case 8: return GPIO_8_GPIO_Port; break;
+#endif
+    default: return GPIO_1_GPIO_Port;
+  }
+}
+
+constexpr uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
+  switch(GPIO_pin){
+    case 1: return GPIO_1_Pin; break;
+    case 2: return GPIO_2_Pin; break;
+    case 3: return GPIO_3_Pin; break;
+    case 4: return GPIO_4_Pin; break;
+#ifdef GPIO_5_Pin
+    case 5: return GPIO_5_Pin; break;
+#endif
+#ifdef GPIO_6_Pin
+    case 6: return GPIO_6_Pin; break;
+#endif
+#ifdef GPIO_7_Pin
+    case 7: return GPIO_7_Pin; break;
+#endif
+#ifdef GPIO_8_Pin
+    case 8: return GPIO_8_Pin; break;
+#endif
+    default: return GPIO_1_Pin;
+  }
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/low_level.cpp

@@ -0,0 +1,822 @@
+/* Includes ------------------------------------------------------------------*/
+
+// Because of broken cmsis_os.h, we need to include arm_math first,
+// otherwise chip specific defines are ommited
+#include <stm32f405xx.h>
+#include <stm32f4xx_hal.h>  // Sets up the correct chip specifc defines required by arm_math
+#define ARM_MATH_CM4
+#include <arm_math.h>
+
+#include <cmsis_os.h>
+#include <math.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <adc.h>
+#include <gpio.h>
+#include <main.h>
+#include <spi.h>
+#include <tim.h>
+#include <utils.hpp>
+
+#include "odrive_main.h"
+
+/* Private defines -----------------------------------------------------------*/
+
+// #define DEBUG_PRINT
+
+/* Private macros ------------------------------------------------------------*/
+/* Private typedef -----------------------------------------------------------*/
+/* Global constant data ------------------------------------------------------*/
+constexpr float adc_full_scale = static_cast<float>(1UL << 12UL);
+constexpr float adc_ref_voltage = 3.3f;
+/* Global variables ----------------------------------------------------------*/
+
+// This value is updated by the DC-bus reading ADC.
+// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
+float vbus_voltage = 12.0f;
+float ibus_ = 0.0f; // exposed for monitoring only
+bool brake_resistor_armed = false;
+bool brake_resistor_saturated = false;
+/* Private constant data -----------------------------------------------------*/
+static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC };
+static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]); 
+/* Private variables ---------------------------------------------------------*/
+
+// Two motors, sampling port A,B,C (coherent with current meas timing)
+static uint16_t GPIO_port_samples [2][num_GPIO];
+/* CPU critical section helpers ----------------------------------------------*/
+
+/* Safety critical functions -------------------------------------------------*/
+
+/*
+* This section contains all accesses to safety critical hardware registers.
+* Specifically, these registers:
+*   Motor0 PWMs:
+*     Timer1.MOE (master output enabled)
+*     Timer1.CCR1 (counter compare register 1)
+*     Timer1.CCR2 (counter compare register 2)
+*     Timer1.CCR3 (counter compare register 3)
+*   Motor1 PWMs:
+*     Timer8.MOE (master output enabled)
+*     Timer8.CCR1 (counter compare register 1)
+*     Timer8.CCR2 (counter compare register 2)
+*     Timer8.CCR3 (counter compare register 3)
+*   Brake resistor PWM:
+*     Timer2.CCR3 (counter compare register 3)
+*     Timer2.CCR4 (counter compare register 4)
+* 
+* The following assumptions are made:
+*   - The hardware operates as described in the datasheet:
+*     http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
+*     This assumption also requires for instance that there are no radiation
+*     caused hardware errors.
+*   - After startup, all variables used in this section are exclusively modified
+*     by the code in this section (this excludes function parameters)
+*     This assumption also requires that there is no memory corruption.
+*   - This code is compiled by a C standard compliant compiler.
+*
+* Furthermore:
+*   - Between calls to safety_critical_arm_motor_pwm and
+*     safety_critical_disarm_motor_pwm the motor's Ibus current is
+*     set to the correct value and update_brake_resistor is called
+*     at a high rate.
+*/
+
+// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
+void low_level_fault(Motor::Error error) {
+    // Disable all motors NOW!
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        safety_critical_disarm_motor_pwm(axes[i]->motor_);
+        axes[i]->motor_.error_ |= error;
+    }
+
+    safety_critical_disarm_brake_resistor();
+}
+
+// @brief Kicks off the arming process of the motor.
+// All calls to this function must clearly originate
+// from user input.
+void safety_critical_arm_motor_pwm(Motor& motor) {
+    uint32_t mask = cpu_enter_critical();
+    if (brake_resistor_armed) {
+        motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
+    }
+    cpu_exit_critical(mask);
+}
+
+// @brief Disarms the motor PWM.
+// After calling this function, it is guaranteed that all three
+// motor phases are floating and will not be enabled again until
+// safety_critical_arm_motor_phases is called.
+// @returns true if the motor was in a state other than disarmed before
+bool safety_critical_disarm_motor_pwm(Motor& motor) {
+    uint32_t mask = cpu_enter_critical();
+    bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
+    motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
+    __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer);
+    cpu_exit_critical(mask);
+    return was_armed;
+}
+
+// @brief Updates the phase timings unless the motor is disarmed.
+//
+// If this is called at a rate higher than the motor's timer period,
+// the actual PMW timings on the pins can be undefined for up to one
+// timer period.
+void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) {
+    uint32_t mask = cpu_enter_critical();
+    if (!brake_resistor_armed) {
+        motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
+    }
+
+    motor.hw_config_.timer->Instance->CCR1 = timings[0];
+    motor.hw_config_.timer->Instance->CCR2 = timings[1];
+    motor.hw_config_.timer->Instance->CCR3 = timings[2];
+
+    if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) {
+        // timings were just loaded into the timer registers
+        // the timer register are buffered, so they won't have an effect
+        // on the output just yet so we need to wait until the next
+        // interrupt before we actually enable the output
+        motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE;
+    } else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) {
+        // now we waited long enough. Enter armed state and
+        // enable the actual PWM outputs.
+        motor.armed_state_ = Motor::ARMED_STATE_ARMED;
+        __HAL_TIM_MOE_ENABLE(motor.hw_config_.timer);  // enable pwm outputs
+    } else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) {
+        // nothing to do, PWM is running, all good
+    } else {
+        // unknown state oh no
+        safety_critical_disarm_motor_pwm(motor);
+    }
+    cpu_exit_critical(mask);
+}
+
+// @brief Arms the brake resistor
+void safety_critical_arm_brake_resistor() {
+    uint32_t mask = cpu_enter_critical();
+    brake_resistor_armed = true;
+    htim2.Instance->CCR3 = 0;
+    htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
+    cpu_exit_critical(mask);
+}
+
+// @brief Disarms the brake resistor and by extension
+// all motor PWM outputs.
+// After calling this, the brake resistor can only be armed again
+// by calling safety_critical_arm_brake_resistor().
+void safety_critical_disarm_brake_resistor() {
+    uint32_t mask = cpu_enter_critical();
+    brake_resistor_armed = false;
+    htim2.Instance->CCR3 = 0;
+    htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        safety_critical_disarm_motor_pwm(axes[i]->motor_);
+    }
+    cpu_exit_critical(mask);
+}
+
+// @brief Updates the brake resistor PWM timings unless
+// the brake resistor is disarmed.
+void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) {
+    if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS)
+        low_level_fault(Motor::ERROR_BRAKE_DEADTIME_VIOLATION);
+    uint32_t mask = cpu_enter_critical();
+    if (brake_resistor_armed) {
+        // Safe update of low and high side timings
+        // To avoid race condition, first reset timings to safe state
+        // ch3 is low side, ch4 is high side
+        htim2.Instance->CCR3 = 0;
+        htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
+        htim2.Instance->CCR3 = low_off;
+        htim2.Instance->CCR4 = high_on;
+    }
+    cpu_exit_critical(mask);
+}
+
+/* Function implementations --------------------------------------------------*/
+
+void start_adc_pwm() {
+    // Enable ADC and interrupts
+    __HAL_ADC_ENABLE(&hadc1);
+    __HAL_ADC_ENABLE(&hadc2);
+    __HAL_ADC_ENABLE(&hadc3);
+    // Warp field stabilize.
+    osDelay(2);
+    __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
+    __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
+    __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC);
+    __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC);
+    __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC);
+
+    // Ensure that debug halting of the core doesn't leave the motor PWM running
+    __HAL_DBGMCU_FREEZE_TIM1();
+    __HAL_DBGMCU_FREEZE_TIM8();
+    __HAL_DBGMCU_FREEZE_TIM13();
+
+    start_pwm(&htim1);
+    start_pwm(&htim8);
+    // TODO: explain why this offset
+    sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128,
+            &htim13);
+
+    // Motor output starts in the disabled state
+    __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1);
+    __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8);
+
+    // Enable the update interrupt (used to coherently sample GPIO)
+    __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
+    __HAL_TIM_ENABLE_IT(&htim8, TIM_IT_UPDATE);
+
+    // Start brake resistor PWM in floating output configuration
+    htim2.Instance->CCR3 = 0;
+    htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
+    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
+    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
+
+    // Disarm motors and arm brake resistor
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        safety_critical_disarm_motor_pwm(axes[i]->motor_);
+    }
+    safety_critical_arm_brake_resistor();
+}
+
+void start_pwm(TIM_HandleTypeDef* htim) {
+    // Init PWM
+    int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
+    htim->Instance->CCR1 = half_load;
+    htim->Instance->CCR2 = half_load;
+    htim->Instance->CCR3 = half_load;
+
+    // This hardware obfustication layer really is getting on my nerves
+    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
+    HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1);
+    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2);
+    HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2);
+    HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3);
+    HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3);
+
+    htim->Instance->CCR4 = 1;
+    HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
+}
+
+/*
+ * Initial intention of this function:
+ * Synchronize TIM1, TIM8 and TIM13 such that:
+ *  1. The triangle waveform of TIM1 leads the triangle waveform of TIM8 by a
+ *     90° phase shift.
+ *  2. The timer update events of TIM1 and TIM8 are symmetrically interleaved.
+ *  3. Each TIM13 reload coincides with a TIM1 lower update event.
+ * 
+ * However right now this function only ensures point (1) and (3) but because
+ * TIM1 and TIM3 only trigger an update on every third reload, this does not
+ * imply (or even allow for) (2).
+ * 
+ * TODO: revisit the timing topic in general.
+ */
+void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
+                 uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
+                 TIM_HandleTypeDef* htim_refbase) {
+    // Store intial timer configs
+    uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE);
+    uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE);
+    uint16_t CR2_store = htim_a->Instance->CR2;
+    uint16_t SMCR_store = htim_b->Instance->SMCR;
+    // Turn off output
+    htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE);
+    htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE);
+    // Disable both timer counters
+    htim_a->Instance->CR1 &= ~TIM_CR1_CEN;
+    htim_b->Instance->CR1 &= ~TIM_CR1_CEN;
+    // Set first timer to send TRGO on counter enable
+    htim_a->Instance->CR2 &= ~TIM_CR2_MMS;
+    htim_a->Instance->CR2 |= TIM_TRGO_ENABLE;
+    // Set Trigger Source of second timer to the TRGO of the first timer
+    htim_b->Instance->SMCR &= ~TIM_SMCR_TS;
+    htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx;
+    // Set 2nd timer to start on trigger
+    htim_b->Instance->SMCR &= ~TIM_SMCR_SMS;
+    htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER;
+    // Dir bit is read only in center aligned mode, so we clear the mode for now
+    uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS;
+    uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS;
+    htim_a->Instance->CR1 &= ~TIM_CR1_CMS;
+    htim_b->Instance->CR1 &= ~TIM_CR1_CMS;
+    // Set both timers to up-counting state
+    htim_a->Instance->CR1 &= ~TIM_CR1_DIR;
+    htim_b->Instance->CR1 &= ~TIM_CR1_DIR;
+    // Restore center aligned mode
+    htim_a->Instance->CR1 |= CMS_store_a;
+    htim_b->Instance->CR1 |= CMS_store_b;
+    // set counter offset
+    htim_a->Instance->CNT = count_offset;
+    htim_b->Instance->CNT = 0;
+    // Set and start reference timebase timer (if used)
+    if (htim_refbase) {
+        htim_refbase->Instance->CNT = count_offset;
+        htim_refbase->Instance->CR1 |= (TIM_CR1_CEN); // start
+    }
+    // Start Timer a
+    htim_a->Instance->CR1 |= (TIM_CR1_CEN);
+    // Restore timer configs
+    htim_a->Instance->CR2 = CR2_store;
+    htim_b->Instance->SMCR = SMCR_store;
+    // restore output
+    htim_a->Instance->BDTR |= MOE_store_a;
+    htim_b->Instance->BDTR |= MOE_store_b;
+}
+
+// @brief ADC1 measurements are written to this buffer by DMA
+uint16_t adc_measurements_[ADC_CHANNEL_COUNT] = { 0 };
+
+// @brief Starts the general purpose ADC on the ADC1 peripheral.
+// The measured ADC voltages can be read with get_adc_voltage().
+//
+// ADC1 is set up to continuously sample all channels 0 to 15 in a
+// round-robin fashion.
+// DMA is used to copy the measured 12-bit values to adc_measurements_.
+//
+// The injected (high priority) channel of ADC1 is used to sample vbus_voltage.
+// This conversion is triggered by TIM1 at the frequency of the motor control loop.
+void start_general_purpose_adc() {
+    ADC_ChannelConfTypeDef sConfig;
+
+    // Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
+    hadc1.Instance = ADC1;
+    hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
+    hadc1.Init.Resolution = ADC_RESOLUTION_12B;
+    hadc1.Init.ScanConvMode = ENABLE;
+    hadc1.Init.ContinuousConvMode = ENABLE;
+    hadc1.Init.DiscontinuousConvMode = DISABLE;
+    hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
+    hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
+    hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
+    hadc1.Init.NbrOfConversion = ADC_CHANNEL_COUNT;
+    hadc1.Init.DMAContinuousRequests = ENABLE;
+    hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
+    if (HAL_ADC_Init(&hadc1) != HAL_OK)
+    {
+        _Error_Handler((char*)__FILE__, __LINE__);
+    }
+
+    // Set up sampling sequence (channel 0 ... channel 15)
+    sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES;
+    for (uint32_t channel = 0; channel < ADC_CHANNEL_COUNT; ++channel) {
+        sConfig.Channel = channel << ADC_CR1_AWDCH_Pos;
+        sConfig.Rank = channel + 1; // rank numbering starts at 1
+        if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
+            _Error_Handler((char*)__FILE__, __LINE__);
+    }
+
+    HAL_ADC_Start_DMA(&hadc1, reinterpret_cast<uint32_t*>(adc_measurements_), ADC_CHANNEL_COUNT);
+}
+
+// @brief Returns the ADC voltage associated with the specified pin.
+// GPIO_set_to_analog() must be called first to put the Pin into
+// analog mode.
+// Returns NaN if the pin has no associated ADC1 channel.
+//
+// On ODrive 3.3 and 3.4 the following pins can be used with this function:
+//  GPIO_1, GPIO_2, GPIO_3, GPIO_4 and some pins that are connected to
+//  on-board sensors (M0_TEMP, M1_TEMP, AUX_TEMP)
+//
+// The ADC values are sampled in background at ~30kHz without
+// any CPU involvement.
+//
+// Details: each of the 16 conversion takes (15+26) ADC clock
+// cycles and the ADC, so the update rate of the entire sequence is:
+//  21000kHz / (15+26) / 16 = 32kHz
+// The true frequency is slightly lower because of the injected vbus
+// measurements
+float get_adc_voltage(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin) {
+    const uint16_t channel = channel_from_gpio(GPIO_port, GPIO_pin);
+    return get_adc_voltage_channel(channel);
+}
+
+// @brief Given a GPIO_port and pin return the associated adc_channel.
+// returns UINT16_MAX if there is no adc_channel;
+uint16_t channel_from_gpio(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin)
+{
+    uint16_t channel = UINT16_MAX;
+    if (GPIO_port == GPIOA) {
+        if (GPIO_pin == GPIO_PIN_0)
+            channel = 0;
+        else if (GPIO_pin == GPIO_PIN_1)
+            channel = 1;
+        else if (GPIO_pin == GPIO_PIN_2)
+            channel = 2;
+        else if (GPIO_pin == GPIO_PIN_3)
+            channel = 3;
+        else if (GPIO_pin == GPIO_PIN_4)
+            channel = 4;
+        else if (GPIO_pin == GPIO_PIN_5)
+            channel = 5;
+        else if (GPIO_pin == GPIO_PIN_6)
+            channel = 6;
+        else if (GPIO_pin == GPIO_PIN_7)
+            channel = 7;
+    } else if (GPIO_port == GPIOB) {
+        if (GPIO_pin == GPIO_PIN_0)
+            channel = 8;
+        else if (GPIO_pin == GPIO_PIN_1)
+            channel = 9;
+    } else if (GPIO_port == GPIOC) {
+        if (GPIO_pin == GPIO_PIN_0)
+            channel = 10;
+        else if (GPIO_pin == GPIO_PIN_1)
+            channel = 11;
+        else if (GPIO_pin == GPIO_PIN_2)
+            channel = 12;
+        else if (GPIO_pin == GPIO_PIN_3)
+            channel = 13;
+        else if (GPIO_pin == GPIO_PIN_4)
+            channel = 14;
+        else if (GPIO_pin == GPIO_PIN_5)
+            channel = 15;
+    }
+    return channel;
+}
+
+// @brief Given an adc channel return the measured voltage.
+// returns NaN if the channel is not valid.
+float get_adc_voltage_channel(uint16_t channel)
+{
+    if (channel < ADC_CHANNEL_COUNT)
+        return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale);
+    else
+        return 0.0f / 0.0f; // NaN
+}
+
+//--------------------------------
+// IRQ Callbacks
+//--------------------------------
+
+void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
+    constexpr float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
+    // Only one conversion in sequence, so only rank1
+    uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
+    vbus_voltage = ADCValue * voltage_scale;
+}
+
+static void decode_hall_samples(Encoder& enc, uint16_t GPIO_samples[num_GPIO]) {
+    GPIO_TypeDef* hall_ports[] = {
+        enc.hw_config_.hallC_port,
+        enc.hw_config_.hallB_port,
+        enc.hw_config_.hallA_port,
+    };
+    uint16_t hall_pins[] = {
+        enc.hw_config_.hallC_pin,
+        enc.hw_config_.hallB_pin,
+        enc.hw_config_.hallA_pin,
+    };
+
+    uint8_t hall_state = 0x0;
+    for (int i = 0; i < 3; ++i) {
+        int port_idx = 0;
+        for (;;) {
+            auto port = GPIOs_to_samp[port_idx];
+            if (port == hall_ports[i])
+                break;
+            ++port_idx;
+        }
+
+        hall_state <<= 1;
+        hall_state |= (GPIO_samples[port_idx] & hall_pins[i]) ? 1 : 0;
+    }
+
+    enc.hall_state_ = hall_state;
+}
+
+// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion.
+// Timing diagram: Firmware/timing_diagram_v3.png
+void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
+#define calib_tau 0.2f  //@TOTO make more easily configurable
+    constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
+
+    // Ensure ADCs are expected ones to simplify the logic below
+    if (!(hadc == &hadc2 || hadc == &hadc3)) {
+        low_level_fault(Motor::ERROR_ADC_FAILED);
+        return;
+    };
+
+    // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion
+    // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
+    // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
+    // If we are counting down, we just sampled in SVM vector 7, with zero current
+    Axis& axis = injected ? *axes[0] : *axes[1];
+    int axis_num = injected ? 0 : 1;
+    Axis& other_axis = injected ? *axes[1] : *axes[0];
+    bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
+    bool current_meas_not_DC_CAL = !counting_down;
+
+    // Check the timing of the sequencing
+    if (current_meas_not_DC_CAL)
+        axis.motor_.log_timing(TIMING_LOG_ADC_CB_I);
+    else
+        axis.motor_.log_timing(TIMING_LOG_ADC_CB_DC);
+
+    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
+
+        // TODO: this is out of place here. However when moving it somewhere
+        // else we have to consider the timing requirements to prevent the SPI
+        // transfers of axis0 and axis1 from conflicting.
+        // Also see comment on sync_timers.
+        if((current_meas_not_DC_CAL && !axis_num) ||
+                (axis_num && !current_meas_not_DC_CAL)){
+            axis.encoder_.abs_spi_start_transaction();
+        }
+    }
+
+    // Load next timings for the motor that we're not currently sampling
+    if (update_timings) {
+        if (!other_axis.motor_.next_timings_valid_) {
+            // the motor control loop failed to update the timings in time
+            // we must assume that it died and therefore float all phases
+            bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_);
+            if (was_armed) {
+                other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED;
+            }
+        } else {
+            other_axis.motor_.next_timings_valid_ = false;
+            safety_critical_apply_motor_pwm_timings(
+                other_axis.motor_, other_axis.motor_.next_timings_
+            );
+        }
+        update_brake_current();
+    }
+
+    uint32_t ADCValue;
+    if (injected) {
+        ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
+    } else {
+        ADCValue = HAL_ADC_GetValue(hadc);
+    }
+    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,
+        // and their interrupts should arrive on the same clock cycle.
+        // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
+        // Therefore we store the value from ADC2 and signal the thread that the
+        // measurement is ready when we receive the ADC3 measurement
+
+        // return or continue
+        if (hadc == &hadc2) {
+            axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB;
+            return;
+        } else {
+            axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
+        }
+        // Prepare hall readings
+        // TODO move this to inside encoder update function
+        decode_hall_samples(axis.encoder_, GPIO_port_samples[axis_num]);
+        // Trigger axis thread
+        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;
+        } else {
+            axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
+        }
+    }
+}
+
+void tim_update_cb(TIM_HandleTypeDef* htim) {
+    
+    // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
+    // If we are counting down, we just sampled in SVM vector 7, with zero current
+    bool counting_down = htim->Instance->CR1 & TIM_CR1_DIR;
+    if (counting_down)
+        return;
+    
+    int sample_ch;
+    Axis* axis;
+    if (htim == &htim1) {
+        sample_ch = 0;
+        axis = axes[0];
+    } else if (htim == &htim8) {
+        sample_ch = 1;
+        axis = axes[1];
+    } else {
+        low_level_fault(Motor::ERROR_UNEXPECTED_TIMER_CALLBACK);
+        return;
+    }
+
+    axis->encoder_.sample_now();
+
+    for (int i = 0; i < num_GPIO; ++i) {
+        GPIO_port_samples[sample_ch][i] = GPIOs_to_samp[i]->IDR;
+    }
+}
+
+// @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) {
+        if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
+            Ibus_sum += axes[i]->motor_.current_control_.Ibus;
+        }
+    }
+    
+    // Don't start braking until -Ibus > regen_current_allowed
+    float brake_current = -Ibus_sum - odrv.config_.max_regen_current;
+    float brake_duty = brake_current * odrv.config_.brake_resistance / vbus_voltage;
+    
+    if (odrv.config_.enable_dc_bus_overvoltage_ramp && (odrv.config_.brake_resistance > 0.0f) && (odrv.config_.dc_bus_overvoltage_ramp_start < odrv.config_.dc_bus_overvoltage_ramp_end)) {
+        brake_duty += std::fmax((vbus_voltage - odrv.config_.dc_bus_overvoltage_ramp_start) / (odrv.config_.dc_bus_overvoltage_ramp_end - odrv.config_.dc_bus_overvoltage_ramp_start), 0.0f);
+    }
+
+    if (std::isnan(brake_duty)) {
+        // Shuts off all motors AND brake resistor, sets error code on all motors.
+        low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN);
+        return;
+    }
+
+    if (brake_duty >= 0.95f) {
+        brake_resistor_saturated = true;
+    }
+
+    // Duty limit at 95% to allow bootstrap caps to charge
+    brake_duty = std::clamp(brake_duty, 0.0f, 0.95f);
+
+    // Special handling to avoid the case 0.0/0.0 == NaN.
+    Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / odrv.config_.brake_resistance) : 0.0f;
+
+    ibus_ += odrv.ibus_report_filter_k_ * (Ibus_sum - ibus_);
+
+    if (Ibus_sum > odrv.config_.dc_max_positive_current) {
+        low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT);
+        return;
+    }
+    if (Ibus_sum < odrv.config_.dc_max_negative_current) {
+        low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT);
+        return;
+    }
+    
+    int high_on = (int)(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
+    int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
+    if (low_off < 0) low_off = 0;
+    safety_critical_apply_brake_resistor_timings(low_off, high_on);
+}
+
+
+/* RC PWM input --------------------------------------------------------------*/
+
+// @brief Returns the ODrive GPIO number for a given
+// TIM2 or TIM5 input capture channel number.
+int tim_2_5_channel_num_to_gpio_num(int channel) {
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+    if (channel >= 1 && channel <= 4) {
+        // the channel numbers just happen to coincide with
+        // the GPIO numbers
+        return channel;
+    } else {
+        return -1;
+    }
+#else
+    // Only ch4 is available on v3.2
+    if (channel == 4) {
+        return 4;
+    } else {
+        return -1;
+    }
+#endif
+}
+// @brief Returns the TIM2 or TIM5 channel number
+// for a given GPIO number.
+uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) {
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+    switch (gpio_num) {
+        case 1: return TIM_CHANNEL_1;
+        case 2: return TIM_CHANNEL_2;
+        case 3: return TIM_CHANNEL_3;
+        case 4: return TIM_CHANNEL_4;
+        default: return 0;
+    }
+#else
+    // Only ch4 is available on v3.2
+    if (gpio_num == 4) {
+        return TIM_CHANNEL_4;
+    } else {
+        return 0;
+    }
+#endif
+}
+
+void pwm_in_init() {
+    GPIO_InitTypeDef GPIO_InitStruct;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF2_TIM5;
+
+    TIM_IC_InitTypeDef sConfigIC;
+    sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE;
+    sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
+    sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
+    sConfigIC.ICFilter = 15;
+
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+    for (int gpio_num = 1; gpio_num <= 4; ++gpio_num) {
+#else
+    int gpio_num = 4; {
+#endif
+        if (fibre::is_endpoint_ref_valid(odrv.config_.pwm_mappings[gpio_num - 1].endpoint)) {
+            GPIO_InitStruct.Pin = get_gpio_pin_by_pin(gpio_num);
+            HAL_GPIO_DeInit(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num));
+            HAL_GPIO_Init(get_gpio_port_by_pin(gpio_num), &GPIO_InitStruct);
+            HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, gpio_num_to_tim_2_5_channel(gpio_num));
+            HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(gpio_num));
+        }
+    }
+}
+
+//TODO: These expressions have integer division by 1MHz, so it will be incorrect for clock speeds of not-integer MHz
+#define TIM_2_5_CLOCK_HZ        TIM_APB1_CLOCK_HZ
+#define PWM_MIN_HIGH_TIME          ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse
+#define PWM_MAX_HIGH_TIME          ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward
+#define PWM_MIN_LEGAL_HIGH_TIME    ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms
+#define PWM_MAX_LEGAL_HIGH_TIME    ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms
+#define PWM_INVERT_INPUT        false
+
+void handle_pulse(int gpio_num, uint32_t high_time) {
+    if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME)
+        return;
+
+    if (high_time < PWM_MIN_HIGH_TIME)
+        high_time = PWM_MIN_HIGH_TIME;
+    if (high_time > PWM_MAX_HIGH_TIME)
+        high_time = PWM_MAX_HIGH_TIME;
+    float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME);
+    float value = odrv.config_.pwm_mappings[gpio_num - 1].min +
+                  (fraction * (odrv.config_.pwm_mappings[gpio_num - 1].max - odrv.config_.pwm_mappings[gpio_num - 1].min));
+
+    fibre::set_endpoint_from_float(odrv.config_.pwm_mappings[gpio_num - 1].endpoint, value);
+}
+
+void pwm_in_cb(int channel, uint32_t timestamp) {
+    static uint32_t last_timestamp[GPIO_COUNT] = { 0 };
+    static bool last_pin_state[GPIO_COUNT] = { false };
+    static bool last_sample_valid[GPIO_COUNT] = { false };
+
+    int gpio_num = tim_2_5_channel_num_to_gpio_num(channel);
+    if (gpio_num < 1 || gpio_num > GPIO_COUNT)
+        return;
+    bool current_pin_state = HAL_GPIO_ReadPin(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)) != GPIO_PIN_RESET;
+
+    if (last_sample_valid[gpio_num - 1]
+        && (last_pin_state[gpio_num - 1] != PWM_INVERT_INPUT)
+        && (current_pin_state == PWM_INVERT_INPUT)) {
+        handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num - 1]);
+    }
+
+    last_timestamp[gpio_num - 1] = timestamp;
+    last_pin_state[gpio_num - 1] = current_pin_state;
+    last_sample_valid[gpio_num - 1] = true;
+}
+
+
+/* Analog speed control input */
+
+static void update_analog_endpoint(const struct PWMMapping_t *map, int gpio)
+{
+    float fraction = get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio)) / 3.3f;
+    float value = map->min + (fraction * (map->max - map->min));
+    fibre::set_endpoint_from_float(map->endpoint, value);
+}
+
+static void analog_polling_thread(void *)
+{
+    while (true) {
+        for (int i = 0; i < GPIO_COUNT; i++) {
+            struct PWMMapping_t *map = &odrv.config_.analog_mappings[i];
+
+            if (fibre::is_endpoint_ref_valid(map->endpoint))
+                update_analog_endpoint(map, i + 1);
+        }
+        osDelay(10);
+    }
+}
+
+void start_analog_thread() {
+    osThreadDef(thread_def, analog_polling_thread, osPriorityLow, 0, 512 / sizeof(StackType_t));
+    osThreadCreate(osThread(thread_def), NULL);
+}
+
+
+void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
+{
+    if(hspi->pRxBuffPtr == (uint8_t*)axes[0]->encoder_.abs_spi_dma_rx_)
+        axes[0]->encoder_.abs_spi_cb();
+    else if (hspi->pRxBuffPtr == (uint8_t*)axes[1]->encoder_.abs_spi_dma_rx_)
+        axes[1]->encoder_.abs_spi_cb();
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/low_level.h

@@ -0,0 +1,76 @@
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __LOW_LEVEL_H
+#define __LOW_LEVEL_H
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include <cmsis_os.h>
+#include <stdbool.h>
+#include <adc.h>
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+#define ADC_CHANNEL_COUNT 16
+extern const float adc_full_scale;
+extern const float adc_ref_voltage;
+/* Exported variables --------------------------------------------------------*/
+extern float vbus_voltage;
+extern float ibus_;
+extern bool brake_resistor_armed;
+extern bool brake_resistor_saturated;
+extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT];
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions --------------------------------------------------------*/
+
+void safety_critical_arm_motor_pwm(Motor& motor);
+bool safety_critical_disarm_motor_pwm(Motor& motor);
+void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]);
+void safety_critical_arm_brake_resistor();
+void safety_critical_disarm_brake_resistor();
+void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on);
+
+// called from STM platform code
+extern "C" {
+void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
+void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
+void tim_update_cb(TIM_HandleTypeDef* htim);
+void pwm_in_cb(int channel, uint32_t timestamp);
+}
+
+// Initalisation
+void start_adc_pwm();
+void start_pwm(TIM_HandleTypeDef* htim);
+void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
+                 uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset,
+                 TIM_HandleTypeDef* htim_refbase = nullptr);
+void start_general_purpose_adc();
+float get_adc_voltage(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin);
+uint16_t channel_from_gpio(const GPIO_TypeDef* const GPIO_port, uint16_t GPIO_pin);
+float get_adc_voltage_channel(uint16_t channel);
+void pwm_in_init();
+void start_analog_thread();
+
+void update_brake_current();
+
+inline uint32_t cpu_enter_critical() {
+    uint32_t primask = __get_PRIMASK();
+    __disable_irq();
+    return primask;
+}
+
+inline void cpu_exit_critical(uint32_t priority_mask) {
+    __set_PRIMASK(priority_mask);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //__LOW_LEVEL_H

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/main.cpp

@@ -0,0 +1,309 @@
+
+#define __MAIN_CPP__
+#include "odrive_main.h"
+#include "nvm_config.hpp"
+
+#include "usart.h"
+#include "freertos_vars.h"
+#include <communication/interface_usb.h>
+#include <communication/interface_uart.h>
+#include <communication/interface_i2c.h>
+#include <communication/interface_can.hpp>
+
+ODriveCAN::Config_t can_config;
+Encoder::Config_t encoder_configs[AXIS_COUNT];
+SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
+Controller::Config_t controller_configs[AXIS_COUNT];
+Motor::Config_t motor_configs[AXIS_COUNT];
+OnboardThermistorCurrentLimiter::Config_t fet_thermistor_configs[AXIS_COUNT];
+OffboardThermistorCurrentLimiter::Config_t motor_thermistor_configs[AXIS_COUNT];
+Axis::Config_t axis_configs[AXIS_COUNT];
+TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT];
+Endstop::Config_t min_endstop_configs[AXIS_COUNT];
+Endstop::Config_t max_endstop_configs[AXIS_COUNT];
+
+std::array<Axis*, AXIS_COUNT> axes;
+ODriveCAN *odCAN = nullptr;
+ODrive odrv{};
+
+typedef Config<
+    BoardConfig_t,
+    ODriveCAN::Config_t,
+    Encoder::Config_t[AXIS_COUNT],
+    SensorlessEstimator::Config_t[AXIS_COUNT],
+    Controller::Config_t[AXIS_COUNT],
+    Motor::Config_t[AXIS_COUNT],
+    OnboardThermistorCurrentLimiter::Config_t[AXIS_COUNT],
+    OffboardThermistorCurrentLimiter::Config_t[AXIS_COUNT],
+    TrapezoidalTrajectory::Config_t[AXIS_COUNT],
+    Endstop::Config_t[AXIS_COUNT],
+    Endstop::Config_t[AXIS_COUNT],
+    Axis::Config_t[AXIS_COUNT]> ConfigFormat;
+
+void ODrive::save_configuration(void) {
+    if (ConfigFormat::safe_store_config(
+            &odrv.config_,
+            &can_config,
+            &encoder_configs,
+            &sensorless_configs,
+            &controller_configs,
+            &motor_configs,
+            &fet_thermistor_configs,
+            &motor_thermistor_configs,
+            &trap_configs,
+            &min_endstop_configs,
+            &max_endstop_configs,
+            &axis_configs)) {
+        printf("saving configuration failed\r\n"); osDelay(5);
+    } else {
+        odrv.user_config_loaded_ = true;
+    }
+}
+
+extern "C" int load_configuration(void) {
+    // Try to load configs
+    if (NVM_init() ||
+        ConfigFormat::safe_load_config(
+                &odrv.config_,
+                &can_config,
+                &encoder_configs,
+                &sensorless_configs,
+                &controller_configs,
+                &motor_configs,
+                &fet_thermistor_configs,
+                &motor_thermistor_configs,
+                &trap_configs,
+                &min_endstop_configs,
+                &max_endstop_configs,
+                &axis_configs)) {
+        //If loading failed, restore defaults
+        odrv.config_ = BoardConfig_t();
+        can_config = ODriveCAN::Config_t();
+        for (size_t i = 0; i < AXIS_COUNT; ++i) {
+            encoder_configs[i] = Encoder::Config_t();
+            sensorless_configs[i] = SensorlessEstimator::Config_t();
+            controller_configs[i] = Controller::Config_t();
+            motor_configs[i] = Motor::Config_t();
+            fet_thermistor_configs[i] = OnboardThermistorCurrentLimiter::Config_t();
+            motor_thermistor_configs[i] = OffboardThermistorCurrentLimiter::Config_t();
+            trap_configs[i] = TrapezoidalTrajectory::Config_t();
+            axis_configs[i] = Axis::Config_t();
+            // Default step/dir pins are different, so we need to explicitly load them
+            Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]);
+            Axis::load_default_can_id(i, axis_configs[i]);
+            min_endstop_configs[i] = Endstop::Config_t();
+            max_endstop_configs[i] = Endstop::Config_t();
+            controller_configs[i].load_encoder_axis = i;
+        }
+    } else {
+        odrv.user_config_loaded_ = true;
+    }
+    return odrv.user_config_loaded_;
+}
+
+void ODrive::erase_configuration(void) {
+    NVM_erase();
+
+    // FIXME: this reboot is a workaround because we don't want the next save_configuration
+    // to write back the old configuration from RAM to NVM. The proper action would
+    // be to reset the values in RAM to default. However right now that's not
+    // practical because several startup actions depend on the config. The
+    // other problem is that the stack overflows if we reset to default here.
+    NVIC_SystemReset();
+}
+
+void ODrive::enter_dfu_mode() {
+    if ((hw_version_major_ == 3) && (hw_version_minor_ >= 5)) {
+        __asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
+        _reboot_cookie = 0xDEADBEEF;
+        NVIC_SystemReset();
+    } else {
+        /*
+        * DFU mode is only allowed on board version >= 3.5 because it can burn
+        * the brake resistor FETs on older boards.
+        * If you really want to use it on an older board, add 3.3k pull-down resistors
+        * to the AUX_L and AUX_H signals and _only then_ uncomment these lines.
+        */
+        //__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
+        //_reboot_cookie = 0xDEADFE75;
+        //NVIC_SystemReset();
+    }
+}
+
+extern "C" int construct_objects(){
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+    if (odrv.config_.enable_i2c_instead_of_can) {
+        // Set up the direction GPIO as input
+        GPIO_InitTypeDef GPIO_InitStruct;
+        GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+        GPIO_InitStruct.Pull = GPIO_PULLUP;
+
+        GPIO_InitStruct.Pin = I2C_A0_PIN;
+        HAL_GPIO_Init(I2C_A0_PORT, &GPIO_InitStruct);
+        GPIO_InitStruct.Pin = I2C_A1_PIN;
+        HAL_GPIO_Init(I2C_A1_PORT, &GPIO_InitStruct);
+        GPIO_InitStruct.Pin = I2C_A2_PIN;
+        HAL_GPIO_Init(I2C_A2_PORT, &GPIO_InitStruct);
+
+        osDelay(1);
+        i2c_stats_.addr = (0xD << 3);
+        i2c_stats_.addr |= HAL_GPIO_ReadPin(I2C_A0_PORT, I2C_A0_PIN) != GPIO_PIN_RESET ? 0x1 : 0;
+        i2c_stats_.addr |= HAL_GPIO_ReadPin(I2C_A1_PORT, I2C_A1_PIN) != GPIO_PIN_RESET ? 0x2 : 0;
+        i2c_stats_.addr |= HAL_GPIO_ReadPin(I2C_A2_PORT, I2C_A2_PIN) != GPIO_PIN_RESET ? 0x4 : 0;
+        MX_I2C1_Init(i2c_stats_.addr);
+    } else
+#endif
+        MX_CAN1_Init();
+
+    HAL_UART_DeInit(&huart4);
+    huart4.Init.BaudRate = odrv.config_.uart_baudrate;
+    HAL_UART_Init(&huart4);
+
+    // Init general user ADC on some GPIOs.
+    GPIO_InitTypeDef GPIO_InitStruct;
+    GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Pin = GPIO_1_Pin;
+    HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
+    GPIO_InitStruct.Pin = GPIO_2_Pin;
+    HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
+    GPIO_InitStruct.Pin = GPIO_3_Pin;
+    HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct);
+    GPIO_InitStruct.Pin = GPIO_4_Pin;
+    HAL_GPIO_Init(GPIO_4_GPIO_Port, &GPIO_InitStruct);
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5
+    GPIO_InitStruct.Pin = GPIO_5_Pin;
+    HAL_GPIO_Init(GPIO_5_GPIO_Port, &GPIO_InitStruct);
+#endif
+
+    // Construct all objects.
+    odCAN = new ODriveCAN(can_config, &hcan1);
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
+                                       encoder_configs[i], motor_configs[i]);
+        SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]);
+        Controller *controller = new Controller(controller_configs[i]);
+
+        OnboardThermistorCurrentLimiter *fet_thermistor = new OnboardThermistorCurrentLimiter(hw_configs[i].thermistor_config,
+                                                                                              fet_thermistor_configs[i]);
+        OffboardThermistorCurrentLimiter *motor_thermistor = new OffboardThermistorCurrentLimiter(motor_thermistor_configs[i]);
+
+        Motor *motor = new Motor(hw_configs[i].motor_config,
+                                 hw_configs[i].gate_driver_config,
+                                 motor_configs[i]);
+        TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
+        Endstop *min_endstop = new Endstop(min_endstop_configs[i]);
+        Endstop *max_endstop = new Endstop(max_endstop_configs[i]);
+        axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
+                *encoder, *sensorless_estimator, *controller, *fet_thermistor,
+                *motor_thermistor, *motor, *trap, *min_endstop, *max_endstop);
+
+        controller_configs[i].parent = controller;
+        encoder_configs[i].parent = encoder;
+        motor_thermistor_configs[i].parent = motor_thermistor;
+        motor_configs[i].parent = motor;
+        min_endstop_configs[i].parent = min_endstop;
+        max_endstop_configs[i].parent = max_endstop;
+        axis_configs[i].parent = axes[i];
+    }
+    return 0;
+}
+
+extern "C" {
+int odrive_main(void);
+void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
+    for(auto& axis : axes){
+        safety_critical_disarm_motor_pwm(axis->motor_);
+    }
+        safety_critical_disarm_brake_resistor();
+    for (;;); // TODO: safe action
+}
+void vApplicationIdleHook(void) {
+    if (odrv.system_stats_.fully_booted) {
+        odrv.system_stats_.uptime = xTaskGetTickCount();
+        odrv.system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
+        odrv.system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
+        odrv.system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
+
+        // Actual usage, in bytes, so we don't have to math
+        odrv.system_stats_.stack_usage_axis0 = axes[0]->stack_size_ - odrv.system_stats_.min_stack_space_axis0;
+        odrv.system_stats_.stack_usage_axis1 = axes[1]->stack_size_ - odrv.system_stats_.min_stack_space_axis1;
+        odrv.system_stats_.stack_usage_comms = stack_size_comm_thread - odrv.system_stats_.min_stack_space_comms;
+        odrv.system_stats_.stack_usage_usb = stack_size_usb_thread - odrv.system_stats_.min_stack_space_usb;
+        odrv.system_stats_.stack_usage_uart = stack_size_uart_thread - odrv.system_stats_.min_stack_space_uart;
+        odrv.system_stats_.stack_usage_usb_irq = stack_size_usb_irq_thread - odrv.system_stats_.min_stack_space_usb_irq;
+        odrv.system_stats_.stack_usage_startup = stack_size_default_task - odrv.system_stats_.min_stack_space_startup;
+        odrv.system_stats_.stack_usage_can = odCAN->stack_size_ - odrv.system_stats_.min_stack_space_can;
+    }
+}
+}
+
+int odrive_main(void) {
+    // Start ADC for temperature measurements and user measurements
+    start_general_purpose_adc();
+
+    // TODO: make dynamically reconfigurable
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+    if (odrv.config_.enable_uart) {
+        SetGPIO12toUART();
+    }
+#endif
+    //osDelay(100);
+    // Init communications (this requires the axis objects to be constructed)
+    init_communication();
+
+    // Start pwm-in compare modules
+    // must happen after communication is initialized
+    pwm_in_init();
+
+    // Set up the CS pins for absolute encoders
+    for(auto& axis : axes){
+        if(axis->encoder_.config_.mode & Encoder::MODE_FLAG_ABS){
+            axis->encoder_.abs_spi_cs_pin_init();
+        }
+    }
+
+    // Setup motors (DRV8301 SPI transactions here)
+    for(auto& axis : axes){
+        axis->motor_.setup();
+    }
+
+    // Setup encoders (Starts encoder SPI transactions)
+    for(auto& axis : axes){
+        axis->encoder_.setup();
+    }
+
+    // Setup anything remaining in each axis
+    for(auto& axis : axes){
+        axis->setup();
+    }
+
+    // Start PWM and enable adc interrupts/callbacks
+    start_adc_pwm();
+
+    // This delay serves two purposes:
+    //  - Let the current sense calibration converge (the current
+    //    sense interrupts are firing in background by now)
+    //  - Allow a user to interrupt the code, e.g. by flashing a new code,
+    //    before it does anything crazy
+    // TODO make timing a function of calibration filter tau
+    osDelay(1500);
+
+    // Start state machine threads. Each thread will go through various calibration
+    // procedures and then run the actual controller loops.
+    // TODO: generalize for AXIS_COUNT != 2
+    for (size_t i = 0; i < AXIS_COUNT; ++i) {
+        axes[i]->start_thread();
+    }
+
+    start_analog_thread();
+
+    odrv.system_stats_.fully_booted = true;
+    return 0;
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/motor.cpp

@@ -0,0 +1,501 @@
+
+#include <algorithm>
+
+#include "drv8301.h"
+#include "odrive_main.h"
+
+
+Motor::Motor(const MotorHardwareConfig_t& hw_config,
+             const GateDriverHardwareConfig_t& gate_driver_config,
+             Config_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,
+        }) {
+    update_current_controller_gains();
+}
+
+// @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() {
+
+    // Reset controller states, integrators, setpoints, etc.
+    axis_->controller_.reset();
+    reset_current_control();
+
+    // Wait until the interrupt handler triggers twice. This gives
+    // the control loop the correct time quota to set up modulation timings.
+    if (!axis_->wait_for_current_meas())
+        return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
+    next_timings_valid_ = false;
+    safety_critical_arm_motor_pwm(*this);
+    return true;
+}
+
+void Motor::reset_current_control() {
+    current_control_.v_current_control_integral_d = 0.0f;
+    current_control_.v_current_control_integral_q = 0.0f;
+    current_control_.acim_rotor_flux = 0.0f;
+    current_control_.Ibus = 0.0f;
+}
+
+// @brief Tune the current controller based on phase resistance and inductance
+// This should be invoked whenever one of these values changes.
+// TODO: allow update on user-request or update automatically via hooks
+void Motor::update_current_controller_gains() {
+    // Calculate current control gains
+    current_control_.p_gain = config_.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;
+}
+
+// @brief Set up the gate drivers
+void Motor::DRV8301_setup() {
+    // for reference:
+    // 20V/V on 500uOhm gives a range of +/- 150A
+    // 40V/V on 500uOhm gives a range of +/- 75A
+    // 20V/V on 666uOhm gives a range of +/- 110A
+    // 40V/V on 666uOhm gives a range of +/- 55A
+
+    // Solve for exact gain, then snap down to have equal or larger range as requested
+    // or largest possible range otherwise
+    constexpr float kMargin = 0.90f;
+    constexpr float kTripMargin = 1.0f; // Trip level is at edge of linear range of amplifer
+    constexpr float max_output_swing = 1.35f; // [V] out of amplifier
+    float max_unity_gain_current = kMargin * max_output_swing * hw_config_.shunt_conductance; // [A]
+    float requested_gain = max_unity_gain_current / config_.requested_current_range; // [V/V]
+
+    // Decoding array for snapping gain
+    std::array<std::pair<float, DRV8301_ShuntAmpGain_e>, 4> gain_choices = { 
+        std::make_pair(10.0f, DRV8301_ShuntAmpGain_10VpV),
+        std::make_pair(20.0f, DRV8301_ShuntAmpGain_20VpV),
+        std::make_pair(40.0f, DRV8301_ShuntAmpGain_40VpV),
+        std::make_pair(80.0f, DRV8301_ShuntAmpGain_80VpV)
+    };
+
+    // We use lower_bound in reverse because it snaps up by default, we want to snap down.
+    auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain, 
+    [](std::pair<float, DRV8301_ShuntAmpGain_e> pair, float val){
+        return pair.first > val;
+    });
+
+    // If we snap to outside the array, clip to smallest val
+    if(gain_snap_down == gain_choices.crend())
+       --gain_snap_down;
+
+    // Values for current controller
+    phase_current_rev_gain_ = 1.0f / gain_snap_down->first;
+    // Clip all current control to actual usable range
+    current_control_.max_allowed_current = max_unity_gain_current * phase_current_rev_gain_;
+    // Set trip level
+    current_control_.overcurrent_trip_level = (kTripMargin / kMargin) * current_control_.max_allowed_current;
+
+    // We now have the gain settings we want to use, lets set up DRV chip
+    DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
+    DRV8301_enable(&gate_driver_);
+    DRV8301_setupSpi(&gate_driver_, local_regs);
+
+    local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;
+    // Overcurrent set to approximately 150A at 100degC. This may need tweaking.
+    local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V;
+    local_regs->Ctrl_Reg_2.GAIN = gain_snap_down->second;
+
+    local_regs->SndCmd = true;
+    DRV8301_writeData(&gate_driver_, local_regs);
+    local_regs->RcvCmd = true;
+    DRV8301_readData(&gate_driver_, local_regs);
+}
+
+// @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);
+    if (nFAULT_state == GPIO_PIN_RESET) {
+        // Update DRV Fault Code
+        gate_driver_exported_.drv_fault = (GateDriverIntf::DrvFault)DRV8301_getFaultType(&gate_driver_);
+        // Update/Cache all SPI device registers
+        // DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
+        // local_regs->RcvCmd = true;
+        // DRV8301_readData(&gate_driver_, local_regs);
+        return false;
+    };
+    return true;
+}
+
+void Motor::set_error(Motor::Error error){
+    error_ |= error;
+    axis_->error_ |= Axis::ERROR_MOTOR_FAILED;
+    safety_critical_disarm_motor_pwm(*this);
+    update_brake_current();
+}
+
+bool Motor::do_checks() {
+    if (!check_DRV_fault()) {
+        set_error(ERROR_DRV_FAULT);
+        return false;
+    }
+
+    return true;
+}
+
+float Motor::effective_current_lim() {
+    // Configured limit
+    float current_lim = config_.current_lim;
+    // Hardware limit
+    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) {
+        current_lim = std::min(current_lim, 0.98f*one_by_sqrt3*vbus_voltage); //gimbal motor is voltage control
+    } else {
+        current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current);
+    }
+
+    // Apply axis current limiters
+    for (const CurrentLimiter* const limiter : axis_->current_limiters_) {
+        current_lim = std::min(current_lim, limiter->get_current_limit(config_.current_lim));
+    }
+
+    effective_current_lim_ = current_lim;
+
+    return effective_current_lim_;
+}
+
+//return the maximum available torque for the motor.
+//Note - for ACIM motors, available torque is allowed to be 0.
+float Motor::max_available_torque() {
+    if (config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
+        float max_torque = effective_current_lim() * config_.torque_constant * current_control_.acim_rotor_flux;
+        max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
+        return max_torque;
+    }
+    else {
+        float max_torque = effective_current_lim() * config_.torque_constant;
+        max_torque = std::clamp(max_torque, 0.0f, config_.torque_lim);
+        return max_torque;
+    }
+}
+
+void Motor::log_timing(TimingLog_t log_idx) {
+    static const uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ);
+    uint16_t timing = clocks_per_cnt * htim13.Instance->CNT; // TODO: Use a hw_config
+
+    if (log_idx < TIMING_LOG_NUM_SLOTS) {
+        timing_log_[log_idx] = 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;
+    return current;
+}
+
+//--------------------------------
+// Measurement and calibration
+//--------------------------------
+
+// TODO check Ibeta balance to verify good motor connection
+bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
+    static const float kI = 10.0f;                                 // [(V/s)/A]
+    static const int num_test_cycles = (int)(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
+    float test_voltage = 0.0f;
+    
+    size_t i = 0;
+    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 set_error(ERROR_PHASE_RESISTANCE_OUT_OF_RANGE), false;
+
+        // Test voltage along phase A
+        if (!enqueue_voltage_timings(test_voltage, 0.0f))
+            return false; // error set inside enqueue_voltage_timings
+        log_timing(TIMING_LOG_MEAS_R);
+
+        return ++i < num_test_cycles;
+    });
+    if (axis_->error_ != Axis::ERROR_NONE)
+        return false;
+
+    //// De-energize motor
+    //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
+    //    return false; // error set inside enqueue_voltage_timings
+
+    float R = test_voltage / test_current;
+    config_.phase_resistance = R;
+    return true; // if we ran to completion that means success
+}
+
+bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
+    float test_voltages[2] = {voltage_low, voltage_high};
+    float Ialphas[2] = {0.0f};
+    static const int num_cycles = 5000;
+
+    size_t t = 0;
+    axis_->run_control_loop([&](){
+        int i = t & 1;
+        Ialphas[i] += -current_meas_.phB - current_meas_.phC;
+
+        // Test voltage along phase A
+        if (!enqueue_voltage_timings(test_voltages[i], 0.0f))
+            return false; // error set inside enqueue_voltage_timings
+        log_timing(TIMING_LOG_MEAS_L);
+
+        return ++t < (num_cycles << 1);
+    });
+    if (axis_->error_ != Axis::ERROR_NONE)
+        return false;
+
+    //// De-energize motor
+    //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
+    //    return false; // error set inside enqueue_voltage_timings
+
+    float v_L = 0.5f * (voltage_high - voltage_low);
+    // Note: A more correct formula would also take into account that there is a finite timestep.
+    // However, the discretisation in the current control loop inverts the same discrepancy
+    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;
+    // TODO arbitrary values set for now
+    if (L < 2e-6f || L > 4000e-6f)
+        return set_error(ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE), false;
+    return true;
+}
+
+
+bool Motor::run_calibration() {
+    float R_calib_max_voltage = config_.resistance_calib_max_voltage;
+    if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT
+        || config_.motor_type == MOTOR_TYPE_ACIM) {
+        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) {
+        // no calibration needed
+    } else {
+        return false;
+    }
+
+    update_current_controller_gains();
+    
+    is_calibrated_ = true;
+    return true;
+}
+
+bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
+    float tA, tB, tC;
+    if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
+        return set_error(ERROR_MODULATION_MAGNITUDE), false;
+    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;
+    return true;
+}
+
+bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
+    float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
+    float mod_alpha = vfactor * v_alpha;
+    float mod_beta = vfactor * v_beta;
+    if (!enqueue_modulation_timings(mod_alpha, mod_beta))
+        return false;
+    log_timing(TIMING_LOG_FOC_VOLTAGE);
+    return true;
+}
+
+// We should probably make FOC Current call FOC Voltage to avoid duplication.
+bool Motor::FOC_voltage(float v_d, float v_q, float pwm_phase) {
+    float c = our_arm_cos_f32(pwm_phase);
+    float s = our_arm_sin_f32(pwm_phase);
+    float v_alpha = c*v_d - s*v_q;
+    float v_beta = c*v_q + s*v_d;
+    return enqueue_voltage_timings(v_alpha, v_beta);
+}
+
+bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase) {
+    // Syntactic sugar
+    CurrentControl_t& ictrl = current_control_;
+
+    // For Reporting
+    ictrl.Iq_setpoint = Iq_des;
+
+    // Check for current sense saturation
+    if (std::abs(current_meas_.phB) > ictrl.overcurrent_trip_level || std::abs(current_meas_.phC) > ictrl.overcurrent_trip_level) {
+        set_error(ERROR_CURRENT_SENSE_SATURATION);
+        return false;
+    }
+
+    // Clarke transform
+    float Ialpha = -current_meas_.phB - current_meas_.phC;
+    float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
+
+    // Park transform
+    float c_I = our_arm_cos_f32(I_phase);
+    float s_I = our_arm_sin_f32(I_phase);
+    float Id = c_I * Ialpha + s_I * Ibeta;
+    float Iq = c_I * Ibeta - s_I * Ialpha;
+    ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured);
+    ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured);
+
+    // Check for violation of current limit
+    float I_trip = effective_current_lim() + config_.current_lim_margin;
+    if (SQ(Id) + SQ(Iq) > SQ(I_trip)) {
+        set_error(ERROR_CURRENT_LIMIT_VIOLATION);
+        return false;
+    }
+
+    // Current error
+    float Ierr_d = Id_des - Id;
+    float Ierr_q = Iq_des - Iq;
+
+    // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
+    // Apply PI control
+    float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain;
+    float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain;
+
+    float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
+    float V_to_mod = 1.0f / mod_to_V;
+    float mod_d = V_to_mod * Vd;
+    float mod_q = V_to_mod * Vq;
+
+    // Vector modulation saturation, lock integrator if saturated
+    // TODO make maximum modulation configurable
+    float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
+    if (mod_scalefactor < 1.0f) {
+        mod_d *= mod_scalefactor;
+        mod_q *= mod_scalefactor;
+        // TODO make decayfactor configurable
+        ictrl.v_current_control_integral_d *= 0.99f;
+        ictrl.v_current_control_integral_q *= 0.99f;
+    } else {
+        ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period);
+        ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period);
+    }
+
+    // Compute estimated bus current
+    ictrl.Ibus = mod_d * Id + mod_q * Iq;
+
+    // Inverse park transform
+    float c_p = our_arm_cos_f32(pwm_phase);
+    float s_p = our_arm_sin_f32(pwm_phase);
+    float mod_alpha = c_p * mod_d - s_p * mod_q;
+    float mod_beta = c_p * mod_q + s_p * mod_d;
+
+    // Report final applied voltage in stationary frame (for sensorles estimator)
+    ictrl.final_v_alpha = mod_to_V * mod_alpha;
+    ictrl.final_v_beta = mod_to_V * mod_beta;
+
+    // Apply SVM
+    if (!enqueue_modulation_timings(mod_alpha, mod_beta))
+        return false; // error set inside enqueue_modulation_timings
+    log_timing(TIMING_LOG_FOC_CURRENT);
+
+    if (axis_->axis_num_ == 0) {
+
+        // Edit these to suit your capture needs
+        float trigger_data = ictrl.v_current_control_integral_d;
+        float trigger_threshold = 0.5f;
+        float sample_data = Ialpha;
+
+        static bool ready = false;
+        static bool capturing = false;
+        if (trigger_data < trigger_threshold) {
+            ready = true;
+        }
+        if (ready && trigger_data >= trigger_threshold) {
+            capturing = true;
+            ready = false;
+        }
+        if (capturing) {
+            oscilloscope[oscilloscope_pos] = sample_data;
+            if (++oscilloscope_pos >= OSCILLOSCOPE_SIZE) {
+                oscilloscope_pos = 0;
+                capturing = false;
+            }
+        }
+    }
+
+    return true;
+}
+
+// torque_setpoint [Nm]
+// phase [rad electrical]
+// phase_vel [rad/s electrical]
+bool Motor::update(float torque_setpoint, float phase, float phase_vel) {
+    float current_setpoint = 0.0f;
+    phase *= config_.direction;
+    phase_vel *= config_.direction;
+
+    if (config_.motor_type == MOTOR_TYPE_ACIM) {
+        current_setpoint = torque_setpoint / (config_.torque_constant * fmax(current_control_.acim_rotor_flux, config_.acim_gain_min_flux));
+    }
+    else {
+        current_setpoint = torque_setpoint / config_.torque_constant;
+    }
+    current_setpoint *= config_.direction;
+
+    // TODO: 2-norm vs independent clamping (current could be sqrt(2) bigger)
+    float ilim = effective_current_lim();
+    float id = std::clamp(current_control_.Id_setpoint, -ilim, ilim);
+    float iq = std::clamp(current_setpoint, -ilim, ilim);
+
+    if (config_.motor_type == MOTOR_TYPE_ACIM) {
+        // Note that the effect of the current commands on the real currents is actually 1.5 PWM cycles later
+        // However the rotor time constant is (usually) so slow that it doesn't matter
+        // So we elect to write it as if the effect is immediate, to have cleaner code
+
+        if (config_.acim_autoflux_enable) {
+            float abs_iq = fabsf(iq);
+            float gain = abs_iq > id ? config_.acim_autoflux_attack_gain : config_.acim_autoflux_decay_gain;
+            id += gain * (abs_iq - id) * current_meas_period;
+            id = std::clamp(id, config_.acim_autoflux_min_Id, ilim);
+            current_control_.Id_setpoint = id;
+        }
+
+        // acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified
+        float dflux_by_dt = config_.acim_slip_velocity * (id - current_control_.acim_rotor_flux);
+        current_control_.acim_rotor_flux += dflux_by_dt * current_meas_period;
+        float slip_velocity = config_.acim_slip_velocity * (iq / current_control_.acim_rotor_flux);
+        // Check for issues with small denominator. Polarity of check to catch NaN too
+        bool acceptable_vel = fabsf(slip_velocity) <= 0.1f * (float)current_meas_hz;
+        if (!acceptable_vel)
+            slip_velocity = 0.0f;
+        phase_vel += slip_velocity;
+        // reporting only:
+        current_control_.async_phase_vel = slip_velocity;
+
+        current_control_.async_phase_offset += slip_velocity * current_meas_period;
+        current_control_.async_phase_offset = wrap_pm_pi(current_control_.async_phase_offset);
+        phase += current_control_.async_phase_offset;
+        phase = wrap_pm_pi(phase);
+    }
+
+    float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;
+
+    // Execute current command
+    switch(config_.motor_type){
+        case MOTOR_TYPE_HIGH_CURRENT: return FOC_current(id, iq, phase, pwm_phase); break;
+        case MOTOR_TYPE_ACIM: return FOC_current(id, iq, phase, pwm_phase); break;
+        case MOTOR_TYPE_GIMBAL: return FOC_voltage(id, iq, pwm_phase); break;
+        default: set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE); return false; break;
+    }
+    return true;
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/motor.hpp

@@ -0,0 +1,165 @@
+#ifndef __MOTOR_HPP
+#define __MOTOR_HPP
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+#include "drv8301.h"
+
+class Motor : public ODriveIntf::MotorIntf {
+public:
+    struct Iph_BC_t {
+        float phB;
+        float phC;
+    };
+
+    struct CurrentControl_t{
+        float p_gain; // [V/A]
+        float i_gain; // [V/As]
+        float v_current_control_integral_d; // [V]
+        float v_current_control_integral_q; // [V]
+        float Ibus; // DC bus current [A]
+        // Voltage applied at end of cycle:
+        float final_v_alpha; // [V]
+        float final_v_beta; // [V]
+        float Id_setpoint; // [A]
+        float Iq_setpoint; // [A]
+        float Iq_measured; // [A]
+        float Id_measured; // [A]
+        float I_measured_report_filter_k;
+        float max_allowed_current; // [A]
+        float overcurrent_trip_level; // [A]
+        float acim_rotor_flux; // [A]
+        float async_phase_vel; // [rad/s electrical]
+        float async_phase_offset; // [rad electrical]
+    };
+
+    // NOTE: for gimbal motors, all units of Nm are instead V.
+    // example: vel_gain is [V/(turn/s)] instead of [Nm/(turn/s)]
+    // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
+    struct Config_t {
+        bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
+        int32_t pole_pairs = 7;
+        float calibration_current = 10.0f;    // [A]
+        float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
+        float phase_inductance = 0.0f;        // to be set by measure_phase_inductance
+        float phase_resistance = 0.0f;        // to be set by measure_phase_resistance
+        float torque_constant = 0.04f;         // [Nm/A] for PM motors, [Nm/A^2] for induction motors. Equal to 8.27/Kv of the motor
+        int32_t direction = 0;                // 1 or -1 (0 = unspecified)
+        MotorType motor_type = MOTOR_TYPE_HIGH_CURRENT;
+        // Read out max_allowed_current to see max supported value for current_lim.
+        // float current_lim = 70.0f; //[A]
+        float current_lim = 10.0f;          //[A]
+        float current_lim_margin = 8.0f;    // Maximum violation of current_lim
+        float torque_lim = std::numeric_limits<float>::infinity();           //[Nm]. 
+        // Value used to compute shunt amplifier gains
+        float requested_current_range = 60.0f; // [A]
+        float current_control_bandwidth = 1000.0f;  // [rad/s]
+        float inverter_temp_limit_lower = 100;
+        float inverter_temp_limit_upper = 120;
+        float acim_slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau
+        float acim_gain_min_flux = 10; // [A]
+        float acim_autoflux_min_Id = 10; // [A]
+        bool acim_autoflux_enable = false;
+        float acim_autoflux_attack_gain = 10.0f;
+        float acim_autoflux_decay_gain = 1.0f;
+
+        // custom property setters
+        Motor* parent = nullptr;
+        void set_pre_calibrated(bool value) {
+            pre_calibrated = value;
+            parent->is_calibrated_ = parent->is_calibrated_ || parent->config_.pre_calibrated;
+        }
+        void set_phase_inductance(float value) { phase_inductance = value; parent->update_current_controller_gains(); }
+        void set_phase_resistance(float value) { phase_resistance = value; parent->update_current_controller_gains(); }
+        void set_current_control_bandwidth(float value) { current_control_bandwidth = value; parent->update_current_controller_gains(); }
+    };
+
+    Motor(const MotorHardwareConfig_t& hw_config,
+         const GateDriverHardwareConfig_t& gate_driver_config,
+         Config_t& config);
+
+    bool arm();
+    void disarm();
+    void setup() {
+        DRV8301_setup();
+    }
+    void reset_current_control();
+
+    void update_current_controller_gains();
+    void DRV8301_setup();
+    bool check_DRV_fault();
+    void set_error(Error error);
+    bool do_checks();
+    float effective_current_lim();
+    float max_available_torque();
+    void log_timing(TimingLog_t log_idx);
+    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);
+    bool run_calibration();
+    bool enqueue_modulation_timings(float mod_alpha, float mod_beta);
+    bool enqueue_voltage_timings(float v_alpha, float v_beta);
+    bool FOC_voltage(float v_d, float v_q, float pwm_phase);
+    bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase);
+    bool update(float current_setpoint, float phase, float phase_vel);
+
+    const MotorHardwareConfig_t& hw_config_;
+    const GateDriverHardwareConfig_t gate_driver_config_;
+    Config_t& config_;
+    Axis* axis_ = nullptr; // set by Axis constructor
+
+//private:
+
+    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
+    };
+    bool next_timings_valid_ = false;
+    uint16_t last_cpu_time_ = 0;
+    int timing_log_index_ = 0;
+    struct {
+        uint16_t& operator[](size_t idx) { return content[idx]; }
+        uint16_t& get(size_t idx) { return content[idx]; }
+        uint16_t content[TIMING_LOG_NUM_SLOTS];
+    } timing_log_;
+
+    // variables exposed on protocol
+    Error error_ = ERROR_NONE;
+    // Do not write to this variable directly!
+    // It is for exclusive use by the safety_critical_... functions.
+    ArmedState armed_state_ = ARMED_STATE_DISARMED; 
+    bool is_calibrated_ = config_.pre_calibrated;
+    Iph_BC_t current_meas_ = {0.0f, 0.0f};
+    Iph_BC_t DC_calib_ = {0.0f, 0.0f};
+    float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
+    CurrentControl_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,
+        .v_current_control_integral_q = 0.0f,
+        .Ibus = 0.0f,
+        .final_v_alpha = 0.0f,
+        .final_v_beta = 0.0f,
+        .Id_setpoint = 0.0f,
+        .Iq_setpoint = 0.0f,
+        .Iq_measured = 0.0f,
+        .Id_measured = 0.0f,
+        .I_measured_report_filter_k = 1.0f,
+        .max_allowed_current = 0.0f,
+        .overcurrent_trip_level = 0.0f,
+        .acim_rotor_flux = 0.0f,
+        .async_phase_vel = 0.0f,
+        .async_phase_offset = 0.0f,
+    };
+    struct : GateDriverIntf {
+        DrvFault drv_fault = DRV_FAULT_NO_FAULT;
+    } gate_driver_exported_;
+    DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
+    float effective_current_lim_ = 10.0f;
+};
+
+#endif // __MOTOR_HPP

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/nvm.c

@@ -0,0 +1,453 @@
+/*
+* Flash-based Non-Volatile Memory (NVM)
+* 
+* This file supports storing and loading persistent configuration based on
+* the STM32 builtin flash memory.
+*
+* The STM32F405xx has 12 flash sectors of heterogeneous size. We use the last
+* two sectors for configuration data. These pages have a size of 128kB each.
+* Setting any bit in these sectors to 0 is always possible, but setting them
+* to 1 requires erasing the whole sector.
+*
+* We consider each sector as an array of 64-bit fields except the first N bytes, which we
+* instead use as an allocation block. The allocation block is a compact bit-field (2 bit per entry)
+* that keeps track of the state of each field (erased, invalid, valid).
+*
+* One sector is always considered the valid (read) sector and the other one is the
+* target for the next write access: they can be considered to be ping-pong or double buffred.
+*
+* When writing a block of data, instead of always erasing the whole writable sector the
+* new data is appended in the erased area. This presumably increases flash life span.
+* The writable sector is only erased if there is not enough space for the new data.
+*
+* On startup, if there is exactly one sector
+* whose last non-erased value has the state "valid" that sector is considered
+* the valid sector. In any other case the selection is undefined.
+*
+*
+* To write a new block of data atomically we first mark all associated fields
+* as "invalid" (in the allocation table) then write the data and then mark the
+* fields as "valid" (in the direction of increasing address).
+*/
+
+#include "nvm.h"
+
+#include <stm32f405xx.h>
+#include <stm32f4xx_hal.h>
+#include <string.h>
+
+#if defined(STM32F405xx)
+
+// refer to page 75 of datasheet:
+// http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
+#define FLASH_SECTOR_10_BASE (const volatile uint8_t*)0x80C0000UL
+#define FLASH_SECTOR_10_SIZE 0x20000UL
+#define FLASH_SECTOR_11_BASE (const volatile uint8_t*)0x80E0000UL
+#define FLASH_SECTOR_11_SIZE 0x20000UL
+
+#define HAL_FLASH_ClearError() __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR | FLASH_FLAG_PGPERR)
+#else
+#error "unknown flash sector size"
+#endif
+
+typedef enum {
+    VALID = 0,
+    INVALID = 1,
+    ERASED = 3
+} field_state_t;
+
+typedef struct {
+    size_t index;               //!< next field to be written to (can be equal to n_data)
+    const uint32_t sector_id;   //!< HAL ID of this sector
+    const size_t n_data;        //!< number of 64-bit fields in this sector
+    const size_t n_reserved;    //!< number of 64-bit fields in this sector that are reserved for the allocation table
+    const volatile uint8_t* const alloc_table;
+    const volatile uint64_t* const data;
+} sector_t;
+
+sector_t sectors[] = { {
+    .sector_id = FLASH_SECTOR_10,
+    .n_data = FLASH_SECTOR_10_SIZE >> 3,
+    .n_reserved = (FLASH_SECTOR_10_SIZE >> 3) >> 5,
+    .alloc_table = FLASH_SECTOR_10_BASE,
+    .data = (uint64_t *)FLASH_SECTOR_10_BASE
+}, {
+    .sector_id = FLASH_SECTOR_11,
+    .n_data = FLASH_SECTOR_11_SIZE >> 3,
+    .n_reserved = (FLASH_SECTOR_11_SIZE >> 3) >> 5,
+    .alloc_table = FLASH_SECTOR_11_BASE,
+    .data = (uint64_t *)FLASH_SECTOR_11_BASE
+}};
+
+uint8_t read_sector_; // 0 or 1 to indicate which sector to read from and which to write to
+size_t n_staging_area_; // number of 64-bit values that were reserved using NVM_start_write
+size_t n_valid_; // number of 64-bit fields that can be read
+
+// @brief Erases a flash sector. This sets all bits in the sector to 1.
+// The sector's current index is reset to the minimum value (n_reserved).
+// @returns 0 on success or a non-zero error code otherwise
+int erase(sector_t *sector) {
+    FLASH_EraseInitTypeDef erase_struct = {
+        .TypeErase = FLASH_TYPEERASE_SECTORS,
+        .Banks = 0, // only used for mass erase
+        .Sector = sector->sector_id,
+        .NbSectors = 1,
+        .VoltageRange = FLASH_VOLTAGE_RANGE_3
+    };
+    HAL_FLASH_Unlock();
+    HAL_FLASH_ClearError();
+    uint32_t sector_error;
+    if (HAL_FLASHEx_Erase(&erase_struct, &sector_error) != HAL_OK)
+        goto fail;
+    sector->index = sector->n_reserved;
+
+    HAL_FLASH_Lock();
+    return 0;
+fail:
+    HAL_FLASH_Lock();
+    //printf("erase failed: %u \r\n", HAL_FLASH_GetError());
+    return HAL_FLASH_GetError(); // non-zero
+}
+
+
+// @brief Writes states into the allocation table.
+// The write operation goes in the direction of increasing indices.
+// @param state: 11: erased, 10: writing, 00: valid data
+// @returns 0 on success or a non-zero error code otherwise
+int set_allocation_state(sector_t *sector, size_t index, size_t count, field_state_t state) {
+    if (index < sector->n_reserved)
+        return -1;
+    if (index + count >= sector->n_data)
+        return -1;
+
+    // expand state to state for 4 values
+    const uint8_t states = (state << 0) | (state << 2) | (state << 4) | (state << 6);
+    
+    // handle unaligned start
+    uint8_t mask = ~(0xff << ((index & 0x3) << 1));
+    count += index & 0x3;
+    index -= index & 0x3;
+
+    HAL_FLASH_Unlock();
+    HAL_FLASH_ClearError();
+
+    // write states
+    for (; count >= 4; count -= 4, index += 4) {
+        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, (uintptr_t)&sector->alloc_table[index >> 2], states | mask) != HAL_OK)
+            goto fail;
+        mask = 0;
+    }
+
+    // handle unaligned end
+    if (count) {
+        mask |= ~(0xff >> ((4 - count) << 1));
+        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, (uintptr_t)&sector->alloc_table[index >> 2], states | mask) != HAL_OK)
+            goto fail;
+    }
+    
+    HAL_FLASH_Lock();
+    return 0;
+fail:
+    HAL_FLASH_Lock();
+    return HAL_FLASH_GetError(); // non-zero
+}
+
+// @brief Reads the allocation table from behind to determine how many fields match the
+// reference state.
+// @param sector: The sector on which to perform the search
+// @param max_index: The maximum index that should be considered
+// @param ref_state: The reference state
+// @param state: Set to the first encountered state that is unequal to ref_state.
+//               Set to ref_state if all encountered states are equal to ref_state.
+// @returns The smallest index that points to a field with ref_state.
+//          This value is at least sector->n_reserved and at most max_index.
+size_t scan_allocation_table(sector_t *sector, size_t max_index, field_state_t ref_state, field_state_t *state) {
+    const uint8_t ref_states = (ref_state << 0) | (ref_state << 2) | (ref_state << 4) | (ref_state << 6);
+    size_t index = (((max_index + 3) >> 2) << 2); // start at the max index but round up to a multiple of 4
+    size_t ignore = index - max_index;
+    uint8_t states = ref_states;
+
+    //printf("scan from %08x to %08x for %02x\r\n", index, sector->n_reserved, ref_states); osDelay(5);
+
+    // read 4 states at a time
+    for (; index >= (sector->n_reserved + 4); index -= 4) {
+        states = sector->alloc_table[(index - 1) >> 2];
+        if (ignore) { // ignore the upper 1, 2 or 3 states if max_index was unaligned
+            uint8_t ignore_mask = ~(0xff >> (ignore << 1));
+            states = (states & ~ignore_mask) | (ref_states & ignore_mask);
+            ignore = 0;
+        }
+        if (states != ref_states)
+            break;
+    }
+
+    // once we encounterd a byte with any state mismatch determine which of the 4 states it is
+    for (; ((states >> 6) == (ref_states & 0x3)) && (index > sector->n_reserved); index--) {
+        states <<= 2;
+    }
+    
+    *state = states >> 6;
+    //printf("(it's %02x)\r\n", index); osDelay(5);
+    return index;
+}
+
+// Loads the head of the NVM data.
+// If this function fails subsequent calls to NVM functions (other than NVM_init or NVM_erase)
+// cause undefined behavior.
+// @returns 0 on success or a non-zero error code otherwise
+int NVM_init(void) {
+    field_state_t sector0_state, sector1_state;
+    sectors[0].index = scan_allocation_table(&sectors[0], sectors[0].n_data,
+                ERASED, &sector0_state);
+    sectors[1].index = scan_allocation_table(&sectors[1], sectors[1].n_data,
+                ERASED, &sector1_state);
+    //printf("sector states: %02x, %02x\r\n", sector0_state, sector1_state); osDelay(5);
+
+    // Select valid sector on a best effort basis
+    // (in unfortunate cases valid_sector might actually point
+    // to an invalid or erased sector)
+    read_sector_ = 0;
+    if (sector1_state == VALID)
+        read_sector_ = 1;
+    
+    // count the number of valid fields
+    sector_t *read_sector = &sectors[read_sector_];
+    uint8_t first_nonvalid_state;
+    size_t min_valid_index = scan_allocation_table(read_sector, read_sector->index,
+        VALID, &first_nonvalid_state);
+    n_valid_ = read_sector->index - min_valid_index;
+    
+    n_staging_area_ = 0;
+
+    int status = 0;
+    /*// bring non-valid sectors into a known state
+    this is not absolutely required
+    if (sector0_state != VALID)
+        status |= erase(&sectors[0]);
+    if (sector1_state != VALID)
+        status |= erase(&sectors[1]);
+    */
+    return status;
+}
+
+// @brief Erases all data in the NVM.
+//
+// If this function fails subsequent calls to NVM functions (other than NVM_init or NVM_erase)
+// cause undefined behavior.
+// Caution: this function may take a long time (like 1 second)
+//
+// @returns 0 on success or a non-zero error code otherwise
+int NVM_erase(void) {
+    read_sector_ = 0;
+    sectors[0].index = sectors[0].n_reserved;
+    sectors[1].index = sectors[1].n_reserved;
+
+    int state = 0;
+    state |= erase(&sectors[0]);
+    state |= erase(&sectors[1]);
+    return state;
+}
+
+// @brief Returns the maximum number of bytes that can be read using NVM_read.
+// This holds until NVM_commit is called.
+size_t NVM_get_max_read_length(void) {
+    return n_valid_ << 3;
+}
+
+// @brief Returns the maximum length (in bytes) that can passed to NVM_start_write.
+// This holds until NVM_commit is called.
+size_t NVM_get_max_write_length(void) {
+    sector_t *target = &sectors[1 - read_sector_];
+    return (target->n_data - target->n_reserved) << 3;
+}
+
+// @brief Reads from the latest committed block in the non-volatile memory.
+// @param offset: offset in bytes (0 meaning the beginning of the valid area)
+// @param data: buffer to write to
+// @param length: length in bytes (if (offset + length) is out of range, the function fails)
+// @returns 0 on success or a non-zero error code otherwise
+int NVM_read(size_t offset, uint8_t *data, size_t length) {
+    if (offset + length > (n_valid_ << 3))
+        return -1;
+    sector_t *read_sector = &sectors[read_sector_];
+    const uint8_t *src_ptr = ((const uint8_t *)&read_sector->data[read_sector->index - n_valid_]) + offset;
+    memcpy(data, src_ptr, length);
+    return 0;
+}
+
+// @brief Starts an atomic write operation.
+//
+// The most recent valid NVM data is not modified or invalidated until NVM_commit is called.
+// The length must be at most equal to the size indicated by NVM_get_max_write_length().
+//
+// @param length: Length of the staging block that should be created
+int NVM_start_write(size_t length) {
+    int status = 0;
+    sector_t *target = &sectors[1 - read_sector_];
+
+    length = (length + 7) >> 3; // round to multiple of 64 bit
+    if (length > target->n_data - target->n_reserved)
+        return -1;
+
+    // make room for the new data
+    if (length > target->n_data - target->index)
+        if ((status = erase(target)))
+            return status;
+
+    // invalidate the fields we're about to write
+    status = set_allocation_state(target, target->index, length, INVALID);
+    if (status)
+        return status;
+
+    n_staging_area_ = length;
+    return 0;
+}
+
+// @brief Writes to the current data block that was opened with NVM_start_write.
+//
+// The operation fails if (offset + length) is larger than the length passed to NVM_start_write.
+// The most recent valid NVM data is not modified or invalidated until NVM_commit is called.
+// Warning: Writing different data to the same area multiple times during a single transaction
+// will cause data corruption.
+//
+// @param offset: The offset in bytes, 0 being the beginning of the staging block.
+// @param data: Pointer to the data that should be written
+// @param length: Data length in bytes
+int NVM_write(size_t offset, uint8_t *data, size_t length) {
+    if (offset + length > (n_staging_area_ << 3))
+        return -1;
+    sector_t *target = &sectors[1 - read_sector_];
+
+    HAL_FLASH_Unlock();
+    HAL_FLASH_ClearError();
+
+    // handle unaligned start
+    for (; (offset & 0x3) && length; ++data, ++offset, --length)
+        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE,
+                ((uintptr_t)&target->data[target->index]) + offset, *data) != HAL_OK)
+            goto fail;
+
+    // write 32-bit values (64-bit doesn't work)
+    for (; length >= 4; data += 4, offset += 4, length -=4)
+        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD,
+                ((uintptr_t)&target->data[target->index]) + offset, *(uint32_t*)data) != HAL_OK)
+            goto fail;
+
+    // handle unaligned end
+    for (; length; ++data, ++offset, --length)
+        if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE,
+                ((uintptr_t)&target->data[target->index]) + offset, *data) != HAL_OK)
+            goto fail;
+
+    HAL_FLASH_Lock();
+    return 0;
+fail:
+    HAL_FLASH_Lock();
+    return HAL_FLASH_GetError(); // non-zero
+}
+
+// @brief Commits the new data to NVM atomically.
+int NVM_commit(void) {
+    sector_t *read_sector = &sectors[read_sector_];
+    sector_t *write_sector = &sectors[1 - read_sector_];
+
+    // mark the newly-written fields as valid
+    int status = set_allocation_state(write_sector, write_sector->index, n_staging_area_, VALID);
+    if (status)
+        return status;
+
+    write_sector->index += n_staging_area_;
+    n_valid_ = n_staging_area_;
+    n_staging_area_ = 0;
+    read_sector_ = 1 - read_sector_;
+
+    // invalidate the other sector
+    if (read_sector->index < read_sector->n_data) {
+        status = set_allocation_state(read_sector, read_sector->index, 1, INVALID);
+        read_sector->index += 1;
+    } else {
+        status = erase(read_sector);
+    }
+
+    return status;
+}
+
+
+#include <cmsis_os.h>
+/** @brief Call this at startup to test/demo the NVM driver
+
+ Expected output when starting with a fully erased NVM
+
+    [1st boot]
+    === NVM TEST ===
+    NVM is empty
+    write 0x00, ..., 0x25 to NVM
+    new data committed to NVM
+    
+    [2nd boot]
+    === NVM TEST ===
+    NVM contains 40 valid bytes:
+    00 01 02 03 04 05 06 07 08 09 0a 0b 0c 0d 0e 0f
+    10 11 12 13 14 15 16 17 18 19 1a 1b 1c 1d 1e 1f
+    20 21 22 23 24 25 ff ff
+    write 0xbd, ..., 0xe2 to NVM
+    new data committed to NVM
+
+    [3rd boot]
+    === NVM TEST ===
+    NVM contains 40 valid bytes:
+    bd be bf c0 c1 c2 c3 c4 c5 c6 c7 c8 c9 ca cb cc
+    cd ce cf d0 d1 d2 d3 d4 d5 d6 d7 d8 d9 da db dc
+    dd de df e0 e1 e2 ff ff
+    write 0xcb, ..., 0xf0 to NVM
+    new data committed to NVM
+*/
+void NVM_demo(void) {
+    const size_t len = 38;
+    uint8_t data[len];
+    int progress = 0;
+    uint8_t seed = 0;
+
+    osDelay(100);
+    printf("=== NVM TEST ===\r\n"); osDelay(5);
+    //NVM_erase();
+    if (progress++, NVM_init() != 0)
+        goto fail;
+    
+    // load bytes from NVM and print them
+    size_t available = NVM_get_max_read_length();
+    if (available) {
+        printf("NVM contains %d valid bytes:\r\n", available); osDelay(5);
+        uint8_t buf[available];
+        if (progress++, NVM_read(0, buf, available) != 0)
+            goto fail;
+        for (size_t pos = 0; pos < available; ++pos) {
+            seed += buf[pos];
+            printf(" %02x", buf[pos]);
+            if ((((pos + 1) % 16) == 0) || ((pos + 1) == available))
+                printf("\r\n");
+            osDelay(2);
+        }
+    } else {
+        printf("NVM is empty\r\n"); osDelay(5);
+    }
+
+    // store new bytes in NVM (data based on seed)
+    printf("write 0x%02x, ..., 0x%02x to NVM\r\n", seed, seed + len - 1); osDelay(5);
+    for (size_t i = 0; i < len; i++)
+        data[i] = seed++;
+    if (progress++, NVM_start_write(len) != 0)
+        goto fail;
+    if (progress++, NVM_write(0, data, len / 2))
+        goto fail;
+    if (progress++, NVM_write(len / 2, &data[len / 2], len - (len / 2)))
+        goto fail;
+    if (progress++, NVM_commit())
+        goto fail;
+    printf("new data committed to NVM\r\n"); osDelay(5);
+
+    return;
+
+fail:
+    printf("NVM test failed at %d!\r\n", progress);
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/nvm.h

@@ -0,0 +1,33 @@
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __NVML_H
+#define __NVM_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include <stdint.h>
+#include <stdlib.h>
+
+/* Exported types ------------------------------------------------------------*/
+/* Exported constants --------------------------------------------------------*/
+/* Exported variables --------------------------------------------------------*/
+/* Exported macro ------------------------------------------------------------*/
+/* Exported functions --------------------------------------------------------*/
+
+int NVM_init(void);
+int NVM_erase(void);
+size_t NVM_get_max_read_length(void);
+size_t NVM_get_max_write_length(void);
+int NVM_read(size_t offset, uint8_t *data, size_t length);
+int NVM_start_write(size_t length);
+int NVM_write(size_t offset, uint8_t *data, size_t length);
+int NVM_commit(void);
+void NVM_demo(void);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif //__NVM_H

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/nvm_config.hpp

@@ -0,0 +1,141 @@
+/*
+* Convenience functions to load and store multiple objects from and to NVM.
+* 
+* The NVM stores consecutive one-to-one copies of arbitrary objects.
+* The types of these objects are passed as template arguments to Config<Ts...>.
+*/
+
+/* Includes ------------------------------------------------------------------*/
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stm32f405xx.h>
+
+#include "nvm.h"
+#include <fibre/crc.hpp>
+
+
+/* Private defines -----------------------------------------------------------*/
+#define CONFIG_CRC16_INIT 0xabcd
+#define CONFIG_CRC16_POLYNOMIAL 0x3d65
+
+/* Private macros ------------------------------------------------------------*/
+/* Private typedef -----------------------------------------------------------*/
+/* Global constant data ------------------------------------------------------*/
+/* Global variables ----------------------------------------------------------*/
+/* Private constant data -----------------------------------------------------*/
+
+// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
+// the config structs, make sure to increment this number:
+static constexpr uint16_t config_version = 0x0001;
+
+/* Private variables ---------------------------------------------------------*/
+/* Private function prototypes -----------------------------------------------*/
+/* Function implementations --------------------------------------------------*/
+
+
+// @brief Manages configuration load and store operations from and to NVM
+//
+// The NVM stores consecutive one-to-one copies of arbitrary objects.
+// The types of these objects are passed as template arguments to Config<Ts...>.
+//
+// Config<Ts...> has two template specializations to implement template recursion:
+// - Config<T, Ts...> handles loading/storing of the first object (type T) and leaves
+//   the rest of the objects to an "inner" class Config<Ts...>.
+// - Config<> represents the leaf of the recursion.
+template<typename ... Ts>
+struct Config;
+
+template<>
+struct Config<> {
+    static size_t get_size() {
+        return 0;
+    }
+    static int load_config(size_t offset, uint16_t* crc16) {
+        return 0;
+    }
+    static int store_config(size_t offset, uint16_t* crc16) {
+        return 0;
+    }
+};
+
+template<typename T, typename ... Ts>
+struct Config<T, Ts...> {
+    static size_t get_size() {
+        return sizeof(T) + Config<Ts...>::get_size();
+    }
+
+    // @brief Loads one or more consecutive objects from the NVM.
+    // During loading this function also calculates the CRC over the loaded data.
+    // @param offset: 0 means that the function should start reading at the beginning
+    // of the last comitted NVM block
+    // @param crc16: the result of the CRC calculation is written to this address
+    // @param val0, vals: the values to be loaded
+    static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) {
+        size_t size = sizeof(T);
+        // save current CRC (in case val0 and crc16 point to the same address)
+        size_t previous_crc16 = *crc16;
+        if (NVM_read(offset, (uint8_t *)val0, size))
+            return -1;
+        *crc16 = calc_crc16<CONFIG_CRC16_POLYNOMIAL>(previous_crc16, (uint8_t *)val0, size);
+        if (Config<Ts...>::load_config(offset + size, crc16, vals...))
+            return -1;
+        return 0;
+    }
+
+    // @brief Stores one or more consecutive objects to the NVM.
+    // During storing this function also calculates the CRC over the stored data.
+    // @param offset: 0 means that the function should start writing at the beginning
+    // of the currently active NVM write block
+    // @param crc16: the result of the CRC calculation is written to this address
+    // @param val0, vals: the values to be stored
+    static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) {
+        size_t size = sizeof(T);
+        if (NVM_write(offset, (uint8_t *)val0, size))
+            return -1;
+        // update CRC _after_ writing (in case val0 and crc16 point to the same address)
+        if (crc16)
+            *crc16 = calc_crc16<CONFIG_CRC16_POLYNOMIAL>(*crc16, (uint8_t *)val0, size);
+        if (Config<Ts...>::store_config(offset + size, crc16, vals...))
+            return -1;
+        return 0;
+    }
+
+    // @brief Loads one or more consecutive objects from the NVM. The loaded data
+    // is validated using a CRC value that is stored at the beginning of the data.
+    static int safe_load_config(T* val0, Ts* ... vals) {
+        //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5);
+        if (Config<T, Ts..., uint16_t>::get_size() > NVM_get_max_read_length())
+            return -1;
+        uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
+        if (Config<T, Ts..., uint16_t>::load_config(0, &crc16, val0, vals..., &crc16))
+            return -1;
+        if (crc16)
+            return -1;
+        return 0;
+    }
+
+    // @brief Stores one or more consecutive objects to the NVM. In addition to the
+    // provided objects, a CRC of the data is stored.
+    //
+    // The CRC includes a version number and thus adds some protection against
+    // changes of the config structs during firmware update. Note that if the total
+    // config data length changes, the CRC validation will fail even if the developer
+    // forgets to update the config version number.
+    static int safe_store_config(const T* val0, const Ts* ... vals) {
+        size_t size = Config<T, Ts...>::get_size() + 2;
+        //printf("config is %d bytes\r\n", size); osDelay(5);
+        if (size > NVM_get_max_write_length())
+            return -1;
+        if (NVM_start_write(size))
+            return -1;
+        uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
+        if (Config<T, Ts...>::store_config(0, &crc16, val0, vals...))
+            return -1;
+        if (Config<uint8_t, uint8_t>::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16))
+            return -1;
+        if (NVM_commit())
+            return -1;
+        return 0;
+    }
+};

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/odrive_main.h

@@ -0,0 +1,297 @@
+#ifndef __ODRIVE_MAIN_H
+#define __ODRIVE_MAIN_H
+
+// Note on central include scheme by Samuel:
+// there are circular dependencies between some of the header files,
+// e.g. the Motor header needs a forward declaration of Axis and vice versa
+// so I figured I'd make one main header that takes care of
+// the forward declarations and right ordering
+// btw this pattern is not so uncommon, for instance IIRC the stdlib uses it too
+
+#ifdef __cplusplus
+#include <fibre/protocol.hpp>
+#include <communication/interface_usb.h>
+#include <communication/interface_i2c.h>
+extern "C" {
+#endif
+
+// STM specific includes
+#include <stm32f4xx_hal.h>  // Sets up the correct chip specifc defines required by arm_math
+#include <can.h>
+#include <i2c.h>
+#define ARM_MATH_CM4 // TODO: might change in future board versions
+#include <arm_math.h>
+
+// OS includes
+#include <cmsis_os.h>
+
+// Hardware configuration
+#if HW_VERSION_MAJOR == 3
+#include "board_config_v3.h"
+#else
+#error "unknown board version"
+#endif
+
+//default timeout waiting for phase measurement signals
+#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
+
+// Period in [s]
+static const float current_meas_period = CURRENT_MEAS_PERIOD;
+
+// Frequency in [Hz]
+static const int current_meas_hz = CURRENT_MEAS_HZ;
+
+// extern const float elec_rad_per_enc;
+extern uint32_t _reboot_cookie;
+
+extern uint64_t serial_number;
+extern char serial_number_str[13];
+
+#ifdef __cplusplus
+}
+
+typedef struct {
+    bool fully_booted;
+    uint32_t uptime; // [ms]
+    uint32_t min_heap_space; // FreeRTOS heap [Bytes]
+    uint32_t min_stack_space_axis0; // minimum remaining space since startup [Bytes]
+    uint32_t min_stack_space_axis1;
+    uint32_t min_stack_space_comms;
+    uint32_t min_stack_space_usb;
+    uint32_t min_stack_space_uart;
+    uint32_t min_stack_space_usb_irq;
+    uint32_t min_stack_space_startup;
+    uint32_t min_stack_space_can;
+
+    uint32_t stack_usage_axis0;
+    uint32_t stack_usage_axis1;
+    uint32_t stack_usage_comms;
+    uint32_t stack_usage_usb;
+    uint32_t stack_usage_uart;
+    uint32_t stack_usage_usb_irq;
+    uint32_t stack_usage_startup;
+    uint32_t stack_usage_can;
+
+    USBStats_t& usb = usb_stats_;
+    I2CStats_t& i2c = i2c_stats_;
+} SystemStats_t;
+
+struct PWMMapping_t {
+    endpoint_ref_t endpoint;
+    float min = 0;
+    float max = 0;
+};
+
+// @brief general user configurable board configuration
+struct BoardConfig_t {
+    bool enable_uart = true;
+    bool enable_i2c_instead_of_can = false;
+    bool enable_ascii_protocol_on_usb = true;
+    float max_regen_current = 0.0f;
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5 && HW_VERSION_VOLTAGE >= 48
+    float brake_resistance = 2.0f;     // [ohm]
+#else
+    float brake_resistance = 0.47f;     // [ohm]
+#endif
+    float dc_bus_undervoltage_trip_level = 8.0f;                        //<! [V] minimum voltage below which the motor stops operating
+    float dc_bus_overvoltage_trip_level = 1.07f * HW_VERSION_VOLTAGE;   //<! [V] maximum voltage above which the motor stops operating.
+                                                                        //<! This protects against cases in which the power supply fails to dissipate
+                                                                        //<! the brake power if the brake resistor is disabled.
+                                                                        //<! The default is 26V for the 24V board version and 52V for the 48V board version.
+
+    /**
+     * If enabled, if the measured DC voltage exceeds `dc_bus_overvoltage_ramp_start`,
+     * the ODrive will sink more power than usual into the the brake resistor
+     * in an attempt to bring the voltage down again.
+     * 
+     * The brake duty cycle is increased by the following amount:
+     *  vbus_voltage == dc_bus_overvoltage_ramp_start  =>  brake_duty_cycle += 0%
+     *  vbus_voltage == dc_bus_overvoltage_ramp_end  =>  brake_duty_cycle += 100%
+     * 
+     * Remarks:
+     *  - This feature is active even when all motors are disarmed.
+     *  - This feature is disabled if `brake_resistance` is non-positive.
+     */
+    bool enable_dc_bus_overvoltage_ramp = false;
+    float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
+                                                                      //!< Do not set this lower than your usual vbus_voltage,
+                                                                      //!< unless you like fried brake resistors.
+    float dc_bus_overvoltage_ramp_end = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`.
+                                                                    //!< Must be larger than `dc_bus_overvoltage_ramp_start`,
+                                                                    //!< otherwise the ramp feature is disabled.
+
+    float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source
+    float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable.
+    PWMMapping_t pwm_mappings[GPIO_COUNT];
+    PWMMapping_t analog_mappings[GPIO_COUNT];
+
+    /**
+     * Defines the baudrate used on the UART interface.
+     * Some baudrates will have a small timing error due to hardware limitations.
+     * 
+     * Here's an (incomplete) list of baudrates for ODrive v3.x:
+     * 
+     *   Configured  | Actual        | Error [%]
+     *  -------------|---------------|-----------
+     *   1.2 KBps    | 1.2 KBps      | 0
+     *   2.4 KBps    | 2.4 KBps      | 0
+     *   9.6 KBps    | 9.6 KBps      | 0
+     *   19.2 KBps   | 19.195 KBps   | 0.02
+     *   38.4 KBps   | 38.391 KBps   | 0.02
+     *   57.6 KBps   | 57.613 KBps   | 0.02
+     *   115.2 KBps  | 115.068 KBps  | 0.11
+     *   230.4 KBps  | 230.769 KBps  | 0.16
+     *   460.8 KBps  | 461.538 KBps  | 0.16
+     *   921.6 KBps  | 913.043 KBps  | 0.93
+     *   1.792 MBps  | 1.826 MBps    | 1.9
+     *   1.8432 MBps | 1.826 MBps    | 0.93
+     * 
+     * For more information refer to Section 30.3.4 and Table 142 (the column with f_PCLK = 42 MHz) in the STM datasheet:
+     * https://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
+     */
+    uint32_t uart_baudrate = 115200;
+};
+
+// Forward Declarations
+class Axis;
+class Motor;
+class ODriveCAN;
+
+constexpr size_t AXIS_COUNT = 2;
+extern std::array<Axis*, AXIS_COUNT> axes;
+extern ODriveCAN *odCAN;
+
+// if you use the oscilloscope feature you can bump up this value
+#define OSCILLOSCOPE_SIZE 4096
+extern float oscilloscope[OSCILLOSCOPE_SIZE];
+extern size_t oscilloscope_pos;
+
+// TODO: move
+// this is technically not thread-safe but practically it might be
+#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \
+inline ENUMTYPE operator | (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) | static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
+inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) & static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
+inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) ^ static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
+inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) |= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
+inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) &= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
+inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) ^= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
+inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_cast<std::underlying_type_t<ENUMTYPE>>(a)); }
+
+
+enum TimingLog_t {
+    TIMING_LOG_GENERAL,
+    TIMING_LOG_ADC_CB_I,
+    TIMING_LOG_ADC_CB_DC,
+    TIMING_LOG_MEAS_R,
+    TIMING_LOG_MEAS_L,
+    TIMING_LOG_ENC_CALIB,
+    TIMING_LOG_IDX_SEARCH,
+    TIMING_LOG_FOC_VOLTAGE,
+    TIMING_LOG_FOC_CURRENT,
+    TIMING_LOG_SPI_START,
+    TIMING_LOG_SAMPLE_NOW,
+    TIMING_LOG_SPI_END,
+    TIMING_LOG_NUM_SLOTS
+};
+
+
+#include "autogen/interfaces.hpp"
+
+// ODrive specific includes
+#include <utils.hpp>
+#include <gpio_utils.hpp>
+#include <low_level.h>
+#include <motor.hpp>
+#include <encoder.hpp>
+#include <sensorless_estimator.hpp>
+#include <controller.hpp>
+#include <current_limiter.hpp>
+#include <thermistor.hpp>
+#include <trapTraj.hpp>
+#include <endstop.hpp>
+#include <axis.hpp>
+#include <communication/communication.h>
+
+// Defined in autogen/version.c based on git-derived version numbers
+extern "C" {
+extern const unsigned char fw_version_major_;
+extern const unsigned char fw_version_minor_;
+extern const unsigned char fw_version_revision_;
+extern const unsigned char fw_version_unreleased_;
+}
+
+
+// general system functions defined in main.cpp
+class ODrive : public ODriveIntf {
+public:
+    void save_configuration() override;
+    void erase_configuration() override;
+    void reboot() override { NVIC_SystemReset(); }
+    void enter_dfu_mode() override;
+
+    float get_oscilloscope_val(uint32_t index) override {
+        return oscilloscope[index];
+    }
+
+    float get_adc_voltage(uint32_t gpio) override {
+        return ::get_adc_voltage(get_gpio_port_by_pin(gpio), get_gpio_pin_by_pin(gpio));
+    }
+
+    int32_t test_function(int32_t delta) override {
+        static int cnt = 0;
+        return cnt += delta;
+    }
+
+    Axis& get_axis(int num) { return *axes[num]; }
+    ODriveCAN& get_can() { return *odCAN; }
+
+    float& vbus_voltage_ = ::vbus_voltage; // TODO: make this the actual variable
+    float& ibus_ = ::ibus_; // TODO: make this the actual variable
+    float ibus_report_filter_k_ = 1.0f;
+
+    const uint64_t& serial_number_ = ::serial_number;
+
+#if HW_VERSION_MAJOR == 3
+    // Determine start address of the OTP struct:
+    // The OTP is organized into 16-byte blocks.
+    // If the first block starts with "0xfe" we use the first block.
+    // If the first block starts with "0x00" and the second block starts with "0xfe",
+    // we use the second block. This gives the user the chance to screw up once.
+    // If none of the above is the case, we consider the OTP invalid (otp_ptr will be NULL).
+    const uint8_t* otp_ptr =
+        (*(uint8_t*)FLASH_OTP_BASE == 0xfe) ? (uint8_t*)FLASH_OTP_BASE :
+        (*(uint8_t*)FLASH_OTP_BASE != 0x00) ? NULL :
+            (*(uint8_t*)(FLASH_OTP_BASE + 0x10) != 0xfe) ? NULL :
+                (uint8_t*)(FLASH_OTP_BASE + 0x10);
+
+    // Read hardware version from OTP if available, otherwise fall back
+    // to software defined version.
+    const uint8_t hw_version_major_ = otp_ptr ? otp_ptr[3] : HW_VERSION_MAJOR;
+    const uint8_t hw_version_minor_ = otp_ptr ? otp_ptr[4] : HW_VERSION_MINOR;
+    const uint8_t hw_version_variant_ = otp_ptr ? otp_ptr[5] : HW_VERSION_VOLTAGE;
+#else
+#error "not implemented"
+#endif
+
+    // the corresponding macros are defined in the autogenerated version.h
+    const uint8_t fw_version_major_ = ::fw_version_major_;
+    const uint8_t fw_version_minor_ = ::fw_version_minor_;
+    const uint8_t fw_version_revision_ = ::fw_version_revision_;
+    const uint8_t fw_version_unreleased_ = ::fw_version_unreleased_; // 0 for official releases, 1 otherwise
+
+    bool& brake_resistor_armed_ = ::brake_resistor_armed; // TODO: make this the actual variable
+    bool& brake_resistor_saturated_ = ::brake_resistor_saturated; // TODO: make this the actual variable
+
+    SystemStats_t system_stats_;
+
+    BoardConfig_t config_;
+    bool user_config_loaded_;
+
+    uint32_t test_property_ = 0;
+};
+
+extern ODrive odrv; // defined in main.cpp
+
+#endif // __cplusplus
+
+#endif /* __ODRIVE_MAIN_H */

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/sensorless_estimator.cpp

@@ -0,0 +1,85 @@
+
+#include "odrive_main.h"
+
+SensorlessEstimator::SensorlessEstimator(Config_t& config) :
+        config_(config)
+    {};
+
+bool SensorlessEstimator::update() {
+    // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
+    // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
+    // In particular, equation 8 (and by extension eqn 4 and 6).
+
+    // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
+    // is the one computed two cycles ago. To get the correct measurement, it was stored twice:
+    // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
+
+    // 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)};
+
+    // Swap sign of I_beta if motor is reversed
+    I_alpha_beta[1] *= axis_->motor_.config_.direction;
+
+    // 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];
+        // flux dynamics (prediction)
+        float x_dot = y;
+        // integrate prediction to current timestep
+        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];
+    }
+
+    // Non-linear observer (see paper eqn 8):
+    float pm_flux_sqr = config_.pm_flux_linkage * config_.pm_flux_linkage;
+    float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
+    float bandwidth_factor = 1.0f / pm_flux_sqr;
+    float eta_factor = 0.5f * (config_.observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
+
+    // alpha-beta vector operations
+    for (int i = 0; i <= 1; ++i) {
+        // 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;
+        // update new eta
+        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 * axis_->motor_.config_.direction;
+
+    // PLL
+    // TODO: the PLL part has some code duplication with the encoder PLL
+    // Pll gains as a function of bandwidth
+    float pll_kp = 2.0f * config_.pll_bandwidth;
+    // Critically damped
+    float pll_ki = 0.25f * (pll_kp * pll_kp);
+    // Check that we don't get problems with discrete time approximation
+    if (!(current_meas_period * pll_kp < 1.0f)) {
+        error_ |= ERROR_UNSTABLE_GAIN;
+        vel_estimate_valid_ = false;
+        return false;
+    }
+
+    // predict PLL phase with velocity
+    pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_erad_);
+    // 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);
+    // update PLL velocity
+    vel_estimate_erad_ += current_meas_period * pll_ki * delta_phase;
+    // convert to mechanical turns/s for controller usage.
+    vel_estimate_ = vel_estimate_erad_ / (std::max((float)axis_->motor_.config_.pole_pairs, 1.0f) * 2.0f * M_PI);
+
+    vel_estimate_valid_ = true;
+    return true;
+};

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/sensorless_estimator.hpp

@@ -0,0 +1,33 @@
+#ifndef __SENSORLESS_ESTIMATOR_HPP
+#define __SENSORLESS_ESTIMATOR_HPP
+
+class SensorlessEstimator : public ODriveIntf::SensorlessEstimatorIntf {
+public:
+    struct Config_t {
+        float observer_gain = 1000.0f; // [rad/s]
+        float pll_bandwidth = 1000.0f;  // [rad/s]
+        float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)]  { 5.51328895422 / (<pole pairs> * <rpm/v>) }
+    };
+
+    explicit SensorlessEstimator(Config_t& config);
+
+    bool update();
+
+    Axis* axis_ = nullptr; // set by Axis constructor
+    Config_t& config_;
+
+    // TODO: expose on protocol
+    Error error_ = ERROR_NONE;
+    float phase_ = 0.0f;                        // [rad]
+    float pll_pos_ = 0.0f;                      // [rad]
+    float vel_estimate_ = 0.0f;                      // [turn/s]
+    float vel_estimate_erad_ = 0.0f;                 // [rad/s]
+    bool vel_estimate_valid_ = false;
+    // float pll_kp_ = 0.0f;                       // [rad/s / rad]
+    // float pll_ki_ = 0.0f;                       // [(rad/s^2) / rad]
+    float flux_state_[2] = {0.0f, 0.0f};        // [Vs]
+    float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
+    bool estimator_good_ = false;
+};
+
+#endif /* __SENSORLESS_ESTIMATOR_HPP */

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/thermistor.cpp

@@ -0,0 +1,80 @@
+#include "odrive_main.h"
+
+#include "low_level.h"
+
+ThermistorCurrentLimiter::ThermistorCurrentLimiter(uint16_t adc_channel,
+                                                   const float* const coefficients,
+                                                   size_t num_coeffs,
+                                                   const float& temp_limit_lower,
+                                                   const float& temp_limit_upper,
+                                                   const bool& enabled) :
+    adc_channel_(adc_channel),
+    coefficients_(coefficients),
+    num_coeffs_(num_coeffs),
+    temperature_(NAN),
+    temp_limit_lower_(temp_limit_lower),
+    temp_limit_upper_(temp_limit_upper),
+    enabled_(enabled),
+    error_(ERROR_NONE)
+{
+}
+
+void ThermistorCurrentLimiter::update() {
+    const float voltage = get_adc_voltage_channel(adc_channel_);
+    const float normalized_voltage = voltage / adc_ref_voltage;
+    temperature_ = horner_fma(normalized_voltage, coefficients_, num_coeffs_);
+}
+
+bool ThermistorCurrentLimiter::do_checks() {
+    if (enabled_ && temperature_ >= temp_limit_upper_ + 5) {
+        error_ = ERROR_OVER_TEMP;
+        axis_->error_ |= Axis::ERROR_OVER_TEMP;
+        return false;
+    }
+    return true;
+}
+
+float ThermistorCurrentLimiter::get_current_limit(float base_current_lim) const {
+    if (!enabled_) {
+        return base_current_lim;
+    }
+
+    const float temp_margin = temp_limit_upper_ - temperature_;
+    const float derating_range = temp_limit_upper_ - temp_limit_lower_;
+    float thermal_current_lim = base_current_lim * (temp_margin / derating_range);
+    if (!(thermal_current_lim >= 0.0f)) { // Funny polarity to also catch NaN
+        thermal_current_lim = 0.0f;
+    }
+
+    return std::min(thermal_current_lim, base_current_lim);
+}
+
+OnboardThermistorCurrentLimiter::OnboardThermistorCurrentLimiter(const ThermistorHardwareConfig_t& hw_config, Config_t& config) :
+    ThermistorCurrentLimiter(hw_config.adc_ch,
+                             hw_config.coeffs,
+                             hw_config.num_coeffs,
+                             config.temp_limit_lower,
+                             config.temp_limit_upper,
+                             config.enabled),
+    config_(config)
+{
+}
+
+OffboardThermistorCurrentLimiter::OffboardThermistorCurrentLimiter(Config_t& config) :
+    ThermistorCurrentLimiter(UINT16_MAX,
+                             &config.thermistor_poly_coeffs[0],
+                             num_coeffs_,
+                             config.temp_limit_lower,
+                             config.temp_limit_upper,
+                             config.enabled),
+    config_(config)
+{
+    decode_pin();
+}
+
+void OffboardThermistorCurrentLimiter::decode_pin() {
+    const GPIO_TypeDef* const port = get_gpio_port_by_pin(config_.gpio_pin);
+    const uint16_t pin = get_gpio_pin_by_pin(config_.gpio_pin);
+
+    adc_channel_ = channel_from_gpio(port, pin);
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/thermistor.hpp

@@ -0,0 +1,74 @@
+#ifndef __THERMISTOR_HPP
+#define __THERMISTOR_HPP
+
+#ifndef __ODRIVE_MAIN_H
+#error "This file should not be included directly. Include odrive_main.h instead."
+#endif
+
+class ThermistorCurrentLimiter : public CurrentLimiter, public ODriveIntf::ThermistorCurrentLimiterIntf {
+public:
+    virtual ~ThermistorCurrentLimiter() = default;
+
+    ThermistorCurrentLimiter(uint16_t adc_channel,
+                             const float* const coefficients,
+                             size_t num_coeffs,
+                             const float& temp_limit_lower,
+                             const float& temp_limit_upper,
+                             const bool& enabled);
+
+    void update();
+    bool do_checks();
+    float get_current_limit(float base_current_lim) const override;
+
+    uint16_t adc_channel_;
+    const float* const coefficients_;
+    const size_t num_coeffs_;
+    float temperature_;
+    const float& temp_limit_lower_;
+    const float& temp_limit_upper_;
+    const bool& enabled_;
+    Error error_;
+    Axis* axis_ = nullptr; // set by Axis constructor
+};
+
+class OnboardThermistorCurrentLimiter : public ThermistorCurrentLimiter, public ODriveIntf::OnboardThermistorCurrentLimiterIntf {
+public:
+    struct Config_t {
+        float temp_limit_lower = 100;
+        float temp_limit_upper = 120;
+        bool enabled = true;
+    };
+
+    virtual ~OnboardThermistorCurrentLimiter() = default;
+    OnboardThermistorCurrentLimiter(const ThermistorHardwareConfig_t& hw_config, Config_t& config);
+
+    Config_t& config_;
+};
+
+class OffboardThermistorCurrentLimiter : public ThermistorCurrentLimiter, public ODriveIntf::OffboardThermistorCurrentLimiterIntf {
+public:
+    static const size_t num_coeffs_ = 4;
+
+    struct Config_t {
+        float thermistor_poly_coeffs[num_coeffs_];
+
+        uint16_t gpio_pin = 4;
+        float temp_limit_lower = 100;
+        float temp_limit_upper = 120;
+        bool enabled = false;
+
+        // custom setters
+        OffboardThermistorCurrentLimiter* parent;
+        void set_gpio_pin(uint16_t value) { gpio_pin = value; parent->decode_pin(); }
+    };
+
+    virtual ~OffboardThermistorCurrentLimiter() = default;
+    OffboardThermistorCurrentLimiter(Config_t& config);
+
+    Config_t& config_;
+
+private:
+    void decode_pin();
+};
+
+#endif // __THERMISTOR_HPP

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/timer.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <algorithm>
+template <class T>
+class Timer {
+   public:
+    void setTimeout(const T timeout) {
+        timeout_ = timeout;
+    }
+
+    void setIncrement(const T increment) {
+        increment_ = increment;
+    }
+
+    void start() {
+        running_ = true;
+    }
+
+    void stop() {
+        running_ = false;
+    }
+
+    // If the timer is started, increment the timer
+    void update() {
+        if (running_)
+            timer_ = std::min<T>(timer_ + increment_, timeout_);
+    }
+
+    void reset() {
+        timer_ = static_cast<T>(0);
+    }
+
+    bool expired() {
+        return timer_ >= timeout_;
+    }
+
+   private:
+    T timer_ = static_cast<T>(0);     // Current state
+    T timeout_ = static_cast<T>(0);   // Time to count
+    T increment_ = static_cast<T>(0);  // Amount to increment each time update() is called
+    bool running_ = false;            // update() only increments if runing_ is true
+};

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/trapTraj.cpp

@@ -0,0 +1,94 @@
+#include <math.h>
+#include "odrive_main.h"
+#include "utils.hpp"
+
+// A sign function where input 0 has positive sign (not 0)
+float sign_hard(float val) {
+    return (std::signbit(val)) ? -1.0f : 1.0f;
+}
+
+// Symbol                     Description
+// Ta, Tv and Td              Duration of the stages of the AL profile
+// Xi and Vi                  Adapted initial conditions for the AL profile
+// Xf                         Position set-point
+// s                          Direction (sign) of the trajectory
+// Vmax, Amax, Dmax and jmax  Kinematic bounds
+// Ar, Dr and Vr              Reached values of acceleration and velocity
+
+TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {}
+
+bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,
+                                            float Vmax, float Amax, float Dmax) {
+    float dX = Xf - Xi;  // Distance to travel
+    float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance
+    float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement
+    float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)
+    Ar_ = s * Amax;  // Maximum Acceleration (signed)
+    Dr_ = -s * Dmax; // Maximum Deceleration (signed)
+    Vr_ = s * Vmax;  // Maximum Velocity (signed)
+
+    // If we start with a speed faster than cruising, then we need to decel instead of accel
+    // aka "double deceleration move" in the paper
+    if ((s * Vi) > (s * Vr_)) {
+        Ar_ = -s * Amax;
+    }
+
+    // Time to accel/decel to/from Vr (cruise speed)
+    Ta_ = (Vr_ - Vi) / Ar_;
+    Td_ = -Vr_ / Dr_;
+
+    // Integral of velocity ramps over the full accel and decel times to get
+    // minimum displacement required to reach cuising speed
+    float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;
+
+    // Are we displacing enough to reach cruising speed?
+    if (s*dX < s*dXmin) {
+        // Short move (triangle profile)
+        Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));
+        Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);
+        Td_ = std::max(0.0f, -Vr_ / Dr_);
+        Tv_ = 0.0f;
+    } else {
+        // Long move (trapezoidal profile)
+        Tv_ = (dX - dXmin) / Vr_;
+    }
+
+    // Fill in the rest of the values used at evaluation-time
+    Tf_ = Ta_ + Tv_ + Td_;
+    Xi_ = Xi;
+    Xf_ = Xf;
+    Vi_ = Vi;
+    yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase
+
+    return true;
+}
+
+TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {
+    Step_t trajStep;
+    if (t < 0.0f) {  // Initial Condition
+        trajStep.Y   = Xi_;
+        trajStep.Yd  = Vi_;
+        trajStep.Ydd = 0.0f;
+    } else if (t < Ta_) {  // Accelerating
+        trajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);
+        trajStep.Yd  = Vi_ + Ar_*t;
+        trajStep.Ydd = Ar_;
+    } else if (t < Ta_ + Tv_) {  // Coasting
+        trajStep.Y   = yAccel_ + Vr_*(t - Ta_);
+        trajStep.Yd  = Vr_;
+        trajStep.Ydd = 0.0f;
+    } else if (t < Tf_) {  // Deceleration
+        float td     = t - Tf_;
+        trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);
+        trajStep.Yd  = Dr_*td;
+        trajStep.Ydd = Dr_;
+    } else if (t >= Tf_) {  // Final Condition
+        trajStep.Y   = Xf_;
+        trajStep.Yd  = 0.0f;
+        trajStep.Ydd = 0.0f;
+    } else {
+        // TODO: report error here
+    }
+
+    return trajStep;
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/trapTraj.hpp

@@ -0,0 +1,44 @@
+#ifndef _TRAP_TRAJ_H
+#define _TRAP_TRAJ_H
+
+class TrapezoidalTrajectory {
+public:
+    struct Config_t {
+        float vel_limit = 2.0f;   // [turn/s]
+        float accel_limit = 0.5f; // [turn/s^2]
+        float decel_limit = 0.5f; // [turn/s^2]
+    };
+    
+    struct Step_t {
+        float Y;
+        float Yd;
+        float Ydd;
+    };
+
+    explicit TrapezoidalTrajectory(Config_t& config);
+    bool planTrapezoidal(float Xf, float Xi, float Vi,
+                         float Vmax, float Amax, float Dmax);
+    Step_t eval(float t);
+
+    Axis* axis_ = nullptr;  // set by Axis constructor
+    Config_t& config_;
+
+    float Xi_;
+    float Xf_;
+    float Vi_;
+
+    float Ar_;
+    float Vr_;
+    float Dr_;
+
+    float Ta_;
+    float Tv_;
+    float Td_;
+    float Tf_;
+
+    float yAccel_;
+
+    float t_;
+};
+
+#endif

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/utils.cpp

@@ -0,0 +1,205 @@
+
+#include <utils.hpp>
+#include <math.h>
+#include <float.h>
+#include <cmsis_os.h>
+#include <stm32f4xx_hal.h>
+
+
+int SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
+    int Sextant;
+
+    if (beta >= 0.0f) {
+        if (alpha >= 0.0f) {
+            //quadrant I
+            if (one_by_sqrt3 * beta > alpha)
+                Sextant = 2; //sextant v2-v3
+            else
+                Sextant = 1; //sextant v1-v2
+
+        } else {
+            //quadrant II
+            if (-one_by_sqrt3 * beta > alpha)
+                Sextant = 3; //sextant v3-v4
+            else
+                Sextant = 2; //sextant v2-v3
+        }
+    } else {
+        if (alpha >= 0.0f) {
+            //quadrant IV
+            if (-one_by_sqrt3 * beta > alpha)
+                Sextant = 5; //sextant v5-v6
+            else
+                Sextant = 6; //sextant v6-v1
+        } else {
+            //quadrant III
+            if (one_by_sqrt3 * beta > alpha)
+                Sextant = 4; //sextant v4-v5
+            else
+                Sextant = 5; //sextant v5-v6
+        }
+    }
+
+    switch (Sextant) {
+        // sextant v1-v2
+        case 1: {
+            // Vector on-times
+            float t1 = alpha - one_by_sqrt3 * beta;
+            float t2 = two_by_sqrt3 * beta;
+
+            // PWM timings
+            *tA = (1.0f - t1 - t2) * 0.5f;
+            *tB = *tA + t1;
+            *tC = *tB + t2;
+        } break;
+
+        // sextant v2-v3
+        case 2: {
+            // Vector on-times
+            float t2 = alpha + one_by_sqrt3 * beta;
+            float t3 = -alpha + one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tB = (1.0f - t2 - t3) * 0.5f;
+            *tA = *tB + t3;
+            *tC = *tA + t2;
+        } break;
+
+        // sextant v3-v4
+        case 3: {
+            // Vector on-times
+            float t3 = two_by_sqrt3 * beta;
+            float t4 = -alpha - one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tB = (1.0f - t3 - t4) * 0.5f;
+            *tC = *tB + t3;
+            *tA = *tC + t4;
+        } break;
+
+        // sextant v4-v5
+        case 4: {
+            // Vector on-times
+            float t4 = -alpha + one_by_sqrt3 * beta;
+            float t5 = -two_by_sqrt3 * beta;
+
+            // PWM timings
+            *tC = (1.0f - t4 - t5) * 0.5f;
+            *tB = *tC + t5;
+            *tA = *tB + t4;
+        } break;
+
+        // sextant v5-v6
+        case 5: {
+            // Vector on-times
+            float t5 = -alpha - one_by_sqrt3 * beta;
+            float t6 = alpha - one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tC = (1.0f - t5 - t6) * 0.5f;
+            *tA = *tC + t5;
+            *tB = *tA + t6;
+        } break;
+
+        // sextant v6-v1
+        case 6: {
+            // Vector on-times
+            float t6 = -two_by_sqrt3 * beta;
+            float t1 = alpha + one_by_sqrt3 * beta;
+
+            // PWM timings
+            *tA = (1.0f - t6 - t1) * 0.5f;
+            *tC = *tA + t1;
+            *tB = *tC + t6;
+        } break;
+    }
+
+    // if any of the results becomes NaN, result_valid will evaluate to false
+    int result_valid =
+            *tA >= 0.0f && *tA <= 1.0f
+         && *tB >= 0.0f && *tB <= 1.0f
+         && *tC >= 0.0f && *tC <= 1.0f;
+    return result_valid ? 0 : -1;
+}
+
+// based on https://math.stackexchange.com/a/1105038/81278
+float fast_atan2(float y, float x) {
+    // a := min (|x|, |y|) / max (|x|, |y|)
+    float abs_y = fabsf(y);
+    float abs_x = fabsf(x);
+    // inject FLT_MIN in denominator to avoid division by zero
+    float a = MACRO_MIN(abs_x, abs_y) / (MACRO_MAX(abs_x, abs_y) + FLT_MIN);
+    // s := a * a
+    float s = a * a;
+    // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a
+    float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a;
+    // if |y| > |x| then r := 1.57079637 - r
+    if (abs_y > abs_x)
+        r = 1.57079637f - r;
+    // if x < 0 then r := 3.14159274 - r
+    if (x < 0.0f)
+        r = 3.14159274f - r;
+    // if y < 0 then r := -r
+    if (y < 0.0f)
+        r = -r;
+
+    return r;
+}
+
+// Evaluate polynomials using Fused Multiply Add intrisic instruction.
+// coeffs[0] is highest order, as per numpy.polyfit
+// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg"
+float horner_fma(float x, const float *coeffs, size_t count) {
+    float result = 0.0f;
+    for (size_t idx = 0; idx < count; ++idx)
+        result = fmaf(result, x, coeffs[idx]);
+    return result;
+}
+
+// Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271
+int mod(int dividend, int divisor){
+    int r = dividend % divisor;
+    return (r < 0) ? (r + divisor) : r;
+}
+
+// @brief: Returns how much time is left until the deadline is reached.
+// If the deadline has already passed, the return value is 0 (except if
+// the deadline is very far in the past)
+uint32_t deadline_to_timeout(uint32_t deadline_ms) {
+    uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
+    uint32_t timeout_ms = deadline_ms - now_ms;
+    return (timeout_ms & 0x80000000) ? 0 : timeout_ms;
+}
+
+// @brief: Converts a timeout to a deadline based on the current time.
+uint32_t timeout_to_deadline(uint32_t timeout_ms) {
+    uint32_t now_ms = (uint32_t)((1000ull * (uint64_t)osKernelSysTick()) / osKernelSysTickFrequency);
+    return now_ms + timeout_ms;
+}
+
+// @brief: Returns a non-zero value if the specified system time (in ms)
+// is in the future or 0 otherwise.
+// If the time lies far in the past this may falsely return a non-zero value.
+int is_in_the_future(uint32_t time_ms) {
+    return deadline_to_timeout(time_ms);
+}
+
+// @brief: Returns number of microseconds since system startup
+uint32_t micros(void) {
+    register uint32_t ms, cycle_cnt;
+    do {
+        ms = HAL_GetTick();
+        cycle_cnt = TIM_TIME_BASE->CNT;
+     } while (ms != HAL_GetTick());
+
+    return (ms * 1000) + cycle_cnt;
+}
+
+// @brief: Busy wait delay for given amount of microseconds (us)
+void delay_us(uint32_t us)
+{
+    uint32_t start = micros();
+    while (micros() - start < (uint32_t) us) {
+        __ASM("nop");
+    }
+}

Makerbase_ODrive/ODrive-fw-v0.5.1/Firmware/MotorControl/utils.hpp

@@ -0,0 +1,132 @@
+
+#ifndef __UTILS_H
+#define __UTILS_H
+
+#include <stdint.h>
+#include <math.h>
+
+/**
+ * @brief Flash size register address
+ */
+#define ID_FLASH_ADDRESS (0x1FFF7A22)
+
+/**
+ * @brief Device ID register address
+ */
+#define ID_DBGMCU_IDCODE (0xE0042000)
+
+/**
+ * "Returns" the device signature
+ *
+ * Possible returns:
+ *    - 0x0413: STM32F405xx/07xx and STM32F415xx/17xx)
+ *    - 0x0419: STM32F42xxx and STM32F43xxx
+ *    - 0x0423: STM32F401xB/C
+ *    - 0x0433: STM32F401xD/E
+ *    - 0x0431: STM32F411xC/E
+ *
+ * Returned data is in 16-bit mode, but only bits 11:0 are valid, bits 15:12 are always 0.
+ * Defined as macro
+ */
+#define STM_ID_GetSignature() ((*(uint16_t *)(ID_DBGMCU_IDCODE)) & 0x0FFF)
+
+/**
+ * "Returns" the device revision
+ *
+ * Revisions possible:
+ *    - 0x1000: Revision A
+ *    - 0x1001: Revision Z
+ *    - 0x1003: Revision Y
+ *    - 0x1007: Revision 1
+ *    - 0x2001: Revision 3
+ *
+ * Returned data is in 16-bit mode.
+ */
+#define STM_ID_GetRevision() (*(uint16_t *)(ID_DBGMCU_IDCODE + 2))
+
+/**
+* "Returns" the Flash size
+*
+* Returned data is in 16-bit mode, returned value is flash size in kB (kilo bytes).
+*/
+#define STM_ID_GetFlashSize() (*(uint16_t *)(ID_FLASH_ADDRESS))
+
+#ifdef M_PI
+#undef M_PI
+#endif
+#define M_PI (3.14159265358979323846f)
+
+#define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y))
+#define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y))
+
+#define SQ(x) ((x) * (x))
+
+#ifdef __cplusplus
+
+#include <array>
+
+/**
+ * @brief Small helper to make array with known size
+ * in contrast to initializer lists the number of arguments
+ * has to match exactly. Whereas initializer lists allow
+ * less arguments.
+ */
+template<class T, class... Tail>
+std::array<T, 1 + sizeof...(Tail)> make_array(T head, Tail... tail)
+{
+    return std::array<T, 1 + sizeof...(Tail)>({ head, tail ... });
+}
+
+extern "C" {
+#endif
+
+static const float one_by_sqrt3 = 0.57735026919f;
+static const float two_by_sqrt3 = 1.15470053838f;
+static const float sqrt3_by_2 = 0.86602540378f;
+
+// like fmodf, but always positive
+static inline float fmodf_pos(float x, float y) {
+    float out = fmodf(x, y);
+    if (out < 0.0f)
+        out += y;
+    return out;
+}
+
+/**
+ * @brief Similar to modulo operator, except that the output range is centered
+ * around zero.
+ * The returned value is always in the range [-pm_range, pm_range).
+ */
+static inline float wrap_pm(float x, float pm_range) {
+    return fmodf_pos(x + pm_range, 2.0f * pm_range) - pm_range;
+}
+
+static inline float wrap_pm_pi(float theta) {
+    return wrap_pm(theta, M_PI);
+}
+
+// Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta
+// as per the magnitude invariant clarke transform
+// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
+// Returns 0 on success, and -1 if the input was out of range
+int SVM(float alpha, float beta, float* tA, float* tB, float* tC);
+
+float fast_atan2(float y, float x);
+float horner_fma(float x, const float *coeffs, size_t count);
+int mod(int dividend, int divisor);
+
+uint32_t deadline_to_timeout(uint32_t deadline_ms);
+uint32_t timeout_to_deadline(uint32_t timeout_ms);
+int is_in_the_future(uint32_t time_ms);
+
+uint32_t micros(void);
+void delay_us(uint32_t us);
+
+float our_arm_sin_f32(float x);
+float our_arm_cos_f32(float x);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  //__UTILS_H