Merge branch 'devel' into libfibre

Author: samuelsadok

.github/workflows/nightly.yaml

@@ -15,13 +15,16 @@ jobs:
     runs-on: ${{ matrix.os }}
     steps:
       - name: Install odrivetool
+        shell: bash
         run: |
+          pip3 list | grep setuptools || echo "setuptools not installed" # show version
+          pip3 install setuptools # TODO: this should be a dependency of odrive
+          pip3 install odrive
+        env:
           # TODO: this is a workaround for https://github.com/pypa/setuptools/issues/2353 and we
           # can remove it as soon as python3-setuptools on GitHub's ubuntu-latest
           # moves to version 50.1+.
-          pip3 list | grep setuptools # show version
-          export SETUPTOOLS_USE_DISTUTILS=stdlib
-          pip3 install odrive
+          SETUPTOOLS_USE_DISTUTILS: stdlib
 
       # This one currently fails because Github Actions runs pip as non-root
       #- name: Check if udev rules were set up properly

CHANGELOG.md

@@ -1,6 +1,7 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 ### Added
+* Added phase balance check to motor calibration and MOTOR_ERROR_UNBALANCED_PHASES to error enums
 * Added polarity and phase offset calibration for hall effect encoders
 * [Mechanical brake support](docs/mechanical-brakes.md)
 * Added periodic sending of encoder position on CAN
@@ -61,6 +62,7 @@ Please add a note of your changes below this heading if you make a Pull Request.
 * `<axis>.encoder.config.offset` was renamed to ``<axis>.encoder.config.phase_offset`
 * `<axis>.encoder.config.offset_float` was renamed to ``<axis>.encoder.config.phase_offset_float`
 * `<odrv>.config.brake_resistance == 0.0` is no longer a valid way to disable the brake resistor. Use `<odrv>.config.enable_brake_resistor` instead.
+* `<odrv>.can.set_baud_rate()` was removed. The baudrate is now automatically updated when writing to `<odrv>.can.config.baud_rate`.
 
 # Releases
 ## [0.5.1] - 2020-09-27

Firmware/Board/v3/Src/can.c

@@ -56,31 +56,46 @@
 
 /* USER CODE END 0 */
 
-CAN_HandleTypeDef hcan1;
-
-/* CAN1 init function */
-void MX_CAN1_Init(void)
-{
-
-  hcan1.Instance = CAN1;
-  hcan1.Init.Prescaler = 8;
-  hcan1.Init.Mode = CAN_MODE_NORMAL;
-  hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ;
-  hcan1.Init.TimeSeg1 = CAN_BS1_16TQ;
-  hcan1.Init.TimeSeg2 = CAN_BS2_4TQ;
-  hcan1.Init.TimeTriggeredMode = DISABLE;
-  hcan1.Init.AutoBusOff = ENABLE;
-  hcan1.Init.AutoWakeUp = ENABLE;
-  hcan1.Init.AutoRetransmission = ENABLE;
-  hcan1.Init.ReceiveFifoLocked = DISABLE;
-  hcan1.Init.TransmitFifoPriority = DISABLE;
-  if (HAL_CAN_Init(&hcan1) != HAL_OK)
-  {
-    _Error_Handler(__FILE__, __LINE__);
+CAN_HandleTypeDef hcan1 = {
+  .Instance = CAN1,
+  .Init = {
+    .Prescaler = 8,
+    .Mode = CAN_MODE_NORMAL,
+    .SyncJumpWidth = CAN_SJW_4TQ,
+    .TimeSeg1 = CAN_BS1_16TQ,
+    .TimeSeg2 = CAN_BS2_4TQ,
+    .TimeTriggeredMode = DISABLE,
+    .AutoBusOff = ENABLE,
+    .AutoWakeUp = ENABLE,
+    .AutoRetransmission = ENABLE,
+    .ReceiveFifoLocked = DISABLE,
+    .TransmitFifoPriority = DISABLE,
   }
+};
 
-}
-
+/* CAN1 init function */
+//void MX_CAN1_Init(void)
+//{
+//
+//  hcan1.Instance = CAN1;
+//  hcan1.Init.Prescaler = 8;
+//  hcan1.Init.Mode = CAN_MODE_NORMAL;
+//  hcan1.Init.SyncJumpWidth = CAN_SJW_4TQ;
+//  hcan1.Init.TimeSeg1 = CAN_BS1_16TQ;
+//  hcan1.Init.TimeSeg2 = CAN_BS2_4TQ;
+//  hcan1.Init.TimeTriggeredMode = DISABLE;
+//  hcan1.Init.AutoBusOff = ENABLE;
+//  hcan1.Init.AutoWakeUp = ENABLE;
+//  hcan1.Init.AutoRetransmission = ENABLE;
+//  hcan1.Init.ReceiveFifoLocked = DISABLE;
+//  hcan1.Init.TransmitFifoPriority = DISABLE;
+//  if (HAL_CAN_Init(&hcan1) != HAL_OK)
+//  {
+//    _Error_Handler(__FILE__, __LINE__);
+//  }
+//
+//}
+//
 void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
 {
 

Firmware/Board/v3/Src/spi.c

@@ -101,13 +101,18 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
     PC11     ------> SPI3_MISO
     PC12     ------> SPI3_MOSI 
     */
-    GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_11|GPIO_PIN_12;
+    GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_12;
     GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-    GPIO_InitStruct.Pull = GPIO_PULLUP; // required for disconnect detection on SPI encoders
+    GPIO_InitStruct.Pull = GPIO_PULLDOWN; // Idle clock and MOSI low.
     GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
     GPIO_InitStruct.Alternate = GPIO_AF6_SPI3;
     HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
+    // MISO pull-up required for disconnect detection on SPI encoders with even parity
+    GPIO_InitStruct.Pin = GPIO_PIN_11;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
     /* SPI3 DMA Init */
     /* SPI3_TX Init */
     hdma_spi3_tx.Instance = DMA1_Stream7;

Firmware/Board/v3/board.cpp

@@ -357,8 +357,6 @@ bool board_init() {
         // mode initialization won't override the CAN mode.
         if (odrv.config_.gpio_modes[15] != ODriveIntf::GPIO_MODE_CAN_A || odrv.config_.gpio_modes[16] != ODriveIntf::GPIO_MODE_CAN_A) {
             odrv.misconfigured_ = true;
-        } else {
-            MX_CAN1_Init();
         }
     }
 

Firmware/Drivers/STM32/stm32_spi_arbiter.cpp

@@ -36,6 +36,7 @@ bool Stm32SpiArbiter::start() {
         HAL_SPI_DeInit(hspi_);
         hspi_->Init = task.config;
         HAL_SPI_Init(hspi_);
+        __HAL_SPI_ENABLE(hspi_);
     }
     task.ncs_gpio.write(false);
 

Firmware/Makefile

@@ -9,8 +9,14 @@ PROGRAMMER_CMD=$(if $(value PROGRAMMER),-c 'hla_serial $(PROGRAMMER)',)
 
 include tup.config # source build configuration to get CONFIG_BOARD_VERSION
 
+ifeq ($(shell python -c "import sys; print(sys.version_info.major)"), 3)
+	PY_CMD := python -B
+else
+	PY_CMD := python3 -B
+endif
+
 ifneq (,$(findstring v3.,$(CONFIG_BOARD_VERSION)))
-  OPENOCD := openocd -f interface/stlink.cfg $(PROGRAMMER_CMD) -f target/stm32f4x.cfg -c init
+  OPENOCD := openocd -f interface/stlink-v2.cfg $(PROGRAMMER_CMD) -f target/stm32f4x.cfg -c init
   GDB := arm-none-eabi-gdb --ex 'target extended-remote | openocd -f "interface/stlink-v2.cfg" -f "target/stm32f4x.cfg" -c "gdb_port pipe; log_output openocd.log"' --ex 'monitor reset halt'
 else ifneq (,$(findstring v4.,$(CONFIG_BOARD_VERSION)))
   OPENOCD := openocd -f interface/stlink.cfg $(PROGRAMMER_CMD) -f target/stm32f7x.cfg -c 'reset_config none separate' -c init
@@ -23,7 +29,7 @@ $(info board version: $(CONFIG_BOARD_VERSION))
 
 all:
 	@tup --quiet --no-environ-check
-	@python interface_generator_stub.py --definitions odrive-interface.yaml --template ../tools/enums_template.j2 --output ../tools/odrive/enums.py
+	@$(PY_CMD) interface_generator_stub.py --definitions odrive-interface.yaml --template ../tools/enums_template.j2 --output ../tools/odrive/enums.py
 
 fibre:
 	(cd fibre-cpp && tup --no-environ-check)

Firmware/MotorControl/axis.cpp

@@ -313,14 +313,18 @@ bool Axis::start_closed_loop_control() {
         motor_.current_control_.Idq_setpoint_src_.connect_to(&motor_.Idq_setpoint_);
         motor_.current_control_.Vdq_setpoint_src_.connect_to(&motor_.Vdq_setpoint_);
 
+        bool is_acim = motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM;
+        // phase
         OutputPort<float>* phase_src = sensorless_mode ? &sensorless_estimator_.phase_ : &encoder_.phase_;
-        motor_.current_control_.phase_src_.connect_to(phase_src);
         acim_estimator_.rotor_phase_src_.connect_to(phase_src);
-        
+        OutputPort<float>* stator_phase_src = is_acim ? &acim_estimator_.stator_phase_ : phase_src;
+        motor_.current_control_.phase_src_.connect_to(stator_phase_src);
+        // phase vel
         OutputPort<float>* phase_vel_src = sensorless_mode ? &sensorless_estimator_.phase_vel_ : &encoder_.phase_vel_;
-        motor_.phase_vel_src_.connect_to(phase_vel_src);
-        motor_.current_control_.phase_vel_src_.connect_to(phase_vel_src);
         acim_estimator_.rotor_phase_vel_src_.connect_to(phase_vel_src);
+        OutputPort<float>* stator_phase_vel_src = is_acim ? &acim_estimator_.stator_phase_vel_ : phase_vel_src;
+        motor_.phase_vel_src_.connect_to(stator_phase_vel_src);
+        motor_.current_control_.phase_vel_src_.connect_to(stator_phase_vel_src);
 
         if (sensorless_mode) {
             // Make the final velocity of the loĉk-in spin the setpoint of the

Firmware/MotorControl/axis.hpp

@@ -187,7 +187,6 @@ class Axis : public ODriveIntf::AxisIntf {
     AxisState requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
     std::array<AxisState, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
     AxisState& current_state_ = task_chain_.front();
-    uint32_t loop_counter_ = 0;
     Homing_t homing_;
     CAN_t can_;
 

Firmware/MotorControl/encoder.hpp

@@ -1,7 +1,9 @@
 #ifndef __ENCODER_HPP
 #define __ENCODER_HPP
 
-#include <arm_math.h>
+class Encoder;
+
+#include <board.h> // needed for arm_math.h
 #include <Drivers/STM32/stm32_spi_arbiter.hpp>
 #include "utils.hpp"
 #include <autogen/interfaces.hpp>

Firmware/MotorControl/main.cpp

@@ -26,8 +26,6 @@ uint32_t _reboot_cookie __attribute__ ((section (".noinit")));
 extern char _estack; // provided by the linker script
 
 
-ODriveCAN::Config_t can_config;
-ODriveCAN *odCAN = nullptr;
 ODrive odrv{};
 
 
@@ -88,7 +86,7 @@ void StatusLedController::update() {
 static bool config_read_all() {
     bool success = board_read_config() &&
            config_manager.read(&odrv.config_) &&
-           config_manager.read(&can_config);
+           config_manager.read(&odrv.can_.config_);
     for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
         success = config_manager.read(&encoders[i].config_) &&
                   config_manager.read(&axes[i].sensorless_estimator_.config_) &&
@@ -108,7 +106,7 @@ static bool config_read_all() {
 static bool config_write_all() {
     bool success = board_write_config() &&
            config_manager.write(&odrv.config_) &&
-           config_manager.write(&can_config);
+           config_manager.write(&odrv.can_.config_);
     for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
         success = config_manager.write(&encoders[i].config_) &&
                   config_manager.write(&axes[i].sensorless_estimator_.config_) &&
@@ -127,7 +125,7 @@ static bool config_write_all() {
 
 static void config_clear_all() {
     odrv.config_ = {};
-    can_config = {};
+    odrv.can_.config_ = {};
     for (size_t i = 0; i < AXIS_COUNT; ++i) {
         encoders[i].config_ = {};
         axes[i].sensorless_estimator_.config_ = {};
@@ -145,7 +143,7 @@ static void config_clear_all() {
 }
 
 static bool config_apply_all() {
-    bool success = true;
+    bool success = odrv.can_.apply_config();
     for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) {
         success = encoders[i].apply_config(motors[i].config_.motor_type)
                && axes[i].controller_.apply_config()
@@ -174,6 +172,12 @@ bool ODrive::save_configuration(void) {
                && config_manager.start_store(&config_size)
                && config_write_all()
                && config_manager.finish_store();
+
+        // FIXME: during save_configuration we might miss some interrupts
+        // because the CPU gets halted during a flash erase. Missing events
+        // (encoder updates, step/dir steps) is not good so to be sure we just
+        // reboot.
+        NVIC_SystemReset();
     }
 
     return success;
@@ -265,24 +269,23 @@ void vApplicationIdleHook(void) {
         odrv.system_stats_.max_stack_usage_usb = stack_size_usb_thread - uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
         odrv.system_stats_.max_stack_usage_uart = stack_size_uart_thread - uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
         odrv.system_stats_.max_stack_usage_startup = stack_size_default_task - uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
-        odrv.system_stats_.max_stack_usage_can = odCAN->stack_size_ - uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
+        odrv.system_stats_.max_stack_usage_can = odrv.can_.stack_size_ - uxTaskGetStackHighWaterMark(odrv.can_.thread_id_) * sizeof(StackType_t);
 
         odrv.system_stats_.stack_size_axis = axes[0].stack_size_;
         odrv.system_stats_.stack_size_usb = stack_size_usb_thread;
         odrv.system_stats_.stack_size_uart = stack_size_uart_thread;
         odrv.system_stats_.stack_size_startup = stack_size_default_task;
-        odrv.system_stats_.stack_size_can = odCAN->stack_size_;
+        odrv.system_stats_.stack_size_can = odrv.can_.stack_size_;
 
         odrv.system_stats_.prio_axis = osThreadGetPriority(axes[0].thread_id_);
         odrv.system_stats_.prio_usb = osThreadGetPriority(usb_thread);
         odrv.system_stats_.prio_uart = osThreadGetPriority(uart_thread);
         odrv.system_stats_.prio_startup = osThreadGetPriority(defaultTaskHandle);
-        odrv.system_stats_.prio_can = osThreadGetPriority(odCAN->thread_id_);
+        odrv.system_stats_.prio_can = osThreadGetPriority(odrv.can_.thread_id_);
 
         status_led_controller.update();
     }
 }
-
 }
 
 /**
@@ -431,9 +434,6 @@ void ODrive::control_loop_cb(uint32_t timestamp) {
             axis.max_endstop_.update();
         }
 
-        MEASURE_TIME(axis.task_times_.can_heartbeat)
-            odCAN->send_cyclic(axis);
-
         MEASURE_TIME(axis.task_times_.controller_update)
             axis.controller_.update(); // uses position and velocity from encoder
 
@@ -827,9 +827,6 @@ extern "C" int main(void) {
     sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
     osSemaphoreWait(sem_can, 0);
 
-    // Construct all objects.
-    odCAN = new ODriveCAN(can_config, &hcan1);
-
     // Create main thread
     osThreadDef(defaultTask, rtos_main, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t));
     defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);

Firmware/MotorControl/motor.cpp

@@ -6,8 +6,8 @@
 
 #include <algorithm>
 
-#define CURRENT_ADC_LOWER_BOUND         (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MIN_VOLT / 3.3f)
-#define CURRENT_ADC_UPPER_BOUND         (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MAX_VOLT / 3.3f)
+static constexpr auto CURRENT_ADC_LOWER_BOUND =        (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MIN_VOLT / 3.3f);
+static constexpr auto CURRENT_ADC_UPPER_BOUND =        (uint32_t)((float)(1 << 12) * CURRENT_SENSE_MAX_VOLT / 3.3f);
 
 /**
  * @brief This control law adjusts the output voltage such that a predefined
@@ -29,6 +29,7 @@ struct ResistanceMeasurementControlLaw : AlphaBetaFrameController {
         if (Ialpha_beta.has_value()) {
             actual_current_ = Ialpha_beta->first;
             test_voltage_ += (kI * current_meas_period) * (target_current_ - actual_current_);
+            I_beta_ += (kIBetaFilt * current_meas_period) * (Ialpha_beta->second - I_beta_);
         } else {
             actual_current_ = 0.0f;
             test_voltage_ = 0.0f;
@@ -63,11 +64,17 @@ struct ResistanceMeasurementControlLaw : AlphaBetaFrameController {
         return test_voltage_ / target_current_;
     }
 
+    float get_Ibeta() {
+        return I_beta_;
+    }
+
     const float kI = 1.0f; // [(V/s)/A]
+    const float kIBetaFilt = 80.0f;
     float max_voltage_ = 0.0f;
     float actual_current_ = 0.0f;
     float target_current_ = 0.0f;
     float test_voltage_ = 0.0f;
+    float I_beta_ = 0.0f; // [A] low pass filtered Ibeta response
     std::optional<float> test_mod_ = NAN;
 };
 
@@ -325,7 +332,7 @@ bool Motor::setup() {
     max_dc_calib_ = 0.1f * max_allowed_current_;
 
     if (!gate_driver_.init())
-        return true;
+        return false;
 
     return true;
 }
@@ -435,6 +442,12 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
         success = false;
     }
 
+    float I_beta = control_law.get_Ibeta();
+    if (is_nan(I_beta) || (abs(I_beta) / test_current) > 0.1f) {
+        disarm_with_error(ERROR_UNBALANCED_PHASES);
+        success = false;
+    }
+
     return success;
 }