Merge branch 'devel' into Endstops

2026-09-20 22:55:00 +08:00 · 2019-04-23 21:48:43 +02:00
parent d90793b788 7eb4f2a584
commit 1e5b82ca8b
52 changed files with 1008 additions and 371 deletions
@@ -33,7 +33,7 @@ install:

 - export TUP_DIR=$HOME/dl/tup_0.7.5-0~16.04.york0_amd64
 - export TUP_ARCHIVE=$HOME/dl/tup_0.7.5-0~16.04.york0_amd64.deb
- export TUP_URL=http://ppa.launchpad.net/jonathonf/tup/ubuntu/pool/main/t/tup/tup_0.7.5-0~16.04.york0_amd64.deb
+- export TUP_URL=http://ppa.launchpad.net/jonathonf/tup/ubuntu/pool/main/t/tup/tup_0.7.8-2~16.04.york0_amd64.deb
 - if [ ! -e $TUP_DIR/bin/tup ]; then wget $TUP_URL -O $TUP_ARCHIVE; dpkg-deb -R $TUP_ARCHIVE $TUP_DIR; fi
 - export PATH=$PATH:$TUP_DIR/usr/bin

@@ -38,6 +38,14 @@ void ODriveArduino::SetVelocity(int motor_number, float velocity, float current_
    serial_ << "v " << motor_number  << " " << velocity << " " << current_feedforward << "\n";
 }

+void ODriveArduino::SetCurrent(int motor_number, float current) {
+    serial_ << "c " << motor_number << " " << current << "\n";
+}
+
+void ODriveArduino::TrapezoidalMove(int motor_number, float position){
+    serial_ << "t " << motor_number << " " << position << "\n";
+}
+
 float ODriveArduino::readFloat() {
    return readString().toFloat();
 }
@@ -26,7 +26,8 @@ public:
    void SetPosition(int motor_number, float position, float velocity_feedforward, float current_feedforward);
    void SetVelocity(int motor_number, float velocity);
    void SetVelocity(int motor_number, float velocity, float current_feedforward);
-
+    void SetCurrent(int motor_number, float current);
+    void TrapezoidalMove(int motor_number, float position);
    // General params
    float readFloat();
    int32_t readInt();
@@ -8,6 +8,7 @@ template<>        inline Print& operator <<(Print &obj, float arg) { obj.print(a

 // Serial to the ODrive
 SoftwareSerial odrive_serial(8, 9); //RX (ODrive TX), TX (ODrive RX)
+// Note: you must also connect GND on ODrive to GND on Arduino!

 // ODrive object
 ODriveArduino odrive(odrive_serial);
@@ -2,14 +2,40 @@
 Please add a note of your changes below this heading if you make a Pull Request.

 ### Added
-* `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
+* Communication watchdog feature.
+* `encoder.set_linear_count(count)` function.
+* Configurable encoder offset calibration distance and speed:`calib_scan_distance` and `calib_scan_omega`
+* Encoder offset calibration debug variable `calib_scan_response`
+* Script to enable using a hall signal as index edge.
+
+### Fixed
+* Encoder index interrupts now disabled when not searching

 # Releases
+## [0.4.8] - 2019-02-25
+### Added
+* `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
+* `f` command to ascii protocol to get encoder position and velocity feedback.
+* `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward.
+* `ss`, `se`, `sr` commands to ascii protocol, for save config, erase config and reboot.
+* `move_incremental` function for relative trajectory moves.
+* `encoder.config.ignore_illegal_hall_state` option.
+* `encoder.config.enable_phase_interpolation` option. Setting to false may reduce jerky pulsations at low speed when using hall sensor feedback.
+* Analog input. Used the same way as the PWM input mappings.
+* Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop.
+* Thermal current limit with linear derating.
+
+### Changed
+* Unified lockin drive modes. Current for index searching and encoder offset calibration now moved to axis.lockin.current.
+
+### Fixed
+* Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM.
+
 ## [0.4.7] - 2018-11-28
 ### Added
 * Overspeed fault
 * Current sense saturation fault.
-* Supress startup transients by sampling encoder estimate into position setpoint when entering closed loop control.
+* Suppress startup transients by sampling encoder estimate into position setpoint when entering closed loop control.
 * Make step dir gpio pins configurable.
 * Configuration variable `encoder.config.zero_count_on_find_idx`, true by default. Set to false to leave the initial encoder count to be where the axis was at boot.
 * Circular position setpoint mode: position setpoints wrapped [0, cpr). Useful for infinite incremental position control.
@@ -6,6 +6,7 @@
                "${workspaceRoot}",
                "${workspaceRoot}/fibre/cpp/include/**",
                "${workspaceRoot}/MotorControl",
+                "${workspaceRoot}/communication",
                "${workspaceRoot}/Drivers/DRV8301",
                "${workspaceRoot}/Board/v3/Inc",
                "${workspaceRoot}/Board/v3/Drivers/CMSIS/Include",
@@ -16,30 +17,21 @@
                "${workspaceRoot}/Board/v3/Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc",
                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS",
                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include",
-                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
-                "${ARM_GCC_ROOT}/arm-none-eabi/include/**",
-                "${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**"
+                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F"
            ],
            "defines": [
                "STM32F405xx",
                "USE_HAL_DRIVER",
                "HW_VERSION_MAJOR=3",
-                "HW_VERSION_MINOR=5",
-                "HW_VERSION_VOLTAGE=24",
+                "HW_VERSION_MINOR=6",
+                "HW_VERSION_VOLTAGE=56",
                "USB_PROTOCOL_NATIVE",
                "__weak=\"__attribute__((weak))\"",
                "__packed=\"__attribute__((__packed__))\"",
                "__GNUC__"
            ],
            "intelliSenseMode": "clang-x64",
-            "browse": {
-                "path": [
-                    "${workspaceRoot}",
-                    "${ARM_GCC_ROOT}"
-                ],
-                "limitSymbolsToIncludedHeaders": true
-            },
-            "compilerPath": "${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
+            "compilerPath": "\"${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe\" -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float",
            "cStandard": "c11",
            "cppStandard": "c++14"
        },
@@ -49,6 +41,7 @@
                "${workspaceRoot}",
                "${workspaceRoot}/fibre/cpp/include/**",
                "${workspaceRoot}/MotorControl",
+                "${workspaceRoot}/communication",
                "${workspaceRoot}/Drivers/DRV8301",
                "${workspaceRoot}/Board/v3/Inc",
                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
@@ -92,6 +85,7 @@
                "${workspaceRoot}",
                "${workspaceRoot}/fibre/cpp/include/**",
                "${workspaceRoot}/MotorControl",
+                "${workspaceRoot}/communication",
                "${workspaceRoot}/Drivers/DRV8301",
                "${workspaceRoot}/Board/v3/Inc",
                "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F",
@@ -171,7 +171,7 @@
 #define CURRENT_MEAS_PERIOD ( (float)2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1) / (float)TIM_1_8_CLOCK_HZ )
 #define CURRENT_MEAS_HZ ( (float)(TIM_1_8_CLOCK_HZ) / (float)(2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1)) )

-#if HW_VERSION_VOLTAGE == 48
+#if HW_VERSION_VOLTAGE >= 48
 #define VBUS_S_DIVIDER_RATIO 19.0f
 #define VBUS_OVERVOLTAGE_LEVEL 52.0f
 #elif HW_VERSION_VOLTAGE == 24
@@ -60,6 +60,7 @@
 #include "usb_device.h"
 extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
 int odrive_main(void);
+int load_configuration(void);
 /* USER CODE END Includes */

 /* Private typedef -----------------------------------------------------------*/
@@ -187,6 +188,9 @@ void MX_FREERTOS_Init(void) {
  sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);

  init_deferred_interrupts();
+
+  // Load persistent configuration (or defaults)
+  load_configuration();
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
@@ -91,22 +91,34 @@ void NMI_Handler(void)
  /* USER CODE END NonMaskableInt_IRQn 1 */
 }

+void get_regs(void** stack_ptr) {
+  void* volatile r0 __attribute__((unused)) = stack_ptr[0];
+  void* volatile r1 __attribute__((unused)) = stack_ptr[1];
+  void* volatile r2 __attribute__((unused)) = stack_ptr[2];
+  void* volatile r3 __attribute__((unused)) = stack_ptr[3];
+
+  void* volatile r12 __attribute__((unused)) = stack_ptr[4];
+  void* volatile lr __attribute__((unused)) = stack_ptr[5];  // Link register
+  void* volatile pc __attribute__((unused)) = stack_ptr[6];  // Program counter
+  void* volatile psr __attribute__((unused)) = stack_ptr[7];  // Program status register
+
+  volatile bool stay_looping = true;
+  while(stay_looping);
+}
+
 /**
 * @brief This function handles Hard fault interrupt.
 */
+__attribute__((naked))
 void HardFault_Handler(void)
 {
-  /* USER CODE BEGIN HardFault_IRQn 0 */
-
-  /* USER CODE END HardFault_IRQn 0 */
-  while (1)
-  {
-    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
-    /* USER CODE END W1_HardFault_IRQn 0 */
-  }
-  /* USER CODE BEGIN HardFault_IRQn 1 */
-
-  /* USER CODE END HardFault_IRQn 1 */
+  __asm(
+    " tst lr, #4     \n\t"
+    " ite eq         \n\t"
+    " mrseq r0, msp  \n\t"
+    " mrsne r0, psp  \n\t"
+    " b get_regs     \n\t"
+  );
 }

 /**
@@ -40,6 +40,10 @@ erase:
 erase_config:
 	$(OPENOCD) -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ init -c reset\ run -c exit

+# Sometimes the STM32 will get it's protection bits set for unknown reasons. Unlock it with this command
+unlock:
+	$(OPENOCD) -c init -c reset\ halt -c stm32f2x\ unlock\ 0
+
 # The one-time programmable memory stores the board version
 # has the following format:
 #  - OTP format version (0xFE: version 1)
@@ -70,8 +74,8 @@ ifeq ($(OTP_CONFIRM),TRUE)
 		-c 'mwb 0x1fff7801 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7802 0x01' -c 'sleep 10' \
 		-c 'mwb 0x1fff7803 3' -c 'sleep 10' \
-		-c 'mwb 0x1fff7804 5' -c 'sleep 10' \
-		-c 'mwb 0x1fff7805 48' -c 'sleep 10' \
+		-c 'mwb 0x1fff7804 6' -c 'sleep 10' \
+		-c 'mwb 0x1fff7805 56' -c 'sleep 10' \
 		-c 'reset run' \
 		-c exit
 	@echo "OK"
@@ -31,6 +31,7 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
    motor_.axis_ = this;
    trap_.axis_ = this;
    decode_step_dir_pins();
+    update_watchdog_settings();
    min_endstop_.axis_ = this;
    max_endstop_.axis_ = this;
 }
@@ -94,6 +95,21 @@ void Axis::decode_step_dir_pins() {
    dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
 }

+// @brief: Setup the watchdog reset value from the configuration watchdog timeout interval. 
+void Axis::update_watchdog_settings() {
+
+    if(config_.watchdog_timeout <= 0.0f) { // watchdog disabled 
+        watchdog_reset_value_ = 0;
+    } else if(config_.watchdog_timeout >= UINT32_MAX / (current_meas_hz+1)) { //overflow! 
+        watchdog_reset_value_ = UINT32_MAX;
+    } else {
+        watchdog_reset_value_ = static_cast<uint32_t>(config_.watchdog_timeout * current_meas_hz);
+    }
+
+    // Do a feed to avoid instant timeout
+    watchdog_feed();
+}
+
 // @brief (de)activates step/dir input
 void Axis::set_step_dir_active(bool active) {
    if (active) {
@@ -149,42 +165,86 @@ bool Axis::do_updates() {
    return check_for_errors();
 }

-float Axis::get_temp() {
-    float adc = adc_measurements_[hw_config_.thermistor_adc_ch];
-    float normalized_voltage = adc / adc_full_scale;
-    return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs);
+// @brief Feed the watchdog to prevent watchdog timeouts.
+void Axis::watchdog_feed() {
+    watchdog_current_value_ = watchdog_reset_value_;
 }

-bool Axis::run_sensorless_spin_up() {
-    // Early Spin-up: spiral up current
+// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired. 
+bool Axis::watchdog_check() {
+    // reset value = 0 means watchdog disabled. 
+    if(watchdog_reset_value_ == 0) 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() {
+    // 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(config_.ramp_up_distance * x);
-        float I_mag = config_.spin_up_current * x;
-        x += current_meas_period / config_.ramp_up_time;
-        if (!motor_.update(I_mag, phase))
-            return error_ |= ERROR_MOTOR_FAILED, false;
+    run_control_loop([&]() {
+        float phase = wrap_pm_pi(config_.lockin.ramp_distance * x);
+        float I_mag = config_.lockin.current * x;
+        x += current_meas_period / config_.lockin.ramp_time;
+        if (!motor_.update(I_mag, phase, 0.0f))
+            return false;
        return x < 1.0f;
    });
-    if (error_ != ERROR_NONE)
-        return false;
    
-    // Late Spin-up: accelerate
-    float vel = config_.ramp_up_distance / config_.ramp_up_time;
-    float phase = wrap_pm_pi(config_.ramp_up_distance);
-    run_control_loop([&](){
-        vel += config_.spin_up_acceleration * current_meas_period;
+    // Spin states
+    float distance = config_.lockin.ramp_distance;
+    float phase = wrap_pm_pi(distance);
+    float vel = distance / config_.lockin.ramp_time;
+
+    // Function of states to check if we are done
+    auto spin_done = [&](bool vel_override = false) -> bool {
+        bool done = false;
+        if (config_.lockin.finish_on_vel || vel_override)
+            done = done || fabsf(vel) >= fabsf(config_.lockin.vel);
+        if (config_.lockin.finish_on_distance)
+            done = done || fabsf(distance) >= fabsf(config_.lockin.finish_distance);
+        if (config_.lockin.finish_on_enc_idx)
+            done = done || encoder_.index_found_;
+        return done;
+    };
+
+    // Accelerate
+    lockin_state_ = LOCKIN_STATE_ACCELERATE;
+    run_control_loop([&]() {
+        vel += config_.lockin.accel * current_meas_period;
+        distance += vel * current_meas_period;
        phase = wrap_pm_pi(phase + vel * current_meas_period);
-        float I_mag = config_.spin_up_current;
-        if (!motor_.update(I_mag, phase))
-            return error_ |= ERROR_MOTOR_FAILED, false;
-        return vel < config_.spin_up_target_vel;
+
+        if (!motor_.update(config_.lockin.current, phase, vel))
+            return false;
+        return !spin_done(true); //vel_override to go to next phase
    });

-    // 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_.spin_up_target_vel;
+    if (!encoder_.index_found_)
+        encoder_.set_idx_subscribe(true);

+    // Constant speed
+    if (!spin_done()) {
+        lockin_state_ = LOCKIN_STATE_CONST_VEL;
+        vel = config_.lockin.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(config_.lockin.current, phase, vel))
+                return false;
+            return !spin_done();
+        });
+    }
+
+    lockin_state_ = LOCKIN_STATE_INACTIVE;
    return check_for_errors();
 }

@@ -198,7 +258,7 @@ bool Axis::run_sensorless_control_loop() {
        float current_setpoint;
        if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
            return error_ |= ERROR_CONTROLLER_FAILED, false;
-        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_))
+        if (!motor_.update(current_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_))
            return false; // set_error should update axis.error_
        return true;
    });
@@ -213,9 +273,10 @@ bool Axis::run_closed_loop_control_loop() {
        // Note that all estimators are updated in the loop prefix in run_control_loop
        float current_setpoint;
        if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
-            return error_ |= ERROR_CONTROLLER_FAILED, false;  //TODO: Make controller.set_error
-        if (!motor_.update(current_setpoint, encoder_.phase_))
-            return false;  // set_error should update axis.error_
+            return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
+        float phase_vel = 2*M_PI * encoder_.vel_estimate_ / (float)encoder_.config_.cpr * motor_.config_.pole_pairs;
+        if (!motor_.update(current_setpoint, encoder_.phase_, phase_vel))
+            return false; // set_error should update axis.error_

        // Handle the homing case
        if (homing_state_ == HOMING_STATE_HOMING) {
@@ -309,48 +370,73 @@ void Axis::run_state_machine_loop() {

        // Note that current_state is a reference to task_chain_[0]

-        // Validate the state before running it
-        if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_)
-            current_state_ = AXIS_STATE_UNDEFINED;
-        if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_)
-            current_state_ = AXIS_STATE_UNDEFINED;
-
        // Run the specified state
        // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
        bool status;
        switch (current_state_) {
-            case AXIS_STATE_MOTOR_CALIBRATION:
+            case AXIS_STATE_MOTOR_CALIBRATION: {
                status = motor_.run_calibration();
-                break;
+            } 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;

-            case AXIS_STATE_ENCODER_INDEX_SEARCH:
                status = encoder_.run_index_search();
-                break;
+            } 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 = controller_.home_axis();
                break;

-            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
+            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
                status = encoder_.run_offset_calibration();
-                break;
+            } break;

-            case AXIS_STATE_SENSORLESS_CONTROL:
-                status = run_sensorless_spin_up(); // TODO: restart if desired
-                if (status)
+            case AXIS_STATE_LOCKIN_SPIN: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                    goto invalid_state_label;
+                status = run_lockin_spin();
+            } break;
+
+            case AXIS_STATE_SENSORLESS_CONTROL: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                        goto invalid_state_label;
+                status = run_lockin_spin(); // 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_.lockin.vel;
                    status = run_sensorless_control_loop();
-                break;
+                }
+            } break;

-            case AXIS_STATE_CLOSED_LOOP_CONTROL:
+            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;
                status = run_closed_loop_control_loop();
-                break;
+            } break;

-            case AXIS_STATE_IDLE:
+            case AXIS_STATE_IDLE: {
                run_idle_loop();
                status = motor_.arm(); // done with idling - try to arm the motor
-                break;
+            } break;

            default:
+            invalid_state_label:
                error_ |= ERROR_INVALID_STATE;
                status = false; // this will set the state to idle
                break;
@@ -27,12 +27,11 @@ public:
        ERROR_ENCODER_FAILED = 0x100, // Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
        ERROR_CONTROLLER_FAILED = 0x200,
        ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
+        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800,
        ERROR_MIN_ENDSTOP_PRESSED = 0x800,
        ERROR_MAX_ENDSTOP_PRESSED = 0x1000
    };

-    // Warning: Do not reorder these enum values.
-    // The state machine uses ">" comparision on them.
    enum State_t {
        AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
        AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
@@ -43,8 +42,21 @@ public:
        AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
        AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
        AXIS_STATE_CLOSED_LOOP_CONTROL = 8,  //<! run closed loop control
+        AXIS_STATE_LOCKIN_SPIN = 9,       //<! run lockin spin
+        AXIS_STATE_ENCODER_DIR_FIND = 10,
        AXIS_STATE_HOMING = 9   //<! run axis homing function
-};
+
+    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;
+    };

    struct Config_t {
        bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
@@ -58,22 +70,26 @@ public:
                                    //   For M0 this has no effect if enable_uart is true
        float counts_per_step = 2.0f;

+        float watchdog_timeout = 0.0f; // [s] (0 disables watchdog)
+
        // Defaults loaded from hw_config in load_configuration in main.cpp
        uint16_t step_gpio_pin = 0;
        uint16_t dir_gpio_pin = 0;

-        // Spinup settings
-        float ramp_up_time = 0.4f;            // [s]
-        float ramp_up_distance = 4 * M_PI;    // [rad]
-        float spin_up_current = 10.0f;        // [A]
-        float spin_up_acceleration = 400.0f;  // [rad/s^2]
-        float spin_up_target_vel = 400.0f;    // [rad/s]
+        LockinConfig_t lockin;
    };

    enum thread_signals {
        M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
    };

+    enum LockinState_t {
+        LOCKIN_STATE_INACTIVE,
+        LOCKIN_STATE_RAMP,
+        LOCKIN_STATE_ACCELERATE,
+        LOCKIN_STATE_CONST_VEL,
+    };
+
    Axis(const AxisHardwareConfig_t& hw_config,
            Config_t& config,
            Encoder& encoder,
@@ -92,6 +108,8 @@ public:
    void step_cb();
    void set_step_dir_active(bool enable);
    void decode_step_dir_pins();
+    void update_watchdog_settings();
+
    static void load_default_step_dir_pin_config(
        const AxisHardwareConfig_t& hw_config, Config_t* config);

@@ -99,7 +117,9 @@ public:
    bool check_PSU_brownout();
    bool do_checks();
    bool do_updates();
-    float get_temp();
+
+    void watchdog_feed();
+    bool watchdog_check();


    // True if there are no errors
@@ -135,8 +155,11 @@ public:
            // 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) {
+            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)
@@ -165,7 +188,7 @@ public:
        }
    }

-    bool run_sensorless_spin_up();
+    bool run_lockin_spin();
    bool run_sensorless_control_loop();
    bool run_closed_loop_control_loop();
    bool run_idle_loop();
@@ -200,8 +223,13 @@ public:
    State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
    State_t& current_state_ = task_chain_[0];
    uint32_t loop_counter_ = 0;
+    LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
    HomingState_t homing_state_ = HOMING_STATE_IDLE;

+    // watchdog
+    uint32_t watchdog_reset_value_ = 0; //computed from config_.watchdog_timeout in update_watchdog_settings()
+    uint32_t watchdog_current_value_= 0;
+
    // Communication protocol definitions
    auto make_protocol_definitions() {
        return make_protocol_member_list(
@@ -210,6 +238,7 @@ public:
            make_protocol_ro_property("current_state", &current_state_),
            make_protocol_property("requested_state", &requested_state_),
            make_protocol_ro_property("loop_counter", &loop_counter_),
+            make_protocol_ro_property("lockin_state", &lockin_state_),
            make_protocol_ro_property("homing_state", &homing_state_),
            make_protocol_object("config",
                make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
@@ -220,24 +249,32 @@ public:
                make_protocol_property("startup_homing", &config_.startup_homing),
                make_protocol_property("enable_step_dir", &config_.enable_step_dir),
                make_protocol_property("counts_per_step", &config_.counts_per_step),
+                make_protocol_property("watchdog_timeout", &config_.watchdog_timeout,
+                    [](void* ctx) { static_cast<Axis*>(ctx)->update_watchdog_settings(); }, this),
                make_protocol_property("step_gpio_pin", &config_.step_gpio_pin,
                    [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
                    [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
-                make_protocol_property("ramp_up_time", &config_.ramp_up_time),
-                make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
-                make_protocol_property("spin_up_current", &config_.spin_up_current),
-                make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
-                make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel)
+                make_protocol_object("lockin",
+                    make_protocol_property("current", &config_.lockin.current),
+                    make_protocol_property("ramp_time", &config_.lockin.ramp_time),
+                    make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),
+                    make_protocol_property("accel", &config_.lockin.accel),
+                    make_protocol_property("vel", &config_.lockin.vel),
+                    make_protocol_property("finish_distance", &config_.lockin.finish_distance),
+                    make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
+                    make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
+                    make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
+                )
            ),
-            make_protocol_function("get_temp", *this, &Axis::get_temp),
            make_protocol_object("motor", motor_.make_protocol_definitions()),
            make_protocol_object("controller", controller_.make_protocol_definitions()),
            make_protocol_object("encoder", encoder_.make_protocol_definitions()),
            make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()),
+            make_protocol_object("trap_traj", trap_.make_protocol_definitions()),
            make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()),
            make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()),
-            make_protocol_object("trap_traj", trap_.make_protocol_definitions())
+            make_protocol_function("watchdog_feed", *this, &Axis::watchdog_feed)
        );
    }
 };
@@ -23,7 +23,6 @@
 typedef struct {
    uint16_t step_gpio_pin;
    uint16_t dir_gpio_pin;
-    size_t thermistor_adc_ch;
    osPriority thread_priority;
 } AxisHardwareConfig_t;

@@ -42,6 +41,7 @@ typedef struct {
    TIM_HandleTypeDef* timer;
    uint16_t control_deadline;
    float shunt_conductance;
+    size_t inverter_thermistor_adc_ch;
 } MotorHardwareConfig_t;
 typedef struct {
    SPI_HandleTypeDef* spi;
@@ -74,7 +74,6 @@ const BoardHardwareConfig_t hw_configs[2] = { {
    .axis_config = {
        .step_gpio_pin = 1,
        .dir_gpio_pin = 2,
-        .thermistor_adc_ch = 15,
        .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1),
    },
    .encoder_config = {
@@ -92,6 +91,7 @@ const BoardHardwareConfig_t hw_configs[2] = { {
        .timer = &htim1,
        .control_deadline = TIM_1_8_PERIOD_CLOCKS,
        .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
+        .inverter_thermistor_adc_ch = 15,
    },
    .gate_driver_config = {
        .spi = &hspi3,
@@ -112,11 +112,6 @@ const BoardHardwareConfig_t hw_configs[2] = { {
 #else
        .step_gpio_pin = 3,
        .dir_gpio_pin = 4,
-#endif
-#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
-        .thermistor_adc_ch = 4,
-#else
-        .thermistor_adc_ch = 1,
 #endif
        .thread_priority = osPriorityHigh,
    },
@@ -135,6 +130,11 @@ const BoardHardwareConfig_t hw_configs[2] = { {
        .timer = &htim8,
        .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
        .shunt_conductance = 1.0f / SHUNT_RESISTANCE,  //[S]
+#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
+        .inverter_thermistor_adc_ch = 4,
+#else
+        .inverter_thermistor_adc_ch = 1,
+#endif
    },
    .gate_driver_config = {
        .spi = &hspi3,
@@ -56,6 +56,15 @@ void Controller::move_to_pos(float goal_point) {
                                 axis_->trap_.config_.decel_limit);
    traj_start_loop_count_ = axis_->loop_counter_;
    config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL;
+    goal_point_ = goal_point;
+}
+
+void Controller::move_incremental(float displacement, bool from_goal_point = true){
+    if(from_goal_point){
+        move_to_pos(goal_point_ + displacement);
+    } else{
+        move_to_pos(pos_setpoint_ + displacement);
+    }
 }

 void Controller::start_anticogging_calibration() {
@@ -195,8 +204,8 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s
    Iq += vel_integrator_current_;

    // Current limiting
-    float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
    bool limited = false;
+    float Ilim = axis_->motor_.effective_current_lim();
    if (Iq > Ilim) {
        limited = true;
        Iq = Ilim;
@@ -35,7 +35,7 @@ public:
    float homing_speed = 2000.0f;   // [counts/s]
    };

-    Controller(Config_t& config);
+    explicit Controller(Config_t& config);
    void reset();
    void set_error(Error_t error);

@@ -45,6 +45,7 @@ public:

    // Trajectory-Planned control
    void move_to_pos(float goal_point);
+    void move_incremental(float displacement, bool from_goal_point);

    bool home_axis();
    
@@ -92,6 +93,8 @@ public:

    uint32_t traj_start_loop_count_ = 0;

+    float goal_point_ = 0.0f;
+
    // Communication protocol definitions
    auto make_protocol_definitions() {
        return make_protocol_member_list(
@@ -119,8 +122,9 @@ public:
                "vel_setpoint", "current_feed_forward"),
            make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
                "current_setpoint"),
+            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"),
+            make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"),
            make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration),
-            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "goal_point"),
            make_protocol_function("home_axis", *this, &Controller::home_axis)
        );
    }
@@ -9,7 +9,7 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
 {
    update_pll_gains();

-    if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) {
+    if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL || config.mode == Encoder::MODE_SINCOS)) {
        is_ready_ = true;
    }
 }
@@ -20,8 +20,7 @@ static void enc_index_cb_wrapper(void* ctx) {

 void Encoder::setup() {
    HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
-    GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
-            GPIO_MODE_IT_RISING, enc_index_cb_wrapper, this);
+    set_idx_subscribe();
 }

 void Encoder::set_error(Error_t error) {
@@ -40,9 +39,8 @@ bool Encoder::do_checks(){
 // 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)
-// TODO: disable interrupt once we found the index
 void Encoder::enc_index_cb() {
-    if (config_.use_index && !index_found_) {
+    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
@@ -56,6 +54,35 @@ void Encoder::enc_index_cb() {
        }
        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() {
+    if (!is_ready_)
+        config_.pre_calibrated = false;
+    if (config_.mode == MODE_INCREMENTAL && !index_found_)
+        config_.pre_calibrated = false;
 }

 // Function that sets the current encoder count to a desired 32-bit value.
@@ -90,36 +117,42 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
    cpu_exit_critical(prim);
 }

-
-// @brief Slowly turns the motor in one direction until the
-// encoder index is found.
-// TODO: Do the scan with current, not voltage!
 bool Encoder::run_index_search() {
-    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;
-    
-    float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed;
-
+    config_.use_index = true;
    index_found_ = false;
-    float phase = 0.0f;
-    axis_->run_control_loop([&](){
-        phase = wrap_pm_pi(phase + omega * current_meas_period);
+    if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
+        axis_->motor_.config_.direction = 1;
+    }

-        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(Motor::TIMING_LOG_IDX_SEARCH);
+    bool orig_finish_on_enc_idx = axis_->config_.lockin.finish_on_enc_idx;
+    axis_->config_.lockin.finish_on_enc_idx = true;
+    bool status = axis_->run_lockin_spin();
+    axis_->config_.lockin.finish_on_enc_idx = orig_finish_on_enc_idx;
+    return status;
+}

-        // continue until the index is found
-        return !index_found_;
-    });
-    return true;
+bool Encoder::run_direction_find() {
+    int32_t init_enc_val = shadow_count_;
+    bool orig_finish_on_distance = axis_->config_.lockin.finish_on_distance;
+    axis_->config_.lockin.finish_on_distance = true;
+    axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
+    bool status = axis_->run_lockin_spin();
+    axis_->config_.lockin.finish_on_distance = orig_finish_on_distance;
+
+    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
@@ -128,9 +161,7 @@ bool Encoder::run_index_search() {
 // TODO: Do the scan with current, not voltage!
 bool Encoder::run_offset_calibration() {
    static const float start_lock_duration = 1.0f;
-    static const float scan_omega = 4.0f * M_PI;
-    static const float scan_distance = 16.0f * M_PI;
-    static const int num_steps = (int)(scan_distance / scan_omega * (float)current_meas_hz);
+    static 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_) {
@@ -167,7 +198,7 @@ bool Encoder::run_offset_calibration() {
    // scan forward
    i = 0;
    axis_->run_control_loop([&](){
-        float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
+        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))
@@ -197,9 +228,9 @@ bool Encoder::run_offset_calibration() {
    //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 = scan_distance / elec_rad_per_enc;
-    float actual_encoder_delta_abs = fabsf(shadow_count_-init_enc_val);
-    if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
+    float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc;
+    calib_scan_response_ = fabsf(shadow_count_-init_enc_val);
+    if(fabsf(calib_scan_response_ - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
    {
        set_error(ERROR_CPR_OUT_OF_RANGE);
        return false;
@@ -208,7 +239,7 @@ bool Encoder::run_offset_calibration() {
    // scan backwards
    i = 0;
    axis_->run_control_loop([&](){
-        float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
+        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))
@@ -242,13 +273,24 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) {
    }
 }

-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
+void Encoder::sample_now() {
+    switch (config_.mode) {
+        case MODE_INCREMENTAL: {
+            tim_cnt_sample_ = (int16_t)hw_config_.timer->Instance->CNT;
+        } break;

-    // Check that we don't get problems with discrete time approximation
-    if (!(current_meas_period * pll_kp_ < 1.0f)) {
-        set_error(ERROR_UNSTABLE_GAIN);
+        case MODE_HALL: {
+            // do nothing: samples already captured in general GPIO capture
+        } break;
+
+        case MODE_SINCOS: {
+            sincos_sample_s_ = (get_adc_voltage(GPIO_3_GPIO_Port, GPIO_3_Pin) / 3.3f) - 0.5f;
+            sincos_sample_c_ = (get_adc_voltage(GPIO_4_GPIO_Port, GPIO_4_Pin) / 3.3f) - 0.5f;
+        } break;
+
+        default: {
+           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
+        } break;
    }
 }

@@ -259,7 +301,7 @@ bool Encoder::update() {
        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)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
+            int16_t delta_enc_16 = (int16_t)tim_cnt_sample_ - (int16_t)shadow_count_;
            delta_enc = (int32_t)delta_enc_16; //sign extend
        } break;

@@ -271,10 +313,23 @@ bool Encoder::update() {
                if (delta_enc > 3)
                    delta_enc -= 6;
            } else {
-                set_error(ERROR_ILLEGAL_HALL_STATE);
-                return false;
+                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;
        
        default: {
           set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
@@ -308,7 +363,7 @@ bool Encoder::update() {
    //// 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) {
+    if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
        interpolation_ = 0.5f;
    // reset interpolation if encoder edge comes
    } else if (delta_enc > 0) {
@@ -19,7 +19,8 @@ public:

    enum Mode_t {
        MODE_INCREMENTAL,
-        MODE_HALL
+        MODE_HALL,
+        MODE_SINCOS
    };

    struct Config_t {
@@ -30,13 +31,18 @@ public:
                                    // be determined by run_offset_calibration.
                                    // In this case the encoder will enter ready
                                    // state as soon as the index is found.
-        float idx_search_speed = 10.0f; // [rad/s electrical]
        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
-        float calib_range = 0.02f;
+        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
    };

    Encoder(const EncoderHardwareConfig_t& hw_config,
@@ -47,17 +53,21 @@ public:
    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 scan_for_enc_idx(float omega, float voltage_magnitude);

    bool run_index_search();
+    bool run_direction_find();
    bool run_offset_calibration();
+    void sample_now();
    bool update();

-    void update_pll_gains();
+

    const EncoderHardwareConfig_t& hw_config_;
    Config_t& config_;
@@ -75,39 +85,53 @@ public:
    float vel_estimate_ = 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

+    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;

    // Communication protocol definitions
    auto make_protocol_definitions() {
        return make_protocol_member_list(
            make_protocol_property("error", &error_),
-            make_protocol_ro_property("is_ready", &is_ready_),
-            make_protocol_ro_property("index_found", const_cast<bool*>(&index_found_)),
+            make_protocol_property("is_ready", &is_ready_),
+            make_protocol_property("index_found", const_cast<bool*>(&index_found_)),
            make_protocol_property("shadow_count", &shadow_count_),
            make_protocol_property("count_in_cpr", &count_in_cpr_),
            make_protocol_property("interpolation", &interpolation_),
-            make_protocol_property("phase", &phase_),
+            make_protocol_ro_property("phase", &phase_),
            make_protocol_property("pos_estimate", &pos_estimate_),
            make_protocol_property("pos_cpr", &pos_cpr_),
-            make_protocol_property("hall_state", &hall_state_),
+            make_protocol_ro_property("hall_state", &hall_state_),
            make_protocol_property("vel_estimate", &vel_estimate_),
+            make_protocol_ro_property("calib_scan_response", &calib_scan_response_),
            // make_protocol_property("pll_kp", &pll_kp_),
            // make_protocol_property("pll_ki", &pll_ki_),
            make_protocol_object("config",
                make_protocol_property("mode", &config_.mode),
-                make_protocol_property("use_index", &config_.use_index),
-                make_protocol_property("pre_calibrated", &config_.pre_calibrated),
-                make_protocol_property("idx_search_speed", &config_.idx_search_speed),
+                make_protocol_property("use_index", &config_.use_index,
+                    [](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
+                make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
+                    [](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
+                make_protocol_property("pre_calibrated", &config_.pre_calibrated,
+                    [](void* ctx) { static_cast<Encoder*>(ctx)->check_pre_calibrated(); }, this),
                make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
                make_protocol_property("cpr", &config_.cpr),
                make_protocol_property("offset", &config_.offset),
                make_protocol_property("offset_float", &config_.offset_float),
+                make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),
                make_protocol_property("bandwidth", &config_.bandwidth,
                    [](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
-                make_protocol_property("calib_range", &config_.calib_range)
-            )
+                make_protocol_property("calib_range", &config_.calib_range),
+                make_protocol_property("calib_scan_distance", &config_.calib_scan_distance),
+                make_protocol_property("calib_scan_omega", &config_.calib_scan_omega),
+                make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
+                make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
+            ),
+            make_protocol_function("set_linear_count", *this, &Encoder::set_linear_count, "count")
        );
    }
 };
@@ -125,7 +125,7 @@ bool safety_critical_disarm_motor_pwm(Motor& motor) {
 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_ARMED;
+        motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
    }

    motor.hw_config_.timer->Instance->CCR1 = timings[0];
@@ -539,6 +539,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
            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();
@@ -553,18 +554,30 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
 }

 void tim_update_cb(TIM_HandleTypeDef* htim) {
-    int portsamples_arr;
+    
+    // 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) {
-        portsamples_arr = 0;
+        sample_ch = 0;
+        axis = axes[0];
    } else if (htim == &htim8) {
-        portsamples_arr = 1;
+        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[portsamples_arr][i] = GPIOs_to_samp[i]->IDR;
+        GPIO_port_samples[sample_ch][i] = GPIOs_to_samp[i]->IDR;
    }
 }

@@ -713,4 +726,33 @@ void pwm_in_cb(int channel, uint32_t timestamp) {
    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));
+    get_endpoint(map->endpoint)->set_from_float(value);
+}
+
+static void analog_polling_thread(void *)
+{
+    while (true) {
+        for (int i = 0; i < GPIO_COUNT; i++) {
+            struct PWMMapping_t *map = &board_config.analog_mappings[i];
+
+            if (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, 4*512);
+    osThreadCreate(osThread(thread_def), NULL);
+}
@@ -51,6 +51,7 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
 void start_general_purpose_adc();
 float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
 void pwm_in_init();
+void start_analog_thread();

 void update_brake_current();

@@ -51,7 +51,7 @@ void save_configuration(void) {
    }
 }

-void load_configuration(void) {
+extern "C" int load_configuration(void) {
    // Try to load configs
    if (NVM_init() ||
        ConfigFormat::safe_load_config(
@@ -81,6 +81,7 @@ void load_configuration(void) {
    } else {
        user_config_loaded_ = true;
    }
+    return user_config_loaded_;
 }

 void erase_configuration(void) {
@@ -107,7 +108,7 @@ void enter_dfu_mode() {

 extern "C" {
 int odrive_main(void);
-void vApplicationStackOverflowHook(void) {
+void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
    for (;;); // TODO: safe action
 }
 void vApplicationIdleHook(void) {
@@ -126,8 +127,6 @@ void vApplicationIdleHook(void) {
 }

 int odrive_main(void) {
-    // Load persistent configuration (or defaults)
-    load_configuration();

 #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
    if (board_config.enable_i2c_instead_of_can) {
@@ -226,6 +225,8 @@ int odrive_main(void) {
        axes[i]->start_thread();
    }

+    start_analog_thread();
+
    system_stats_.fully_booted = true;
    return 0;
 }
@@ -143,14 +143,54 @@ void Motor::set_error(Motor::Error_t error){
    update_brake_current();
 }

+float Motor::get_inverter_temp() {
+    float adc = adc_measurements_[hw_config_.inverter_thermistor_adc_ch];
+    float normalized_voltage = adc / adc_full_scale;
+    return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs);
+}
+
+bool Motor::update_thermal_limits() {
+    float fet_temp = get_inverter_temp();
+    float temp_margin = config_.inverter_temp_limit_upper - fet_temp;
+    float derating_range = config_.inverter_temp_limit_upper - config_.inverter_temp_limit_lower;
+    thermal_current_lim_ = config_.current_lim * (temp_margin / derating_range);
+    if (!(thermal_current_lim_ >= 0.0f)) { //Funny polarity to also catch NaN
+        thermal_current_lim_ = 0.0f;
+    }
+    if (fet_temp > config_.inverter_temp_limit_upper + 5) {
+        set_error(ERROR_INVERTER_OVER_TEMP);
+        return false;
+    }
+    return true;
+}
+
 bool Motor::do_checks() {
    if (!check_DRV_fault()) {
        set_error(ERROR_DRV_FAULT);
        return false;
    }
+    if (!update_thermal_limits()) {
+        //error already set in function
+        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);
+    } else {
+        current_lim = std::min(current_lim, axis_->motor_.current_control_.max_allowed_current);
+    }
+    // Thermal limit
+    current_lim = std::min(current_lim, thermal_current_lim_);
+
+    return current_lim;
+}
+
 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
@@ -236,7 +276,7 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {

    config_.phase_inductance = L;
    // TODO arbitrary values set for now
-    if (L < 1e-6f || L > 2500e-6f)
+    if (L < 2e-6f || L > 4000e-6f)
        return set_error(ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE), false;
    return true;
 }
@@ -282,17 +322,16 @@ bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
    return true;
 }

-// TODO: This doesn't update brake current
 // We should probably make FOC Current call FOC Voltage to avoid duplication.
-bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
-    float c = our_arm_cos_f32(phase);
-    float s = our_arm_sin_f32(phase);
+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 phase) {
+bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase) {
    // Syntactic sugar
    CurrentControl_t& ictrl = current_control_;

@@ -310,11 +349,12 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
    float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);

    // Park transform
-    float c = our_arm_cos_f32(phase);
-    float s = our_arm_sin_f32(phase);
-    float Id = c * Ialpha + s * Ibeta;
-    float Iq = c * Ibeta - s * Ialpha;
-    ictrl.Iq_measured = Iq;
+    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);

    // Current error
    float Ierr_d = Id_des - Id;
@@ -348,8 +388,10 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
    ictrl.Ibus = mod_d * Id + mod_q * Iq;

    // Inverse park transform
-    float mod_alpha = c * mod_d - s * mod_q;
-    float mod_beta  = c * mod_q + s * mod_d;
+    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;
@@ -364,19 +406,22 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
 }


-bool Motor::update(float current_setpoint, float phase) {
+bool Motor::update(float current_setpoint, float phase, float phase_vel) {
    current_setpoint *= config_.direction;
    phase *= config_.direction;
+    phase_vel *= config_.direction;
+
+    float pwm_phase = phase + 1.5f * current_meas_period * phase_vel;

    // Execute current command
    // TODO: move this into the mot
    if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
-        if(!FOC_current(0.0f, current_setpoint, phase)){
+        if(!FOC_current(0.0f, current_setpoint, phase, pwm_phase)){
            return false;
        }
    } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
        //In gimbal motor mode, current is reinterptreted as voltage.
-        if(!FOC_voltage(0.0f, current_setpoint, phase))
+        if(!FOC_voltage(0.0f, current_setpoint, pwm_phase))
            return false;
    } else {
        set_error(ERROR_NOT_IMPLEMENTED_MOTOR_TYPE);
@@ -21,7 +21,8 @@ public:
        ERROR_MODULATION_MAGNITUDE = 0x0080,
        ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100,
        ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200,
-        ERROR_CURRENT_SENSE_SATURATION = 0x0400
+        ERROR_CURRENT_SENSE_SATURATION = 0x0400,
+        ERROR_INVERTER_OVER_TEMP = 0x0800
    };

    enum MotorType_t {
@@ -46,6 +47,8 @@ public:
        float final_v_beta; // [V]
        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]
    };
@@ -60,7 +63,7 @@ public:
        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
-        int32_t direction = 1;                // 1 or -1
+        int32_t direction = 0;                // 1 or -1 (0 = unspecified)
        MotorType_t 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]
@@ -68,6 +71,8 @@ public:
        // 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;
    };

    enum TimingLog_t {
@@ -106,6 +111,9 @@ public:
    bool check_DRV_fault();
    void set_error(Error_t error);
    bool do_checks();
+    float get_inverter_temp();
+    bool update_thermal_limits();
+    float effective_current_lim();
    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);
@@ -113,9 +121,9 @@ public:
    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 phase);
-    bool FOC_current(float Id_des, float Iq_des, float phase);
-    bool update(float current_setpoint, float phase);
+    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_;
@@ -154,11 +162,14 @@ public:
        .final_v_beta = 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,
    };
    DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault;
    DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
+    float thermal_current_lim_ = 10.0f;  //[A]

    // Communication protocol definitions
    auto make_protocol_definitions() {
@@ -171,6 +182,8 @@ public:
            make_protocol_property("DC_calib_phB", &DC_calib_.phB),
            make_protocol_property("DC_calib_phC", &DC_calib_.phC),
            make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_),
+            make_protocol_ro_property("thermal_current_lim", &thermal_current_lim_),
+            make_protocol_function("get_inverter_temp", *this, &Motor::get_inverter_temp),
            make_protocol_object("current_control",
                make_protocol_property("p_gain", &current_control_.p_gain),
                make_protocol_property("i_gain", &current_control_.i_gain),
@@ -181,6 +194,8 @@ public:
                make_protocol_property("final_v_beta", &current_control_.final_v_beta),
                make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
                make_protocol_property("Iq_measured", &current_control_.Iq_measured),
+                make_protocol_property("Id_measured", &current_control_.Id_measured),
+                make_protocol_property("I_measured_report_filter_k", &current_control_.I_measured_report_filter_k),
                make_protocol_ro_property("max_allowed_current", &current_control_.max_allowed_current),
                make_protocol_ro_property("overcurrent_trip_level", &current_control_.overcurrent_trip_level)
            ),
@@ -212,6 +227,8 @@ public:
                make_protocol_property("direction", &config_.direction),
                make_protocol_property("motor_type", &config_.motor_type),
                make_protocol_property("current_lim", &config_.current_lim),
+                make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower),
+                make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper),
                make_protocol_property("requested_current_range", &config_.requested_current_range),
                make_protocol_property("current_control_bandwidth", &config_.current_control_bandwidth,
                    [](void* ctx) { static_cast<Motor*>(ctx)->update_current_controller_gains(); }, this)
@@ -77,11 +77,12 @@ struct BoardConfig_t {
    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.08f * HW_VERSION_VOLTAGE;   //<! [V] maximum voltage above 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.
    PWMMapping_t pwm_mappings[GPIO_COUNT];
+    PWMMapping_t analog_mappings[GPIO_COUNT];
 };
 extern BoardConfig_t board_config;
 extern bool user_config_loaded_;
@@ -14,7 +14,7 @@ public:
        float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)]  { 5.51328895422 / (<pole pairs> * <rpm/v>) }
    };

-    SensorlessEstimator(Config_t& config);
+    explicit SensorlessEstimator(Config_t& config);

    bool update();

@@ -9,13 +9,14 @@ public:
        float decel_limit = 5000.0f; // [count/s^2]
        float A_per_css = 0.0f;      // [A/(count/s^2)]
    };
+    
    struct Step_t {
        float Y;
        float Yd;
        float Ydd;
    };

-    TrapezoidalTrajectory(Config_t& config);
+    explicit TrapezoidalTrajectory(Config_t& config);
    bool planTrapezoidal(float Xf, float Xi, float Vi,
                         float Vmax, float Amax, float Dmax);
    Step_t eval(float t);
@@ -31,6 +31,14 @@ elseif boardversion == "v3.5-48V" then
    boarddir = 'Board/v3'
    FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=5"
    FLAGS += "-DHW_VERSION_VOLTAGE=48"
+elseif boardversion == "v3.6-24V" then
+    boarddir = 'Board/v3'
+    FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=6"
+    FLAGS += "-DHW_VERSION_VOLTAGE=24"
+elseif boardversion == "v3.6-56V" then
+    boarddir = 'Board/v3'
+    FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=6"
+    FLAGS += "-DHW_VERSION_VOLTAGE=56"
 elseif boardversion == "" then
    error("board version not specified - take a look at tup.config.default")
 else
@@ -91,10 +99,6 @@ FLAGS += '-mfpu=fpv4-sp-d16'
 FLAGS += '-mfloat-abi=hard'
 FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'}

-- debug build
-FLAGS += '-g -gdwarf-2'
-
-
 -- linker flags
 LDFLAGS += '-T'..boarddir..'/STM32F405RGTx_FLASH.ld'
 LDFLAGS += '-L'..boarddir..'/Drivers/CMSIS/Lib' -- lib dir
@@ -102,10 +106,15 @@ LDFLAGS += '-lc -lm -lnosys -larm_cortexM4lf_math' -- libs
 LDFLAGS += '-mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float -Wl,--cref -Wl,--gc-sections'
 LDFLAGS += '-Wl,--undefined=uxTopUsedPriority'

+-- debug build
+if tup.getconfig("DEBUG") == "true" then
+    FLAGS += '-g -gdwarf-2'
+    OPT += '-Og'
+else
+    OPT += '-O2'
+end

 -- common flags for ASM, C and C++
-OPT += '-Og'
-- OPT += '-O0'
 OPT += '-ffast-math -fno-finite-math-only'
 tup.append_table(FLAGS, OPT)
 tup.append_table(LDFLAGS, OPT)
@@ -56,14 +56,14 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags)
        compiler_flags += '-fstack-usage'
    end

-    gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
+    local gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
        -- convert include list to flags
        inc_flags = {}
        for _,inc in pairs(includes) do
            inc_flags += "-I"..inc
        end
        -- todo: vary build directory
-        obj_file = builddir.."/"..src:gsub("/","_")..".o"
+        obj_file = builddir.."/obj/"..src:gsub("/","_")..".o"
        outputs.object_files += obj_file
        if gen_su_file then
            su_file = builddir.."/"..src:gsub("/","_")..".su"
@@ -162,7 +162,7 @@ function build(args)

        outputs.includes = {}
        for _,inc in pairs(args.includes) do
-            table.insert(outputs.includes, tup.nodevariable(inc))
+            table.insert(outputs.includes, inc)
        end
        if args.name != nil then
            all_packages[args.name] = outputs
@@ -15,6 +15,7 @@ export CONFIG_STRICT=true
 rm -rdf build
 mkdir -p build
 env | grep ^CONFIG > tup.config
+tup init
 tup generate ./tup_build.sh
 bash -xe ./tup_build.sh

@@ -99,7 +99,28 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
                vel_feed_forward = 0.0f;
            if (numscan < 4)
                current_feed_forward = 0.0f;
-            axes[motor_number]->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward);
+            Axis* axis = axes[motor_number];
+            axis->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward);
+            axis->watchdog_feed();
+        }
+
+    } else if (cmd[0] == 'q') { // position control with limits
+        unsigned motor_number;
+        float pos_setpoint, vel_limit, current_lim;
+        int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &current_lim);
+        if (numscan < 2) {
+            respond(response_channel, use_checksum, "invalid command format");
+        } else if (motor_number >= AXIS_COUNT) {
+            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
+        } else {
+            Axis* axis = axes[motor_number];
+            axis->controller_.pos_setpoint_ = pos_setpoint;
+            if (numscan >= 3)
+                axis->controller_.config_.vel_limit = vel_limit;
+            if (numscan >= 4)
+                axis->motor_.config_.current_lim = current_lim;
+
+            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'v') { // velocity control
@@ -113,7 +134,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
        } else {
            if (numscan < 3)
                current_feed_forward = 0.0f;
-            axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward);
+            Axis* axis = axes[motor_number];
+            axis->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward);
+            axis->watchdog_feed();
        }

    } else if (cmd[0] == 'c') { // current control
@@ -125,7 +148,9 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
-            axes[motor_number]->controller_.set_current_setpoint(current_setpoint);
+            Axis* axis = axes[motor_number];
+            axis->controller_.set_current_setpoint(current_setpoint);
+            axis->watchdog_feed();
        }

    } else if (cmd[0] == 't') { // trapezoidal trajectory
@@ -137,13 +162,29 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
        } else if (motor_number >= AXIS_COUNT) {
            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
        } else {
-            axes[motor_number]->controller_.move_to_pos(goal_point);
+            Axis* axis = axes[motor_number];
+            axis->controller_.move_to_pos(goal_point);
+            axis->watchdog_feed();
+        }
+
+    } else if (cmd[0] == 'f') { // feedback
+        unsigned motor_number;
+        int numscan = sscanf(cmd, "f %u", &motor_number);
+        if (numscan < 1) {
+            respond(response_channel, use_checksum, "invalid command format");
+        } else if (motor_number >= AXIS_COUNT) {
+            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
+        } else {
+            respond(response_channel, use_checksum, "%f %f",
+                    (double)axes[motor_number]->encoder_.pos_estimate_,
+                    (double)axes[motor_number]->encoder_.vel_estimate_);
        }

    } else if (cmd[0] == 'h') {  // Help
        respond(response_channel, use_checksum, "Please see documentation for more details");
        respond(response_channel, use_checksum, "");
        respond(response_channel, use_checksum, "Available commands syntax reference:");
+        respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim");
        respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff");
        respond(response_channel, use_checksum, "Velocity: v axis vel I-ff");
        respond(response_channel, use_checksum, "Current: c axis I");
@@ -151,6 +192,10 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
        respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state");
        respond(response_channel, use_checksum, "Read: r property");
        respond(response_channel, use_checksum, "Write: w property value");
+        respond(response_channel, use_checksum, "");
+        respond(response_channel, use_checksum, "Save config: ss");
+        respond(response_channel, use_checksum, "Erase config: se");
+        respond(response_channel, use_checksum, "Reboot: sr");

    } else if (cmd[0] == 'i'){ // Dump device info
        // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature());
@@ -160,6 +205,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
        respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION);
        respond(response_channel, use_checksum, "Serial number: %s", serial_number_str);

+    } else if (cmd[0] == 's'){ // System
+        if(cmd[1] == 's') { // Save config
+            save_configuration();
+        } else if (cmd[1] == 'e'){ // Erase config
+            erase_configuration();
+        } else if (cmd[1] == 'b'){ // Reboot
+            NVIC_SystemReset();
+        }
+
    } else if (cmd[0] == 'r') { // read property
        char name[MAX_LINE_LENGTH];
        int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name);
@@ -196,6 +250,17 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
            }
        }

+    }else if (cmd[0] == 'u') { // Update axis watchdog. 
+        unsigned motor_number;
+        int numscan = sscanf(cmd, "u %u", &motor_number);
+        if(numscan < 1){
+            respond(response_channel, use_checksum, "invalid command format");
+        } else if (motor_number >= AXIS_COUNT) {
+            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
+        }else {
+            axes[motor_number]->watchdog_feed();
+        }
+
    } else if (cmd[0] != 0) {
        respond(response_channel, use_checksum, "unknown command");
    }
@@ -84,7 +84,7 @@ void init_communication(void) {
    printf("hi!\r\n");

    // Start command handling thread
-    osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 6000 /* in 32-bit words */); // TODO: fix stack issues
+    osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 8000 /* in 32-bit words */); // TODO: fix stack issues
    comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL);

    while (!endpoint_list_valid)
@@ -163,8 +163,11 @@ static inline auto make_obj_tree() {
            make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])),
            make_protocol_object("gpio3_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[2])),
 #endif
-            make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3]))
-        ),
+            make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3])),
+
+            make_protocol_object("gpio3_analog_mapping", make_protocol_definitions(board_config.analog_mappings[2])),
+            make_protocol_object("gpio4_analog_mapping", make_protocol_definitions(board_config.analog_mappings[3]))
+            ),
        make_protocol_object("axis0", axes[0]->make_protocol_definitions()),
        make_protocol_object("axis1", axes[1]->make_protocol_definitions()),
        make_protocol_object("can", can1_ctx.make_protocol_definitions()),
@@ -177,6 +177,6 @@ void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) {

 void start_usb_server() {
    // Start USB communication thread
-    osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512);
+    osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 1024);
    usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL);
 }
@@ -82,61 +82,19 @@ typedef struct {
    uint16_t endpoint_id;
 } endpoint_ref_t;

+#include <cstring>

 template<typename T, typename = typename std::enable_if_t<!std::is_const<T>::value>>
-inline size_t write_le(T value, uint8_t* buffer);
+inline size_t write_le(T value, uint8_t* buffer){
+    //TODO: add static_assert that this is still a little endian machine
+    std::memcpy(&buffer[0], &value, sizeof(value));
+    return sizeof(value);
+}

 template<typename T>
-inline size_t read_le(T* value, const uint8_t* buffer);
-
-template<>
-inline size_t write_le<bool>(bool value, uint8_t* buffer) {
-    buffer[0] = value ? 1 : 0;
-    return 1;
-}
-
-template<>
-inline size_t write_le<uint8_t>(uint8_t value, uint8_t* buffer) {
-    buffer[0] = value;
-    return 1;
-}
-
-template<>
-inline size_t write_le<uint16_t>(uint16_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    return 2;
-}
-
-template<>
-inline size_t write_le<uint32_t>(uint32_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    buffer[2] = (value >> 16) & 0xff;
-    buffer[3] = (value >> 24) & 0xff;
-    return 4;
-}
-
-template<>
-inline size_t write_le<int32_t>(int32_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    buffer[2] = (value >> 16) & 0xff;
-    buffer[3] = (value >> 24) & 0xff;
-    return 4;
-}
-
-template<>
-inline size_t write_le<uint64_t>(uint64_t value, uint8_t* buffer) {
-    buffer[0] = (value >> 0) & 0xff;
-    buffer[1] = (value >> 8) & 0xff;
-    buffer[2] = (value >> 16) & 0xff;
-    buffer[3] = (value >> 24) & 0xff;
-    buffer[4] = (value >> 32) & 0xff;
-    buffer[5] = (value >> 40) & 0xff;
-    buffer[6] = (value >> 48) & 0xff;
-    buffer[7] = (value >> 56) & 0xff;
-    return 8;
+typename std::enable_if_t<std::is_const<T>::value, size_t>
+write_le(T value, uint8_t* buffer) {
+    return write_le<std::remove_const_t<T>>(value, buffer);
 }

 template<>
@@ -148,65 +106,17 @@ inline size_t write_le<float>(float value, uint8_t* buffer) {
 }

 template<typename T>
-typename std::enable_if_t<std::is_const<T>::value, size_t>
-write_le(T value, uint8_t* buffer) {
-    return write_le<std::remove_const_t<T>>(value, buffer);
-}
-
-template<>
-inline size_t read_le<bool>(bool* value, const uint8_t* buffer) {
-    *value = buffer[0];
-    return 1;
-}
-
-template<>
-inline size_t read_le<uint8_t>(uint8_t* value, const uint8_t* buffer) {
-    *value = buffer[0];
-    return 1;
-}
-
-template<>
-inline size_t read_le<uint16_t>(uint16_t* value, const uint8_t* buffer) {
-    *value = (static_cast<uint16_t>(buffer[0]) << 0) |
-             (static_cast<uint16_t>(buffer[1]) << 8);
-    return 2;
-}
-
-template<>
-inline size_t read_le<int32_t>(int32_t* value, const uint8_t* buffer) {
-    *value = (static_cast<int32_t>(buffer[0]) << 0) |
-             (static_cast<int32_t>(buffer[1]) << 8) |
-             (static_cast<int32_t>(buffer[2]) << 16) |
-             (static_cast<int32_t>(buffer[3]) << 24);
-    return 4;
-}
-
-template<>
-inline size_t read_le<uint32_t>(uint32_t* value, const uint8_t* buffer) {
-    *value = (static_cast<uint32_t>(buffer[0]) << 0) |
-             (static_cast<uint32_t>(buffer[1]) << 8) |
-             (static_cast<uint32_t>(buffer[2]) << 16) |
-             (static_cast<uint32_t>(buffer[3]) << 24);
-    return 4;
-}
-
-template<>
-inline size_t read_le<uint64_t>(uint64_t* value, const uint8_t* buffer) {
-    *value = (static_cast<uint64_t>(buffer[0]) << 0) |
-             (static_cast<uint64_t>(buffer[1]) << 8) |
-             (static_cast<uint64_t>(buffer[2]) << 16) |
-             (static_cast<uint64_t>(buffer[3]) << 24) |
-             (static_cast<uint64_t>(buffer[4]) << 32) |
-             (static_cast<uint64_t>(buffer[5]) << 40) |
-             (static_cast<uint64_t>(buffer[6]) << 48) |
-             (static_cast<uint64_t>(buffer[7]) << 56);
-    return 8;
+inline size_t read_le(T* value, const uint8_t* buffer){
+    // TODO: add static_assert that this is still a little endian machine
+    std::memcpy(value, buffer, sizeof(*value));
+    return sizeof(*value);
 }

 template<>
 inline size_t read_le<float>(float* value, const uint8_t* buffer) {
    static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected");
    static_assert(std::numeric_limits<float>::is_iec559, "IEEE 754 floating point expected");
+
    return read_le(reinterpret_cast<uint32_t*>(value), buffer);
 }

@@ -499,6 +409,14 @@ inline constexpr const char* get_default_json_modifier<float>() {
    return "\"type\":\"float\",\"access\":\"rw\"";
 }
 template<>
+inline constexpr const char* get_default_json_modifier<const int64_t>() {
+    return "\"type\":\"int64\",\"access\":\"r\"";
+}
+template<>
+inline constexpr const char* get_default_json_modifier<int64_t>() {
+    return "\"type\":\"int64\",\"access\":\"rw\"";
+}
+template<>
 inline constexpr const char* get_default_json_modifier<const uint64_t>() {
    return "\"type\":\"uint64\",\"access\":\"r\"";
 }
@@ -5,7 +5,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags)
    -- add some default compiler flags
    compiler_flags += '-fstack-usage'

-    gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
+    local gcc_generic_compiler = function(compiler, compiler_flags, gen_su_file, src, flags, includes, outputs)
        -- resolve source path
        src = tostring(src)

@@ -1,9 +1,11 @@
 #!/usr/bin/python2

 # run openocd (0.9.0) with :
-# $ openocd -f stlink-v2-1.cfg -f stm32f4x.cfg &> /dev/null"
+# $ openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg &> /dev/null &
 # then run
 # $ python2 sampler.py path_to_myelf_with_symbols
+# ctrl-c to stop sampling.
+# To terminate the openocd session, enter command "fg" then do ctrl-c.

 import sys
 import time
@@ -111,10 +113,10 @@ if __name__ == '__main__':

            cur = time.time()
            if cur - start > 1.0:
-                tmp = sorted(countmap.items(), key=operator.itemgetter(1), reverse=True)
+                tmp = sorted(countmap.items(), key=operator.itemgetter(1)) #, reverse=True)
                for k, v in tmp:
-                    # print('{:05.2f}% {}'.format((v * 100.) / total, k))
-                    print('{:06.2f} clocks : {}'.format((v * 8192) / total, k))
+                    print('{:05.2f}% {}'.format((v * 100.) / total, k))
+                    # print('{:06.2f} clocks : {}'.format((v * 10500) / total, k))
                start = cur
                print('{} Samples'.format(total))
                print('')
@@ -3,6 +3,7 @@
 #CONFIG_BOARD_VERSION=v3.5-24V
 CONFIG_USB_PROTOCOL=native
 CONFIG_UART_PROTOCOL=ascii
+CONFIG_DEBUG=false

 # Uncomment this to error on compilation warnings
 #CONFIG_STRICT=true
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2016-2018 ODrive Robotics
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
@@ -11,6 +11,16 @@
 		}
 	],
 	"settings": {
+
+		"c-cpp-flylint.cppcheck.includePaths": [
+			"${workspaceRoot}",
+			"${workspaceRoot}/fibre/cpp/include/fibre",
+			"${workspaceRoot}/communication",
+			"${workspaceRoot}/MotorControl",
+		],
+		"c-cpp-flylint.cppcheck.platform": "avr8",
+		"c-cpp-flylint.cppcheck.standard": ["c99","c++14"],
+
 		"files.associations": {
 			"memory": "cpp",
 			"utility": "cpp",
@@ -70,7 +70,7 @@ GEM
      listen (= 3.1.5)
      mercenary (~> 0.3)
      minima (= 2.4.0)
-      nokogiri (>= 1.8.1, < 2.0)
+      nokogiri (>= 1.8.5, < 2.0)
      rouge (= 2.2.1)
      terminal-table (~> 1.4)
    github-pages-health-check (1.4.0)
@@ -81,7 +81,7 @@ GEM
      typhoeus (~> 1.3)
    html-pipeline (2.7.1)
      activesupport (>= 2)
-      nokogiri (>= 1.4)
+      nokogiri (>= 1.8.5)
    http_parser.rb (0.6.0)
    i18n (0.9.5)
      concurrent-ruby (~> 1.0)
@@ -207,7 +207,7 @@ GEM
    minitest (5.11.3)
    multipart-post (2.0.0)
    net-dns (0.8.0)
-    nokogiri (1.8.2)
+    nokogiri (>= 1.8.5)
      mini_portile2 (~> 2.3.0)
    octokit (4.8.0)
      sawyer (~> 0.8.0, >= 0.5.3)
@@ -7,7 +7,7 @@
    * **Windows:** Use the Zadig utility to set the ODrive's driver to "usbser". Windows will then make the device available as COM port. You can use [PuTTY](https://www.chiark.greenend.org.uk/~sgtatham/putty/) to manually send commands or  open the COM port using your favorite programming language 
    * **Linux/macOS:** Run `/dev/tty*` to list all serial ports. The ODrive will show up as `/dev/ttyACM0` on Linux and `/dev/tty.usbmodem[...]` on macOS. Once you know the name, you can use `screen /dev/ttyACM0` (with the correct name) to send commands manually or open the device using your favorite programming language. Serial ports on Unix can be opened, written to and read from like a normal file.
 * **Via UART:** Connect the ODrive's TX (GPIO1) to your host's RX. Connect your ODrive's RX (GPIO2) to your host's TX. The logic level of the ODrive is 3.3V.
-    * **Arduino:** You can use the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) to talk to the ODrive.
+    * **Arduino:** You can use the [ODrive Arduino library](https://github.com/madcowswe/ODrive/tree/master/Arduino/ODriveArduino) to talk to the ODrive.
    * **Windows/Linux/macOS:** You can use an FTDI USB-UART cable to connect to the ODrive.

 ## Command format
@@ -36,7 +36,23 @@ Example: `t 0 -20000`

 For general moving around of the axis, this is the recommended command.

+This command updates the watchdog timer for the motor. 
+
 #### Motor Position command
+For basic use where you send one setpoint at at a time, use the `q` command.
+If you have a realtime controller that is streaming setpoints and tracking a trajectory, use the `p` command.
+
+```
+q motor position velocity_lim current_lim
+```
+* `q` for position
+* `motor` is the motor number, `0` or `1`.
+* `position` is the desired position, in encoder counts.
+* `velocity_lim` is the velocity limit, in counts/s (optional).
+* `current_lim` is the current limit, in A (optional).
+
+Example: `q 0 -20000 10000 10`
+
 ```
 p motor position velocity_ff current_ff
 ```
@@ -50,6 +66,7 @@ Example: `p 0 -20000 0 0`

 Note that if you don't know what feed-forward is or what it's used for, simply omit it.

+This command updates the watchdog timer for the motor. 

 #### Motor Velocity command
 ```
@@ -64,6 +81,8 @@ Example: `v 0 1000 0`

 Note that if you don't know what feed-forward is or what it's used for, simply omit it.

+This command updates the watchdog timer for the motor. 
+
 #### Motor Current command
 ```
 c motor current
@@ -72,6 +91,29 @@ c motor current
 * `motor` is the motor number, `0` or `1`.
 * `current` is the desired current in A.

+This command updates the watchdog timer for the motor. 
+
+#### Request feedback
+```
+f motor
+
+response:
+pos vel
+```
+* `f` for feedback
+* `pos` is the encoder position in counts (float)
+* `vel` is the encoder velocity in counts/s (float)
+
+#### Update motor watchdog
+```
+u motor
+```
+* `u` for /u/pdate.
+* `motor` is the motor number, `0` or `1`.
+
+This command updates the watchdog timer for the motor, without changing any
+setpoints. 
+
 #### Parameter reading/writing

 Not all parameters can be accessed via the ASCII protocol but at least all parameters with float and integer type are supported.
@@ -90,3 +132,8 @@ Not all parameters can be accessed via the ASCII protocol but at least all param
   * `property` name of the property, as seen in ODrive Tool
   * `value` text representation of the value to be written
   * Example: `w axis0.controller.pos_setpoint -123.456`
+
+#### System commands:
+* `ss` - Save config
+* `se` - Erase config
+* `sr` - Reboot
@@ -79,7 +79,7 @@ An upcoming feature will enable automatic tuning. Until then, here is a rough tu
 * Back down `vel_gain` to 50% of the vibrating value.
 * Increase `pos_gain` by around 30% per iteration until you see some overshoot.
 * Back down `pos_gain` until you do not have overshoot anymore.
-* The integrator is not easily tuned, nor is it strictly required. Tune at your own discretion.
+* The integrator can be set to `0.5 * bandwidth * vel_gain`, where `bandwidth` is the overall resulting tracking bandwidth of your system. Say your tuning made it track commands with a settling time of 100ms: this means the bandwidth was 1/100ms or 10. In this case you should set the `vel_integrator_gain = 0.5 * 10 * vel_gain`.

 ## System monitoring commands

@@ -5,7 +5,7 @@ As such it assumes that you know things like how to use Git, what a compiler is,

 The official releases are maintained on the `master` branch. However since you are a developer, you are encouraged to use the `devel` branch, as it contains the latest features.

-The project is under active development, so make sure to check the [Changelog](CHANGELOG.md) to keep track of updates.
+The project is under active development, so make sure to check the [Changelog](../CHANGELOG.md) to keep track of updates.

 ### Table of contents

@@ -70,7 +70,8 @@ sudo pacman -S tup
 #### Mac
 First install [Homebrew](https://brew.sh/). Then you can run these commands in Terminal:
 ```bash
-brew cask install gcc-arm-embedded
+brew tap osx-cross/arm
+brew install arm-gcc-bin
 brew cask install osxfuse && brew install tup
 brew install openocd
 ```
@@ -80,7 +81,9 @@ __Note__: make sure these programs are not only installed but also added to your

 Some instructions in this document may assume that you're using a bash command prompt, such as the Windows 10 built-in bash or [Git](https://git-scm.com/download/win) bash.

-* [ARM compiler](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads)  __Note__: After installing, create an environment variable named `ARM_GCC_ROOT` whose value is the path you installed to.  e.g. `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`.  This variable is used to locate include files for the c/c++ Visual Studio Code extension.
+* [ARM compiler](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads)  
+  * __Note 1__: After installing, create an environment variable named `ARM_GCC_ROOT` whose value is the path you installed to.  e.g. `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`.  This variable is used to locate include files for the c/c++ Visual Studio Code extension.
+  * __Note 2__: 8-2018-q4-major seems to have a bug on Windows.  Please use 7-2018-q2-update.
 * [Tup](http://gittup.org/tup/index.html)
 * [Make for Windows](http://gnuwin32.sourceforge.net/packages/make.htm)
 * [OpenOCD](http://gnuarmeclipse.github.io/openocd/install/). Also follow the instructions on the ST-LINK/V2 drivers.
@@ -40,7 +40,7 @@ permalink: /

  </div></details>

-* A power supply (12V-24V for the 24V board variant, 12V-48V for the 48V board variant). A battery is also fine.
+* A power supply (12V-24V for the 24V board variant, 12V-48V for the 48V board variant). A battery is also fine. Some advice on choosing a power supply can be found [here](https://things-in-motion.blogspot.com/2018/12/how-to-select-right-power-source-for.html).
  <details><summary markdown="span">What voltage variant do I have?</summary><div markdown="block">
  On all ODrives shipped July 2018 or after have a silkscreen label clearly indicating the voltage variant.

@@ -56,14 +56,19 @@ All non-power I/O is 3.3V output and 5V tolerant on input, on ODrive v3.3 and ne

 ### Wiring up the motors
 * Connect the motor phases into the 3-phase screw terminals. It is not recommended to use a clip-on connector such as an alligator clip, as this can cause issues with the phase resistance/inductance measurements. 
-* Connect the power source to the DC terminals. Make sure to pay attention to the polarity.
-* Do not apply power just yet.

 ### Wiring up the encoders
 Connect the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 3.3k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups.

 ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281)

+### Safety & Power UP
+<div class="alert">
+ Always think safety before powering up the ODrive if motors are attached. Consider what might happen if the motor spins as soon as power is applied.
+</div>
+* Unlike some devices, the ODrive does not recieve power over the USB port so the 24/48 volt power input is required even just to communicate with it using USB. It is ok to power up the ODrive before or after connecting the USB cable.
+* To power up the ODrive, connect the power source to the DC terminals. Make sure to pay attention to the polarity. A small spark is normal. This is caused by the capacitors charging up.
+
 ## Downloading and Installing Tools
 Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first.

@@ -189,6 +194,7 @@ This is the resistance of the brake resistor. If you are not using it, you may s
 `odrv0.axis0.motor.config.pole_pairs`  
 This is the number of **magnet poles** in the rotor, **divided by two**. To find this, you can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._
 If you can't see them, try sliding a magnet around the rotor, and counting how many times it stops. This will be the number of **pole pairs**. If you use a magnetic piece of metal instead of a magnet, you will get the number of **magnet poles**.
+
 `odrv0.axis0.motor.config.motor_type`  
 This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and gimbal motors (`MOTOR_TYPE_GIMBAL`).
 <details><summary markdown="span">Which <code>motor_type</code> to choose?</summary><div markdown="block">
@@ -255,6 +261,7 @@ You can also directly control the current of the motor, which is proportional to


 ### Trajectory control
+While in position control mode, use the `move_to_pos` or `move_incremental` functions. See the **Usage** section for details<br>
 This mode lets you smoothly accelerate, coast, and decelerate the axis from one position to another. With raw position control, the controller simply tries to go to the setpoint as quickly as possible. Using a trajectory lets you tune the feedback gains more aggressively to reject disturbance, while keeping smooth motion.

 ![Taptraj](TrapTrajPosVel.PNG)<br>
@@ -284,10 +291,22 @@ Keep in mind that you must still set your safety limits as before.  I recommend
 #### Usage
 Use the `move_to_pos` function to move to an absolute position:
 ```
-<odrv>.<axis>.controller.move_to_pos(<Float>)
+<odrv>.<axis>.controller.move_to_pos(your_absolute_pos)
 ```

+Use the `move_incremental` function to move to a relative position.
+To set the goal relative to the current actual position, use `from_goal_point = False`
+To set the goal relative to the previous destination, use `from_goal_point = True`
+```
+<odrv>.<axis>.controller.move_incremental(pos_increment, from_goal_point)
+```
+
+You can also execute a move with the [appropriate ascii command](ascii-protocol.md#motor-trajectory-command).
+
 ### Circular position control
+
+To enable Circular position control, set `axis.controller.config.setpoints_in_cpr = True`
+
 This mode is useful for continuos incremental position movement. For example a robot rolling indefinitely, or an extruder motor or conveyor belt moving with controlled increments indefinitely.
 In the regular position mode, the `pos_setpoint` would grow to a very large value and would lose precision due to floating point rounding.

@@ -312,6 +331,18 @@ You can now control the current with `axis.controller.current_setpoint = 3` [A].

 *Note: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder.*

+
+## Watchdog Timer
+Each axis has a configurable watchdog timer that can stop the motors if the
+control connection to the ODrive is interrupted.
+
+Each axis has a configurable watchdog timeout: `axis.config.watchdog_timeout`,
+measured in seconds. A value of `0` disables the watchdog functionality. Any value
+`> 0` will stop the motors if the watchdog has not been fed in the configured
+time interval. 
+
+The watchdog is fed using the `axis.watchdog_feed()` method of each axis. 
+
 ## What's next?
 You can now:
 * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance.
@@ -18,6 +18,8 @@ You may wire the motor phases in any order into a motor connector on the ODrive,
 | Green     | Z         |
 | Black     | GND       |

+Note: In order to ber compatible with encoder inputs, the ODrive doesn't have any filtering capacitors on the pins where the hall sensors connect. Therefore to get a reliable hall signal, it is recommended that you add some filter capacitors to these pins. You can see instructions [here](https://discourse.odriverobotics.com/t/encoder-error-error-illegal-hall-state/1047/7?u=madcowswe).
+

 ### Hoverboard motor configuration
 Standard 6.5 inch hoverboard hub motors have 30 permanent magnet poles, and thus 15 pole pairs. If you have a different motor you need to count the magnets or have a reliable datasheet for this information.
@@ -118,7 +120,7 @@ odrv0.axis0.requested_state = AXIS_STATE_IDLE
 Hopefully you got your motor to spin! Feel free to repeat all of the above for the other axis if appropriate.

 ### PWM input
-If you want to drive your hoverboard wheels around with an RC remote contro you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link.
+If you want to drive your hoverboard wheels around with an RC remote control you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link.
 Lets use GPIO 3/4 for the velocity inputs so that we don't have to disable UART.
 Then let's map the full stick range of these inputs to some suitable velocity setpoint range.
 We also have to reboot to activate the PWM input.
@@ -159,6 +161,9 @@ odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
 odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL
 ```

+### Safety
+Be sure to setup the Failsafe feature on your RC Receiver so that if connection is lost between the remote and the receiver, the receiver outputs 0 and 0 for the velocity setpoint of both axes (or whatever is safest for your configuration). Also note that if the receiver turns off (loss of power, etc) or if the signal from the receiver to the ODrive is lost (wire comes unplugged, etc), the ODrive will continue the last commanded velocity setpoint. There is currently no timeout function in the ODrive for PWM inputs.
+
 ### Automatic startup
 Try to reboot and then activate AXIS_STATE_CLOSED_LOOP_CONTROL on both axis. Check that everything is operational and works as expected.
 If so, you can now make the ODrive turn on the motor power automatically after booting. This is useful if you are going to be running the ODrive without a PC or other logic board.
@@ -33,6 +33,7 @@ The ODrive can be controlled over various ports and protocols. If you're comfort
 (*) ODrive v3.5 and later

 Notes:
+* You must also connect GND between ODrive and your other board.
 * ODrive v3.3 and onward have 5V tolerant GPIO pins.
 * ODrive v3.5 and later have some noise supression filters on the default step/dir pins
 * You can change the step/dir pins using `axis.config.<step/dir>_gpio_pin`.
@@ -127,6 +128,8 @@ As an example, we'll configure GPIO4 to control the angle of axis 0. We want the
    ```
 5. With the ODrive powered off, connect the RC receiver ground to the ODrive's GND and one of the RC receiver signals to GPIO4. You may try to power the receiver from the ODrive's 5V supply if it doesn't draw too much power. Power up the the RC transmitter. You should now be able to control axis 0 from one of the RC sticks.

+Be sure to setup the Failsafe feature on your RC Receiver so that if connection is lost between the remote and the receiver, the receiver outputs 0 for the velocity setpoint of both axes (or whatever is safest for your configuration). Also note that if the receiver turns off (loss of power, etc) or if the signal from the receiver to the ODrive is lost (wire comes unplugged, etc), the ODrive will continue the last commanded velocity setpoint. There is currently no timeout function in the ODrive for PWM inputs.
+
 ## Ports
 Note: when you use an existing library you don't have to deal with the specifics described in this section.

@@ -116,6 +116,7 @@ You can use the DfuSe app from ST.
 1. Force the ODrive into DFU mode, as per the instructions above "How to force DFU mode".
 1. In the top left it should now be connected to "STM Device in DFU Mode".
   1. If it doesn't appear, it may be because the driver is set to libusb by Zadig. We need to set it back to the original driver. Follow [these instructions](https://github.com/pbatard/libwdi/wiki/FAQ#Help_Zadig_replaced_the_driver_for_the_wrong_device_How_do_I_restore_it).
+   2. If, after doing the above step, the ODrive still installs itself as a libusb device in Device Manager, you can try to delete the libusb driver (this is OK, since we can use Zadig to install it again). You can simply delete the file `C:\Windows\System32\drivers\libusb0.sys`.
 1. In the bottom right section called "Upgrade or Verify Action" click the button "Choose...".
 1. Locate the `ODriveFirmware.dfu` we made before.
 1. Click button "Upgrade".
@@ -135,9 +136,40 @@ In the Firmware directory, after finishing building the firmware:
 sudo dfu-util -a 0 -s 0x08000000 -D build/ODriveFirmware.bin
 ```

-#### MacOS
-**This section needs more detail. Please consider adding detail if you got it to work.**
-You may be able to use [dfu-util](http://dfu-util.sourceforge.net/) to upgrade the firmware. The command should be similar to the Linux instructions.
+#### macOS
+
+First, you need to install the arm development tools to copy the binary into the appropriate format.
+
+```text
+$ brew cask install gcc-arm-embedded
+```
+
+Then convert the binary to .bin format
+
+```text
+$ arm-none-eabi-objcopy -O binary ODriveFirmware_v3.5-48V.elf ODriveFirmware_v3.5-48V.bin
+```
+
+Install `dfu-util`:
+
+```text
+$ sudo port install dfu-util   # via MacPorts; for HomeBrew use "brew install dfu-util"
+```
+
+Find the correct device serial number to use:
+
+```text
+$ dfu-util --list              # list the DFU capable devices
+[...]
+Found DFU: [0483:df11] ver=2200, devnum=5, cfg=1, intf=0, path="20-2", alt=0, 
+ name="@Internal Flash  /0x08000000/04*016Kg,01*064Kg,07*128Kg", serial="388237123123"
+```
+
+Finally, flash the firmware using the found serial number:
+
+```text
+$ sudo dfu-util -S 388237123123 -a 0 -s 0x08000000 -D ODriveFirmware_v3.5-48V.bin
+```

 ## Flashing with an STLink

@@ -20,6 +20,7 @@ The following sections will give some guidance on the most common errors. You ma
 * Axis error flags defined [here](../Firmware/MotorControl/axis.hpp).
 * Motor error flags defined [here](../Firmware/MotorControl/motor.hpp).
 * Encoder error flags defined [here](../Firmware/MotorControl/encoder.hpp).
+* Controller error flags defined [here](../Firmware/MotorControl/controller.hpp).
 * Sensorless estimator error flags defined [here](../Firmware/MotorControl/sensorless_estimator.hpp).

 ## Common Axis Errors 
@@ -36,9 +37,9 @@ You can monitor your PUS voltage using liveplotter in odrive tool by entering `s

 * `ERROR_DC_BUS_OVER_VOLTAGE = 0x04`

-Confirm that you have a break resistor of the correct value connected securly and that `odrv0.config.brake_resistance` is set to the value of your break resistor. 
+Confirm that you have a brake resistor of the correct value connected securly and that `odrv0.config.brake_resistance` is set to the value of your brake resistor. 

-You can monitor your PUS voltage using liveplotter in odrive tool by entering `start_liveplotter(lambda: [odrv0.vbus_voltage])`. If during a move you see the voltage rise above your PSU's nominal set voltage then you have your break resistance set too low. This may happen if you are using long wires or small gauge wires to connect your break resistor to your odrive which will added extra resistance. This extra resistance needs to be accounted for to prevent this voltage spike. If you have checked all your connections you can also try increasing your break resistance by ~ 0.01 Ohm at a time to a maximum of 0.05 greater than your break resistor value.
+You can monitor your PUS voltage using liveplotter in odrive tool by entering `start_liveplotter(lambda: [odrv0.vbus_voltage])`. If during a move you see the voltage rise above your PSU's nominal set voltage then you have your brake resistance set too low. This may happen if you are using long wires or small gauge wires to connect your brake resistor to your odrive which will added extra resistance. This extra resistance needs to be accounted for to prevent this voltage spike. If you have checked all your connections you can also try increasing your brake resistance by ~ 0.01 Ohm at a time to a maximum of 0.05 greater than your brake resistor value.

 ## Common Motor Errors 

@@ -58,7 +59,8 @@ Some motors will have a considerably different phase resistance and inductance t

 In general, you need
 ```text
-resistance_calib_max_voltage > calibration_current * phase_resistance`.
+resistance_calib_max_voltage > calibration_current * phase_resistance
+resistance_calib_max_voltage < 0.5 * vbus_voltage
 ```

 * `ERROR_DRV_FAULT = 0x0008`
@@ -72,6 +74,12 @@ power supply of the DRV8301 gate driver chips, thus tripping its under-voltage f

 To resolve this issue you can limit the M0 current to 40A. The lowest current at which the DRV fault was observed is 45A on one test motor and 50A on another test motor. Refer to [this post](https://discourse.odriverobotics.com/t/drv-fault-on-odrive-v3-4/558) for instructions for a hardware fix.

+* `ERROR_MODULATION_MAGNITUDE = 0x0080`
+
+The bus voltage was insufficent to push the requested current through the motor. Reduce `motor.config.calibration_current` and/or `motor.config.current_lim`, for errors at calibration-time and closed loop control respectively.
+
+For gimbal motors, it is recommended to set the calibration_current and current_lim to half your bus voltage, or less.
+
 ## Common Encoder Errors

 * `ERROR_CPR_OUT_OF_RANGE = 0x02`
@@ -10,6 +10,8 @@ AXIS_STATE_SENSORLESS_CONTROL = 5
 AXIS_STATE_ENCODER_INDEX_SEARCH = 6
 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
 AXIS_STATE_CLOSED_LOOP_CONTROL = 8
+AXIS_STATE_LOCKIN_SPIN = 9
+AXIS_STATE_ENCODER_DIR_FIND = 10
 AXIS_STATE_HOMING = 9

 class errors:
@@ -26,6 +28,7 @@ class errors:
        ERROR_ENCODER_FAILED = 0x100 # Go to encoder.hpp for information, check odrvX.axisX.encoder.error for error value
        ERROR_CONTROLLER_FAILED = 0x200
        ERROR_POS_CTRL_DURING_SENSORLESS = 0x400
+        ERROR_WATCHDOG_TIMER_EXPIRED = 0x800

    class motor:
        ERROR_NONE = 0
@@ -62,6 +65,7 @@ CTRL_MODE_VOLTAGE_CONTROL = 0
 CTRL_MODE_CURRENT_CONTROL = 1
 CTRL_MODE_VELOCITY_CONTROL = 2
 CTRL_MODE_POSITION_CONTROL = 3
+CTRL_MODE_TRAJECTORY_CONTROL = 4

 ENCODER_MODE_INCREMENTAL = 0
 ENCODER_MODE_HALL = 1
@@ -32,9 +32,10 @@ class OperationAbortedException(Exception):
    pass

 def dump_errors(odrv, clear=False):
-    axes = [axis for name, axis in odrv._remote_attributes.items() if 'axis' in name]
-    for num, axis in enumerate(axes):
-        print('Axis{}:'.format(num))
+    axes = [(name, axis) for name, axis in odrv._remote_attributes.items() if 'axis' in name]
+    axes.sort()
+    for name, axis in axes:
+        print(name)

        # Flatten axis and submodules
        # (name, remote_obj, errorcode)
@@ -59,7 +60,7 @@ def dump_errors(odrv, clear=False):
            else:
                print(prefix + _VT100Colors['green'] + "no error" + _VT100Colors['default'])

-data_rate = 100
+data_rate = 10
 plot_rate = 10
 num_samples = 1000
 def start_liveplotter(get_var_callback):
@@ -106,6 +107,7 @@ def start_liveplotter(get_var_callback):
        while not cancellation_token.is_set():
            plt.clf()
            plt.plot(vals)
+            plt.legend(list(range(len(vals))))
            fig.canvas.draw()
            fig.canvas.start_event_loop(1/plot_rate)

@@ -190,17 +192,6 @@ def usb_burn_in_test(get_var_callback, cancellation_token):
                print("read {} values".format(i))
    threading.Thread(target=fetch_data, daemon=True).start()

-def setup_udev_rules(logger):
-    if platform.system() != 'Linux':
-        logger.error("This command only makes sense on Linux")
-    if os.getuid() != 0:
-        logger.warn("you should run this as root, otherwise it will probably not work")
-    with open('/etc/udev/rules.d/91-odrive.rules', 'w') as file:
-        file.write('SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d3[0-9]", MODE="0666"\n')
-    subprocess.check_call(["udevadm", "control", "--reload-rules"])
-    subprocess.check_call(["udevadm", "trigger"])
-    logger.info('udev rules configured successfully')
-
 def yes_no_prompt(question, default=None):
    if default is None:
        question += " [y/n] "
@@ -3,6 +3,7 @@ import re
 import subprocess
 import os
 import sys
+import platform

 def version_str_to_tuple(version_string):
    """
@@ -78,3 +79,15 @@ if __name__ == '__main__':
    args.output.write('#define FW_VERSION_MINOR {}\n'.format(minor))
    args.output.write('#define FW_VERSION_REVISION {}\n'.format(revision))
    args.output.write('#define FW_VERSION_UNRELEASED {}\n'.format(1 if unreleased else 0))
+
+def setup_udev_rules(logger):
+  if platform.system() != 'Linux':
+      logger.error("This command only makes sense on Linux")
+      return
+  if os.getuid() != 0:
+      logger.warn("you should run this as root, otherwise it will probably not work")
+  with open('/etc/udev/rules.d/91-odrive.rules', 'w') as file:
+      file.write('SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d3[0-9]", MODE="0666", ENV{ID_MM_DEVICE_IGNORE}="1"\n')
+  subprocess.check_call(["udevadm", "control", "--reload-rules"])
+  subprocess.check_call(["udevadm", "trigger"])
+  logger.info('udev rules configured successfully')
@@ -177,7 +177,7 @@ try:
        rate_test(my_odrive)

    elif args.command == 'udev-setup':
-        from odrive.utils import setup_udev_rules
+        from odrive.version import setup_udev_rules
        setup_udev_rules(logger)
    
    elif args.command == 'generate-code':
--- a/Show More
+++ b/Show More