From b32c87df5a92fb2bdff2f96124992b0080b13484 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 2 Mar 2018 13:00:59 -0800 Subject: [PATCH 001/112] [TEMP] refactoring: compile works, motors initialize as usual --- Firmware/.gitignore | 5 +- Firmware/Board/v3.3/Inc/gpio.h | 6 +- Firmware/Board/v3.3/Inc/main.h | 2 +- .../FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 2 +- Firmware/Board/v3.3/Src/freertos.c | 34 +- Firmware/Board/v3.3/Src/gpio.c | 142 +- Firmware/Board/v3.3/Src/stm32f4xx_it.c | 2 +- Firmware/Board/v3.3/Src/syscalls.c | 3 + Firmware/Board/v3.3/Src/usbd_cdc_if.c | 2 +- Firmware/MotorControl/axis.cpp | 298 +++- Firmware/MotorControl/axis.h | 63 - Firmware/MotorControl/axis.hpp | 209 +++ Firmware/MotorControl/board_config_v3.3.h | 112 ++ Firmware/MotorControl/commands.cpp | 9 +- Firmware/MotorControl/config.h | 16 - Firmware/MotorControl/controller.cpp | 130 ++ Firmware/MotorControl/controller.hpp | 72 + Firmware/MotorControl/encoder.cpp | 204 +++ Firmware/MotorControl/encoder.hpp | 42 + Firmware/MotorControl/example.json | 37 + Firmware/MotorControl/legacy_commands.c | 15 +- Firmware/MotorControl/low_level.c | 1459 ----------------- Firmware/MotorControl/low_level.cpp | 270 +++ Firmware/MotorControl/low_level.h | 233 +-- Firmware/MotorControl/main.cpp | 110 ++ Firmware/MotorControl/motor.cpp | 322 ++++ Firmware/MotorControl/motor.hpp | 147 ++ Firmware/MotorControl/nvm_config.hpp | 142 ++ .../MotorControl/sensorless_estimator.cpp | 101 ++ .../MotorControl/sensorless_estimator.hpp | 24 + Firmware/MotorControl/utils.c | 2 - Firmware/MotorControl/utils.h | 4 + Firmware/Tupfile.lua | 8 +- 33 files changed, 2298 insertions(+), 1929 deletions(-) delete mode 100644 Firmware/MotorControl/axis.h create mode 100644 Firmware/MotorControl/axis.hpp create mode 100644 Firmware/MotorControl/board_config_v3.3.h delete mode 100644 Firmware/MotorControl/config.h create mode 100644 Firmware/MotorControl/controller.cpp create mode 100644 Firmware/MotorControl/controller.hpp create mode 100644 Firmware/MotorControl/encoder.cpp create mode 100644 Firmware/MotorControl/encoder.hpp create mode 100644 Firmware/MotorControl/example.json delete mode 100644 Firmware/MotorControl/low_level.c create mode 100644 Firmware/MotorControl/low_level.cpp create mode 100644 Firmware/MotorControl/main.cpp create mode 100644 Firmware/MotorControl/motor.cpp create mode 100644 Firmware/MotorControl/motor.hpp create mode 100644 Firmware/MotorControl/nvm_config.hpp create mode 100644 Firmware/MotorControl/sensorless_estimator.cpp create mode 100644 Firmware/MotorControl/sensorless_estimator.hpp diff --git a/Firmware/.gitignore b/Firmware/.gitignore index 4770c39a..c17ea7e8 100644 --- a/Firmware/.gitignore +++ b/Firmware/.gitignore @@ -15,8 +15,11 @@ Odrive.xml .settings/ .project +# VSCode stuff +/.vscode/.cortex-debug.*.state.json + # STM32CubeMX (in case you put it in this folder, or a symlink) STM32CubeMX #gdb log -openocd.log \ No newline at end of file +openocd.log diff --git a/Firmware/Board/v3.3/Inc/gpio.h b/Firmware/Board/v3.3/Inc/gpio.h index 74ce09da..b0e21c22 100644 --- a/Firmware/Board/v3.3/Inc/gpio.h +++ b/Firmware/Board/v3.3/Inc/gpio.h @@ -59,7 +59,7 @@ #include "main.h" /* USER CODE BEGIN Includes */ - +#include /* USER CODE END Includes */ /* USER CODE BEGIN Private defines */ @@ -73,6 +73,10 @@ void MX_GPIO_Init(void); void SetGPIO12toUART(); void SetGPIO12toStepDir(); void SetupENCIndexGPIO(); +bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, + uint32_t pull_up_down, + void (*callback)(void*), void* ctx); +void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); /* USER CODE END Prototypes */ diff --git a/Firmware/Board/v3.3/Inc/main.h b/Firmware/Board/v3.3/Inc/main.h index a9650093..8ba31542 100644 --- a/Firmware/Board/v3.3/Inc/main.h +++ b/Firmware/Board/v3.3/Inc/main.h @@ -54,7 +54,7 @@ #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 4 -// #define HW_VERSION_HIGH_VOLTAGE true +#define HW_VERSION_HIGH_VOLTAGE true #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 diff --git a/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index 467cb745..3e00082a 100644 --- a/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t; /// Thread Definition structure contains startup information of a thread. /// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS. typedef struct os_thread_def { - char *name; ///< Thread name + const char *name; ///< Thread name os_pthread pthread; ///< start address of thread function osPriority tpriority; ///< initial thread priority uint32_t instances; ///< maximum number of instances of that thread function diff --git a/Firmware/Board/v3.3/Src/freertos.c b/Firmware/Board/v3.3/Src/freertos.c index 7794558c..601ff3e9 100644 --- a/Firmware/Board/v3.3/Src/freertos.c +++ b/Firmware/Board/v3.3/Src/freertos.c @@ -53,10 +53,11 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include "low_level.h" +//#include "low_level.h" #include "axis_c_interface.h" -#include "commands.h" -#include "config.h" +//#include "commands.h" +//#include "config.h" +int odrive_main(void); /* USER CODE END Includes */ /* Variables -----------------------------------------------------------------*/ @@ -67,8 +68,6 @@ osThreadId defaultTaskHandle; osSemaphoreId sem_usb_irq; // List of threads -osThreadId thread_motor_0; -osThreadId thread_motor_1; osThreadId thread_cmd_parse; /* USER CODE END Variables */ @@ -110,7 +109,7 @@ void MX_FREERTOS_Init(void) { sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1); osSemaphoreWait(sem_usb_rx, 0); // Remove a token. - // Create a semaphore for USB RX + // Create a semaphore for USB TX osSemaphoreDef(sem_usb_tx); sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); @@ -142,28 +141,7 @@ void StartDefaultTask(void const * argument) /* USER CODE BEGIN StartDefaultTask */ - // Init and load persistent configuration - init_configuration(); - - // Init communications - init_communication(); - - // Init motor control - init_motor_control(); - - // Start motor threads - osThreadDef(task_motor_0, axis_thread_entry, osPriorityHigh+1, 0, 512); - osThreadDef(task_motor_1, axis_thread_entry, osPriorityHigh, 0, 512); - thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]); - thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]); - - // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512); - thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); - - // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); - thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); + odrive_main(); //If we get to here, then the default task is done. vTaskDelete(defaultTaskHandle); diff --git a/Firmware/Board/v3.3/Src/gpio.c b/Firmware/Board/v3.3/Src/gpio.c index 693ab7b1..44948a1c 100644 --- a/Firmware/Board/v3.3/Src/gpio.c +++ b/Firmware/Board/v3.3/Src/gpio.c @@ -50,7 +50,7 @@ /* Includes ------------------------------------------------------------------*/ #include "gpio.h" /* USER CODE BEGIN 0 */ -#include "low_level.h" +#include #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 @@ -158,10 +158,44 @@ void MX_GPIO_Init(void) /* USER CODE BEGIN 2 */ #endif // End GPIO Include +// @brief Returns the IRQ number associated with a certain pin. +// Note that all GPIOs with the same pin number map to the same IRQn, +// no matter which port they belong to. +IRQn_Type get_irq_number(uint16_t pin) { + uint16_t pin_number = 0; + while (pin) { + pin >>= 1; + pin_number++; + } + switch (pin_number) { + case 0: return EXTI0_IRQn; + case 1: return EXTI1_IRQn; + case 2: return EXTI2_IRQn; + case 3: return EXTI3_IRQn; + case 4: return EXTI4_IRQn; + case 5: + case 6: + case 7: + case 8: + case 9: return EXTI9_5_IRQn; + case 10: + case 11: + case 12: + case 13: + case 14: + case 15: return EXTI15_10_IRQn; + default: return 0; // impossible + } +} + +// @brief Puts the GPIO's 1 and 2 into UART mode. +// This will disable any interrupt subscribers of these GPIOs. void SetGPIO12toUART() { GPIO_InitTypeDef GPIO_InitStruct; - HAL_NVIC_DisableIRQ(EXTI0_IRQn); + // make sure nothing is hogging the GPIO's + GPIO_unsubscribe(GPIO_1_GPIO_Port, GPIO_1_Pin); + GPIO_unsubscribe(GPIO_2_GPIO_Port, GPIO_2_Pin); GPIO_InitStruct.Pin = GPIO_1_Pin; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -178,59 +212,73 @@ void SetGPIO12toUART() { HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); } -void SetGPIO12toStepDir() { +// Expected subscriptions: 2x step signal + 2x encoder index signal +#define MAX_SUBSCRIPTIONS 10 +struct subscription_t { + GPIO_TypeDef* GPIO_port; + uint16_t GPIO_pin; + void (*callback)(void*); + void* ctx; +} subscriptions[MAX_SUBSCRIPTIONS] = { 0 }; +size_t n_subscriptions = 0; + +// Sets up the specified GPIO to trigger the specified callback +// on a rising edge of the GPIO. +// @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN +bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, + uint32_t pull_up_down, + void (*callback)(void*), void* ctx) { + + // Register handler (or reuse existing registration) + // TODO: make thread safe + struct subscription_t* subscription = NULL; + for (size_t i = 0; i < n_subscriptions; ++i) { + if (subscriptions[i].GPIO_port == GPIO_port && + subscriptions[i].GPIO_pin == GPIO_pin) + subscription = &subscriptions[i]; + } + if (!subscription) { + if (n_subscriptions >= MAX_SUBSCRIPTIONS) + return false; + subscription = &subscriptions[n_subscriptions++]; + } + + *subscription = (struct subscription_t){ + .GPIO_port = GPIO_port, + .GPIO_pin = GPIO_pin, + .callback = callback, + .ctx = ctx + }; + + // Set up GPIO GPIO_InitTypeDef GPIO_InitStruct; - - GPIO_InitStruct.Pin = GPIO_1_Pin; + GPIO_InitStruct.Pin = GPIO_pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct); + GPIO_InitStruct.Pull = pull_up_down; + HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct); - GPIO_InitStruct.Pin = GPIO_2_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); - - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI0_IRQn); + // Enable interrupt + HAL_NVIC_SetPriority(get_irq_number(GPIO_pin), 0, 0); + HAL_NVIC_EnableIRQ(get_irq_number(GPIO_pin)); + return true; } -//TODO: Enable index on only one channel -void SetupENCIndexGPIO(){ - GPIO_InitTypeDef GPIO_InitStruct; - - /*Configure GPIO pins : PAPin PAPin */ - GPIO_InitStruct.Pin = M0_ENC_Z_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(M0_ENC_Z_GPIO_Port, &GPIO_InitStruct); - - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); - - /*Configure GPIO pins : PBPin PBPin */ - GPIO_InitStruct.Pin = M1_ENC_Z_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(M1_ENC_Z_GPIO_Port, &GPIO_InitStruct); - - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI3_IRQn); +void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { + for (size_t i = 0; i < n_subscriptions; ++i) { + if (subscriptions[i].GPIO_port == GPIO_port && + subscriptions[i].GPIO_pin == GPIO_pin) { + subscriptions[i].callback = NULL; + subscriptions[i].ctx = NULL; + } + } } - //Dispatch processing of external interrupts based on source -void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { - //Step signals for M0 and M1 - if (GPIO_Pin & GPIO_1_Pin || GPIO_Pin & GPIO_3_Pin) { - step_cb(GPIO_Pin); - } else if(GPIO_Pin & M0_ENC_Z_Pin){ - enc_index_cb(GPIO_Pin, 0); - } else if(GPIO_Pin & M1_ENC_Z_Pin){ - enc_index_cb(GPIO_Pin, 1); +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) { + for (size_t i = 0; i < n_subscriptions; ++i) { + if (subscriptions[i].GPIO_pin == GPIO_pin) // TODO: check for port + if (subscriptions[i].callback) + subscriptions[i].callback(subscriptions[i].ctx); } } diff --git a/Firmware/Board/v3.3/Src/stm32f4xx_it.c b/Firmware/Board/v3.3/Src/stm32f4xx_it.c index 064e1036..48450d1b 100644 --- a/Firmware/Board/v3.3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3.3/Src/stm32f4xx_it.c @@ -209,7 +209,7 @@ void ADC_IRQHandler(void) // The HAL's ADC handling mechanism adds many clock cycles of overhead // So we bypass it and handle the logic ourselves. - //@TODO add vbus meaasurement on adc1 here + //@TODO add vbus measurement on adc1 here ADC_IRQ_Dispatch(&hadc1, &vbus_sense_adc_cb); ADC_IRQ_Dispatch(&hadc2, &pwm_trig_adc_cb); ADC_IRQ_Dispatch(&hadc3, &pwm_trig_adc_cb); diff --git a/Firmware/Board/v3.3/Src/syscalls.c b/Firmware/Board/v3.3/Src/syscalls.c index 3ddb5701..d546266c 100644 --- a/Firmware/Board/v3.3/Src/syscalls.c +++ b/Firmware/Board/v3.3/Src/syscalls.c @@ -23,6 +23,7 @@ static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; int _write(int file, char* data, int len) { +#if 0 // TODO: revert! //number of bytes written int written = 0; switch (serial_printf_select) { @@ -57,6 +58,8 @@ int _write(int file, char* data, int len) { } return written; +#endif + return len; } void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { diff --git a/Firmware/Board/v3.3/Src/usbd_cdc_if.c b/Firmware/Board/v3.3/Src/usbd_cdc_if.c index b38385f7..ff11152b 100644 --- a/Firmware/Board/v3.3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3.3/Src/usbd_cdc_if.c @@ -274,7 +274,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ - set_cmd_buffer(Buf, *Len); + //set_cmd_buffer(Buf, *Len); TODO: revert! osSemaphoreRelease(sem_usb_rx); return (USBD_OK); diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 553c7190..1ee31a0d 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -1,65 +1,239 @@ -#include "axis.h" #include -#include "legacy_commands.h" +#include +#include "gpio.h" -//TODO: goal of refactor is to kick this out completely -extern "C" { -#include "low_level.h" +#include "utils.h" +#include "axis.hpp" + +Axis::Axis(const AxisHardwareConfig_t& hw_config, + AxisConfig_t& config, + Encoder& encoder, + SensorlessEstimator& sensorless_estimator, + Controller& controller, + Motor& motor) + : hw_config(hw_config), + config(config), + encoder(encoder), + sensorless_estimator(sensorless_estimator), + controller(controller), + motor(motor) +{ + encoder.axis = this; + sensorless_estimator.axis = this; + controller.axis = this; + motor.axis = this; } -//TODO: Make it really clear where this is loaded. -AxisConfig axis_configs[2]; //TODO: get a constexpr for num motors - -// C interface -extern "C" { -void axis_thread_entry(void const* temp_motor_ptr) { - Motor_t* motor = (Motor_t*)temp_motor_ptr; - - //TODO: explicit axis number assignment - //for now we search for it - uint8_t ax_number = 0; - while (&motors[ax_number] != motor) - ++ax_number; - - Axis axis(axis_configs[ax_number], ax_number, motor); - axis.StateMachineLoop(); -} -} // extern "C" - -void Axis::SetupLegacyMappings() { - // Legacy reachability from C - legacy_motor_ref_->axis_legacy.enable_control = &enable_control_; - - // override for compatibility with legacy comms paradigm - // TODO next gen comms - exposed_bools[4 * axis_number_ + 1] = &enable_control_; - exposed_bools[4 * axis_number_ + 2] = &do_calibration_; +static void step_cb_wrapper(void* ctx) { + reinterpret_cast(ctx)->step_cb(); } -Axis::Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref) - : axis_number_(axis_number), - enable_control_(config.enable_control_at_start), - do_calibration_(config.do_calibration_at_start), - config_(config), - legacy_motor_ref_(legacy_motor_ref) { - SetupLegacyMappings(); +void Axis::setup() { + encoder.setup(); + motor.setup(); } -void Axis::StateMachineLoop() { +void Axis::start_thread() { + osThreadDef(thread_def, run_state_machine_loop, hw_config.thread_priority, 0, 512); + thread_id = osThreadCreate(osThread(thread_def), this); + thread_id_valid = true; +} + +void Axis::signal_thread(thread_signals sig) { + if (thread_id_valid) + osSignalSet(thread_id, sig); +} + +// step/direction interface +void Axis::step_cb() { + if (enable_step_dir) { + GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config.dir_port, hw_config.dir_pin); + float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + controller.pos_setpoint += dir * config.counts_per_step; + } +}; + +void Axis::set_step_dir_enabled(bool enable) { + if (enable) { + // Set up the direction GPIO as input + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = hw_config.dir_pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(hw_config.dir_port, &GPIO_InitStruct); + + // Subscribe to rising edges of the step GPIO + GPIO_subscribe(hw_config.step_port, hw_config.step_pin, GPIO_PULLDOWN, + step_cb_wrapper, this); + + enable_step_dir = true; + } else { + enable_step_dir = false; + + // Unsubscribe from step GPIO + GPIO_unsubscribe(hw_config.step_port, hw_config.step_pin); + } +} + +//Returns true if everything is OK (no fault) +bool Axis::check_PSU_brownout() { + if(vbus_voltage < config.dc_bus_brownout_trip_level) + return false; + return true; +} + +// Returns true if everything is ok. Sets motor->error and returns false otherwise. +bool Axis::do_checks() { + if (!motor.check_DRV_fault()) { + motor.error = ERROR_DRV_FAULT; + return false; + } + if (!check_PSU_brownout()) { + motor.error = ERROR_DC_BUS_BROWNOUT; + return false; + } + return true; +} + +bool Axis::run_sensorless_spin_up() { + // Early Spin-up: spiral up current + float x = 0.0f; + run_control_loop([&](){ + float phase = wrap_pm_pi(config.ramp_up_distance * x); + float I_mag = config.spin_up_current * x; + x += current_meas_period / config.ramp_up_time; + if (!motor.update(I_mag, phase)) + return false; + return x < 1.0f; + }); + if (x < 1.0f) + 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; + phase = wrap_pm_pi(phase + vel * current_meas_period); + float I_mag = config.spin_up_current; + if (!motor.update(I_mag, phase)) + return false; + return vel < config.spin_up_target_vel; + }); + return vel >= config.spin_up_target_vel; +} + +// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. +bool Axis::run_sensorless_control_loop() { + run_control_loop([this](){ + float pos_estimate, vel_estimate, phase, current_setpoint; + + // We update the encoder just in case someone needs the output for testing + encoder.update(nullptr, nullptr, nullptr); + if (!sensorless_estimator.update(&pos_estimate, &vel_estimate, &phase)) + return false; + if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return false; + return motor.update(current_setpoint, phase); + }); + return false; +} + +bool Axis::run_closed_loop_control_loop() { + run_control_loop([this](){ + float pos_estimate, vel_estimate, phase, current_setpoint; + + // We update the sensorless estimator just in case someone needs the output for testing + sensorless_estimator.update(nullptr, nullptr, nullptr); + if (!encoder.update(&pos_estimate, &vel_estimate, &phase)) + return false; + if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return false; + return motor.update(current_setpoint, phase); + }); + return false; +} + +bool Axis::run_idle_loop() { + // TODO: allow preemption + for (;;) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT; + break; + } + } + return false; +} + +void Axis::run_state_machine_loop() { //TODO: Move this somewhere else + // TODO: respect changes of CPR // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f - int encoder_cpr = legacy_motor_ref_->encoder.encoder_cpr; - legacy_motor_ref_->anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); - if (legacy_motor_ref_->anticogging.cogging_map != NULL) { + int encoder_cpr = encoder.config.cpr; + controller.anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); + if (controller.anticogging.cogging_map != NULL) { for (int i = 0; i < encoder_cpr; i++) { - legacy_motor_ref_->anticogging.cogging_map[i] = 0.0f; + controller.anticogging.cogging_map[i] = 0.0f; } } - legacy_motor_ref_->motor_thread = osThreadGetId(); - legacy_motor_ref_->thread_ready = true; + enum AxisState_t { + AXIS_STATE_MOTOR_CALIBRATION, + AXIS_STATE_ENCODER_CALIBRATION, + AXIS_STATE_SENSORLESS_SPINUP, + AXIS_STATE_SENSORLESS_CONTROL, + AXIS_STATE_CLOSED_LOOP_CONTROL, + AXIS_STATE_IDLE + }; + AxisState_t axis_state = AXIS_STATE_MOTOR_CALIBRATION; + + for (;;) { + switch (axis_state) { + case AXIS_STATE_MOTOR_CALIBRATION: + if (!config.enable_motor_calibration || motor.run_calibration()) { + axis_state = AXIS_STATE_ENCODER_CALIBRATION; + } else { + axis_state = AXIS_STATE_IDLE; + } + break; + case AXIS_STATE_ENCODER_CALIBRATION: + if (!config.enable_encoder_calibration || encoder.run_calibration()) { + axis_state = config.enable_control ? + config.sensorless ? + AXIS_STATE_SENSORLESS_SPINUP : + AXIS_STATE_CLOSED_LOOP_CONTROL : + AXIS_STATE_IDLE; + if (axis_state != AXIS_STATE_IDLE) + set_step_dir_enabled(config.enable_step_dir_after_calibration); + } else { + axis_state = AXIS_STATE_IDLE; + } + break; + case AXIS_STATE_SENSORLESS_SPINUP: + if (run_sensorless_spin_up()) { + axis_state = AXIS_STATE_SENSORLESS_CONTROL; + } else { + axis_state = AXIS_STATE_IDLE; + } + break; + case AXIS_STATE_SENSORLESS_CONTROL: + run_sensorless_control_loop(); + axis_state = AXIS_STATE_IDLE; // TODO: restart if desired + break; + case AXIS_STATE_CLOSED_LOOP_CONTROL: + run_closed_loop_control_loop(); + axis_state = AXIS_STATE_IDLE; + break; + case AXIS_STATE_IDLE: + default: + run_idle_loop(); + break; + } + } + +/* bool calibration_ok = false; for (;;) { // Keep rotor estimation up to date while idling @@ -68,30 +242,26 @@ void Axis::StateMachineLoop() { if (do_calibration_) { do_calibration_ = false; - - __HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); // enable pwm outputs - calibration_ok = motor_calibration(legacy_motor_ref_); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); // disables pwm outputs + calibration_ok = motor.do_calibration(); + if (calibration_ok) + calibration_ok = encoder.do_calibration(); } if (calibration_ok && enable_control_) { - legacy_motor_ref_->enable_step_dir = true; - __HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); - - bool spin_up_ok = true; - if (legacy_motor_ref_->rotor_mode == ROTOR_MODE_SENSORLESS) - spin_up_ok = spin_up_sensorless(legacy_motor_ref_); - if (spin_up_ok) - control_motor_loop(legacy_motor_ref_); - - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); - legacy_motor_ref_->enable_step_dir = false; + enable_step_dir = true; + if (rotor_mode == ROTOR_MODE_SENSORLESS) { + bool spin_up_ok = do_sensorless_spin_up(); + if (spin_up_ok) + do_sensorless_control(); + } else { + do_closed_loop_control(); + } if (enable_control_) { // if control is still enabled, we exited because of error calibration_ok = false; enable_control_ = false; } } - } - legacy_motor_ref_->thread_ready = false; -} \ No newline at end of file + }*/ + thread_id_valid = false; +} diff --git a/Firmware/MotorControl/axis.h b/Firmware/MotorControl/axis.h deleted file mode 100644 index 455515b1..00000000 --- a/Firmware/MotorControl/axis.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef __AXIS_HPP -#define __AXIS_HPP - -//TODO: goal of refactor is to kick this out completely -extern "C" { -#include "low_level.h" -} - -//Outside axis: - //command handler - //callback dispatch - -// TODO: decide if we want to consolidate all default configs in one file for ease of use? -struct AxisConfig { - bool enable_control_at_start = true; - bool do_calibration_at_start = true; -}; -extern AxisConfig axis_configs[]; - -class Axis { -public: - //thread/os/system management - //timing log - //thread id - //etc. - //state machine - //control mode - //control_en/calib_ok - //error state - //motor - //current controller - //contains rotor phase logic - //motor level calibration routines - //low_level (implementation specifics) - //DRV driver - //adc callback handling - //pwm queueing - //rotor estimator - //kick out rotor phase logic - //pos/vel controller - //step/dir handler - - // Object operation requires ptr to legacy object for now, TODO: get rid of this dep - Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref); - - // Infinite loop that does calibration and enters main control loop as appropriate - void StateMachineLoop(); - - uint8_t axis_number_; - - bool enable_control_; - bool do_calibration_; - - AxisConfig& config_; - - Motor_t* legacy_motor_ref_; - -private: - void SetupLegacyMappings(); - -}; - -#endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp new file mode 100644 index 00000000..f8bbf79e --- /dev/null +++ b/Firmware/MotorControl/axis.hpp @@ -0,0 +1,209 @@ +#ifndef __AXIS_HPP +#define __AXIS_HPP + +#include +#include +#include + +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include + + +#include + +/*class Estimator { +public: + virtual float get_position_estimation(void); + virtual float get_velocity_estimation(void); +};*/ +/* +class Motor { +public: + // @brief Updates the current control loop + virtual void update(float current_ref); +};*/ + +// The Axis declaration is needed in the other header files +class Axis; + +#include +#include +#include +#include +#include + +//default timeout waiting for phase measurement signals +#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] + +static const float current_meas_period = CURRENT_MEAS_PERIOD; +static const int current_meas_hz = CURRENT_MEAS_HZ; +extern float vbus_voltage; +extern float brake_resistance; // [ohm] + +constexpr size_t AXIS_COUNT = 2; +extern Axis *axes[AXIS_COUNT]; + +/* +class Controller { +public: + // @brief Updates the controller loop(s) + virtual void update(void); +};*/ + + +//Outside axis: + //command handler + //callback dispatch + +typedef enum { + ROTOR_MODE_ENCODER, + ROTOR_MODE_SENSORLESS, + ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing +} Rotor_mode_t; + +// TODO: decide if we want to consolidate all default configs in one file for ease of use? +struct AxisConfig_t { + bool enable_motor_calibration = true; + bool enable_encoder_calibration = true; + bool enable_control = true; + bool sensorless = false; + bool enable_step_dir_after_calibration = true; // For M0 this has no effect if enable_uart is true + float counts_per_step = 2.0f; + float dc_bus_brownout_trip_level = 8.0f; // [V] + Rotor_mode_t rotor_mode = ROTOR_MODE_ENCODER; + + // 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] +}; + +class Axis { +public: + //thread/os/system management + //timing log + //thread id + //etc. + //state machine + //control mode + //control_en/calib_ok + //error state + //motor + //current controller + //contains rotor phase logic + //motor level calibration routines + //low_level (implementation specifics) + //DRV driver + //adc callback handling + //pwm queueing + //rotor estimator + //kick out rotor phase logic + //pos/vel controller + //step/dir handler + + enum thread_signals { + M_SIGNAL_PH_CURRENT_MEAS = 1u << 0 + }; + + Axis(const AxisHardwareConfig_t& hw_config, + AxisConfig_t& config, + Encoder& encoder, + SensorlessEstimator& sensorless_estimator, + Controller& controller, + Motor& motor); + + void setup(); + void start_thread(); + void signal_thread(thread_signals sig); + + // Infinite loop that does calibration and enters main control loop as appropriate + void run_state_machine_loop(); + static void run_state_machine_loop(const void* ctx) { + const_cast(reinterpret_cast(ctx))->run_state_machine_loop(); + }; + + void step_cb(); + void set_step_dir_enabled(bool enable); + + bool check_DRV_fault(); + bool check_PSU_brownout(); + bool do_checks(); + + // TODO: check if this uses dynamic memory + + + // @brief Runs the update handler at the frequency of the current measurements. + // + // The loop runs until one of the following conditions: + // - the update handler returns false + // - the current measurement times out + // - do_checks() becomes false + // - update_handler doesn't finish in time + // + // The function arms the motor at the beginning of the control loop and disarms it at + // the end of the control loop. + // Note that if this function returns, this should generally be considered an error condition, + // unless the termination was deliberately caused by the update_handler because it was of the + // opinion that the loop's task was completed. + // @tparam T Must be a callable type that takes no arguments and returns a bool + template + void run_control_loop(const T& update_handler) { + motor.arm(); + while (true /*enable_control*/) { // TODO: check for state change + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT; + break; + } + + // Proactively set phase voltages to 0. If the control deadline is missed, + // the voltages will go to zero. + motor.enqueue_voltage_timings(0.0f, 0.0f); + + if (!do_checks()) + break; + + if (!update_handler()) + break; + + update_brake_current(); + + // Check we meet deadlines after queueing + motor.last_cpu_time = motor.check_timing(); + if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) { + motor.error = ERROR_PHASE_RESISTANCE_TIMING; + break; + } + ++loop_counter; + + // TODO: maybe we should just abort automatically as soon as error is set + } + + // We are exiting control: disarm motor, reset Ibus, and update brake current + motor.disarm(); + motor.current_control.Ibus = 0.0f; + update_brake_current(); + } + + bool run_sensorless_spin_up(); + bool run_sensorless_control_loop(); + bool run_closed_loop_control_loop(); + bool run_idle_loop(); + + const AxisHardwareConfig_t& hw_config; + AxisConfig_t& config; + + Encoder& encoder; + SensorlessEstimator& sensorless_estimator; + Controller& controller; + Motor& motor; + + osThreadId thread_id; + volatile bool thread_id_valid = false; + bool enable_step_dir = false; //auto enabled after calibration + uint32_t loop_counter = 0; +}; + +#endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/board_config_v3.3.h b/Firmware/MotorControl/board_config_v3.3.h new file mode 100644 index 00000000..9ef2d2d6 --- /dev/null +++ b/Firmware/MotorControl/board_config_v3.3.h @@ -0,0 +1,112 @@ + +#ifndef __BOARD_CONFIG_H +#define __BOARD_CONFIG_H + +// STM specific includes +#include +#include +#include +#include + +#if HW_VERSION_MAJOR == 3 +#if HW_VERSION_MINOR <= 3 +#define SHUNT_RESISTANCE (675e-6f) +#else +#define SHUNT_RESISTANCE (500e-6f) +#endif +#endif + + +struct AxisHardwareConfig_t { + GPIO_TypeDef* step_port; + uint16_t step_pin; + GPIO_TypeDef* dir_port; + uint16_t dir_pin; + osPriority thread_priority; +}; + +struct EncoderHardwareConfig_t { + TIM_HandleTypeDef* timer; + GPIO_TypeDef* index_port; + uint16_t index_pin; +}; +struct MotorHardwareConfig_t { + TIM_HandleTypeDef* timer; + uint16_t control_deadline; + float shunt_conductance; +}; +struct GateDriverHardwareConfig_t { + SPI_HandleTypeDef* spi; + GPIO_TypeDef* enable_port; + uint16_t enable_pin; + GPIO_TypeDef* nCS_port; + uint16_t nCS_pin; + GPIO_TypeDef* nFAULT_port; + uint16_t nFAULT_pin; +}; +struct BoardHardwareConfig_t { + AxisHardwareConfig_t axis_config; + EncoderHardwareConfig_t encoder_config; + MotorHardwareConfig_t motor_config; + GateDriverHardwareConfig_t gate_driver_config; +}; + +const BoardHardwareConfig_t hw_configs[] = { { + .axis_config = { + .step_port = GPIO_1_GPIO_Port, + .step_pin = GPIO_1_Pin, + .dir_port = GPIO_2_GPIO_Port, + .dir_pin = GPIO_2_Pin, + .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1), + }, + .encoder_config = { + .timer = &htim3, + .index_port = M0_ENC_Z_GPIO_Port, + .index_pin = M0_ENC_Z_Pin, + }, + .motor_config = { + .timer = &htim1, + .control_deadline = TIM_1_8_PERIOD_CLOCKS, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + }, + .gate_driver_config = { + .spi = &hspi3, + // Note: this board has the EN_Gate pin shared! + .enable_port = EN_GATE_GPIO_Port, + .enable_pin = EN_GATE_Pin, + .nCS_port = M0_nCS_GPIO_Port, + .nCS_pin = M0_nCS_Pin, + .nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors + .nFAULT_pin = nFAULT_Pin, + } +},{ + .axis_config = { + .step_port = GPIO_3_GPIO_Port, + .step_pin = GPIO_3_Pin, + .dir_port = GPIO_4_GPIO_Port, + .dir_pin = GPIO_4_Pin, + .thread_priority = osPriorityHigh, + }, + .encoder_config = { + .timer = &htim4, + .index_port = M1_ENC_Z_GPIO_Port, + .index_pin = M1_ENC_Z_Pin, + }, + .motor_config = { + .timer = &htim8, + .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + }, + .gate_driver_config = { + .spi = &hspi3, + // Note: this board has the EN_Gate pin shared! + .enable_port = EN_GATE_GPIO_Port, + .enable_pin = EN_GATE_Pin, + .nCS_port = M1_nCS_GPIO_Port, + .nCS_pin = M1_nCS_Pin, + .nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors + .nFAULT_pin = nFAULT_Pin, + } +} }; + +#endif // __BOARD_CONFIG_H diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index df27c46d..d95646b4 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -1,4 +1,4 @@ - +#if 0 /* Includes ------------------------------------------------------------------*/ // TODO: remove this option @@ -8,7 +8,7 @@ #include "commands.h" #include "low_level.h" -#include "axis.h" +#include "axis.hpp" #include "protocol.hpp" #include "freertos_vars.h" #include "utils.h" @@ -145,7 +145,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain), - Endpoint::make_property("thread_ready", &motors[0].thread_ready), + Endpoint::make_property("thread_id_valid", &motors[0].thread_id_valid), Endpoint::make_property("control_deadline", &motors[0].control_deadline), Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time), Endpoint::make_property("loop_counter", &motors[0].loop_counter), @@ -232,7 +232,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain), - Endpoint::make_property("thread_ready", &motors[1].thread_ready), + Endpoint::make_property("thread_id_valid", &motors[1].thread_id_valid), Endpoint::make_property("control_deadline", &motors[1].control_deadline), Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time), Endpoint::make_property("loop_counter", &motors[1].loop_counter), @@ -509,3 +509,4 @@ void usb_update_thread() { vTaskDelete(osThreadGetId()); } +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/config.h b/Firmware/MotorControl/config.h deleted file mode 100644 index 57fafcca..00000000 --- a/Firmware/MotorControl/config.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef __CONFIG_H -#define __CONFIG_H - -#ifdef __cplusplus -extern "C" { -#endif - -void init_configuration(void); -void save_configuration(void); -void erase_configuration(void); - -#ifdef __cplusplus -} -#endif - -#endif /* __CONFIG_H */ diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp new file mode 100644 index 00000000..9f385d2d --- /dev/null +++ b/Firmware/MotorControl/controller.cpp @@ -0,0 +1,130 @@ + +#include "axis.hpp" + + +Controller::Controller(ControllerConfig_t& config) : + config(config) +{} + +//-------------------------------- +// Command Handling +//-------------------------------- + +void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) { + pos_setpoint = pos_setpoint; + vel_setpoint = vel_feed_forward; + current_setpoint = current_feed_forward; + config.control_mode = CTRL_MODE_POSITION_CONTROL; +#ifdef DEBUG_PRINT + printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); +#endif +} + +void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) { + vel_setpoint = vel_setpoint; + current_setpoint = current_feed_forward; + config.control_mode = CTRL_MODE_VELOCITY_CONTROL; +#ifdef DEBUG_PRINT + printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); +#endif +} + +void Controller::set_current_setpoint(float current_setpoint) { + current_setpoint = current_setpoint; + config.control_mode = CTRL_MODE_CURRENT_CONTROL; +#ifdef DEBUG_PRINT + printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); +#endif +} + +/* + * This anti-cogging implementation iterates through each encoder position, + * waits for zero velocity & position error, + * then samples the current required to maintain that position. + * + * This holding current is added as a feedforward term in the control loop. + */ +bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate) { + if (anticogging.calib_anticogging && anticogging.cogging_map != NULL) { + float pos_err = anticogging.index - pos_estimate; + if (fabsf(pos_err) <= anticogging.calib_pos_threshold && + fabsf(vel_estimate) < anticogging.calib_vel_threshold) { + anticogging.cogging_map[anticogging.index++] = vel_integrator_current; + } + if (anticogging.index < axis->encoder.config.cpr) { // TODO: remove the dependency on encoder CPR + set_pos_setpoint(anticogging.index, 0.0f, 0.0f); + return false; + } else { + anticogging.index = 0; + set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home + anticogging.use_anticogging = true; // We're good to go, enable anti-cogging + anticogging.calib_anticogging = false; + return true; + } + } + return false; +} + +bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { + // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(pos_estimate, vel_estimate); + + // Position control + // TODO Decide if we want to use encoder or pll position here + float vel_des = vel_setpoint; + if (config.control_mode >= CTRL_MODE_POSITION_CONTROL) { + float pos_err = pos_setpoint - pos_estimate; + vel_des += config.pos_gain * pos_err; + } + + // Velocity limiting + float vel_lim = config.vel_limit; + if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des < -vel_lim) vel_des = -vel_lim; + + // Velocity control + float Iq = current_setpoint; + + // Anti-cogging is enabled after calibration + // We get the current position and apply a current feed-forward + // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) + if (anticogging.use_anticogging) { + Iq += anticogging.cogging_map[mod(pos_estimate, axis->encoder.config.cpr)]; + } + + float v_err = vel_des - vel_estimate; + if (config.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + Iq += config.vel_gain * v_err; + } + + // Velocity integral action before limiting + 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; + if (Iq > Ilim) { + limited = true; + Iq = Ilim; + } + if (Iq < -Ilim) { + limited = true; + Iq = -Ilim; + } + + // Velocity integrator (behaviour dependent on limiting) + if (config.control_mode < CTRL_MODE_VELOCITY_CONTROL) { + // reset integral if not in use + vel_integrator_current = 0.0f; + } else { + if (limited) { + // TODO make decayfactor configurable + vel_integrator_current *= 0.99f; + } else { + vel_integrator_current += (config.vel_integrator_gain * current_meas_period) * v_err; + } + } + + if (current_setpoint_output) *current_setpoint_output = Iq; + return true; +} diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp new file mode 100644 index 00000000..79dda566 --- /dev/null +++ b/Firmware/MotorControl/controller.hpp @@ -0,0 +1,72 @@ + +// Note: these should be sorted from lowest level of control to +// highest level of control, to allow "<" style comparisons. +typedef enum { + CTRL_MODE_VOLTAGE_CONTROL = 0, + CTRL_MODE_CURRENT_CONTROL = 1, + CTRL_MODE_VELOCITY_CONTROL = 2, + CTRL_MODE_POSITION_CONTROL = 3 +} Motor_control_mode_t; + +struct ControllerConfig_t { + Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t + float pos_gain = 20.0f; // [(counts/s) / counts] + float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_limit = 20000.0f; // [counts/s] +}; + +class Controller { +public: + Controller(ControllerConfig_t& config); + + void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); + void set_vel_setpoint(float vel_setpoint, float current_feed_forward); + void set_current_setpoint(float current_setpoint); + + // TODO: make this more similar to other calibration loops + bool anti_cogging_calibration(float pos_estimate, float vel_estimate); + + bool update(float pos_estimate, float vel_estimate, float* current_setpoint); + + ControllerConfig_t& config; + Axis* axis = nullptr; // set by Axis constructor + + float pos_setpoint = 0.0f; + float vel_setpoint = 0.0f; + // float vel_setpoint = 800.0f; + // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] + float vel_integrator_current = 0.0f; // [A] + float current_setpoint = 0.0f; // [A] + + typedef struct { + int index; + float *cogging_map; + bool use_anticogging; + bool calib_anticogging; + float calib_pos_threshold; + float calib_vel_threshold; + } Anticogging_t; + Anticogging_t anticogging = { + .index = 0, + .cogging_map = nullptr, + .use_anticogging = false, + .calib_anticogging = false, + .calib_pos_threshold = 1.0f, + .calib_vel_threshold = 1.0f, + }; + + // Cache for remote procedure calls arguments TODO: remove + struct { + float pos_setpoint; + float vel_feed_forward; + float current_feed_forward; + } set_pos_setpoint_args; + struct { + float vel_setpoint; + float current_feed_forward; + } set_vel_setpoint_args; + struct { + float current_setpoint; + } set_current_setpoint_args; +}; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp new file mode 100644 index 00000000..8432cf83 --- /dev/null +++ b/Firmware/MotorControl/encoder.cpp @@ -0,0 +1,204 @@ + +//#include "encoder.hpp" +#include "axis.hpp" + + +Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, + EncoderConfig_t& config) : + hw_config(hw_config), + config(config) +{ + // Calculate encoder pll gains + // This calculation is currently identical to the PLL in SensorlessEstimator + float pll_bandwidth = 1000.0f; // [rad/s] + pll_kp = 2.0f * pll_bandwidth; + + // Critically damped + pll_ki = 0.25f * (pll_kp * pll_kp); +} + +static void enc_index_cb_wrapper(void* ctx) { + reinterpret_cast(ctx)->enc_index_cb(); +} + +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, + enc_index_cb_wrapper, this); +} + +//-------------------- +// Hardware Dependent +//-------------------- + +// Triggered when an encoder passes over the "Index" pin +// TODO: only arm index edge interrupt when we know encoder has powered up +// TODO: disarm interrupt once we found the index +void Encoder::enc_index_cb() { + if (!index_found) { + set_count(0); + index_found = true; + } +} + +// Function that sets the current encoder count to a desired 32-bit value. +void Encoder::set_count(uint32_t count) { + // Disable interrupts to make a critical section to avoid race condition + uint32_t prim = __get_PRIMASK(); + __disable_irq(); + state = count; + hw_config.timer->Instance->CNT = count; + pll_pos = (float)count; + __set_PRIMASK(prim); +} + + +// TODO: Do the scan with current, not voltage! +// TODO: add check_timing +bool Encoder::calib_enc_offset(float voltage_magnitude) { + static const float start_lock_duration = 1.0f; + static const float scan_duration = 1.0f; + static const float scan_range = 16.0f * M_PI; + static const size_t num_steps = scan_duration * current_meas_hz; + + // go to motor zero phase for start_lock_duration to get ready to scan + size_t i = 0; + axis->run_control_loop([&](){ + axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f); + return ++i < start_lock_duration * current_meas_hz; + }); + + int32_t init_enc_val = (int16_t)hw_config.timer->Instance->CNT; + int64_t encvaluesum = 0; + + // scan forward + i = 0; + axis->run_control_loop([&](){ + float phase = wrap_pm_pi(scan_range * (float)i / (float)num_steps - scan_range / 2.0f); + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + + encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + + return ++i < num_steps; + }); + if (i < num_steps) + return false; + + //TODO avoid recomputing elec_rad_per_enc every time + float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); + float expected_encoder_delta = scan_range / elec_rad_per_enc; + float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range) + { + axis->motor.error = ERROR_ENCODER_CPR_OUT_OF_RANGE; + return false; + } + // check direction + if ((int16_t)hw_config.timer->Instance->CNT > init_enc_val + 8) { + // motor same dir as encoder + axis->motor.config.direction = 1; + } else if ((int16_t)hw_config.timer->Instance->CNT < init_enc_val - 8) { + // motor opposite dir as encoder + axis->motor.config.direction = -1; + } else { + // Encoder response error + axis->motor.error = ERROR_ENCODER_RESPONSE; + return false; + } + + // scan backwards + i = 0; + axis->run_control_loop([&](){ + float phase = wrap_pm_pi(-scan_range * (float)i / (float)num_steps + scan_range / 2.0f); + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + + encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + + return ++i < num_steps; + }); + if (i < num_steps) + return false; + + int offset = encvaluesum / (num_steps * 2); + config.offset = offset; + config.calibrated = true; + return true; +} + +bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { + index_found = false; + float phase = 0.0f; + axis->run_control_loop([&](){ + phase = wrap_pm_pi(phase + omega * current_meas_period); + + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + + // continue until the index is found + return !index_found; + }); + return index_found; +} + +bool Encoder::run_calibration() { + float enc_calibration_voltage; + if (axis->motor.config.motor_type == MOTOR_TYPE_HIGH_CURRENT) + enc_calibration_voltage = axis->motor.config.calibration_current * axis->motor.config.phase_resistance; + else if (axis->motor.config.motor_type == MOTOR_TYPE_GIMBAL) + enc_calibration_voltage = axis->motor.config.calibration_current; + else + return false; + + if (config.use_index && !index_found) + if (!scan_for_enc_idx( + /*(float)(axis->motor.config.direction) * */ config.idx_search_speed, + enc_calibration_voltage)) + return false; + if (!config.calibrated) + if (!calib_enc_offset(enc_calibration_voltage)) + return false; + return true; +} + +bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp < 1.0f)) { + axis->motor.error = ERROR_CALIBRATION_TIMING; + return false; + } + + // update internal encoder state + int16_t delta_enc = (int16_t)hw_config.timer->Instance->CNT - (int16_t)state; + state += (int32_t)delta_enc; + + // compute electrical phase + int corrected_enc = state % config.cpr; + corrected_enc -= config.offset; + //corrected_enc *= axis->motor.config.direction; TODO: verify if this still works + //TODO avoid recomputing elec_rad_per_enc every time + float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); + float ph = elec_rad_per_enc * (float)corrected_enc; + // ph = fmodf(ph, 2*M_PI); + phase = wrap_pm_pi(ph); + + // run pll (for now pll is in units of encoder counts) + // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? + // Predict current pos + pll_pos += current_meas_period * pll_vel; + // discrete phase detector + float delta_pos = (float)(state - (int32_t)floorf(pll_pos)); + // pll feedback + pll_pos += current_meas_period * pll_kp * delta_pos; + pll_vel += current_meas_period * pll_ki * delta_pos; + + // Assign output arguments + if (*pos_estimate) *pos_estimate = pll_pos; + if (*vel_estimate) *vel_estimate = pll_vel; + if (*phase_output) *phase_output = phase; + return true; +} diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp new file mode 100644 index 00000000..856f00e5 --- /dev/null +++ b/Firmware/MotorControl/encoder.hpp @@ -0,0 +1,42 @@ +#ifndef __ENCODER_HPP +#define __ENCODER_HPP + +struct EncoderConfig_t { + bool use_index = false; + bool calibrated = false; + float idx_search_speed = 10.0f; // [rad/s electrical] + int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, + int32_t offset = 0; + float calib_range = 0.02; +}; + +class Encoder { +public: + Encoder(const EncoderHardwareConfig_t& hw_config, + EncoderConfig_t& config); + + void setup(); + + void enc_index_cb(); + + void set_count(uint32_t count); + bool calib_enc_offset(float voltage_magnitude); + bool scan_for_enc_idx(float omega, float voltage_magnitude); + + bool update(float* pos_estimate, float* vel_estimate, float* phase); + bool run_calibration(); + + const EncoderHardwareConfig_t& hw_config; + EncoderConfig_t& config; + Axis* axis = nullptr; // set by Axis constructor + + volatile bool index_found = false; + int32_t state = 0; + float phase = 0.0f; // [rad] + float pll_pos = 0.0f; // [rad] + float pll_vel = 0.0f; // [rad/s] + float pll_kp = 0.0f; // [rad/s / rad] + float pll_ki = 0.0f; // [(rad/s^2) / rad] +}; + +#endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/example.json b/Firmware/MotorControl/example.json new file mode 100644 index 00000000..7d3c4dc6 --- /dev/null +++ b/Firmware/MotorControl/example.json @@ -0,0 +1,37 @@ +[ + { + "name": "", + "id": 0, + "type": "json" + }, + { + "name": "subscriptions", + "id": 1, + "type": "int32[]" + }, + { + "name": "motor0", + "id": 2, + "type": "tree", + "content": [ + { + "name": "pos_setpoint", + "id": 3, + "type": "float", + "access": "rw" + }, + { + "name": "pos_gain", + "id": 4, + "type": "float", + "access": "rw" + }, + { + "name": "vel_setpoint", + "id": 5, + "type": "float", + "access": "rw" + } + ] + } +] \ No newline at end of file diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c index 0de2acdf..87df7140 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -1,7 +1,7 @@ /* Includes ------------------------------------------------------------------*/ #include "legacy_commands.h" #include - +#if 0 /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ @@ -88,14 +88,14 @@ int* exposed_ints[] = { }; bool* exposed_bools[] = { - &motors[0].thread_ready, // ro + &motors[0].thread_id_valid, // ro //For now these are written by Axis::SetupLegacyMappings - NULL, // &motors[0].enable_control, // rw - NULL, // &motors[0].do_calibration, // rw + &axis[0].enable_control, // rw + &axis[0].do_calibration, // rw NULL, // &motors[0].calibration_ok, // ro - &motors[1].thread_ready, // ro - NULL, // &motors[1].enable_control, // rw - NULL, // &motors[1].do_calibration, // rw + &motors[1].thread_id_valid, // ro + &axis[1].enable_control, // rw + &axis[1].do_calibration, // rw NULL, // &motors[1].calibration_ok, // ro }; @@ -289,3 +289,4 @@ static void print_monitoring(int limit) { } printf("\n"); } +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c deleted file mode 100644 index ad06623f..00000000 --- a/Firmware/MotorControl/low_level.c +++ /dev/null @@ -1,1459 +0,0 @@ -/* Includes ------------------------------------------------------------------*/ - -// Because of broken cmsis_os.h, we need to include arm_math first, -// otherwise chip specific defines are ommited -#include -#include // Sets up the correct chip specifc defines required by arm_math -#define ARM_MATH_CM4 -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -/* Private defines -----------------------------------------------------------*/ - -// #define DEBUG_PRINT - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -// This value is updated by the DC-bus reading ADC. -// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late -float vbus_voltage = 12.0f; - -#if HW_VERSION_MAJOR == 3 -#if HW_VERSION_MINOR <= 3 -#define SHUNT_RESISTANCE (675e-6f) -#else -#define SHUNT_RESISTANCE (500e-6f) -#endif -#endif - -// TODO: Migrate to C++, clearly we are actually doing object oriented code here... -// TODO: For nice encapsulation, consider not having the motor objects public - -// NOTE: for gimbal motors, all units of A are instead V. -// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] -// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. -Motor_t motors[] = { - { - // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pole_pairs = 7, // This value is correct for N5065 motors and Turnigy SK3 series. - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - // .vel_setpoint = 800.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] - // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - .dc_bus_brownout_trip_level = 8.0f, // [V] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = TIM_1_8_PERIOD_CLOCKS, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M0_nCS_GPIO_Port, - .nCSgpioNumber = M0_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .motor_type = MOTOR_TYPE_HIGH_CURRENT, - // .motor_type = MOTOR_TYPE_GIMBAL, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .max_allowed_current = 0.0f, - }, - // .rotor_mode = ROTOR_MODE_SENSORLESS, - // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim3, - .use_index = false, - .index_found = false, - .calibrated = false, - .idx_search_speed = 10.0f, // [rad/s electrical] - .encoder_cpr = (2048 * 4), // Default resolution of CUI-AMT102 encoder, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 1, // 1 or -1 - .encoder_calib_range = 0.02, - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .loop_counter = 0, - .timing_log_index = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - .drv_fault = DRV8301_FaultType_NoFault, - }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pole_pairs = 7, // This value is correct for N5065 motors and Turnigy SK3 series. - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - .dc_bus_brownout_trip_level = 8.0f, // [V] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M1_nCS_GPIO_Port, - .nCSgpioNumber = M1_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .motor_type = MOTOR_TYPE_HIGH_CURRENT, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .max_allowed_current = 0.0f, - }, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim4, - .use_index = false, - .index_found = false, - .calibrated = false, - .idx_search_speed = 10.0f, // [rad/s electrical] - .encoder_cpr = (2048 * 4), // Default resolution of CUI-AMT102 encoder, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 1, // 1 or -1 - .encoder_calib_range = 0.02, - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .loop_counter = 0, - .timing_log_index = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - .drv_fault = DRV8301_FaultType_NoFault, - } -}; -const size_t num_motors = sizeof(motors) / sizeof(motors[0]); - -float brake_resistance = 0.47f; // [ohm] - -/* Private constant data -----------------------------------------------------*/ -static const float one_by_sqrt3 = 0.57735026919f; -static const float sqrt3_by_2 = 0.86602540378f; -static const float current_meas_period = CURRENT_MEAS_PERIOD; -static const int current_meas_hz = CURRENT_MEAS_HZ; - -/* Private variables ---------------------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -//-------------------------------- -// Command Handling -//-------------------------------- - -void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - motor->pos_setpoint = pos_setpoint; - motor->vel_setpoint = vel_feed_forward; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_POSITION_CONTROL; -#ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) { - motor->vel_setpoint = vel_setpoint; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_VELOCITY_CONTROL; -#ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_current_setpoint(Motor_t* motor, float current_setpoint) { - motor->current_setpoint = current_setpoint; - motor->control_mode = CTRL_MODE_CURRENT_CONTROL; -#ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); -#endif -} - -//-------------------------------- -// Utility -//-------------------------------- - -uint16_t check_timing(Motor_t* motor) { - TIM_HandleTypeDef* htim = motor->motor_timer; - uint16_t timing = htim->Instance->CNT; - bool down = htim->Instance->CR1 & TIM_CR1_DIR; - if (down) { - uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; - timing = TIM_1_8_PERIOD_CLOCKS + delta; - } - - if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { - motor->timing_log_index = 0; - } - motor->timing_log[motor->timing_log_index] = timing; - - return timing; -} - -void global_fault(int error) { - // Disable motors NOW! - for (int i = 0; i < num_motors; ++i) { - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); - } - // Set fault codes, etc. - for (int i = 0; i < num_motors; ++i) { - motors[i].error = error; - *(motors[i].axis_legacy.enable_control) = false; - } - // disable brake resistor - set_brake_current(0.0f); -} - -float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1 << 11); - float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; - float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; - float current = shunt_volt * motor->shunt_conductance; - return current; -} - -//-------------------------------- -// Initalisation -//-------------------------------- - -// Initalises the low level motor control and then starts the motor control threads -void init_motor_control() { - // Init gate drivers - DRV8301_setup(&motors[0]); - DRV8301_setup(&motors[1]); - - // Start PWM and enable adc interrupts/callbacks - start_adc_pwm(); - - // Start Encoders - HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); - HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); - //TODO: Enable index on only one channel - if (motors[0].encoder.use_index || motors[1].encoder.use_index) { - SetupENCIndexGPIO(); - } - - // Wait for current sense calibration to converge - // TODO make timing a function of calibration filter tau - osDelay(1500); -} - -// Set up the gate drivers -void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); - - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - // 20V/V on 666uOhm gives a range of +/- 110A - // 40V/V on 666uOhm gives a range of +/- 55A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f / 10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f / 20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f / 40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f / 80.0f; - break; - } - - float margin = 0.90f; - float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); - - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); -} - -void start_adc_pwm() { - // Enable ADC and interrupts - __HAL_ADC_ENABLE(&hadc1); - __HAL_ADC_ENABLE(&hadc2); - __HAL_ADC_ENABLE(&hadc3); - // Warp field stabilize. - osDelay(2); - __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); - - // Ensure that debug halting of the core doesn't leave the motor PWM running - __HAL_DBGMCU_FREEZE_TIM1(); - __HAL_DBGMCU_FREEZE_TIM8(); - - start_pwm(&htim1); - start_pwm(&htim8); - // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); - - // Motor output starts in the disabled state - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); - - // Start brake resistor PWM in floating output configuration - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); -} - -void start_pwm(TIM_HandleTypeDef* htim) { - // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS / 2; - htim->Instance->CCR1 = half_load; - htim->Instance->CCR2 = half_load; - htim->Instance->CCR3 = half_load; - - // This hardware obfustication layer really is getting on my nerves - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); - - htim->Instance->CCR4 = 1; - HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); -} - -void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - // Store intial timer configs - uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t CR2_store = htim_a->Instance->CR2; - uint16_t SMCR_store = htim_b->Instance->SMCR; - // Turn off output - htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); - htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); - // Disable both timer counters - htim_a->Instance->CR1 &= ~TIM_CR1_CEN; - htim_b->Instance->CR1 &= ~TIM_CR1_CEN; - // Set first timer to send TRGO on counter enable - htim_a->Instance->CR2 &= ~TIM_CR2_MMS; - htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; - // Set Trigger Source of second timer to the TRGO of the first timer - htim_b->Instance->SMCR &= ~TIM_SMCR_TS; - htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; - // Set 2nd timer to start on trigger - htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; - htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; - // Dir bit is read only in center aligned mode, so we clear the mode for now - uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; - uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; - htim_a->Instance->CR1 &= ~TIM_CR1_CMS; - htim_b->Instance->CR1 &= ~TIM_CR1_CMS; - // Set both timers to up-counting state - htim_a->Instance->CR1 &= ~TIM_CR1_DIR; - htim_b->Instance->CR1 &= ~TIM_CR1_DIR; - // Restore center aligned mode - htim_a->Instance->CR1 |= CMS_store_a; - htim_b->Instance->CR1 |= CMS_store_b; - // set counter offset - htim_a->Instance->CNT = count_offset; - htim_b->Instance->CNT = 0; - // Start Timer a - htim_a->Instance->CR1 |= (TIM_CR1_CEN); - // Restore timer configs - htim_a->Instance->CR2 = CR2_store; - htim_b->Instance->SMCR = SMCR_store; - // restore output - htim_a->Instance->BDTR |= MOE_store_a; - htim_b->Instance->BDTR |= MOE_store_b; -} - -//-------------------------------- -// IRQ Callbacks -//-------------------------------- - -// step/direction interface -void step_cb(uint16_t GPIO_Pin) { - GPIO_PinState dir_pin; - float dir; - switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; - } -} - -// Triggered when an encoder passes over the "Index" pin -// TODO: only arm index edge interrupt when we know encoder has powered up -void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { - Motor_t* motor = &motors[motor_index]; - if (!motor->encoder.index_found) { - setEncoderCount(motor, 0); - motor->encoder.index_found = true; - } - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - if(GPIO_Pin == M0_ENC_Z_Pin){ - HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); - } else { - HAL_NVIC_DisableIRQ(EXTI3_IRQn); - } -} - -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); - // Only one conversion in sequence, so only rank1 - uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - vbus_voltage = ADCValue * voltage_scale; -} - -// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. -// TODO: Document how the phasing is done, link to timing diagram -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { -#define calib_tau 0.2f //@TOTO make more easily configurable - static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; - - // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)) { - global_fault(ERROR_ADC_FAILED); - return; - }; - - // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion - // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion - // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current - // If we are counting down, we just sampled in SVM vector 7, with zero current - Motor_t* motor = injected ? &motors[0] : &motors[1]; - bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - - bool current_meas_not_DC_CAL; - if (motor == &motors[1] && counting_down) { - // We are measuring M1 DC_CAL here - current_meas_not_DC_CAL = false; - // Load next timings for M0 (only once is sufficient) - if (hadc == &hadc2) { - motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; - motors[0].motor_timer->Instance->CCR2 = motors[0].next_timings[1]; - motors[0].motor_timer->Instance->CCR3 = motors[0].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[0] && !counting_down) { - // We are measuring M0 current here - current_meas_not_DC_CAL = true; - // Load next timings for M1 (only once is sufficient) - if (hadc == &hadc2) { - motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; - motors[1].motor_timer->Instance->CCR2 = motors[1].next_timings[1]; - motors[1].motor_timer->Instance->CCR3 = motors[1].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[1] && !counting_down) { - // We are measuring M1 current here - current_meas_not_DC_CAL = true; - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[0] && counting_down) { - // We are measuring M0 DC_CAL here - current_meas_not_DC_CAL = false; - // Check the timing of the sequencing - check_timing(motor); - - } else { - global_fault(ERROR_PWM_SRC_FAIL); - return; - } - - uint32_t ADCValue; - if (injected) { - ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - } else { - ADCValue = HAL_ADC_GetValue(hadc); - } - float current = phase_current_from_adcval(motor, ADCValue); - - if (current_meas_not_DC_CAL) { - // ADC2 and ADC3 record the phB and phC currents concurrently, - // and their interrupts should arrive on the same clock cycle. - // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. - // Therefore we store the value from ADC2 and signal the thread that the - // measurement is ready when we receive the ADC3 measurement - - // return or continue - if (hadc == &hadc2) { - motor->current_meas.phB = current - motor->DC_calib.phB; - return; - } else { - motor->current_meas.phC = current - motor->DC_calib.phC; - } - // Trigger motor thread - if (motor->thread_ready) - osSignalSet(motor->motor_thread, M_SIGNAL_PH_CURRENT_MEAS); - } else { - // DC_CAL measurement - if (hadc == &hadc2) { - motor->DC_calib.phB += (current - motor->DC_calib.phB) * calib_filter_k; - } else { - motor->DC_calib.phC += (current - motor->DC_calib.phC) * calib_filter_k; - } - } -} - -//-------------------------------- -// Measurement and calibration -//-------------------------------- - -// TODO check Ibeta balance to verify good motor connection -bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s - float test_voltage = 0.0f; - for (int i = 0; i < num_test_cycles; ++i) { - osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal) { - motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - - float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); - test_voltage += (kI * current_meas_period) * (test_current - Ialpha); - if (test_voltage > max_voltage) test_voltage = max_voltage; - if (test_voltage < -max_voltage) test_voltage = -max_voltage; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltage, 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_RESISTANCE_TIMING; - return false; - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float R = test_voltage / test_current; - motor->phase_resistance = R; - if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { - motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) { - float test_voltages[2] = {voltage_low, voltage_high}; - float Ialphas[2] = {0.0f}; - static const int num_cycles = 5000; - - for (int t = 0; t < num_cycles; ++t) { - for (int i = 0; i < 2; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - - Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltages[i], 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_INDUCTANCE_TIMING; - return false; - } - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float v_L = 0.5f * (voltage_high - voltage_low); - // Note: A more correct formula would also take into account that there is a finite timestep. - // However, the discretisation in the current control loop inverts the same discrepancy - float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); - float L = v_L / dI_by_dt; - - motor->phase_inductance = L; - // TODO arbitrary values set for now - if (L < 1e-6f || L > 500e-6f) { - motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -// TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { - static const float start_lock_duration = 1.0f; - static const int num_steps = 1024*2; - static const float dt_step = 1.0f / 500.0f; - static const float scan_range = 16.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) - - // go to motor zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - queue_voltage_timings(motor, voltage_magnitude, 0.0f); - } - - int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; - int32_t encvaluesum = 0; - - // scan forwards - for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = motor->pole_pairs * 2 * M_PI * (1.0f / (float)(motor->encoder.encoder_cpr)); - float expected_encoder_delta = scan_range / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > motor->encoder.encoder_calib_range) - { - motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; - return false; - } - // check direction - if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { - // motor same dir as encoder - motor->encoder.motor_dir = 1; - } else if ((int16_t)motor->encoder.encoder_timer->Instance->CNT < init_enc_val - 8) { - // motor opposite dir as encoder - motor->encoder.motor_dir = -1; - } else { - // Encoder response error - motor->error = ERROR_ENCODER_RESPONSE; - return false; - } - // scan backwards - for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - int offset = encvaluesum / (num_steps * 2); - motor->encoder.encoder_offset = offset; - motor->encoder.calibrated = true; - return true; -} - -bool motor_calibration(Motor_t* motor) { - motor->error = ERROR_NO_ERROR; - - float R_calib_max_voltage = motor->resistance_calib_max_voltage; - float enc_calibration_voltage = 0.0f; - if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if (!measure_phase_resistance(motor, motor->calibration_current, R_calib_max_voltage)) - return false; - enc_calibration_voltage = motor->calibration_current * motor->phase_resistance; - - if (!measure_phase_inductance(motor, -R_calib_max_voltage, R_calib_max_voltage)) - return false; - } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - enc_calibration_voltage = motor->calibration_current; - } else { - return false; - } - - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (motor->encoder.use_index && !motor->encoder.index_found) - if (!scan_for_enc_idx(motor, - (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed, - enc_calibration_voltage)) - return false; - if (!motor->encoder.calibrated) - if (!calib_enc_offset(motor, enc_calibration_voltage)) - return false; - } - - // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] - motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; - float plant_pole = motor->phase_resistance / motor->phase_inductance; - motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; - - // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] - motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { - motor->error = ERROR_CALIBRATION_TIMING; - return false; - } - // Critically damped - motor->encoder.pll_ki = 0.25f * (motor->encoder.pll_kp * motor->encoder.pll_kp); - - // sensorless pll same as encoder (for now) - motor->sensorless.pll_kp = motor->encoder.pll_kp; - motor->sensorless.pll_ki = motor->encoder.pll_ki; - - return true; -} - -/* - * This anti-cogging implementation iterates through each encoder position, - * waits for zero velocity & position error, - * then samples the current required to maintain that position. - * - * This holding current is added as a feedforward term in the control loop. - */ -bool anti_cogging_calibration(Motor_t* motor) { - if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { - float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { - motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; - } - if (motor->anticogging.index < motor->encoder.encoder_cpr) { - set_pos_setpoint(motor, motor->anticogging.index, 0.0f, 0.0f); - return false; - } else { - motor->anticogging.index = 0; - set_pos_setpoint(motor, 0.0f, 0.0f, 0.0f); // Send the motor home - motor->anticogging.use_anticogging = true; // We're good to go, enable anti-cogging - motor->anticogging.calib_anticogging = false; - return true; - } - } - return false; -} - -//-------------------------------- -// Test functions -//-------------------------------- - -bool scan_for_enc_idx(Motor_t* motor, float omega, float voltage_magnitude) { - for (;;) { - for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { - osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - if (!do_checks(motor)) - return false; - - if (motor->encoder.index_found) - return true; - - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_SCAN_MOTOR_TIMING; - return false; - } - } - } -} - -//-------------------------------- -// Main motor control -//-------------------------------- - -void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { - case ROTOR_MODE_ENCODER: - case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { - //for convenience - Encoder_t* encoder = &motor->encoder; - - // update internal encoder state - int16_t delta_enc = (int16_t)encoder->encoder_timer->Instance->CNT - (int16_t)encoder->encoder_state; - encoder->encoder_state += (int32_t)delta_enc; - - // compute electrical phase - int corrected_enc = encoder->encoder_state % motor->encoder.encoder_cpr; - corrected_enc -= encoder->encoder_offset; - corrected_enc *= encoder->motor_dir; - //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = motor->pole_pairs * 2 * M_PI * (1.0f / (float)(motor->encoder.encoder_cpr)); - float ph = elec_rad_per_enc * (float)corrected_enc; - // ph = fmodf(ph, 2*M_PI); - encoder->phase = wrap_pm_pi(ph); - - // run pll (for now pll is in units of encoder counts) - // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? - // Predict current pos - encoder->pll_pos += current_meas_period * encoder->pll_vel; - // discrete phase detector - float delta_pos = (float)(encoder->encoder_state - (int32_t)floorf(encoder->pll_pos)); - // pll feedback - encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; - encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; - } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; - case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer - // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf - // In particular, equation 8 (and by extension eqn 4 and 6). - - // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle - // is the one computed two cycles ago. To get the correct measurement, it was stored twice: - // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. - - //for convenience - Sensorless_t* sensorless = &motor->sensorless; - - // Clarke transform - float I_alpha_beta[2] = { - -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; - - // alpha-beta vector operations - float eta[2]; - for (int i = 0; i <= 1; ++i) { - // y is the total flux-driving voltage (see paper eqn 4) - float y = -motor->phase_resistance * I_alpha_beta[i] + sensorless->V_alpha_beta_memory[i]; - // flux dynamics (prediction) - float x_dot = y; - // integrate prediction to current timestep - sensorless->flux_state[i] += x_dot * current_meas_period; - - // eta is the estimated permanent magnet flux (see paper eqn 6) - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = sensorless->pm_flux_linkage * sensorless->pm_flux_linkage; - float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; - float bandwidth_factor = 1.0f / (sensorless->pm_flux_linkage * sensorless->pm_flux_linkage); - float eta_factor = 0.5f * (sensorless->observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); - - static float eta_factor_avg_test = 0.0f; - eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); - - // alpha-beta vector operations - for (int i = 0; i <= 1; ++i) { - // add observer action to flux estimate dynamics - float x_dot = eta_factor * eta[i]; - // convert action to discrete-time - sensorless->flux_state[i] += x_dot * current_meas_period; - // update new eta - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Flux state estimation done, store V_alpha_beta for next timestep - sensorless->V_alpha_beta_memory[0] = motor->current_control.final_v_alpha; - sensorless->V_alpha_beta_memory[1] = motor->current_control.final_v_beta; - - // PLL - // predict PLL phase with velocity - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_vel); - // update PLL phase with observer permanent magnet phase - sensorless->phase = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(sensorless->phase - sensorless->pll_pos); - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_kp * delta_phase); - // update PLL velocity - sensorless->pll_vel += current_meas_period * sensorless->pll_ki * delta_phase; - - //TODO TEMP TEST HACK - // static int trigger_ctr = 0; - // if (++trigger_ctr >= 3*current_meas_hz) { - // trigger_ctr = 0; - - // //Change to sensorless units - // motor->vel_gain = 15.0f / 200.0f; - // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; - - // //Change mode - // motor->rotor_mode = ROTOR_MODE_SENSORLESS; - // } - - } break; - default: - //TODO error handling - break; - } -} - -bool using_encoder(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - return true; - else - return false; -} - -bool using_sensorless(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) - return true; - else - return false; -} - -float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.phase; - else if (using_sensorless(motor)) - return motor->sensorless.phase; - else - //TODO error handling - return 0.0f; -} - -float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.pll_vel; - else if (using_sensorless(motor)) - return motor->sensorless.pll_vel; - else - //TODO error handling - return 0.0f; -} - -// Function that sets the current encoder count to a desired 32-bit value. -void setEncoderCount(Motor_t* motor, uint32_t count) { - // Disable interrupts to make a critical section to avoid race condition - uint32_t prim = __get_PRIMASK(); - __disable_irq(); - motor->encoder.encoder_state = count; - motor->encoder.encoder_timer->Instance->CNT = count; - motor->encoder.pll_pos = (float)count; - __set_PRIMASK(prim); -} - -bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { - // wait for new timestep - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_SPIN_UP_TIMEOUT; - return false; - } - - if (!do_checks(motor)) - return false; - // run estimator - if (!loop_updates(motor)) - return false; - - // override the phase during spinup - motor->sensorless.phase = phase; - // run current control (with the phase override) - FOC_current(motor, I_mag, 0.0f); - - return true; -} - -bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; - static const float ramp_up_distance = 4 * M_PI; - float ramp_step = current_meas_period / ramp_up_time; - - float phase = 0.0f; - float vel = ramp_up_distance / ramp_up_time; - float I_mag = 0.0f; - - // spiral up current - for (float x = 0.0f; x < 1.0f; x += ramp_step) { - phase = wrap_pm_pi(ramp_up_distance * x); - I_mag = motor->sensorless.spin_up_current * x; - if (!spin_up_timestep(motor, phase, I_mag)) - return false; - } - - // accelerate - while (vel < motor->sensorless.spin_up_target_vel) { - vel += motor->sensorless.spin_up_acceleration * current_meas_period; - phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - return false; - } - - // // test keep spinning - // while (true) { - // phase = wrap_pm_pi(phase + vel * current_meas_period); - // if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - // return false; - // } - - return true; - - // TODO: check pll vel (abs ratio, 0.8) -} - -void update_brake_current() { - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: set_brake_current will clip negative values to 0.0f - set_brake_current(-Ibus_sum); -} - -void set_brake_current(float brake_current) { - if (brake_current < 0.0f) brake_current = 0.0f; - float brake_duty = brake_current * brake_resistance / vbus_voltage; - - // Duty limit at 90% to allow bootstrap caps to charge - if (brake_duty > 0.9f) brake_duty = 0.9f; - int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - - // Safe update of low and high side timings - // To avoid race condition, first reset timings to safe state - // ch3 is low side, ch4 is high side - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - htim2.Instance->CCR3 = low_off; - htim2.Instance->CCR4 = high_on; -} - -void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) { - float tA, tB, tC; - SVM(mod_alpha, mod_beta, &tA, &tB, &tC); - motor->next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); -} - -void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { - float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); - float mod_alpha = vfactor * v_alpha; - float mod_beta = vfactor * v_beta; - queue_modulation_timings(motor, mod_alpha, mod_beta); -} - -// TODO: This doesn't update brake current -// We should probably make FOC Current call FOC Voltage to avoid duplication. -bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - if (!(check_timing(motor) < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return false; - } - return true; -} - -bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { - Current_control_t* ictrl = &motor->current_control; - - // For Reporting - ictrl->Iq_setpoint = Iq_des; - - // Clarke transform - float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; - float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); - - // Park transform - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float Id = c * Ialpha + s * Ibeta; - float Iq = c * Ibeta - s * Ialpha; - ictrl->Iq_measured = Iq; - - // Current error - float Ierr_d = Id_des - Id; - float Ierr_q = Iq_des - Iq; - - // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) - // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; - - float mod_to_V = (2.0f / 3.0f) * vbus_voltage; - float V_to_mod = 1.0f / mod_to_V; - float mod_d = V_to_mod * Vd; - float mod_q = V_to_mod * Vq; - - // Vector modulation saturation, lock integrator if saturated - // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); - if (mod_scalefactor < 1.0f) { - mod_d *= mod_scalefactor; - mod_q *= mod_scalefactor; - // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; - } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); - } - - // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; - - // Inverse park transform - float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; - - // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; - - // Apply SVM - queue_modulation_timings(motor, mod_alpha, mod_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_TIMING; - return false; - } - - update_brake_current(); - return true; -} - -//Returns true if everything is OK (no fault) -bool check_DRV_fault(Motor_t* motor) { - //TODO: make this pin configurable per motor ch - GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin); - return (nFAULT_state == GPIO_PIN_RESET) ? false : true; -} - -//Returns true if everything is OK (no fault) -bool check_PSU_brownout(Motor_t* motor) { - if(vbus_voltage < motor->dc_bus_brownout_trip_level) - return false; - return true; -} - -// Returns true if everything is ok. Sets motor->error and returns false otherwise. -bool do_checks(Motor_t* motor) { - if (!check_DRV_fault(motor)) { - motor->error = ERROR_DRV_FAULT; - // Update DRV Fault Code - motor->drv_fault = DRV8301_getFaultType(&motor->gate_driver); - // Update/Cache all SPI device registers - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - local_regs->RcvCmd = true; - DRV8301_readData(&motor->gate_driver, local_regs); - return false; - } - if (!check_PSU_brownout(motor)) { - motor->error = ERROR_DC_BUS_BROWNOUT; - return false; - } - return true; -} - -bool loop_updates(Motor_t* motor) { - update_rotor(motor); - return true; -} - -void control_motor_loop(Motor_t* motor) { - while (*(motor->axis_legacy.enable_control)) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; - break; - } - - if (!do_checks(motor)) - break; - if (!loop_updates(motor)) - break; - - // Only runs if anticogging.calib_anticogging is true; non-blocking - anti_cogging_calibration(motor); - - // Position control - // TODO Decide if we want to use encoder or pll position here - float vel_des = motor->vel_setpoint; - if (motor->control_mode >= CTRL_MODE_POSITION_CONTROL) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) { - motor->error = ERROR_POS_CTRL_DURING_SENSORLESS; - break; - } - float pos_err = motor->pos_setpoint - motor->encoder.pll_pos; - vel_des += motor->pos_gain * pos_err; - } - - // Velocity limiting - float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; - if (vel_des < -vel_lim) vel_des = -vel_lim; - - // Velocity control - float Iq = motor->current_setpoint; - - // Anti-cogging is enabled after calibration - // We get the current position and apply a current feed-forward - // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (motor->anticogging.use_anticogging) { - Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, motor->encoder.encoder_cpr)]; - } - - float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += motor->vel_gain * v_err; - } - - // Velocity integral action before limiting - Iq += motor->vel_integrator_current; - - // Apply motor direction correction - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - Iq *= motor->encoder.motor_dir; - } - - // Current limiting - float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); - bool limited = false; - if (Iq > Ilim) { - limited = true; - Iq = Ilim; - } - if (Iq < -Ilim) { - limited = true; - Iq = -Ilim; - } - - // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { - // reset integral if not in use - motor->vel_integrator_current = 0.0f; - } else { - if (limited) { - // TODO make decayfactor configurable - motor->vel_integrator_current *= 0.99f; - } else { - motor->vel_integrator_current += (motor->vel_integrator_gain * current_meas_period) * v_err; - } - } - - // Execute current command - if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current - } - } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_voltage - } - } else { - motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; - break; - } - - ++(motor->loop_counter); - } - - //We are exiting control, reset Ibus, and update brake current - motor->current_control.Ibus = 0.0f; - update_brake_current(); -} diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp new file mode 100644 index 00000000..8c907a5a --- /dev/null +++ b/Firmware/MotorControl/low_level.cpp @@ -0,0 +1,270 @@ +/* Includes ------------------------------------------------------------------*/ + +// Because of broken cmsis_os.h, we need to include arm_math first, +// otherwise chip specific defines are ommited +#include +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +/* Private defines -----------------------------------------------------------*/ + +// #define DEBUG_PRINT + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +// This value is updated by the DC-bus reading ADC. +// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late +float vbus_voltage = 12.0f; + +// TODO: Migrate to C++, clearly we are actually doing object oriented code here... + +float brake_resistance = 0.47f; // [ohm] + +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void start_adc_pwm() { + // Enable ADC and interrupts + __HAL_ADC_ENABLE(&hadc1); + __HAL_ADC_ENABLE(&hadc2); + __HAL_ADC_ENABLE(&hadc3); + // Warp field stabilize. + osDelay(2); + __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); + __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); + + // Ensure that debug halting of the core doesn't leave the motor PWM running + __HAL_DBGMCU_FREEZE_TIM1(); + __HAL_DBGMCU_FREEZE_TIM8(); + + start_pwm(&htim1); + start_pwm(&htim8); + // TODO: explain why this offset + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); + + // Motor output starts in the disabled state + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); + + // Start brake resistor PWM in floating output configuration + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); +} + +void global_fault(Error_t error) { + // Disable motors NOW! + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->motor.disarm(); + } + // Set fault codes, etc. + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->motor.error = error; + // TODO: update axis_state + } + // disable brake resistor + set_brake_current(0.0f); +} + +void start_pwm(TIM_HandleTypeDef* htim) { + // Init PWM + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; + htim->Instance->CCR1 = half_load; + htim->Instance->CCR2 = half_load; + htim->Instance->CCR3 = half_load; + + // This hardware obfustication layer really is getting on my nerves + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); + + htim->Instance->CCR4 = 1; + HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); +} + +void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { + // Store intial timer configs + uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); + uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); + uint16_t CR2_store = htim_a->Instance->CR2; + uint16_t SMCR_store = htim_b->Instance->SMCR; + // Turn off output + htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); + htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); + // Disable both timer counters + htim_a->Instance->CR1 &= ~TIM_CR1_CEN; + htim_b->Instance->CR1 &= ~TIM_CR1_CEN; + // Set first timer to send TRGO on counter enable + htim_a->Instance->CR2 &= ~TIM_CR2_MMS; + htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; + // Set Trigger Source of second timer to the TRGO of the first timer + htim_b->Instance->SMCR &= ~TIM_SMCR_TS; + htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; + // Set 2nd timer to start on trigger + htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; + htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; + // Dir bit is read only in center aligned mode, so we clear the mode for now + uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; + uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; + htim_a->Instance->CR1 &= ~TIM_CR1_CMS; + htim_b->Instance->CR1 &= ~TIM_CR1_CMS; + // Set both timers to up-counting state + htim_a->Instance->CR1 &= ~TIM_CR1_DIR; + htim_b->Instance->CR1 &= ~TIM_CR1_DIR; + // Restore center aligned mode + htim_a->Instance->CR1 |= CMS_store_a; + htim_b->Instance->CR1 |= CMS_store_b; + // set counter offset + htim_a->Instance->CNT = count_offset; + htim_b->Instance->CNT = 0; + // Start Timer a + htim_a->Instance->CR1 |= (TIM_CR1_CEN); + // Restore timer configs + htim_a->Instance->CR2 = CR2_store; + htim_b->Instance->SMCR = SMCR_store; + // restore output + htim_a->Instance->BDTR |= MOE_store_a; + htim_b->Instance->BDTR |= MOE_store_b; +} + +//-------------------------------- +// IRQ Callbacks +//-------------------------------- + + +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { + static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); + // Only one conversion in sequence, so only rank1 + uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + vbus_voltage = ADCValue * voltage_scale; +} + +// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. +// TODO: Document how the phasing is done, link to timing diagram +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { +#define calib_tau 0.2f //@TOTO make more easily configurable + static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; + + // Ensure ADCs are expected ones to simplify the logic below + if (!(hadc == &hadc2 || hadc == &hadc3)) { + global_fault(ERROR_ADC_FAILED); + return; + }; + + // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion + // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + Axis& axis = injected ? *axes[0] : *axes[1]; + Axis& other_axis = injected ? *axes[1] : *axes[0]; + bool counting_down = axis.motor.hw_config.timer->Instance->CR1 & TIM_CR1_DIR; + + bool current_meas_not_DC_CAL = !counting_down; + if (&axis == axes[1] && counting_down) { + // Load next timings for M0 (only once is sufficient) + if (hadc == &hadc2) { + other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; + other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; + other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; + } + } else if (&axis == axes[0] && !counting_down) { + // Load next timings for M1 (only once is sufficient) + if (hadc == &hadc2) { + other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; + other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; + other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; + } + } + + // Check the timing of the sequencing + axis.motor.check_timing(); + + uint32_t ADCValue; + if (injected) { + ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + } else { + ADCValue = HAL_ADC_GetValue(hadc); + } + float current = axis.motor.phase_current_from_adcval(ADCValue); + + if (current_meas_not_DC_CAL) { + // ADC2 and ADC3 record the phB and phC currents concurrently, + // and their interrupts should arrive on the same clock cycle. + // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. + // Therefore we store the value from ADC2 and signal the thread that the + // measurement is ready when we receive the ADC3 measurement + + // return or continue + if (hadc == &hadc2) { + axis.motor.current_meas.phB = current - axis.motor.DC_calib.phB; + return; + } else { + axis.motor.current_meas.phC = current - axis.motor.DC_calib.phC; + } + // Trigger axis thread + axis.signal_thread(Axis::thread_signals::M_SIGNAL_PH_CURRENT_MEAS); + } else { + // DC_CAL measurement + if (hadc == &hadc2) { + axis.motor.DC_calib.phB += (current - axis.motor.DC_calib.phB) * calib_filter_k; + } else { + axis.motor.DC_calib.phC += (current - axis.motor.DC_calib.phC) * calib_filter_k; + } + } +} + +void update_brake_current() { + float Ibus_sum = 0.0f; + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Ibus_sum += axes[i]->motor.current_control.Ibus; + } + // Note: set_brake_current will clip negative values to 0.0f + set_brake_current(-Ibus_sum); +} + +void set_brake_current(float brake_current) { + if (brake_current < 0.0f) brake_current = 0.0f; + float brake_duty = brake_current * brake_resistance / vbus_voltage; + + // Duty limit at 90% to allow bootstrap caps to charge + if (brake_duty > 0.9f) brake_duty = 0.9f; + int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; +} diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e591e778..2b414786 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -8,191 +8,10 @@ extern "C" { /* Includes ------------------------------------------------------------------*/ #include -#include "drv8301.h" - -//default timeout waiting for phase measurement signals -#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] +#include +#include /* Exported types ------------------------------------------------------------*/ -typedef enum { - M_SIGNAL_PH_CURRENT_MEAS = 1u << 0 -} Motor_thread_signals_t; - -typedef struct { - int index; - float *cogging_map; - bool use_anticogging; - bool calib_anticogging; - float calib_pos_threshold; - float calib_vel_threshold; -} Anticogging_t; - -typedef enum { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_TIMING, - ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_TIMING, - ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ENCODER_RESPONSE, - ERROR_ENCODER_MEASUREMENT_TIMEOUT, - ERROR_ADC_FAILED, - ERROR_CALIBRATION_TIMING, - ERROR_FOC_TIMING, - ERROR_FOC_MEASUREMENT_TIMEOUT, - ERROR_SCAN_MOTOR_TIMING, - ERROR_FOC_VOLTAGE_TIMING, - ERROR_GATEDRIVER_INVALID_GAIN, - ERROR_PWM_SRC_FAIL, - ERROR_UNEXPECTED_STEP_SRC, - ERROR_POS_CTRL_DURING_SENSORLESS, - ERROR_SPIN_UP_TIMEOUT, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_ENCODER_CPR_OUT_OF_RANGE, - ERROR_DC_BUS_BROWNOUT, -} Error_t; - -// Note: these should be sorted from lowest level of control to -// highest level of control, to allow "<" style comparisons. -typedef enum { - CTRL_MODE_VOLTAGE_CONTROL = 0, - CTRL_MODE_CURRENT_CONTROL = 1, - CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3 -} Motor_control_mode_t; - -typedef enum { - MOTOR_TYPE_HIGH_CURRENT = 0, - // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented - MOTOR_TYPE_GIMBAL = 2 -} Motor_type_t; - -typedef struct { - float phB; - float phC; -} Iph_BC_t; - -typedef struct { - float current_lim; // [A] - float p_gain; // [V/A] - float i_gain; // [V/As] - float v_current_control_integral_d; // [V] - float v_current_control_integral_q; // [V] - float Ibus; // DC bus current [A] - // Voltage applied at end of cycle: - float final_v_alpha; // [V] - float final_v_beta; // [V] - float Iq_setpoint; - float Iq_measured; - float max_allowed_current; -} Current_control_t; - -typedef enum { - ROTOR_MODE_ENCODER, - ROTOR_MODE_SENSORLESS, - ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing -} Rotor_mode_t; - -typedef struct { - float phase; - float pll_pos; - float pll_vel; - float pll_kp; - float pll_ki; - float observer_gain; // [rad/s] - float flux_state[2]; // [Vs] - float V_alpha_beta_memory[2]; // [V] - float pm_flux_linkage; // [V / (rad/s)] - bool estimator_good; - float spin_up_current; // [A] - float spin_up_acceleration; // [rad/s^2] - float spin_up_target_vel; // [rad/s] -} Sensorless_t; - -typedef struct { - TIM_HandleTypeDef* encoder_timer; - bool use_index; - bool index_found; - bool calibrated; - float idx_search_speed; - int32_t encoder_cpr; - int32_t encoder_offset; - int32_t encoder_state; - int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder. - float encoder_calib_range; - float phase; - float pll_pos; - float pll_vel; - float pll_kp; - float pll_ki; -} Encoder_t; - -typedef struct { - bool* enable_control; -} Axis_legacy_t; - -#define TIMING_LOG_SIZE 16 -typedef struct { - Axis_legacy_t axis_legacy; - Motor_control_mode_t control_mode; - bool enable_step_dir; - float counts_per_step; - Error_t error; - int32_t pole_pairs; - float pos_setpoint; - float pos_gain; - float vel_setpoint; - float vel_gain; - float vel_integrator_gain; - float vel_integrator_current; - float vel_limit; - float current_setpoint; - float calibration_current; - float resistance_calib_max_voltage; - float dc_bus_brownout_trip_level; - float phase_inductance; - float phase_resistance; - osThreadId motor_thread; - bool thread_ready; - // bool enable_control; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true - // bool do_calibration; // trigger motor calibration. will be reset to false after self test - // bool calibration_ok; - TIM_HandleTypeDef* motor_timer; - uint16_t next_timings[3]; - uint16_t control_deadline; - uint16_t last_cpu_time; - Iph_BC_t current_meas; - Iph_BC_t DC_calib; - DRV8301_Obj gate_driver; - DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers - Motor_type_t motor_type; - float shunt_conductance; - float phase_current_rev_gain; //Reverse gain for ADC to Amps - Current_control_t current_control; - Rotor_mode_t rotor_mode; - Encoder_t encoder; - Sensorless_t sensorless; - uint32_t loop_counter; - int timing_log_index; - uint16_t timing_log[TIMING_LOG_SIZE]; - // Cache for remote procedure calls arguments - struct { - float pos_setpoint; - float vel_feed_forward; - float current_feed_forward; - } set_pos_setpoint_args; - struct { - float vel_setpoint; - float current_feed_forward; - } set_vel_setpoint_args; - struct { - float current_setpoint; - } set_current_setpoint_args; - Anticogging_t anticogging; - DRV8301_FaultType_e drv_fault; -} Motor_t; typedef struct{ int type; @@ -200,74 +19,26 @@ typedef struct{ } monitoring_slot; /* Exported constants --------------------------------------------------------*/ -extern const size_t num_motors; extern const float elec_rad_per_enc; /* Exported variables --------------------------------------------------------*/ -extern float vbus_voltage; -extern float brake_resistance; -extern Motor_t motors[]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ //Note: to control without feed forward, set feed forward terms to 0.0f. -void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward); -void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward); -void set_current_setpoint(Motor_t* motor, float current_setpoint); -void step_cb(uint16_t GPIO_Pin); -void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index); void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); -void safe_assert(int arg); -void init_motor_control(); -void setEncoderCount(Motor_t* motor, uint32_t count); - -bool anti_cogging_calibration(Motor_t* motor); - -bool motor_calibration(Motor_t* motor); - - -//// Old private: // Utility -uint16_t check_timing(Motor_t* motor); void global_fault(int error); -float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue); // Initalisation -void DRV8301_setup(Motor_t* motor); void start_adc_pwm(); void start_pwm(TIM_HandleTypeDef* htim); void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); -// IRQ Callbacks (are all public) -// Measurement and calibrationa -bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage); -bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high); -bool calib_enc_offset(Motor_t* motor, float voltage_magnitude); -bool scan_for_enc_idx(Motor_t* motor, float v_d, float v_q); -bool anti_cogging_calibration(Motor_t* motor); -// Test functions -void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude); -// Main motor control -bool do_checks(Motor_t* motor); -bool loop_updates(Motor_t* motor); -void update_rotor(Motor_t* motor); -bool using_encoder(Motor_t* motor); -bool using_sensorless(Motor_t* motor); -float get_rotor_phase(Motor_t* motor); -float get_pll_vel(Motor_t* motor); -bool spin_up_sensorless(Motor_t* motor); void update_brake_current(); void set_brake_current(float brake_current); -void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); -void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); -bool FOC_voltage(Motor_t* motor, float v_d, float v_q); -bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); -void control_motor_loop(Motor_t* motor); - -//motor thread moved to axis object -//void motor_thread(void const * argument); #ifdef __cplusplus } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp new file mode 100644 index 00000000..1a2bb3e9 --- /dev/null +++ b/Firmware/MotorControl/main.cpp @@ -0,0 +1,110 @@ + +#include +#include + + +EncoderConfig_t encoder_configs[AXIS_COUNT]; +ControllerConfig_t controller_configs[AXIS_COUNT]; +MotorConfig_t motor_configs[AXIS_COUNT]; +AxisConfig_t axis_configs[AXIS_COUNT]; +Axis *axes[AXIS_COUNT]; + +bool enable_uart; + +typedef Config ConfigFormat; + +void save_configuration(void) { + if (ConfigFormat::safe_store_config( + &axis_configs, + &motor_configs, + &brake_resistance, + &enable_uart)) { + //printf("saving configuration failed\r\n"); osDelay(5); + } +} + +void load_configuration() { + if (NVM_init() || + ConfigFormat::safe_load_config( + &axis_configs, + &motor_configs, + &brake_resistance, + &enable_uart)) { + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axis_configs[i] = AxisConfig_t(); + motor_configs[i] = MotorConfig_t(); + } + brake_resistance = 0.47f; + enable_uart = true; + } +} + +void erase_configuration(void) { + NVM_erase(); +} + +extern "C" { +int odrive_main(void); +} + +int odrive_main(void) { + // Load persistent configuration (or defaults) + load_configuration(); + + // Construct all objects. + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Encoder *encoder = new Encoder(hw_configs[i].encoder_config, + encoder_configs[i]); + SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(); + Controller *controller = new Controller(controller_configs[i]); + Motor *motor = new Motor(hw_configs[i].motor_config, + hw_configs[i].gate_driver_config, + motor_configs[i]); + axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], + *encoder, *sensorless_estimator, *controller, *motor); + } + + // TODO: make dynamically reconfigurable + if (enable_uart) { + axes[0]->config.enable_step_dir_after_calibration = false; + axes[0]->set_step_dir_enabled(false); + SetGPIO12toUART(); + } +/* + // Init communications (this requires the axis objects to be constructed) + init_communication(); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512); + thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); + */ + + // Setup hardware for all components + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->setup(); + } + + // Start PWM and enable adc interrupts/callbacks + start_adc_pwm(); + + // This delay serves two purposes: + // - Let the current sense calibration converge (the current + // sense interrupts are firing in background by now) + // - Allow a user to interrupt the code, e.g. by flashing a new code, + // before it does anything crazy + // TODO make timing a function of calibration filter tau + osDelay(1500); + + // Start state machine threads. Each thread will go through various calibration + // procedures and then run the actual controller loops. + // TODO: generalize for AXIS_COUNT != 2 + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->start_thread(); + } + + return 0; +} diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp new file mode 100644 index 00000000..0e78235f --- /dev/null +++ b/Firmware/MotorControl/motor.cpp @@ -0,0 +1,322 @@ + +#include + +#include "drv8301.h" +//#include "motor.hpp" +#include + + +Motor::Motor(const MotorHardwareConfig_t& hw_config, + const GateDriverHardwareConfig_t& gate_driver_config, + MotorConfig_t& config) : + hw_config(hw_config), + gate_driver_config(gate_driver_config), + config(config), + gate_driver({ + .spiHandle = gate_driver_config.spi, + .EngpioHandle = gate_driver_config.enable_port, + .EngpioNumber = gate_driver_config.enable_pin, + .nCSgpioHandle = gate_driver_config.nCS_port, + .nCSgpioNumber = gate_driver_config.nCS_pin, + }) +{ +} + +void Motor::arm() { + __HAL_TIM_MOE_ENABLE(hw_config.timer); // enable pwm outputs +} + +void Motor::disarm() { + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config.timer); // disables pwm outputs +} + +// Set up the gate drivers +void Motor::DRV8301_setup() { + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + + DRV8301_enable(&gate_driver); + DRV8301_setupSpi(&gate_driver, local_regs); + + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + // 20V/V on 666uOhm gives a range of +/- 110A + // 40V/V on 666uOhm gives a range of +/- 55A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; + + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + phase_current_rev_gain = 1.0f / 80.0f; + break; + } + + float margin = 0.90f; + float max_input = margin * 0.3f * hw_config.shunt_conductance; + float max_swing = margin * 1.6f * hw_config.shunt_conductance * phase_current_rev_gain; + current_control.max_allowed_current = std::min(max_input, max_swing); + + local_regs->SndCmd = true; + DRV8301_writeData(&gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(&gate_driver, local_regs); +} + +//Returns true if everything is OK (no fault) +bool Motor::check_DRV_fault() { + //TODO: make this pin configurable per motor ch + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config.nFAULT_port, gate_driver_config.nFAULT_pin); + if (nFAULT_state == GPIO_PIN_RESET) { + // Update DRV Fault Code + drv_fault = DRV8301_getFaultType(&gate_driver); + // Update/Cache all SPI device registers + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + local_regs->RcvCmd = true; + DRV8301_readData(&gate_driver, local_regs); + return false; + }; + return true; +} + +uint16_t Motor::check_timing() { + TIM_HandleTypeDef* htim = hw_config.timer; + uint16_t timing = htim->Instance->CNT; + bool down = htim->Instance->CR1 & TIM_CR1_DIR; + if (down) { + uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; + timing = TIM_1_8_PERIOD_CLOCKS + delta; + } + + if (++(timing_log_index) == TIMING_LOG_SIZE) { + timing_log_index = 0; + } + timing_log[timing_log_index] = timing; + + return timing; +} + +float Motor::phase_current_from_adcval(uint32_t ADCValue) { + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; + float shunt_volt = amp_out_volt * phase_current_rev_gain; + float current = shunt_volt * hw_config.shunt_conductance; + return current; +} + +//-------------------------------- +// Measurement and calibration +//-------------------------------- + +// TODO check Ibeta balance to verify good motor connection +bool Motor::measure_phase_resistance(float test_current, float max_voltage) { + static const float kI = 10.0f; // [(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + float test_voltage = 0.0f; + + size_t i = 0; + axis->run_control_loop([&](){ + float Ialpha = -(current_meas.phB + current_meas.phC); + test_voltage += (kI * current_meas_period) * (test_current - Ialpha); + if (test_voltage > max_voltage || test_voltage < -max_voltage) { + error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; + return false; + } + + // Test voltage along phase A + enqueue_voltage_timings(test_voltage, 0.0f); + + return ++i < num_test_cycles; + }); + + //// De-energize motor + //enqueue_voltage_timings(motor, 0.0f, 0.0f); + + float R = test_voltage / test_current; + config.phase_resistance = R; + return i == num_test_cycles; // if we ran to completion that means success +} + +bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { + float test_voltages[2] = {voltage_low, voltage_high}; + float Ialphas[2] = {0.0f}; + static const int num_cycles = 5000; + + size_t t = 0; + axis->run_control_loop([&](){ + int i = t & 1; + Ialphas[i] += -current_meas.phB - current_meas.phC; + + // Test voltage along phase A + enqueue_voltage_timings(test_voltages[i], 0.0f); + + return ++t < (num_cycles << 1); + }); + + if (t != (num_cycles << 1)) + return false; // the loop aborted prematurely + + //// De-energize motor + //enqueue_voltage_timings(motor, 0.0f, 0.0f); + + float v_L = 0.5f * (voltage_high - voltage_low); + // Note: A more correct formula would also take into account that there is a finite timestep. + // However, the discretisation in the current control loop inverts the same discrepancy + float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); + float L = v_L / dI_by_dt; + + config.phase_inductance = L; + // TODO arbitrary values set for now + if (L < 1e-6f || L > 500e-6f) { + error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; + return false; + } + return true; +} + + +bool Motor::run_calibration() { + error = ERROR_NO_ERROR; + + float R_calib_max_voltage = config.resistance_calib_max_voltage; + if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(config.calibration_current, R_calib_max_voltage)) + return false; + if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage)) + return false; + } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + // no calibration needed + } else { + return false; + } + + // Calculate current control gains + float current_control_bandwidth = 1000.0f; // [rad/s] + current_control.p_gain = current_control_bandwidth * config.phase_inductance; + float plant_pole = config.phase_resistance / config.phase_inductance; + current_control.i_gain = plant_pole * current_control.p_gain; + + return true; +} + +void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { + float tA, tB, tC; + SVM(mod_alpha, mod_beta, &tA, &tB, &tC); + next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); +} + +void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { + float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); + float mod_alpha = vfactor * v_alpha; + float mod_beta = vfactor * v_beta; + enqueue_modulation_timings(mod_alpha, mod_beta); +} + +// 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 = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c*v_d - s*v_q; + float v_beta = c*v_q + s*v_d; + enqueue_voltage_timings(v_alpha, v_beta); + return true; +} + +bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { + Current_control_t* ictrl = ¤t_control; + + // For Reporting + ictrl->Iq_setpoint = Iq_des; + + // Clarke transform + float Ialpha = -current_meas.phB - current_meas.phC; + float Ibeta = one_by_sqrt3 * (current_meas.phB - current_meas.phC); + + // Park transform + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; + ictrl->Iq_measured = Iq; + + // Current error + float Ierr_d = Id_des - Id; + float Ierr_q = Iq_des - Iq; + + // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) + // Apply PI control + float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; + float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + + float mod_to_V = (2.0f / 3.0f) * vbus_voltage; + float V_to_mod = 1.0f / mod_to_V; + float mod_d = V_to_mod * Vd; + float mod_q = V_to_mod * Vq; + + // Vector modulation saturation, lock integrator if saturated + // TODO make maximum modulation configurable + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { + mod_d *= mod_scalefactor; + mod_q *= mod_scalefactor; + // TODO make decayfactor configurable + ictrl->v_current_control_integral_d *= 0.99f; + ictrl->v_current_control_integral_q *= 0.99f; + } else { + ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); + ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + } + + // Compute estimated bus current + ictrl->Ibus = mod_d * Id + mod_q * Iq; + + // Inverse park transform + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; + + // Report final applied voltage in stationary frame (for sensorles estimator) + ictrl->final_v_alpha = mod_to_V * mod_alpha; + ictrl->final_v_beta = mod_to_V * mod_beta; + + // Apply SVM + enqueue_modulation_timings(mod_alpha, mod_beta); + + update_brake_current(); + return true; +} + + +bool Motor::update(float current_setpoint, float phase) { + current_setpoint *= config.direction; + phase *= config.direction; + + // Execute current command + // TODO: move this into the mot + if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if(!FOC_current(0.0f, current_setpoint, 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)) + return false; + } else { + error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + return false; + } + return true; +} diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp new file mode 100644 index 00000000..3825b012 --- /dev/null +++ b/Firmware/MotorControl/motor.hpp @@ -0,0 +1,147 @@ +#ifndef __MOTOR_HPP +#define __MOTOR_HPP + +// The Motor declaration is needed in the axis header +//class Motor; +#include + +#include "drv8301.h" + +typedef enum { + ERROR_NO_ERROR, + ERROR_PHASE_RESISTANCE_TIMING, + ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT, + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, + ERROR_PHASE_INDUCTANCE_TIMING, + ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, + ERROR_ENCODER_RESPONSE, + ERROR_ENCODER_MEASUREMENT_TIMEOUT, + ERROR_ADC_FAILED, + ERROR_CALIBRATION_TIMING, + ERROR_FOC_TIMING, + ERROR_FOC_MEASUREMENT_TIMEOUT, + ERROR_SCAN_MOTOR_TIMING, + ERROR_FOC_VOLTAGE_TIMING, + ERROR_GATEDRIVER_INVALID_GAIN, + ERROR_PWM_SRC_FAIL, + ERROR_UNEXPECTED_STEP_SRC, + ERROR_POS_CTRL_DURING_SENSORLESS, + ERROR_SPIN_UP_TIMEOUT, + ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_ENCODER_CPR_OUT_OF_RANGE, + ERROR_DC_BUS_BROWNOUT, +} Error_t; + +typedef enum { + MOTOR_TYPE_HIGH_CURRENT = 0, + // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented + MOTOR_TYPE_GIMBAL = 2 +} Motor_type_t; + +typedef struct { + float phB; + float phC; +} Iph_BC_t; + +typedef struct { + float p_gain; // [V/A] + float i_gain; // [V/As] + float v_current_control_integral_d; // [V] + float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] + // Voltage applied at end of cycle: + float final_v_alpha; // [V] + float final_v_beta; // [V] + float Iq_setpoint; + float Iq_measured; + float max_allowed_current; +} Current_control_t; + +// NOTE: for gimbal motors, all units of A are instead V. +// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] +// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. +typedef struct { + int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. + float calibration_current = 10.0f; // [A] + float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + 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 + Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT; + + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // float current_lim = 75.0f; //[A] + float current_lim = 10.0f; //[A] +} MotorConfig_t; + +#define TIMING_LOG_SIZE 16 + +class Motor { +public: + Motor(const MotorHardwareConfig_t& hw_config, + const GateDriverHardwareConfig_t& gate_driver_config, + MotorConfig_t& config); + + void arm(); + void disarm(); + void setup() { + DRV8301_setup(); + } + void DRV8301_setup(); + bool check_DRV_fault(); + uint16_t check_timing(); + float phase_current_from_adcval(uint32_t ADCValue); + bool measure_phase_resistance(float test_current, float max_voltage); + bool measure_phase_inductance(float voltage_low, float voltage_high); + bool run_calibration(); + void enqueue_modulation_timings(float mod_alpha, float mod_beta); + void 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); + + const MotorHardwareConfig_t& hw_config; + const GateDriverHardwareConfig_t gate_driver_config; + MotorConfig_t& config; + Axis* axis = nullptr; // set by Axis constructor + +//private: + + DRV8301_Obj gate_driver; // initialized in constructor + + Error_t error = ERROR_NO_ERROR; + // bool enable_control = true; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true + // bool do_calibration = true; // trigger motor calibration. will be reset to false after self test + // bool calibration_ok = false; + uint16_t next_timings[3] = { + TIM_1_8_PERIOD_CLOCKS / 2, + TIM_1_8_PERIOD_CLOCKS / 2, + TIM_1_8_PERIOD_CLOCKS / 2 + }; + uint16_t last_cpu_time = 0; + Iph_BC_t current_meas = {0.0f, 0.0f}; + Iph_BC_t DC_calib = {0.0f, 0.0f}; + DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup) + float shunt_conductance = 1.0f / SHUNT_RESISTANCE; //[S] + float phase_current_rev_gain = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) + Current_control_t current_control = { + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .v_current_control_integral_d = 0.0f, + .v_current_control_integral_q = 0.0f, + .Ibus = 0.0f, + .final_v_alpha = 0.0f, + .final_v_beta = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, + .max_allowed_current = 0.0f, + }; + int timing_log_index = 0; + uint16_t timing_log[TIMING_LOG_SIZE] = { 0 }; + DRV8301_FaultType_e drv_fault = DRV8301_FaultType_NoFault; +}; + +#endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp new file mode 100644 index 00000000..bf6c6415 --- /dev/null +++ b/Firmware/MotorControl/nvm_config.hpp @@ -0,0 +1,142 @@ +/* +* Convenience functions to load and store multiple objects from and to NVM. +* +* The NVM stores consecutive one-to-one copies of arbitrary objects. +* The types of these objects are passed as template arguments to Config. +*/ + +/* Includes ------------------------------------------------------------------*/ + +#include +#include +#include + +#include "nvm.h" +#include "crc.hpp" +#include "low_level.h" +#include "axis.hpp" + + +/* Private defines -----------------------------------------------------------*/ +#define CONFIG_CRC16_INIT 0xabcd + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ + +// IMPORTANT: if you change, reorder or otherwise modify any of the fields in +// the config structs, make sure to increment this number: +static constexpr uint16_t config_version = 0x0001; + +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + + +// @brief Manages configuration load and store operations from and to NVM +// +// The NVM stores consecutive one-to-one copies of arbitrary objects. +// The types of these objects are passed as template arguments to Config. +// +// Config has two template specializations to implement template recursion: +// - Config handles loading/storing of the first object (type T) and leaves +// the rest of the objects to an "inner" class Config. +// - Config<> represents the leaf of the recursion. +template +struct Config; + +template<> +struct Config<> { + static size_t get_size() { + return 0; + } + static int load_config(size_t offset, uint16_t* crc16) { + return 0; + } + static int store_config(size_t offset, uint16_t* crc16) { + return 0; + } +}; + +template +struct Config { + static size_t get_size() { + return sizeof(T) + Config::get_size(); + } + + // @brief Loads one or more consecutive objects from the NVM. + // During loading this function also calculates the CRC over the loaded data. + // @param offset: 0 means that the function should start reading at the beginning + // of the last comitted NVM block + // @param crc16: the result of the CRC calculation is written to this address + // @param val0, vals: the values to be loaded + static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) { + size_t size = sizeof(T); + // save current CRC (in case val0 and crc16 point to the same address) + size_t previous_crc16 = *crc16; + if (NVM_read(offset, (uint8_t *)val0, size)) + return -1; + *crc16 = calc_crc16(previous_crc16, (uint8_t *)val0, size); + if (Config::load_config(offset + size, crc16, vals...)) + return -1; + return 0; + } + + // @brief Stores one or more consecutive objects to the NVM. + // During storing this function also calculates the CRC over the stored data. + // @param offset: 0 means that the function should start writing at the beginning + // of the currently active NVM write block + // @param crc16: the result of the CRC calculation is written to this address + // @param val0, vals: the values to be stored + static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) { + size_t size = sizeof(T); + if (NVM_write(offset, (uint8_t *)val0, size)) + return -1; + // update CRC _after_ writing (in case val0 and crc16 point to the same address) + if (crc16) + *crc16 = calc_crc16(*crc16, (uint8_t *)val0, size); + if (Config::store_config(offset + size, crc16, vals...)) + return -1; + return 0; + } + + // @brief Loads one or more consecutive objects from the NVM. The loaded data + // is validated using a CRC value that is stored at the beginning of the data. + static int safe_load_config(T* val0, Ts* ... vals) { + //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5); + if (Config::get_size() > NVM_get_max_read_length()) + return -1; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; + if (Config::load_config(0, &crc16, val0, vals..., &crc16)) + return -1; + if (crc16) + return -1; + return 0; + } + + // @brief Stores one or more consecutive objects to the NVM. In addition to the + // provided objects, a CRC of the data is stored. + // + // The CRC includes a version number and thus adds some protection against + // changes of the config structs during firmware update. Note that if the total + // config data length changes, the CRC validation will fail even if the developer + // forgets to update the config version number. + static int safe_store_config(const T* val0, const Ts* ... vals) { + size_t size = Config::get_size() + 2; + //printf("config is %d bytes\r\n", size); osDelay(5); + if (size > NVM_get_max_write_length()) + return -1; + if (NVM_start_write(size)) + return -1; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; + if (Config::store_config(0, &crc16, val0, vals...)) + return -1; + if (Config::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16)) + return -1; + if (NVM_commit()) + return -1; + return 0; + } +}; diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp new file mode 100644 index 00000000..664ed9e7 --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -0,0 +1,101 @@ + +//#include "sensorless_estimator.hpp" +#include + +SensorlessEstimator::SensorlessEstimator() +{ + // Calculate pll gains + // This calculation is currently identical to the PLL in Encoder + float pll_bandwidth = 1000.0f; // [rad/s] + pll_kp = 2.0f * pll_bandwidth; + + // Critically damped + pll_ki = 0.25f * (pll_kp * pll_kp); +} + +bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) { + // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer + // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf + // In particular, equation 8 (and by extension eqn 4 and 6). + + // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle + // is the one computed two cycles ago. To get the correct measurement, it was stored twice: + // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. + + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp < 1.0f)) { + axis->motor.error = ERROR_CALIBRATION_TIMING; + return false; + } + + // Clarke transform + float I_alpha_beta[2] = { + -axis->motor.current_meas.phB - axis->motor.current_meas.phC, + one_by_sqrt3 * (axis->motor.current_meas.phB - axis->motor.current_meas.phC)}; + + // alpha-beta vector operations + float eta[2]; + for (int i = 0; i <= 1; ++i) { + // y is the total flux-driving voltage (see paper eqn 4) + float y = -axis->motor.config.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory[i]; + // flux dynamics (prediction) + float x_dot = y; + // integrate prediction to current timestep + flux_state[i] += x_dot * current_meas_period; + + // eta is the estimated permanent magnet flux (see paper eqn 6) + eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + } + + // Non-linear observer (see paper eqn 8): + float pm_flux_sqr = pm_flux_linkage * pm_flux_linkage; + float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; + float bandwidth_factor = 1.0f / (pm_flux_linkage * pm_flux_linkage); + float eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); + + static float eta_factor_avg_test = 0.0f; + eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); + + // alpha-beta vector operations + for (int i = 0; i <= 1; ++i) { + // add observer action to flux estimate dynamics + float x_dot = eta_factor * eta[i]; + // convert action to discrete-time + flux_state[i] += x_dot * current_meas_period; + // update new eta + eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + } + + // Flux state estimation done, store V_alpha_beta for next timestep + V_alpha_beta_memory[0] = axis->motor.current_control.final_v_alpha; + V_alpha_beta_memory[1] = axis->motor.current_control.final_v_beta; + + // PLL + // TODO: the PLL part has some code duplication with the encoder PLL + // predict PLL phase with velocity + pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_vel); + // update PLL phase with observer permanent magnet phase + phase = fast_atan2(eta[1], eta[0]); + float delta_phase = wrap_pm_pi(phase - pll_pos); + pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_kp * delta_phase); + // update PLL velocity + pll_vel += current_meas_period * pll_ki * delta_phase; + + //TODO TEMP TEST HACK + // static int trigger_ctr = 0; + // if (++trigger_ctr >= 3*current_meas_hz) { + // trigger_ctr = 0; + + // //Change to sensorless units + // motor->vel_gain = 15.0f / 200.0f; + // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; + + // //Change mode + // motor->rotor_mode = ROTOR_MODE_SENSORLESS; + // } + + if (pos_estimate) *pos_estimate = pll_pos; + if (vel_estimate) *vel_estimate = pll_vel; + if (phase_output) *phase_output = phase; + return true; +}; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp new file mode 100644 index 00000000..9f276aa3 --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -0,0 +1,24 @@ +#ifndef __SENSORLESS_ESTIMATOR_HPP +#define __SENSORLESS_ESTIMATOR_HPP + +class SensorlessEstimator { +public: + SensorlessEstimator(); + + bool update(float* pos_estimate, float* vel_estimate, float* phase); + + Axis* axis = nullptr; // set by Axis constructor + + float phase = 0.0f; // [rad] + float pll_pos = 0.0f; // [rad] + float pll_vel = 0.0f; // [rad/s] + float pll_kp = 0.0f; // [rad/s / rad] + float pll_ki = 0.0f; // [(rad/s^2) / rad] + float observer_gain = 1000.0f; // [rad/s] + float flux_state[2] = {0.0f, 0.0f}; // [Vs] + float V_alpha_beta_memory[2] = {0.0f, 0.0f}; // [V] + float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } + bool estimator_good = false; +}; + +#endif /* __SENSORLESS_ESTIMATOR_HPP */ diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 032a996a..741810ed 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -4,8 +4,6 @@ #include #include -static const float one_by_sqrt3 = 0.57735026919f; -static const float two_by_sqrt3 = 1.15470053838f; int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { int Sextant; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index bf329605..a4d7a6e0 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -78,6 +78,10 @@ extern "C" { #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +static const float one_by_sqrt3 = 0.57735026919f; +static const float two_by_sqrt3 = 1.15470053838f; +static const float sqrt3_by_2 = 0.86602540378f; + // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index dec05271..e3908cde 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -65,12 +65,16 @@ build{ sources={ 'MotorControl/utils.c', 'MotorControl/legacy_commands.c', - 'MotorControl/low_level.c', + 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', 'MotorControl/commands.cpp', 'MotorControl/protocol.cpp', - 'MotorControl/config.cpp' + 'MotorControl/motor.cpp', + 'MotorControl/encoder.cpp', + 'MotorControl/controller.cpp', + 'MotorControl/sensorless_estimator.cpp', + 'MotorControl/main.cpp' }, includes={ 'MotorControl' From fb5f5fe03c836cd8ea96d86237d64f9cfee5ebcd Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 4 Mar 2018 15:33:53 -0800 Subject: [PATCH 002/112] change thread entry signature to take a non-const pointer --- .../Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 4 ++-- Firmware/Board/v3.3/Src/freertos.c | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index 3e00082a..1dd31f8a 100644 --- a/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -270,11 +270,11 @@ typedef enum { /// Entry point of a thread. /// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS. -typedef void (*os_pthread) (void const *argument); +typedef void (*os_pthread) (void *argument); /// Entry point of a timer call back function. /// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS. -typedef void (*os_ptimer) (void const *argument); +typedef void (*os_ptimer) (void *argument); // >>> the following data type definitions may shall adapted towards a specific RTOS diff --git a/Firmware/Board/v3.3/Src/freertos.c b/Firmware/Board/v3.3/Src/freertos.c index 601ff3e9..06caaf74 100644 --- a/Firmware/Board/v3.3/Src/freertos.c +++ b/Firmware/Board/v3.3/Src/freertos.c @@ -72,7 +72,7 @@ osThreadId thread_cmd_parse; /* USER CODE END Variables */ /* Function prototypes -------------------------------------------------------*/ -void StartDefaultTask(void const * argument); +void StartDefaultTask(void * argument); extern void MX_USB_DEVICE_Init(void); void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ @@ -134,7 +134,7 @@ void MX_FREERTOS_Init(void) { } /* StartDefaultTask function */ -void StartDefaultTask(void const * argument) +void StartDefaultTask(void * argument) { /* init code for USB_DEVICE */ MX_USB_DEVICE_Init(); From 97aedd46e9925b63d6c540fadb2442900780a321 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 4 Mar 2018 15:49:48 -0800 Subject: [PATCH 003/112] Sanitize error handling and state machine, move common stuff to odrive_main.hpp --- Firmware/MotorControl/axis.cpp | 191 +++++++++--------- Firmware/MotorControl/axis.hpp | 162 +++++---------- ...{board_config_v3.3.h => board_config_v3.h} | 0 Firmware/MotorControl/commands.cpp | 2 +- Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/controller.hpp | 8 + Firmware/MotorControl/encoder.cpp | 16 +- Firmware/MotorControl/encoder.hpp | 12 ++ Firmware/MotorControl/low_level.cpp | 8 +- Firmware/MotorControl/low_level.h | 3 - Firmware/MotorControl/main.cpp | 4 +- Firmware/MotorControl/motor.cpp | 31 +-- Firmware/MotorControl/motor.hpp | 43 ++-- Firmware/MotorControl/nvm_config.hpp | 2 - Firmware/MotorControl/odrive_main.hpp | 46 +++++ .../MotorControl/sensorless_estimator.cpp | 4 +- .../MotorControl/sensorless_estimator.hpp | 6 + 17 files changed, 271 insertions(+), 269 deletions(-) rename Firmware/MotorControl/{board_config_v3.3.h => board_config_v3.h} (100%) create mode 100644 Firmware/MotorControl/odrive_main.hpp diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 1ee31a0d..f23cc91f 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -4,7 +4,7 @@ #include "gpio.h" #include "utils.h" -#include "axis.hpp" +#include "odrive_main.hpp" Axis::Axis(const AxisHardwareConfig_t& hw_config, AxisConfig_t& config, @@ -25,26 +25,35 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, motor.axis = this; } -static void step_cb_wrapper(void* ctx) { - reinterpret_cast(ctx)->step_cb(); -} - +// @brief Sets up all components of the axis, +// such as gate driver and encoder hardware. void Axis::setup() { encoder.setup(); motor.setup(); } +static void run_state_machine_loop_wrapper(void* ctx) { + reinterpret_cast(ctx)->run_state_machine_loop(); +} + +// @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { - osThreadDef(thread_def, run_state_machine_loop, hw_config.thread_priority, 0, 512); + osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config.thread_priority, 0, 512); thread_id = osThreadCreate(osThread(thread_def), this); thread_id_valid = true; } +// @brief Unblocks the control loop thread. +// This is called from the current sense interrupt handler. void Axis::signal_thread(thread_signals sig) { if (thread_id_valid) osSignalSet(thread_id, sig); } +static void step_cb_wrapper(void* ctx) { + reinterpret_cast(ctx)->step_cb(); +} + // step/direction interface void Axis::step_cb() { if (enable_step_dir) { @@ -54,6 +63,7 @@ void Axis::step_cb() { } }; +// @brief Enables or disables step/dir input void Axis::set_step_dir_enabled(bool enable) { if (enable) { // Set up the direction GPIO as input @@ -76,23 +86,20 @@ void Axis::set_step_dir_enabled(bool enable) { } } -//Returns true if everything is OK (no fault) +// @brief Returns true if the power supply is within range bool Axis::check_PSU_brownout() { if(vbus_voltage < config.dc_bus_brownout_trip_level) - return false; + return error = ERROR_BAD_VOLTAGE, false; return true; } -// Returns true if everything is ok. Sets motor->error and returns false otherwise. +// @brief Returns true if everything is ok. +// Sets error and returns false otherwise. bool Axis::do_checks() { - if (!motor.check_DRV_fault()) { - motor.error = ERROR_DRV_FAULT; - return false; - } - if (!check_PSU_brownout()) { - motor.error = ERROR_DC_BUS_BROWNOUT; - return false; - } + if (!motor.do_checks()) + return error = ERROR_MOTOR_FAILED, false; + if (!check_PSU_brownout()) + return error = ERROR_BAD_VOLTAGE, false; return true; } @@ -104,10 +111,10 @@ bool Axis::run_sensorless_spin_up() { float I_mag = config.spin_up_current * x; x += current_meas_period / config.ramp_up_time; if (!motor.update(I_mag, phase)) - return false; + return error = ERROR_MOTOR_FAILED, false; return x < 1.0f; }); - if (x < 1.0f) + if (error != ERROR_NO_ERROR) return false; // Late Spin-up: accelerate @@ -118,10 +125,10 @@ bool Axis::run_sensorless_spin_up() { phase = wrap_pm_pi(phase + vel * current_meas_period); float I_mag = config.spin_up_current; if (!motor.update(I_mag, phase)) - return false; + return error = ERROR_MOTOR_FAILED, false; return vel < config.spin_up_target_vel; }); - return vel >= config.spin_up_target_vel; + return error == ERROR_NO_ERROR; } // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. @@ -129,15 +136,20 @@ bool Axis::run_sensorless_control_loop() { run_control_loop([this](){ float pos_estimate, vel_estimate, phase, current_setpoint; + if (controller.config.control_mode >= CTRL_MODE_POSITION_CONTROL) + return error = ERROR_POS_CTRL_DURING_SENSORLESS, false; + // We update the encoder just in case someone needs the output for testing encoder.update(nullptr, nullptr, nullptr); if (!sensorless_estimator.update(&pos_estimate, &vel_estimate, &phase)) - return false; + return error = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return false; - return motor.update(current_setpoint, phase); + return error = ERROR_CONTROLLER_FAILED, false; + if (!motor.update(current_setpoint, phase)) + return error = ERROR_MOTOR_FAILED, false; + return true; }); - return false; + return error == ERROR_NO_ERROR; } bool Axis::run_closed_loop_control_loop() { @@ -147,30 +159,30 @@ bool Axis::run_closed_loop_control_loop() { // We update the sensorless estimator just in case someone needs the output for testing sensorless_estimator.update(nullptr, nullptr, nullptr); if (!encoder.update(&pos_estimate, &vel_estimate, &phase)) - return false; + return error = ERROR_ENCODER_FAILED, false; if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return false; - return motor.update(current_setpoint, phase); + return error = ERROR_CONTROLLER_FAILED, false; + if (!motor.update(current_setpoint, phase)) + return error = ERROR_MOTOR_FAILED, false; + return true; }); - return false; + return error == ERROR_NO_ERROR; } bool Axis::run_idle_loop() { - // TODO: allow preemption - for (;;) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT; - break; - } + while (requested_state == AXIS_STATE_DONT_CARE) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) + return error = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; } - return false; + return error == ERROR_NO_ERROR; } +// Infinite loop that does calibration and enters main control loop as appropriate void Axis::run_state_machine_loop() { - //TODO: Move this somewhere else - // TODO: respect changes of CPR // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f + // TODO: Move this somewhere else + // TODO: respect changes of CPR int encoder_cpr = encoder.config.cpr; controller.anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); if (controller.anticogging.cogging_map != NULL) { @@ -179,89 +191,76 @@ void Axis::run_state_machine_loop() { } } - enum AxisState_t { - AXIS_STATE_MOTOR_CALIBRATION, - AXIS_STATE_ENCODER_CALIBRATION, - AXIS_STATE_SENSORLESS_SPINUP, - AXIS_STATE_SENSORLESS_CONTROL, - AXIS_STATE_CLOSED_LOOP_CONTROL, - AXIS_STATE_IDLE - }; - AxisState_t axis_state = AXIS_STATE_MOTOR_CALIBRATION; + current_state = AXIS_STATE_MOTOR_CALIBRATION; + bool force_state = false; for (;;) { - switch (axis_state) { + AxisState_t next_state = AXIS_STATE_DONT_CARE; + + switch (current_state) { + case AXIS_STATE_MOTOR_CALIBRATION: - if (!config.enable_motor_calibration || motor.run_calibration()) { - axis_state = AXIS_STATE_ENCODER_CALIBRATION; - } else { - axis_state = AXIS_STATE_IDLE; + { + bool skip = !force_state && !config.enable_motor_calibration; + if (skip || motor.run_calibration()) { + next_state = AXIS_STATE_ENCODER_CALIBRATION; + } else { + next_state = AXIS_STATE_IDLE; + } } break; + case AXIS_STATE_ENCODER_CALIBRATION: - if (!config.enable_encoder_calibration || encoder.run_calibration()) { - axis_state = config.enable_control ? - config.sensorless ? - AXIS_STATE_SENSORLESS_SPINUP : - AXIS_STATE_CLOSED_LOOP_CONTROL : - AXIS_STATE_IDLE; - if (axis_state != AXIS_STATE_IDLE) - set_step_dir_enabled(config.enable_step_dir_after_calibration); - } else { - axis_state = AXIS_STATE_IDLE; + { + bool skip = !force_state && !config.enable_encoder_calibration; + if (skip || encoder.run_calibration()) { + next_state = config.enable_closed_loop_control ? + AXIS_STATE_CLOSED_LOOP_CONTROL : + config.enable_sensorless_control ? + AXIS_STATE_SENSORLESS_SPINUP : + AXIS_STATE_IDLE; + if (next_state != AXIS_STATE_IDLE) + set_step_dir_enabled(config.enable_step_dir); + } else { + next_state = AXIS_STATE_IDLE; + } } break; + case AXIS_STATE_SENSORLESS_SPINUP: if (run_sensorless_spin_up()) { - axis_state = AXIS_STATE_SENSORLESS_CONTROL; + next_state = AXIS_STATE_SENSORLESS_CONTROL; } else { - axis_state = AXIS_STATE_IDLE; + next_state = AXIS_STATE_IDLE; } break; + case AXIS_STATE_SENSORLESS_CONTROL: run_sensorless_control_loop(); - axis_state = AXIS_STATE_IDLE; // TODO: restart if desired + next_state = AXIS_STATE_IDLE; // TODO: restart if desired break; + case AXIS_STATE_CLOSED_LOOP_CONTROL: run_closed_loop_control_loop(); - axis_state = AXIS_STATE_IDLE; + next_state = AXIS_STATE_IDLE; break; + case AXIS_STATE_IDLE: default: + current_state = AXIS_STATE_IDLE; run_idle_loop(); break; } + + if (requested_state != AXIS_STATE_DONT_CARE) { + current_state = requested_state; + requested_state = AXIS_STATE_DONT_CARE; + force_state = true; + } else { + current_state = next_state; + force_state = false; + } } -/* - bool calibration_ok = false; - for (;;) { - // Keep rotor estimation up to date while idling - osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - loop_updates(legacy_motor_ref_); - - if (do_calibration_) { - do_calibration_ = false; - calibration_ok = motor.do_calibration(); - if (calibration_ok) - calibration_ok = encoder.do_calibration(); - } - - if (calibration_ok && enable_control_) { - enable_step_dir = true; - if (rotor_mode == ROTOR_MODE_SENSORLESS) { - bool spin_up_ok = do_sensorless_spin_up(); - if (spin_up_ok) - do_sensorless_control(); - } else { - do_closed_loop_control(); - } - - if (enable_control_) { // if control is still enabled, we exited because of error - calibration_ok = false; - enable_control_ = false; - } - } - }*/ thread_id_valid = false; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index f8bbf79e..72931eeb 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -1,77 +1,32 @@ #ifndef __AXIS_HPP #define __AXIS_HPP -#include -#include -#include - -#include // Sets up the correct chip specifc defines required by arm_math -#define ARM_MATH_CM4 // TODO: might change in future board versions -#include +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif -#include +enum AxisState_t { + AXIS_STATE_STARTUP, + AXIS_STATE_MOTOR_CALIBRATION, + AXIS_STATE_ENCODER_CALIBRATION, + AXIS_STATE_SENSORLESS_SPINUP, + AXIS_STATE_SENSORLESS_CONTROL, + AXIS_STATE_CLOSED_LOOP_CONTROL, + AXIS_STATE_IDLE, + AXIS_STATE_DONT_CARE // used to indicate that no state request is pending +}; -/*class Estimator { -public: - virtual float get_position_estimation(void); - virtual float get_velocity_estimation(void); -};*/ -/* -class Motor { -public: - // @brief Updates the current control loop - virtual void update(float current_ref); -};*/ - -// The Axis declaration is needed in the other header files -class Axis; - -#include -#include -#include -#include -#include - -//default timeout waiting for phase measurement signals -#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] - -static const float current_meas_period = CURRENT_MEAS_PERIOD; -static const int current_meas_hz = CURRENT_MEAS_HZ; -extern float vbus_voltage; -extern float brake_resistance; // [ohm] - -constexpr size_t AXIS_COUNT = 2; -extern Axis *axes[AXIS_COUNT]; - -/* -class Controller { -public: - // @brief Updates the controller loop(s) - virtual void update(void); -};*/ - - -//Outside axis: - //command handler - //callback dispatch - -typedef enum { - ROTOR_MODE_ENCODER, - ROTOR_MODE_SENSORLESS, - ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing -} Rotor_mode_t; - -// TODO: decide if we want to consolidate all default configs in one file for ease of use? struct AxisConfig_t { - bool enable_motor_calibration = true; - bool enable_encoder_calibration = true; - bool enable_control = true; - bool sensorless = false; - bool enable_step_dir_after_calibration = true; // For M0 this has no effect if enable_uart is true + bool enable_motor_calibration = true; //(reinterpret_cast(ctx))->run_state_machine_loop(); - }; - void step_cb(); void set_step_dir_enabled(bool enable); @@ -132,29 +72,32 @@ public: bool check_PSU_brownout(); bool do_checks(); - // TODO: check if this uses dynamic memory - - - // @brief Runs the update handler at the frequency of the current measurements. + // @brief Runs the specified update handler at the frequency of the current measurements. // // The loop runs until one of the following conditions: // - the update handler returns false // - the current measurement times out - // - do_checks() becomes false + // - the health checks fail (brownout, driver fault line) // - update_handler doesn't finish in time // // The function arms the motor at the beginning of the control loop and disarms it at // the end of the control loop. - // Note that if this function returns, this should generally be considered an error condition, - // unless the termination was deliberately caused by the update_handler because it was of the - // opinion that the loop's task was completed. + // + // If the function returns, it is guaranteed that error is non-zero, except if the cause for the exit + // reason for the loop termination was a negative return value of update_handler or an external + // state change request (requested_state != AXIS_STATE_DONT_CARE). + // Under all exit conditions the motor is disarmed and the brake current set to zero. + // Furthermore, if the update_handler does not set the phase voltages in time, they will + // go to zero. + // // @tparam T Must be a callable type that takes no arguments and returns a bool template void run_control_loop(const T& update_handler) { motor.arm(); - while (true /*enable_control*/) { // TODO: check for state change + while (requested_state == AXIS_STATE_DONT_CARE + && error == ERROR_NO_ERROR /* error may be set by interrupt handler */ ) { if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT; + error = ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } @@ -162,10 +105,10 @@ public: // the voltages will go to zero. motor.enqueue_voltage_timings(0.0f, 0.0f); - if (!do_checks()) + if (!do_checks()) // error set during function call break; - if (!update_handler()) + if (!update_handler()) // error set during function call break; update_brake_current(); @@ -173,12 +116,10 @@ public: // Check we meet deadlines after queueing motor.last_cpu_time = motor.check_timing(); if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) { - motor.error = ERROR_PHASE_RESISTANCE_TIMING; + error = ERROR_CONTROL_LOOP_TIMEOUT; break; } ++loop_counter; - - // TODO: maybe we should just abort automatically as soon as error is set } // We are exiting control: disarm motor, reset Ibus, and update brake current @@ -192,6 +133,8 @@ public: bool run_closed_loop_control_loop(); bool run_idle_loop(); + void run_state_machine_loop(); + const AxisHardwareConfig_t& hw_config; AxisConfig_t& config; @@ -200,9 +143,12 @@ public: Controller& controller; Motor& motor; + Error_t error = ERROR_NO_ERROR; osThreadId thread_id; volatile bool thread_id_valid = false; - bool enable_step_dir = false; //auto enabled after calibration + bool enable_step_dir = false; // auto enabled after calibration, based on enable_step_dir_after_calibration + AxisState_t current_state = AXIS_STATE_STARTUP; + AxisState_t requested_state = AXIS_STATE_DONT_CARE; uint32_t loop_counter = 0; }; diff --git a/Firmware/MotorControl/board_config_v3.3.h b/Firmware/MotorControl/board_config_v3.h similarity index 100% rename from Firmware/MotorControl/board_config_v3.3.h rename to Firmware/MotorControl/board_config_v3.h diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index d95646b4..727fea65 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -8,7 +8,7 @@ #include "commands.h" #include "low_level.h" -#include "axis.hpp" +#include "odrive_main.hpp" #include "protocol.hpp" #include "freertos_vars.h" #include "utils.h" diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 9f385d2d..b3ccd70c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -1,5 +1,5 @@ -#include "axis.hpp" +#include "odrive_main.hpp" Controller::Controller(ControllerConfig_t& config) : diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 79dda566..8b168083 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -1,3 +1,9 @@ +#ifndef __CONTROLLER_HPP +#define __CONTROLLER_HPP + +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif // Note: these should be sorted from lowest level of control to // highest level of control, to allow "<" style comparisons. @@ -70,3 +76,5 @@ public: float current_setpoint; } set_current_setpoint_args; }; + +#endif // __CONTROLLER_HPP diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 8432cf83..1ad2b258 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -1,6 +1,6 @@ //#include "encoder.hpp" -#include "axis.hpp" +#include "odrive_main.hpp" Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, @@ -67,6 +67,8 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f); return ++i < start_lock_duration * current_meas_hz; }); + if (axis->error != Axis::ERROR_NO_ERROR) + return false; int32_t init_enc_val = (int16_t)hw_config.timer->Instance->CNT; int64_t encvaluesum = 0; @@ -83,7 +85,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { return ++i < num_steps; }); - if (i < num_steps) + if (axis->error != Axis::ERROR_NO_ERROR) return false; //TODO avoid recomputing elec_rad_per_enc every time @@ -92,7 +94,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range) { - axis->motor.error = ERROR_ENCODER_CPR_OUT_OF_RANGE; + error = ERROR_CPR_OUT_OF_RANGE; return false; } // check direction @@ -104,7 +106,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { axis->motor.config.direction = -1; } else { // Encoder response error - axis->motor.error = ERROR_ENCODER_RESPONSE; + error = ERROR_RESPONSE; return false; } @@ -120,7 +122,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { return ++i < num_steps; }); - if (i < num_steps) + if (axis->error != Axis::ERROR_NO_ERROR) return false; int offset = encvaluesum / (num_steps * 2); @@ -142,7 +144,7 @@ bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { // continue until the index is found return !index_found; }); - return index_found; + return axis->error == Axis::ERROR_NO_ERROR; } bool Encoder::run_calibration() { @@ -168,7 +170,7 @@ bool Encoder::run_calibration() { bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp < 1.0f)) { - axis->motor.error = ERROR_CALIBRATION_TIMING; + error = ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 856f00e5..6ab419b8 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -1,6 +1,10 @@ #ifndef __ENCODER_HPP #define __ENCODER_HPP +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif + struct EncoderConfig_t { bool use_index = false; bool calibrated = false; @@ -12,6 +16,13 @@ struct EncoderConfig_t { class Encoder { public: + enum Error_t { + ERROR_NONE, + ERROR_NUMERICAL, + ERROR_CPR_OUT_OF_RANGE, + ERROR_RESPONSE, + }; + Encoder(const EncoderHardwareConfig_t& hw_config, EncoderConfig_t& config); @@ -30,6 +41,7 @@ public: EncoderConfig_t& config; Axis* axis = nullptr; // set by Axis constructor + Error_t error = ERROR_NONE; volatile bool index_found = false; int32_t state = 0; float phase = 0.0f; // [rad] diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 8c907a5a..1dc791e9 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -21,7 +21,7 @@ #include #include -#include +#include "odrive_main.hpp" /* Private defines -----------------------------------------------------------*/ @@ -76,7 +76,7 @@ void start_adc_pwm() { HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -void global_fault(Error_t error) { +void halt_motors(Motor::Error_t error) { // Disable motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { axes[i]->motor.disarm(); @@ -84,7 +84,7 @@ void global_fault(Error_t error) { // Set fault codes, etc. for (size_t i = 0; i < AXIS_COUNT; ++i) { axes[i]->motor.error = error; - // TODO: update axis_state + axes[i]->error = Axis::ERROR_MOTOR_FAILED; } // disable brake resistor set_brake_current(0.0f); @@ -175,7 +175,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Ensure ADCs are expected ones to simplify the logic below if (!(hadc == &hadc2 || hadc == &hadc3)) { - global_fault(ERROR_ADC_FAILED); + halt_motors(Motor::ERROR_ADC_FAILED); return; }; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 2b414786..7654dfaa 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -19,7 +19,6 @@ typedef struct{ } monitoring_slot; /* Exported constants --------------------------------------------------------*/ -extern const float elec_rad_per_enc; /* Exported variables --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ @@ -29,8 +28,6 @@ extern const float elec_rad_per_enc; void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); -// Utility -void global_fault(int error); // Initalisation void start_adc_pwm(); void start_pwm(TIM_HandleTypeDef* htim); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 1a2bb3e9..b6a10061 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,5 +1,5 @@ -#include +#include "odrive_main.hpp" #include @@ -66,7 +66,7 @@ int odrive_main(void) { // TODO: make dynamically reconfigurable if (enable_uart) { - axes[0]->config.enable_step_dir_after_calibration = false; + axes[0]->config.enable_step_dir = false; axes[0]->set_step_dir_enabled(false); SetGPIO12toUART(); } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 0e78235f..044f4264 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -3,7 +3,7 @@ #include "drv8301.h" //#include "motor.hpp" -#include +#include "odrive_main.hpp" Motor::Motor(const MotorHardwareConfig_t& hw_config, @@ -90,6 +90,14 @@ bool Motor::check_DRV_fault() { return true; } +bool Motor::do_checks() { + if (!check_DRV_fault()) { + error = ERROR_DRV_FAULT; + return false; + } + return true; +} + uint16_t Motor::check_timing() { TIM_HandleTypeDef* htim = hw_config.timer; uint16_t timing = htim->Instance->CNT; @@ -129,23 +137,23 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { axis->run_control_loop([&](){ float Ialpha = -(current_meas.phB + current_meas.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); - if (test_voltage > max_voltage || test_voltage < -max_voltage) { - error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; - return false; - } + if (test_voltage > max_voltage || test_voltage < -max_voltage) + return error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A enqueue_voltage_timings(test_voltage, 0.0f); return ++i < num_test_cycles; }); + if (axis->error != Axis::ERROR_NO_ERROR) + return false; //// De-energize motor //enqueue_voltage_timings(motor, 0.0f, 0.0f); float R = test_voltage / test_current; config.phase_resistance = R; - return i == num_test_cycles; // if we ran to completion that means success + return true; // if we ran to completion that means success } bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { @@ -163,9 +171,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { return ++t < (num_cycles << 1); }); - - if (t != (num_cycles << 1)) - return false; // the loop aborted prematurely + if (axis->error != Axis::ERROR_NO_ERROR) + return false; //// De-energize motor //enqueue_voltage_timings(motor, 0.0f, 0.0f); @@ -178,10 +185,8 @@ 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 > 500e-6f) { - error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; - return false; - } + if (L < 1e-6f || L > 500e-6f) + return error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; return true; } diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 3825b012..b2029271 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -1,39 +1,12 @@ #ifndef __MOTOR_HPP #define __MOTOR_HPP -// The Motor declaration is needed in the axis header -//class Motor; -#include +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif #include "drv8301.h" -typedef enum { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_TIMING, - ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_TIMING, - ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ENCODER_RESPONSE, - ERROR_ENCODER_MEASUREMENT_TIMEOUT, - ERROR_ADC_FAILED, - ERROR_CALIBRATION_TIMING, - ERROR_FOC_TIMING, - ERROR_FOC_MEASUREMENT_TIMEOUT, - ERROR_SCAN_MOTOR_TIMING, - ERROR_FOC_VOLTAGE_TIMING, - ERROR_GATEDRIVER_INVALID_GAIN, - ERROR_PWM_SRC_FAIL, - ERROR_UNEXPECTED_STEP_SRC, - ERROR_POS_CTRL_DURING_SENSORLESS, - ERROR_SPIN_UP_TIMEOUT, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_ENCODER_CPR_OUT_OF_RANGE, - ERROR_DC_BUS_BROWNOUT, -} Error_t; - typedef enum { MOTOR_TYPE_HIGH_CURRENT = 0, // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented @@ -81,6 +54,15 @@ typedef struct { class Motor { public: + enum Error_t { + ERROR_NO_ERROR, + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, + ERROR_ADC_FAILED, + ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + }; + Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_t& config); @@ -92,6 +74,7 @@ public: } void DRV8301_setup(); bool check_DRV_fault(); + bool do_checks(); uint16_t check_timing(); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp index bf6c6415..7784322b 100644 --- a/Firmware/MotorControl/nvm_config.hpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -13,8 +13,6 @@ #include "nvm.h" #include "crc.hpp" -#include "low_level.h" -#include "axis.hpp" /* Private defines -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp new file mode 100644 index 00000000..0f927d01 --- /dev/null +++ b/Firmware/MotorControl/odrive_main.hpp @@ -0,0 +1,46 @@ +#ifndef __ODRIVE_MAIN_HPP +#define __ODRIVE_MAIN_HPP + +// stdlib includes +#include + +// System includes +#include + +// STM specific includes +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include + +// Hardware configuration +#if HW_VERSION_MAJOR == 3 +#include +#else +#error "unknown board version" +#endif + +class Axis; + +//default timeout waiting for phase measurement signals +#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] + +static const float current_meas_period = CURRENT_MEAS_PERIOD; +static const int current_meas_hz = CURRENT_MEAS_HZ; +extern float vbus_voltage; +extern float brake_resistance; // [ohm] +extern const float elec_rad_per_enc; + +constexpr size_t AXIS_COUNT = 2; +extern Axis *axes[AXIS_COUNT]; + + +// ODrive specific includes +#include +#include +#include +#include +#include +#include +#include + +#endif /* __ODRIVE_MAIN_HPP */ diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 664ed9e7..1ba30015 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,6 +1,6 @@ //#include "sensorless_estimator.hpp" -#include +#include "odrive_main.hpp" SensorlessEstimator::SensorlessEstimator() { @@ -24,7 +24,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp < 1.0f)) { - axis->motor.error = ERROR_CALIBRATION_TIMING; + error = ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 9f276aa3..b256e24c 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -3,12 +3,18 @@ class SensorlessEstimator { public: + enum Error_t { + ERROR_NONE, + ERROR_NUMERICAL, + }; + SensorlessEstimator(); bool update(float* pos_estimate, float* vel_estimate, float* phase); Axis* axis = nullptr; // set by Axis constructor + Error_t error = ERROR_NONE; float phase = 0.0f; // [rad] float pll_pos = 0.0f; // [rad] float pll_vel = 0.0f; // [rad/s] From f5081352b367ad51f82574a37830badef4a6cd1d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 8 Mar 2018 12:56:24 -0800 Subject: [PATCH 004/112] refactor protocol --- Firmware/Board/v3.3/Inc/FreeRTOSConfig.h | 2 +- Firmware/Board/v3.3/Src/freertos.c | 2 +- Firmware/Board/v3.3/Src/syscalls.c | 3 - Firmware/Board/v3.3/Src/usbd_cdc_if.c | 2 +- Firmware/MotorControl/axis.hpp | 6 +- Firmware/MotorControl/commands.h | 4 +- Firmware/MotorControl/controller.hpp | 19 +- Firmware/MotorControl/encoder.cpp | 12 +- Firmware/MotorControl/legacy_commands.c | 4 +- Firmware/MotorControl/main.cpp | 15 +- Firmware/MotorControl/motor.hpp | 58 +- Firmware/MotorControl/odrive_main.hpp | 7 + Firmware/MotorControl/protocol.cpp | 136 +++-- Firmware/MotorControl/protocol.hpp | 495 +++++++++++++----- .../MotorControl/sensorless_estimator.hpp | 1 + 15 files changed, 521 insertions(+), 245 deletions(-) diff --git a/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h index 0028db5d..3a55d727 100644 --- a/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h +++ b/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h @@ -102,7 +102,7 @@ #define configTICK_RATE_HZ ((TickType_t)1000) #define configMAX_PRIORITIES ( 7 ) #define configMINIMAL_STACK_SIZE ((uint16_t)128) -#define configTOTAL_HEAP_SIZE ((size_t)15360) +#define configTOTAL_HEAP_SIZE ((size_t)15360*3) #define configMAX_TASK_NAME_LEN ( 16 ) #define configUSE_16_BIT_TICKS 0 #define configUSE_MUTEXES 1 diff --git a/Firmware/Board/v3.3/Src/freertos.c b/Firmware/Board/v3.3/Src/freertos.c index 06caaf74..a6ccbf25 100644 --- a/Firmware/Board/v3.3/Src/freertos.c +++ b/Firmware/Board/v3.3/Src/freertos.c @@ -121,7 +121,7 @@ void MX_FREERTOS_Init(void) { /* Create the thread(s) */ /* definition and creation of defaultTask */ - osThreadDef(defaultTask, StartDefaultTask, osPriorityIdle, 0, 256); + osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 256); defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); /* USER CODE BEGIN RTOS_THREADS */ diff --git a/Firmware/Board/v3.3/Src/syscalls.c b/Firmware/Board/v3.3/Src/syscalls.c index d546266c..3ddb5701 100644 --- a/Firmware/Board/v3.3/Src/syscalls.c +++ b/Firmware/Board/v3.3/Src/syscalls.c @@ -23,7 +23,6 @@ static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; int _write(int file, char* data, int len) { -#if 0 // TODO: revert! //number of bytes written int written = 0; switch (serial_printf_select) { @@ -58,8 +57,6 @@ int _write(int file, char* data, int len) { } return written; -#endif - return len; } void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { diff --git a/Firmware/Board/v3.3/Src/usbd_cdc_if.c b/Firmware/Board/v3.3/Src/usbd_cdc_if.c index ff11152b..b38385f7 100644 --- a/Firmware/Board/v3.3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3.3/Src/usbd_cdc_if.c @@ -274,7 +274,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ - //set_cmd_buffer(Buf, *Len); TODO: revert! + set_cmd_buffer(Buf, *Len); osSemaphoreRelease(sem_usb_rx); return (USBD_OK); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 72931eeb..ff52a51b 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -143,10 +143,12 @@ public: Controller& controller; Motor& motor; - Error_t error = ERROR_NO_ERROR; osThreadId thread_id; volatile bool thread_id_valid = false; - bool enable_step_dir = false; // auto enabled after calibration, based on enable_step_dir_after_calibration + + // variables exposed on protocol + Error_t error = ERROR_NO_ERROR; + bool enable_step_dir = false; // auto enabled after calibration, based on config.enable_step_dir AxisState_t current_state = AXIS_STATE_STARTUP; AxisState_t requested_state = AXIS_STATE_DONT_CARE; uint32_t loop_counter = 0; diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/commands.h index 0d9d7c87..2c82ad88 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/commands.h @@ -35,9 +35,9 @@ extern "C" { #endif void init_communication(void); -void communication_task(void const * argument); +void communication_task(void * ctx); void set_cmd_buffer(uint8_t *buf, uint32_t len); -void usb_update_thread(); +void usb_update_thread(void * ctx); void USB_receive_packet(const uint8_t *buffer, size_t length); #ifdef __cplusplus diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 8b168083..df6e22e9 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -18,6 +18,7 @@ struct ControllerConfig_t { Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t float pos_gain = 20.0f; // [(counts/s) / counts] float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] float vel_limit = 20000.0f; // [counts/s] }; @@ -38,12 +39,11 @@ public: ControllerConfig_t& config; Axis* axis = nullptr; // set by Axis constructor - float pos_setpoint = 0.0f; - float vel_setpoint = 0.0f; - // float vel_setpoint = 800.0f; - // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_current = 0.0f; // [A] - float current_setpoint = 0.0f; // [A] + // TODO: anticogging overhaul: + // - expose selected (all?) variables on protocol + // - make calibration user experience similar to motor & encoder calibration + // - use python tools to Fourier transform and write back the smoothed map or Fourier coefficients + // - make the calibration persistent typedef struct { int index; @@ -62,6 +62,13 @@ public: .calib_vel_threshold = 1.0f, }; + // variables exposed on protocol + float pos_setpoint = 0.0f; + float vel_setpoint = 0.0f; + // float vel_setpoint = 800.0f; + float vel_integrator_current = 0.0f; // [A] + float current_setpoint = 0.0f; // [A] + // Cache for remote procedure calls arguments TODO: remove struct { float pos_setpoint; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 1ad2b258..f4d81da7 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -57,9 +57,9 @@ void Encoder::set_count(uint32_t count) { // TODO: add check_timing bool Encoder::calib_enc_offset(float voltage_magnitude) { static const float start_lock_duration = 1.0f; - static const float scan_duration = 1.0f; - static const float scan_range = 16.0f * M_PI; - static const size_t num_steps = scan_duration * current_meas_hz; + static const float scan_omega = 4.0f * M_PI; + static const float scan_distance = 16.0f * M_PI; + static const size_t num_steps = scan_distance / scan_omega * current_meas_hz; // go to motor zero phase for start_lock_duration to get ready to scan size_t i = 0; @@ -76,7 +76,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { // scan forward i = 0; axis->run_control_loop([&](){ - float phase = wrap_pm_pi(scan_range * (float)i / (float)num_steps - scan_range / 2.0f); + float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); axis->motor.enqueue_voltage_timings(v_alpha, v_beta); @@ -90,7 +90,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); - float expected_encoder_delta = scan_range / elec_rad_per_enc; + float expected_encoder_delta = scan_distance / elec_rad_per_enc; float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range) { @@ -113,7 +113,7 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { // scan backwards i = 0; axis->run_control_loop([&](){ - float phase = wrap_pm_pi(-scan_range * (float)i / (float)num_steps + scan_range / 2.0f); + float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); axis->motor.enqueue_voltage_timings(v_alpha, v_beta); diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c index 87df7140..29a6c33a 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -1,7 +1,7 @@ /* Includes ------------------------------------------------------------------*/ #include "legacy_commands.h" #include -#if 0 + /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ @@ -10,7 +10,7 @@ // recently recieved a command. In the future we may want to separate // debug printf and the main serial comms. SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART; - +#if 0 /* Private constant data -----------------------------------------------------*/ // variables exposed to usb/serial interface via set/get/monitor diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index b6a10061..6dde3afd 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -23,7 +23,7 @@ void save_configuration(void) { } } -void load_configuration() { +void load_configuration(void) { if (NVM_init() || ConfigFormat::safe_load_config( &axis_configs, @@ -65,24 +65,17 @@ int odrive_main(void) { } // TODO: make dynamically reconfigurable +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (enable_uart) { axes[0]->config.enable_step_dir = false; axes[0]->set_step_dir_enabled(false); SetGPIO12toUART(); } -/* +#endif + //osDelay(100); // Init communications (this requires the axis objects to be constructed) init_communication(); - // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512); - thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); - - // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); - thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); - */ - // Setup hardware for all components for (size_t i = 0; i < AXIS_COUNT; ++i) { axes[i]->setup(); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index b2029271..877921f9 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -94,21 +94,20 @@ public: //private: DRV8301_Obj gate_driver; // initialized in constructor - - Error_t error = ERROR_NO_ERROR; - // bool enable_control = true; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true - // bool do_calibration = true; // trigger motor calibration. will be reset to false after self test - // bool calibration_ok = false; uint16_t next_timings[3] = { TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2 }; uint16_t last_cpu_time = 0; + int timing_log_index = 0; + uint16_t timing_log[TIMING_LOG_SIZE] = { 0 }; + + // variables exposed on protocol + Error_t error = ERROR_NO_ERROR; Iph_BC_t current_meas = {0.0f, 0.0f}; Iph_BC_t DC_calib = {0.0f, 0.0f}; - DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup) - float shunt_conductance = 1.0f / SHUNT_RESISTANCE; //[S] + const float shunt_conductance = 1.0f / SHUNT_RESISTANCE; //[S] float phase_current_rev_gain = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) Current_control_t current_control = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement @@ -122,9 +121,50 @@ public: .Iq_measured = 0.0f, .max_allowed_current = 0.0f, }; - int timing_log_index = 0; - uint16_t timing_log[TIMING_LOG_SIZE] = { 0 }; DRV8301_FaultType_e drv_fault = DRV8301_FaultType_NoFault; + DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup) + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_property("error", reinterpret_cast(&this->error)), + make_protocol_ro_property("current_meas.phB", &this->current_meas.phB), + make_protocol_ro_property("current_meas.phC", &this->current_meas.phC), + make_protocol_property("DC_calib.phB", &this->DC_calib.phB), + make_protocol_property("DC_calib.phC", &this->DC_calib.phC), + make_protocol_property("shunt_conductance", &this->shunt_conductance), + make_protocol_property("phase_current_rev_gain", &this->phase_current_rev_gain), + make_protocol_object("current_control", + make_protocol_property("p_gain", &this->current_control.p_gain), + make_protocol_property("i_gain", &this->current_control.i_gain), + make_protocol_property("v_current_control_integral_d", &this->current_control.v_current_control_integral_d), + make_protocol_property("v_current_control_integral_q", &this->current_control.v_current_control_integral_q), + make_protocol_property("Ibus", &this->current_control.Ibus), + make_protocol_property("final_v_alpha", &this->current_control.final_v_alpha), + make_protocol_property("final_v_beta", &this->current_control.final_v_beta), + make_protocol_property("Iq_setpoint", &this->current_control.Iq_setpoint), + make_protocol_property("Iq_measured", &this->current_control.Iq_measured), + make_protocol_property("max_allowed_current", &this->current_control.max_allowed_current) + ), + make_protocol_object("gate_driver", + make_protocol_ro_property("drv_fault", reinterpret_cast(&this->drv_fault)), + make_protocol_ro_property("status_reg_1", &this->gate_driver_regs.Stat_Reg_1_Value), + make_protocol_ro_property("status_reg_2", &this->gate_driver_regs.Stat_Reg_2_Value), + make_protocol_ro_property("ctrl_reg_1", &this->gate_driver_regs.Ctrl_Reg_1_Value), + make_protocol_ro_property("ctrl_reg_2", &this->gate_driver_regs.Ctrl_Reg_2_Value) + ), + make_protocol_object("config", + make_protocol_property("pole_pairs", &this->config.pole_pairs), + make_protocol_property("calibration_current", &this->config.calibration_current), + make_protocol_property("resistance_calib_max_voltage", &this->config.resistance_calib_max_voltage), + make_protocol_property("phase_inductance", &this->config.phase_inductance), + make_protocol_property("phase_resistance", &this->config.phase_resistance), + make_protocol_property("direction", &this->config.direction), + make_protocol_property("motor_type", reinterpret_cast(&this->config.motor_type)), + make_protocol_property("current_lim", &this->config.current_lim) + ) + ); + } }; #endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 0f927d01..e9bb285d 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -35,6 +35,7 @@ extern Axis *axes[AXIS_COUNT]; // ODrive specific includes +#include #include #include #include @@ -43,4 +44,10 @@ extern Axis *axes[AXIS_COUNT]; #include #include +#include // TODO: remove + +// defined in main.cpp +void save_configuration(void); +void erase_configuration(void); + #endif /* __ODRIVE_MAIN_HPP */ diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index 56f9afb5..3e0336fa 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -8,16 +8,7 @@ #include /* Private defines -----------------------------------------------------------*/ -// Note that this option cannot be used to debug UART because it prints on UART -//#define DEGUG_PROTOCOL /* Private macros ------------------------------------------------------------*/ - -#ifdef DEGUG_PROTOCOL -#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0) -#else -#define LOG_PROTO(...) ((void) 0) -#endif - /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ /* Global variables ----------------------------------------------------------*/ @@ -46,48 +37,6 @@ void hexdump(const uint8_t* buf, size_t len) { } #endif -static inline int write_string(const char* str, StreamSink* output) { - return output->process_bytes(reinterpret_cast(str), strlen(str)); -} - -void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const { - if (type_ == CLOSE_TREE) { - write_string("]}", output); - *need_comma = true; - } else { - if (*need_comma) - write_string(",", output); - - // write name - write_string("{\"name\":\"", output); - if (name_) - write_string(name_, output); - - // write endpoint ID - write_string("\",\"id\":", output); - char id_buf[10]; - snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf - write_string(id_buf, output); - - // write additional JSON data - if (json_modifier_ && json_modifier_[0]) { - write_string(",", output); - write_string(json_modifier_, output); - } - - if (type_ == BEGIN_OBJECT) { - write_string(",\"members\":[", output); - *need_comma = false; - } else if (type_ == BEGIN_FUNCTION) { - write_string(",\"arguments\":[", output); - *need_comma = false; - } else if (type_ == PROPERTY) { - write_string("}", output); - *need_comma = true; - } - } -} - int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) { @@ -157,36 +106,80 @@ int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length } -// Calculates the CRC16 of the JSON interface descriptor. -// The init value is the protocol version. -uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) { - CRC16Calculator crc16_calculator(PROTOCOL_VERSION); +class JSONDescriptorEndpoint : Endpoint { +public: + static constexpr size_t endpoint_count = 1; + void write_json(size_t id, StreamSink* output); + void register_endpoints(Endpoint** list, size_t id, size_t length); + void handle(const uint8_t* input, size_t input_length, StreamSink* output); +}; - uint8_t offset[4] = { 0 }; - interface_query(offset, sizeof(offset), &crc16_calculator); +JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint(); +EndpointProvider* application_endpoints; +uint16_t json_crc_; - return crc16_calculator.get_crc16(); +Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; +size_t n_endpoints_ = 0; +EndpointProvider* endpoint_provider_ = nullptr; + +void JSONDescriptorEndpoint::write_json(size_t id, StreamSink* output) { + write_string("{\"name\":\"\",", output); + + // write endpoint ID + write_string("\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + write_string(",\"type\":\"json\",\"access\":\"r\"}", output); } +void JSONDescriptorEndpoint::register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + +}; + // Returns part of the JSON interface definition. -void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) { +void JSONDescriptorEndpoint::handle(const uint8_t* input, size_t input_length, StreamSink* output) { // The request must contain a 32 bit integer to specify an offset if (input_length < 4) return; uint32_t offset = 0; read_le(&offset, input); NullStreamSink output_with_offset = NullStreamSink(offset, *output); - - bool need_comma = false; + + size_t id = 0; write_string("[", &output_with_offset); - for (size_t i = 0; i < n_endpoints_; ++i) { - get_endpoint(i)->write_json(i, &need_comma, &output_with_offset); - if (!output->get_free_space()) - return; // return early if the output cannot take more bytes - } + json_file_endpoint.write_json(id, &output_with_offset); + id += decltype(json_file_endpoint)::endpoint_count; + write_string(",", &output_with_offset); + application_endpoints->write_json(id, &output_with_offset); write_string("]", &output_with_offset); } +void set_application_endpoints(EndpointProvider* endpoints) { + application_endpoints = endpoints; + + n_endpoints_ = 0; + json_file_endpoint.register_endpoints(endpoints_, 0, MAX_ENDPOINTS); + n_endpoints_ += decltype(json_file_endpoint)::endpoint_count; + application_endpoints->register_endpoints(endpoints_, n_endpoints_, MAX_ENDPOINTS); + n_endpoints_ += application_endpoints->get_endpoint_count(); + + // Calculates the CRC16 of the JSON file. + // The init value is the protocol version. + CRC16Calculator crc16_calculator(PROTOCOL_VERSION); + uint8_t offset[4] = { 0 }; + json_file_endpoint.handle(offset, sizeof(offset), &crc16_calculator); + json_crc_ = crc16_calculator.get_crc16(); + + + CRC16Calculator crc16_calculator2(PROTOCOL_VERSION); + endpoints_[0]->handle(offset, sizeof(offset), &crc16_calculator2); + json_crc_ = crc16_calculator2.get_crc16(); +} + int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) { LOG_PROTO("got packet of length %d: \r\n", length); hexdump(buffer, length); @@ -205,10 +198,15 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_ bool expect_response = endpoint_id & 0x8000; endpoint_id &= 0x7fff; - const Endpoint* endpoint = get_endpoint(endpoint_id); - if (!endpoint) + if (endpoint_id >= n_endpoints_) return -1; + Endpoint* endpoint = endpoints_[endpoint_id]; + if (!endpoint) { + LOG_PROTO("critical: no endpoint at %d", endpoint_id); + return -1; + } + // Verify packet trailer. The expected trailer value depends on the selected endpoint. // For endpoint 0 this is just the protocol version, for all other endpoints it's a // CRC over the entire JSON descriptor tree (this may change in future versions). @@ -218,7 +216,7 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_ LOG_PROTO("trailer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_trailer, actual_trailer); return -1; } - LOG_PROTO("trailer ok\r\n"); + LOG_PROTO("trailer ok for endpoint %d\r\n", endpoint_id); // TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible? diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index 8b2e1edd..c4e1652f 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -13,6 +13,14 @@ see protocol.md for the protocol specification #include #include "crc.hpp" +// Note that this option cannot be used to debug UART because it prints on UART +//#define DEBUG_PROTOCOL +#ifdef DEBUG_PROTOCOL +#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0) +#else +#define LOG_PROTO(...) ((void) 0) +#endif + constexpr uint8_t SYNC_BYTE = 0xAA; constexpr uint8_t CRC8_INIT = 0x42; @@ -74,7 +82,8 @@ template<> inline size_t write_le(float value, uint8_t* buffer) { static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); - return write_le(*reinterpret_cast(&value), buffer); + const uint32_t * value_as_uint32 = reinterpret_cast(&value); + return write_le(*value_as_uint32, buffer); } template<> @@ -268,15 +277,6 @@ private: }; - -typedef enum { - PROPERTY, - BEGIN_OBJECT, - BEGIN_FUNCTION, - CLOSE_TREE -} EndpointType_t; - - // @brief Endpoint request handler // // When passed a valid endpoint context, implementing functions shall handle an @@ -293,8 +293,7 @@ typedef std::function -void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - const T* value = reinterpret_cast(ctx); +void default_readwrite_endpoint_handler(const T* value, const uint8_t* input, size_t input_length, StreamSink* output) { // If the old value was requested, call the corresponding little endian serialization function if (output) { // TODO: make buffer size dependent on the type @@ -306,11 +305,9 @@ void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input } template -void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - T* value = reinterpret_cast(ctx); - +void default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { // Read the endpoint value into output - default_read_endpoint_handler(ctx, input, input_length, output); + default_readwrite_endpoint_handler(const_cast(value), input, input_length, output); // If a new value was passed, call the corresponding little endian deserialization function uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type @@ -318,124 +315,80 @@ void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t read_le(value, input); } -static void trigger_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - (void) input; - (void) input_length; - (void) output; - std::function function = reinterpret_cast(ctx); - function(); -} template static inline const char* get_default_json_modifier(); template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"float\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"float\",\"access\":\"rw\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"int32\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"int32\",\"access\":\"rw\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint32\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint32\",\"access\":\"rw\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint16\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint16\",\"access\":\"rw\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint8\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint8\",\"access\":\"rw\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"bool\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"bool\",\"access\":\"rw\""; } +constexpr size_t MAX_ENDPOINTS = 100; + class Endpoint { public: - const char* const name_; - - Endpoint(const char* name, EndpointType_t type, EndpointHandler handler, const char* json_modifier, void *ctx) : - name_(name), - type_(type), - handler_(handler), - json_modifier_(json_modifier), - ctx_(ctx) - { - } - - template - static Endpoint make_property(const char* name, const T* ctx) { - return Endpoint(name, PROPERTY, - default_read_endpoint_handler, - get_default_json_modifier(), - const_cast(ctx) /* it's safe to cast the const away here because we - know that the default_read_endpoint_handler immediately adds it back */); - } - - template - static Endpoint make_property(const char* name, T* ctx) { - return Endpoint(name, PROPERTY, - default_readwrite_endpoint_handler, - get_default_json_modifier(), ctx); - } - - static Endpoint make_object(const char* name) { - return Endpoint(name, BEGIN_OBJECT, nullptr, - "\"type\":\"object\"", nullptr); - } - - static Endpoint make_function(const char* name, void(*function)(void)) { - return Endpoint(name, BEGIN_FUNCTION, trigger_endpoint_handler, - "\"type\":\"function\"", reinterpret_cast(function)); - } - - static Endpoint close_tree() { - return Endpoint(nullptr, CLOSE_TREE, nullptr, nullptr, nullptr); - } - - void write_json(size_t id, bool* need_comma, StreamSink* output) const; - - void handle(const uint8_t* input, size_t input_length, StreamSink* output) const { - if (handler_) - return handler_(ctx_, input, input_length, output); - } - -private: - const EndpointType_t type_; - const EndpointHandler handler_; - const char* json_modifier_; - void* const ctx_; + //const char* const name_; + virtual void handle(const uint8_t* input, size_t input_length, StreamSink* output) = 0; }; +class EndpointProvider { +public: + virtual size_t get_endpoint_count() = 0; + virtual void write_json(size_t id, StreamSink* output) = 0; + virtual void register_endpoints(Endpoint** list, size_t id, size_t length) = 0; +}; + + +static inline int write_string(const char* str, StreamSink* output) { + return output->process_bytes(reinterpret_cast(str), strlen(str)); +} + /* @brief Handles the communication protocol on one channel. * @@ -446,55 +399,333 @@ private: */ class BidirectionalPacketBasedChannel : public PacketSink { public: - BidirectionalPacketBasedChannel(const Endpoint* endpoints, size_t n_endpoints, PacketSink& output) : - global_endpoints_(endpoints), - n_endpoints_(NUM_CHANNEL_SPECIFIC_ENDPOINTS + n_endpoints), - output_(output), - json_crc_(calculate_json_crc16()) - { - } + BidirectionalPacketBasedChannel(PacketSink& output) : + output_(output) + { } int process_packet(const uint8_t* buffer, size_t length); - private: - - uint16_t calculate_json_crc16(void); - void interface_query(const uint8_t* input, size_t input_length, StreamSink* output); - - static void interface_query_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - reinterpret_cast(ctx)->interface_query(input, input_length, output); - } - - static void subscription_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - reinterpret_cast(ctx)->subscription(input, input_length, output); - } - - const Endpoint channel_specific_endpoints_[1] = { - Endpoint("", PROPERTY, BidirectionalPacketBasedChannel::interface_query_handler, "\"type\":\"json\",\"access\":\"rw\"", this), - //Endpoint("subscriptions", PROPERTY, BidirectionalPacketBasedChannel::subscription_handler, nullptr, this) - }; - static constexpr size_t NUM_CHANNEL_SPECIFIC_ENDPOINTS = sizeof(channel_specific_endpoints_) / sizeof(channel_specific_endpoints_[0]); - - const Endpoint* get_endpoint(size_t index) { - if (index < NUM_CHANNEL_SPECIFIC_ENDPOINTS){ - return &channel_specific_endpoints_[index]; - } else if (index < n_endpoints_) { - return &global_endpoints_[index - NUM_CHANNEL_SPECIFIC_ENDPOINTS]; - } else { - return nullptr; - } - } - - void subscription(const uint8_t* input, size_t input_length, StreamSink* output) { - // TODO: handle - return; - } - - const Endpoint * const global_endpoints_; - size_t n_endpoints_; PacketSink& output_; uint8_t tx_buf_[TX_BUF_SIZE]; - const uint16_t json_crc_; }; + +template +struct MemberList; + +template<> +struct MemberList<> { +public: + static constexpr size_t endpoint_count = 0; + size_t get_endpoint_count() { return endpoint_count; } + static constexpr bool is_empty = true; + void write_json(size_t id, StreamSink* output) { + // no action + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { + // no action + } + std::tuple<> get_names_as_tuple() const { return std::tuple<>(); } +}; + +template +struct MemberList { +public: + static constexpr size_t endpoint_count = TMember::endpoint_count + MemberList::endpoint_count; + size_t get_endpoint_count() { return endpoint_count; } + static constexpr bool is_empty = false; + + MemberList(TMember&& this_member, TMembers&&... subsequent_members) : + this_member_(std::forward(this_member)), + subsequent_members_(std::forward(subsequent_members)...) {} + + MemberList(TMember&& this_member, MemberList&& subsequent_members) : + this_member_(std::forward(this_member)), + subsequent_members_(std::forward(subsequent_members)) {} + + // @brief Move constructor +/* MemberList(MemberList&& other) : + this_member_(std::move(other.this_member_)), + subsequent_members_(std::move(other.subsequent_members_)) {}*/ + + void write_json(size_t id, StreamSink* output) /*final*/ { + this_member_.write_json(id, output); + if (!MemberList::is_empty) + write_string(",", output); + subsequent_members_.write_json(id + TMember::endpoint_count, output); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) /*final*/ { + this_member_.register_endpoints(list, id, length); + subsequent_members_.register_endpoints(list, id + TMember::endpoint_count, length); + } + + TMember this_member_; + MemberList subsequent_members_; +}; + +template +MemberList make_protocol_member_list(TMembers&&... member_list) { + return MemberList(std::forward(member_list)...); +} + +template +class ProtocolObject { +public: + ProtocolObject(const char * name, TMembers&&... member_list) : + name_(name), + member_list_(std::forward(member_list)...) {} + + static constexpr size_t endpoint_count = MemberList::endpoint_count; + + void write_json(size_t id, StreamSink* output) { + write_string("{\"name\":\"", output); + write_string(name_, output); + write_string("\",\"type\":\"object\",\"members\":[", output); + member_list_.write_json(id, output), + write_string("]}", output); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + member_list_.register_endpoints(list, id, length); + } + + const char * name_; + MemberList member_list_; +}; + +template +ProtocolObject make_protocol_object(const char * name, TMembers&&... member_list) { + return ProtocolObject(name, std::forward(member_list)...); +} + +template +class ProtocolProperty : Endpoint { +public: + static constexpr const char * json_modifier = get_default_json_modifier(); + static constexpr size_t endpoint_count = 1; + + ProtocolProperty(const char * name, TProperty* property) + : name_(name), property_(property) + {} + +// ProtocolProperty(const ProtocolProperty&) = delete; + + // @brief Move constructor + ProtocolProperty(ProtocolProperty&& other) : + Endpoint(std::move(other)), + name_(std::move(other.name_)), + property_(other.property_) + {} + + //constexpr ProtocolProperty& operator=(const ProtocolProperty& other) = delete; + /*constexpr ProtocolProperty& operator=(const ProtocolProperty& other) { + //Endpoint(std::move(other)), + //name_(std::move(other.name_)), + //property_(other.property_) + name_ = other.name_; + property_ = other.property_; + return *this; + }*/ + + /*ProtocolProperty& operator=(ProtocolProperty&& other) + : name_(other.name_), property_(other.property_) + {} + ProtocolProperty& operator=(const ProtocolProperty& other) + : name_(other.name_), property_(other.property_) + {}*/ + + void write_json(size_t id, StreamSink* output) { + // write name + write_string("{\"name\":\"", output); + LOG_PROTO("json: this at %x, name at %x is s\r\n", (uintptr_t)this, (uintptr_t)name_); + //LOG_PROTO("json\r\n"); + write_string(name_, output); + + // write endpoint ID + write_string("\",\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + // write additional JSON data + if (json_modifier && json_modifier[0]) { + write_string(",", output); + write_string(json_modifier, output); + } + + write_string("}", output); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + } + void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + default_readwrite_endpoint_handler(property_, input, input_length, output); + } + /*void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + handle(input, input_length, output); + }*/ + + const char * name_; + TProperty* property_; +}; + +template +ProtocolProperty make_protocol_property(const char * name, TProperty* property) { + return ProtocolProperty(name, property); +}; + +template +ProtocolProperty make_protocol_ro_property(const char * name, const TProperty* property) { + return ProtocolProperty(name, property); +}; + + + +template +class FunctionTraits { +public: + template> + static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args, TUnpackedArgs ... args) { + return invoke(obj, func_ptr, packed_args, args..., std::get(packed_args)); + } + + template + static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args, TArgs ... args) { + return (obj.*func_ptr)(args...); + } +}; + +/* @brief Invoke a class member function with a variable number of arguments that are supplied as a tuple + +Example usage: + +class MyClass { +public: + int MyFunction(int a, int b) { + return 0; + } +}; + +MyClass my_object; +std::tuple my_args(3, 4); // arguments are supplied as a tuple +int result = invoke_function_with_tuple(my_object, &MyClass::MyFunction, my_args); +*/ +template +TRet invoke_function_with_tuple(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args) { + return FunctionTraits::template invoke<0>(obj, func_ptr, packed_args); +} + + +template +struct PropertyListFactory; + +template<> +struct PropertyListFactory<> { + template + static MemberList<> make_property_list(std::array names, std::tuple& values) { + return MemberList<>(); + } +}; + +template +struct PropertyListFactory { + template + static MemberList, ProtocolProperty...> + make_property_list(std::array names, std::tuple& values) { + return MemberList, ProtocolProperty...>( + make_protocol_property(std::get(names), std::get(values)), + PropertyListFactory::template make_property_list(names, values) + ); + } +}; + + +template +class ProtocolFunction : Endpoint { +public: + static constexpr size_t endpoint_count = 1 + MemberList...>::endpoint_count; + template + ProtocolFunction(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) : + name_(name), all_arg_names_{names...}, obj_(obj), func_ptr_(func_ptr), + input_properties_(PropertyListFactory::template make_property_list<0>(all_arg_names_, in_args_)) + { + LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + } + + ProtocolFunction(const ProtocolFunction& other) : + name_(other.name_), all_arg_names_(other.all_arg_names_), obj_(other.obj_), func_ptr_(other.func_ptr_), + input_properties_(PropertyListFactory::template make_property_list<0>( + all_arg_names_, in_args_)) + { + LOG_PROTO("COPIED! my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + } + + void write_json(size_t id, StreamSink* output) { + // write name + write_string("{\"name\":\"", output); + write_string(name_, output); + + // write endpoint ID + write_string("\",\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + // write arguments + write_string(",\"type\":\"function\",\"arguments\":[", output); + input_properties_.write_json(id + 1, output), + write_string("]}", output); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + input_properties_.register_endpoints(list, id + 1, length); + } + + void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + (void) input; + (void) input_length; + (void) output; + LOG_PROTO("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + LOG_PROTO("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_)); + invoke_function_with_tuple(obj_, func_ptr_, in_args_); + } + + const char * name_; + std::array all_arg_names_; // TODO: remove + TObj& obj_; + TRet(TObj::*func_ptr_)(TArgs...); + std::tuple in_args_; + MemberList...> input_properties_; +}; + +template> +ProtocolFunction make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { + return ProtocolFunction(name, obj, func_ptr, names...); +} + + + +template +class EndpointProvider_from_MemberList : public EndpointProvider { +public: + EndpointProvider_from_MemberList(T& member_list) : member_list_(member_list) {} + size_t get_endpoint_count() final { + return member_list_.get_endpoint_count(); + } + void write_json(size_t id, StreamSink* output) final { + return member_list_.write_json(id, output); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) final { + return member_list_.register_endpoints(list, id, length); + } + T& member_list_; +}; + +void set_application_endpoints(EndpointProvider* endpoints); + #endif diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index b256e24c..18b291ca 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -14,6 +14,7 @@ public: Axis* axis = nullptr; // set by Axis constructor + // TODO: expose on protocol Error_t error = ERROR_NONE; float phase = 0.0f; // [rad] float pll_pos = 0.0f; // [rad] From 9486f9ed15c994d704ccd7dd20bfd0560e3e1496 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 13:37:07 -0800 Subject: [PATCH 005/112] make stuff working again, improve state machine design, add underscore to member names --- Firmware/Board/v3.3/Src/usbd_cdc_if.c | 2 +- Firmware/MotorControl/axis.cpp | 270 ++++----- Firmware/MotorControl/axis.hpp | 136 +++-- Firmware/MotorControl/commands.cpp | 512 ------------------ Firmware/MotorControl/communication.cpp | 293 ++++++++++ .../{commands.h => communication.h} | 10 - Firmware/MotorControl/controller.cpp | 91 ++-- Firmware/MotorControl/controller.hpp | 58 +- Firmware/MotorControl/encoder.cpp | 132 ++--- Firmware/MotorControl/encoder.hpp | 49 +- Firmware/MotorControl/low_level.cpp | 86 +-- Firmware/MotorControl/low_level.h | 7 - Firmware/MotorControl/main.cpp | 6 +- Firmware/MotorControl/motor.cpp | 169 +++--- Firmware/MotorControl/motor.hpp | 101 ++-- Firmware/MotorControl/odrive_main.hpp | 2 - Firmware/MotorControl/protocol.cpp | 9 +- Firmware/MotorControl/protocol.hpp | 34 +- .../MotorControl/sensorless_estimator.cpp | 48 +- .../MotorControl/sensorless_estimator.hpp | 24 +- Firmware/Tupfile.lua | 2 +- 21 files changed, 972 insertions(+), 1069 deletions(-) delete mode 100644 Firmware/MotorControl/commands.cpp create mode 100644 Firmware/MotorControl/communication.cpp rename Firmware/MotorControl/{commands.h => communication.h} (81%) diff --git a/Firmware/Board/v3.3/Src/usbd_cdc_if.c b/Firmware/Board/v3.3/Src/usbd_cdc_if.c index b38385f7..bf94296b 100644 --- a/Firmware/Board/v3.3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3.3/Src/usbd_cdc_if.c @@ -52,7 +52,7 @@ #include "cmsis_os.h" #include "freertos_vars.h" #include "utils.h" -#include "commands.h" +#include "communication.h" #include /* USER CODE END INCLUDE */ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f23cc91f..dd14249f 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -12,24 +12,24 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, SensorlessEstimator& sensorless_estimator, Controller& controller, Motor& motor) - : hw_config(hw_config), - config(config), - encoder(encoder), - sensorless_estimator(sensorless_estimator), - controller(controller), - motor(motor) + : hw_config_(hw_config), + config_(config), + encoder_(encoder), + sensorless_estimator_(sensorless_estimator), + controller_(controller), + motor_(motor) { - encoder.axis = this; - sensorless_estimator.axis = this; - controller.axis = this; - motor.axis = this; + encoder_.axis_ = this; + sensorless_estimator_.axis_ = this; + controller_.axis_ = this; + motor_.axis_ = this; } // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. void Axis::setup() { - encoder.setup(); - motor.setup(); + encoder_.setup(); + motor_.setup(); } static void run_state_machine_loop_wrapper(void* ctx) { @@ -38,16 +38,24 @@ static void run_state_machine_loop_wrapper(void* ctx) { // @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { - osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config.thread_priority, 0, 512); - thread_id = osThreadCreate(osThread(thread_def), this); - thread_id_valid = true; + osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 512); + thread_id_ = osThreadCreate(osThread(thread_def), this); + thread_id_valid_ = true; } // @brief Unblocks the control loop thread. // This is called from the current sense interrupt handler. -void Axis::signal_thread(thread_signals sig) { - if (thread_id_valid) - osSignalSet(thread_id, sig); +void Axis::signal_current_meas() { + if (thread_id_valid_) + osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS); +} + +// @brief Blocks until a current measurement is completed +// @returns True on success, false otherwise +bool Axis::wait_for_current_meas() { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) + return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; + return true; } static void step_cb_wrapper(void* ctx) { @@ -56,10 +64,10 @@ static void step_cb_wrapper(void* ctx) { // step/direction interface void Axis::step_cb() { - if (enable_step_dir) { - GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config.dir_port, hw_config.dir_pin); + if (enable_step_dir_) { + GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config_.dir_port, hw_config_.dir_pin); float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - controller.pos_setpoint += dir * config.counts_per_step; + controller_.pos_setpoint_ += dir * config_.counts_per_step; } }; @@ -68,38 +76,38 @@ void Axis::set_step_dir_enabled(bool enable) { if (enable) { // Set up the direction GPIO as input GPIO_InitTypeDef GPIO_InitStruct; - GPIO_InitStruct.Pin = hw_config.dir_pin; + GPIO_InitStruct.Pin = hw_config_.dir_pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(hw_config.dir_port, &GPIO_InitStruct); + HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct); // Subscribe to rising edges of the step GPIO - GPIO_subscribe(hw_config.step_port, hw_config.step_pin, GPIO_PULLDOWN, + GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN, step_cb_wrapper, this); - enable_step_dir = true; + enable_step_dir_ = true; } else { - enable_step_dir = false; + enable_step_dir_ = false; // Unsubscribe from step GPIO - GPIO_unsubscribe(hw_config.step_port, hw_config.step_pin); + GPIO_unsubscribe(hw_config_.step_port, hw_config_.step_pin); } } // @brief Returns true if the power supply is within range bool Axis::check_PSU_brownout() { - if(vbus_voltage < config.dc_bus_brownout_trip_level) - return error = ERROR_BAD_VOLTAGE, false; + if(vbus_voltage < config_.dc_bus_brownout_trip_level) + return error_ = ERROR_BAD_VOLTAGE, false; return true; } // @brief Returns true if everything is ok. // Sets error and returns false otherwise. bool Axis::do_checks() { - if (!motor.do_checks()) - return error = ERROR_MOTOR_FAILED, false; + if (!motor_.do_checks()) + return error_ = ERROR_MOTOR_FAILED, false; if (!check_PSU_brownout()) - return error = ERROR_BAD_VOLTAGE, false; + return error_ = ERROR_BAD_VOLTAGE, false; return true; } @@ -107,74 +115,81 @@ bool Axis::run_sensorless_spin_up() { // Early Spin-up: spiral up current float x = 0.0f; run_control_loop([&](){ - float phase = wrap_pm_pi(config.ramp_up_distance * x); - float I_mag = config.spin_up_current * x; - x += current_meas_period / config.ramp_up_time; - if (!motor.update(I_mag, phase)) - return error = ERROR_MOTOR_FAILED, false; + float phase = wrap_pm_pi(config_.ramp_up_distance * x); + float I_mag = config_.spin_up_current * x; + x += current_meas_period / config_.ramp_up_time; + if (!motor_.update(I_mag, phase)) + return error_ = ERROR_MOTOR_FAILED, false; return x < 1.0f; }); - if (error != ERROR_NO_ERROR) + if (error_ != ERROR_NO_ERROR) return false; // Late Spin-up: accelerate - float vel = config.ramp_up_distance / config.ramp_up_time; - float phase = wrap_pm_pi(config.ramp_up_distance); + float vel = config_.ramp_up_distance / config_.ramp_up_time; + float phase = wrap_pm_pi(config_.ramp_up_distance); run_control_loop([&](){ - vel += config.spin_up_acceleration * current_meas_period; + vel += config_.spin_up_acceleration * current_meas_period; phase = wrap_pm_pi(phase + vel * current_meas_period); - float I_mag = config.spin_up_current; - if (!motor.update(I_mag, phase)) - return error = ERROR_MOTOR_FAILED, false; - return vel < config.spin_up_target_vel; + float I_mag = config_.spin_up_current; + if (!motor_.update(I_mag, phase)) + return error_ = ERROR_MOTOR_FAILED, false; + return vel < config_.spin_up_target_vel; }); - return error == ERROR_NO_ERROR; + return error_ == ERROR_NO_ERROR; } // Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. bool Axis::run_sensorless_control_loop() { + set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ float pos_estimate, vel_estimate, phase, current_setpoint; - if (controller.config.control_mode >= CTRL_MODE_POSITION_CONTROL) - return error = ERROR_POS_CTRL_DURING_SENSORLESS, false; + if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) + return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false; // We update the encoder just in case someone needs the output for testing - encoder.update(nullptr, nullptr, nullptr); - if (!sensorless_estimator.update(&pos_estimate, &vel_estimate, &phase)) - return error = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; - if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error = ERROR_CONTROLLER_FAILED, false; - if (!motor.update(current_setpoint, phase)) - return error = ERROR_MOTOR_FAILED, false; + encoder_.update(nullptr, nullptr, nullptr); + if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase)) + return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; + if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return error_ = ERROR_CONTROLLER_FAILED, false; + if (!motor_.update(current_setpoint, phase)) + return error_ = ERROR_MOTOR_FAILED, false; return true; }); - return error == ERROR_NO_ERROR; + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; } bool Axis::run_closed_loop_control_loop() { + set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ float pos_estimate, vel_estimate, phase, current_setpoint; // We update the sensorless estimator just in case someone needs the output for testing - sensorless_estimator.update(nullptr, nullptr, nullptr); - if (!encoder.update(&pos_estimate, &vel_estimate, &phase)) - return error = ERROR_ENCODER_FAILED, false; - if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error = ERROR_CONTROLLER_FAILED, false; - if (!motor.update(current_setpoint, phase)) - return error = ERROR_MOTOR_FAILED, false; + sensorless_estimator_.update(nullptr, nullptr, nullptr); + if (!encoder_.update(&pos_estimate, &vel_estimate, &phase)) + return error_ = ERROR_ENCODER_FAILED, false; + if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return error_ = ERROR_CONTROLLER_FAILED, false; + if (!motor_.update(current_setpoint, phase)) + return error_ = ERROR_MOTOR_FAILED, false; return true; }); - return error == ERROR_NO_ERROR; + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; } bool Axis::run_idle_loop() { - while (requested_state == AXIS_STATE_DONT_CARE) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) - return error = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; - } - return error == ERROR_NO_ERROR; + // run_control_loop ignores missed modulation timing updates + // if and only if we're in AXIS_STATE_IDLE + run_control_loop([this](){ + sensorless_estimator_.update(nullptr, nullptr, nullptr); + encoder_.update(nullptr, nullptr, nullptr); + return true; + }); + return error_ == ERROR_NO_ERROR; } // Infinite loop that does calibration and enters main control loop as appropriate @@ -183,84 +198,91 @@ void Axis::run_state_machine_loop() { // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f // TODO: Move this somewhere else // TODO: respect changes of CPR - int encoder_cpr = encoder.config.cpr; - controller.anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); - if (controller.anticogging.cogging_map != NULL) { + int encoder_cpr = encoder_.config_.cpr; + controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); + if (controller_.anticogging_.cogging_map != NULL) { for (int i = 0; i < encoder_cpr; i++) { - controller.anticogging.cogging_map[i] = 0.0f; + controller_.anticogging_.cogging_map[i] = 0.0f; } } - current_state = AXIS_STATE_MOTOR_CALIBRATION; - bool force_state = false; + // arm! + motor_.arm(); for (;;) { - AxisState_t next_state = AXIS_STATE_DONT_CARE; - - switch (current_state) { - - case AXIS_STATE_MOTOR_CALIBRATION: - { - bool skip = !force_state && !config.enable_motor_calibration; - if (skip || motor.run_calibration()) { - next_state = AXIS_STATE_ENCODER_CALIBRATION; - } else { - next_state = AXIS_STATE_IDLE; - } + // Load the task chain if a specific request is pending + if (requested_state_ != AXIS_STATE_UNDEFINED) { + size_t pos = 0; + if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) { + if (config_.startup_motor_calibration) + task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; + if (config_.startup_encoder_calibration) + task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (config_.startup_closed_loop_control) + task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; + else if (config_.startup_sensorless_control) + task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL; + task_chain_[pos++] = AXIS_STATE_IDLE; + } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) { + task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; + task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + task_chain_[pos++] = AXIS_STATE_IDLE; + } else if (requested_state_ != AXIS_STATE_UNDEFINED) { + task_chain_[pos++] = requested_state_; + task_chain_[pos++] = AXIS_STATE_IDLE; } + task_chain_[pos++] = AXIS_STATE_UNDEFINED; + // TODO: bounds checking + requested_state_ = AXIS_STATE_UNDEFINED; + } + + // Note that current_state is a reference to task_chain_[0] + + // Validate the state before running it + if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_) + current_state_ = AXIS_STATE_UNDEFINED; + if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_) + current_state_ = AXIS_STATE_UNDEFINED; + + // Run the specified state + // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED + bool status; + switch (current_state_) { + case AXIS_STATE_MOTOR_CALIBRATION: + status = motor_.run_calibration(); break; case AXIS_STATE_ENCODER_CALIBRATION: - { - bool skip = !force_state && !config.enable_encoder_calibration; - if (skip || encoder.run_calibration()) { - next_state = config.enable_closed_loop_control ? - AXIS_STATE_CLOSED_LOOP_CONTROL : - config.enable_sensorless_control ? - AXIS_STATE_SENSORLESS_SPINUP : - AXIS_STATE_IDLE; - if (next_state != AXIS_STATE_IDLE) - set_step_dir_enabled(config.enable_step_dir); - } else { - next_state = AXIS_STATE_IDLE; - } - } - break; - - case AXIS_STATE_SENSORLESS_SPINUP: - if (run_sensorless_spin_up()) { - next_state = AXIS_STATE_SENSORLESS_CONTROL; - } else { - next_state = AXIS_STATE_IDLE; - } + status = encoder_.run_calibration(); break; case AXIS_STATE_SENSORLESS_CONTROL: - run_sensorless_control_loop(); - next_state = AXIS_STATE_IDLE; // TODO: restart if desired + status = run_sensorless_spin_up(); // TODO: restart if desired + if (status) + status = run_sensorless_control_loop(); break; case AXIS_STATE_CLOSED_LOOP_CONTROL: - run_closed_loop_control_loop(); - next_state = AXIS_STATE_IDLE; + status = run_closed_loop_control_loop(); break; case AXIS_STATE_IDLE: - default: - current_state = AXIS_STATE_IDLE; run_idle_loop(); + status = motor_.arm(); // done with idling - try to arm the motor + break; + + default: + error_ = ERROR_INVALID_STATE; + status = false; // this will set the state to idle break; } - if (requested_state != AXIS_STATE_DONT_CARE) { - current_state = requested_state; - requested_state = AXIS_STATE_DONT_CARE; - force_state = true; - } else { - current_state = next_state; - force_state = false; - } + // If the state failed, go to idle, else advance task chain + if (!status) + current_state_ = AXIS_STATE_IDLE; + else + memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0])); } - thread_id_valid = false; + thread_id_valid_ = false; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index ff52a51b..6f401154 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,23 +5,24 @@ #error "This file should not be included directly. Include odrive_main.hpp instead." #endif - +// Warning: Do not reorder these enum values. +// The state machine uses ">" comparision on them. enum AxisState_t { - AXIS_STATE_STARTUP, - AXIS_STATE_MOTOR_CALIBRATION, - AXIS_STATE_ENCODER_CALIBRATION, - AXIS_STATE_SENSORLESS_SPINUP, - AXIS_STATE_SENSORLESS_CONTROL, - AXIS_STATE_CLOSED_LOOP_CONTROL, - AXIS_STATE_IDLE, - AXIS_STATE_DONT_CARE // used to indicate that no state request is pending + AXIS_STATE_UNDEFINED, // void run_control_loop(const T& update_handler) { - motor.arm(); - while (requested_state == AXIS_STATE_DONT_CARE - && error == ERROR_NO_ERROR /* error may be set by interrupt handler */ ) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - error = ERROR_CURRENT_MEASUREMENT_TIMEOUT; + while (requested_state_ == AXIS_STATE_UNDEFINED) { + if (motor_.error_ != Motor::ERROR_NO_ERROR) { + error_ = ERROR_MOTOR_FAILED; + break; + } + if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) { + error_ = ERROR_CONTROL_LOOP_TIMEOUT; break; } - - // Proactively set phase voltages to 0. If the control deadline is missed, - // the voltages will go to zero. - motor.enqueue_voltage_timings(0.0f, 0.0f); if (!do_checks()) // error set during function call break; @@ -111,21 +113,15 @@ public: if (!update_handler()) // error set during function call break; - update_brake_current(); - // Check we meet deadlines after queueing - motor.last_cpu_time = motor.check_timing(); - if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) { - error = ERROR_CONTROL_LOOP_TIMEOUT; + ++loop_counter_; + + // Wait until the current measurement interrupt fires + if (!wait_for_current_meas()) { // error set by function call + motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases break; } - ++loop_counter; } - - // We are exiting control: disarm motor, reset Ibus, and update brake current - motor.disarm(); - motor.current_control.Ibus = 0.0f; - update_brake_current(); } bool run_sensorless_spin_up(); @@ -135,23 +131,57 @@ public: void run_state_machine_loop(); - const AxisHardwareConfig_t& hw_config; - AxisConfig_t& config; + const AxisHardwareConfig_t& hw_config_; + AxisConfig_t& config_; - Encoder& encoder; - SensorlessEstimator& sensorless_estimator; - Controller& controller; - Motor& motor; + Encoder& encoder_; + SensorlessEstimator& sensorless_estimator_; + Controller& controller_; + Motor& motor_; - osThreadId thread_id; - volatile bool thread_id_valid = false; + osThreadId thread_id_; + volatile bool thread_id_valid_ = false; // variables exposed on protocol - Error_t error = ERROR_NO_ERROR; - bool enable_step_dir = false; // auto enabled after calibration, based on config.enable_step_dir - AxisState_t current_state = AXIS_STATE_STARTUP; - AxisState_t requested_state = AXIS_STATE_DONT_CARE; - uint32_t loop_counter = 0; + Error_t error_ = ERROR_NO_ERROR; + bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler + // whenever there's no active control loop that + // sets the timings. The flag must be explicitly + // cleared by a call to motors.arm(). + bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir + AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; + AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; + AxisState_t& current_state_ = task_chain_[0]; + uint32_t loop_counter_ = 0; + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_ro_property("error", &error_), + make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_), + make_protocol_property("enable_step_dir", &enable_step_dir_), + make_protocol_ro_property("current_state", ¤t_state_), + make_protocol_property("requested_state", &requested_state_), + make_protocol_ro_property("loop_counter", &loop_counter_), + make_protocol_object("config", + make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration), + make_protocol_property("startup_encoder_calibration", &config_.startup_encoder_calibration), + make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control), + make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control), + make_protocol_property("enable_step_dir", &config_.enable_step_dir), + make_protocol_property("counts_per_step", &config_.counts_per_step), + make_protocol_property("dc_bus_brownout_trip_level", &config_.dc_bus_brownout_trip_level), + make_protocol_property("ramp_up_time", &config_.ramp_up_time), + make_protocol_property("ramp_up_distance", &config_.ramp_up_distance), + make_protocol_property("spin_up_current", &config_.spin_up_current), + make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration), + make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel) + ), + make_protocol_object("motor", motor_.make_protocol_definitions()), + make_protocol_object("controller", controller_.make_protocol_definitions()), + make_protocol_object("encoder", encoder_.make_protocol_definitions()) + ); + } }; #endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp deleted file mode 100644 index 727fea65..00000000 --- a/Firmware/MotorControl/commands.cpp +++ /dev/null @@ -1,512 +0,0 @@ -#if 0 -/* Includes ------------------------------------------------------------------*/ - -// TODO: remove this option -// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py -// todo: make clean switches for protocol -#define ENABLE_LEGACY_PROTOCOL - -#include "commands.h" -#include "low_level.h" -#include "odrive_main.hpp" -#include "protocol.hpp" -#include "freertos_vars.h" -#include "utils.h" -#include "config.h" - -#ifdef ENABLE_LEGACY_PROTOCOL -#include "legacy_commands.h" -#endif - -#include -#include -#include -#include -#include -#include - -#define UART_TX_BUFFER_SIZE 64 - -/* Private defines -----------------------------------------------------------*/ -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ - -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern USBD_HandleTypeDef hUsbDeviceFS; - -/* Private constant data -----------------------------------------------------*/ -// TODO: make command to switch gpio_mode during run-time -#if defined(USE_GPIO_MODE_STEP_DIR) -static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir -#elif !defined(UART_PROTOCOL_NONE) -static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx -#else -static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured -#endif - -/* Private variables ---------------------------------------------------------*/ - -static uint8_t* usb_buf; -static uint32_t usb_len; - -// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -static thread_local uint32_t deadline_ms = 0; - -/* Variables exposed to USB & UART via read/write commands */ -// TODO: include range information in JSON description - - -// TODO: Autogenerate these functions -void motors_0_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[0], - motors[0].set_pos_setpoint_args.pos_setpoint, - motors[0].set_pos_setpoint_args.vel_feed_forward, - motors[0].set_pos_setpoint_args.current_feed_forward); -} -void motors_0_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[0], - motors[0].set_vel_setpoint_args.vel_setpoint, - motors[0].set_vel_setpoint_args.current_feed_forward); -} -void motors_0_set_current_setpoint_func(void) { - set_current_setpoint(&motors[0], - motors[0].set_current_setpoint_args.current_setpoint); -} -void motors_1_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[1], - motors[1].set_pos_setpoint_args.pos_setpoint, - motors[1].set_pos_setpoint_args.vel_feed_forward, - motors[1].set_pos_setpoint_args.current_feed_forward); -} -void motors_1_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[1], - motors[1].set_vel_setpoint_args.vel_setpoint, - motors[1].set_vel_setpoint_args.current_feed_forward); -} -void motors_1_set_current_setpoint_func(void) { - set_current_setpoint(&motors[1], - motors[1].set_current_setpoint_args.current_setpoint); -} -void motors_run_anticogging_calibration_func() { - for (uint8_t i = 0; i < num_motors; i++) { - // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) { - motors[i].anticogging.calib_anticogging = true; - } - } -} - -// This table specifies which fields and functions are exposed on the USB and UART ports. -// TODO: Autogenerate this table. It will come up again very soon in the Arduino library. -// clang-format off -const Endpoint endpoints[] = { - Endpoint::make_property("vbus_voltage", const_cast(&vbus_voltage)), - Endpoint::make_property("UUID_0", (const uint32_t*)(ID_UNIQUE_ADDRESS + 0*4)), - Endpoint::make_property("UUID_1", (const uint32_t*)(ID_UNIQUE_ADDRESS + 1*4)), - Endpoint::make_property("UUID_2", (const uint32_t*)(ID_UNIQUE_ADDRESS + 2*4)), - Endpoint::make_function("run_anticogging_calibration", &motors_run_anticogging_calibration_func), - // No parameters, but still requires a close_tree() - Endpoint::close_tree(), - Endpoint::make_object("config"), - Endpoint::make_property("brake_resistance", &brake_resistance), - Endpoint::close_tree(), - Endpoint::make_object("axis0"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control", &axis_configs[0].enable_control_at_start), - Endpoint::make_property("do_calibration", &axis_configs[0].do_calibration_at_start), - Endpoint::close_tree(), - Endpoint::close_tree(), - Endpoint::make_object("motor0"), - Endpoint::make_object("config"), - Endpoint::make_property("control_mode", reinterpret_cast(&motors[0].control_mode)), - Endpoint::make_property("counts_per_step", &motors[0].counts_per_step), - Endpoint::make_property("pole_pairs", &motors[0].pole_pairs), - Endpoint::make_property("pos_gain", &motors[0].pos_gain), - Endpoint::make_property("vel_gain", &motors[0].vel_gain), - Endpoint::make_property("vel_integrator_gain", &motors[0].vel_integrator_gain), - Endpoint::make_property("vel_limit", &motors[0].vel_limit), - Endpoint::make_property("calibration_current", &motors[0].calibration_current), - Endpoint::make_property("resistance_calib_max_voltage", &motors[0].resistance_calib_max_voltage), - Endpoint::make_property("phase_inductance", &motors[0].phase_inductance), - Endpoint::make_property("phase_resistance", &motors[0].phase_resistance), - Endpoint::make_property("motor_type", reinterpret_cast(&motors[0].motor_type)), - Endpoint::make_property("rotor_mode", reinterpret_cast(&motors[0].rotor_mode)), - Endpoint::close_tree(), - Endpoint::make_property("error", reinterpret_cast(&motors[0].error)), - Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint), - Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint), - Endpoint::make_property("vel_integrator_current", &motors[0].vel_integrator_current), - Endpoint::make_property("current_setpoint", &motors[0].current_setpoint), - Endpoint::make_property("current_meas_phB", const_cast(&motors[0].current_meas.phB)), - Endpoint::make_property("current_meas_phC", const_cast(&motors[0].current_meas.phC)), - Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB), - Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), - Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), - Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain), - Endpoint::make_property("thread_id_valid", &motors[0].thread_id_valid), - Endpoint::make_property("control_deadline", &motors[0].control_deadline), - Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time), - Endpoint::make_property("loop_counter", &motors[0].loop_counter), - Endpoint::make_object("current_control"), - Endpoint::make_object("config"), - Endpoint::make_property("current_lim", &motors[0].current_control.current_lim), - Endpoint::close_tree(), - Endpoint::make_property("p_gain", &motors[0].current_control.p_gain), - Endpoint::make_property("i_gain", &motors[0].current_control.i_gain), - Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d), - Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_setpoint", &motors[0].current_control.Iq_setpoint), - Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured), - Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), - Endpoint::close_tree(), - Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_fault", reinterpret_cast(&motors[0].drv_fault)), - Endpoint::make_property("status_reg_1", (&motors[0].gate_driver_regs.Stat_Reg_1_Value)), - Endpoint::make_property("status_reg_2", (&motors[0].gate_driver_regs.Stat_Reg_2_Value)), - Endpoint::make_property("ctrl_reg_1", (&motors[0].gate_driver_regs.Ctrl_Reg_1_Value)), - Endpoint::make_property("ctrl_reg_2", (&motors[0].gate_driver_regs.Ctrl_Reg_2_Value)), - Endpoint::close_tree(), - Endpoint::make_object("encoder"), - Endpoint::make_object("config"), - Endpoint::make_property("use_index", &motors[0].encoder.use_index), - Endpoint::make_property("calibrated", &motors[0].encoder.calibrated), - Endpoint::make_property("idx_search_speed", &motors[0].encoder.idx_search_speed), - Endpoint::make_property("cpr", &motors[0].encoder.encoder_cpr), - Endpoint::make_property("offset", &motors[0].encoder.encoder_offset), - Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_property("phase", const_cast(&motors[0].encoder.phase)), - Endpoint::make_property("pll_pos", &motors[0].encoder.pll_pos), - Endpoint::make_property("pll_vel", &motors[0].encoder.pll_vel), - Endpoint::make_property("pll_kp", &motors[0].encoder.pll_kp), - Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki), - Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset), - Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state), - Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func), - Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint), - Endpoint::make_property("vel_feed_forward", &motors[0].set_pos_setpoint_args.vel_feed_forward), - Endpoint::make_property("current_feed_forward", &motors[0].set_pos_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_vel_setpoint", &motors_0_set_vel_setpoint_func), - Endpoint::make_property("vel_setpoint", &motors[0].set_vel_setpoint_args.vel_setpoint), - Endpoint::make_property("current_feed_forward", &motors[0].set_vel_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_current_setpoint", &motors_0_set_current_setpoint_func), - Endpoint::make_property("current_setpoint", &motors[0].set_current_setpoint_args.current_setpoint), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor0 - Endpoint::make_object("axis1"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control", &axis_configs[1].enable_control_at_start), - Endpoint::make_property("do_calibration", &axis_configs[1].do_calibration_at_start), - Endpoint::close_tree(), - Endpoint::close_tree(), - Endpoint::make_object("motor1"), - Endpoint::make_object("config"), - Endpoint::make_property("control_mode", reinterpret_cast(&motors[1].control_mode)), - Endpoint::make_property("counts_per_step", &motors[1].counts_per_step), - Endpoint::make_property("pole_pairs", &motors[1].pole_pairs), - Endpoint::make_property("pos_gain", &motors[1].pos_gain), - Endpoint::make_property("vel_gain", &motors[1].vel_gain), - Endpoint::make_property("vel_integrator_gain", &motors[1].vel_integrator_gain), - Endpoint::make_property("vel_limit", &motors[1].vel_limit), - Endpoint::make_property("calibration_current", &motors[1].calibration_current), - Endpoint::make_property("resistance_calib_max_voltage", &motors[1].resistance_calib_max_voltage), - Endpoint::make_property("phase_inductance", &motors[1].phase_inductance), - Endpoint::make_property("phase_resistance", &motors[1].phase_resistance), - Endpoint::make_property("motor_type", reinterpret_cast(&motors[1].motor_type)), - Endpoint::make_property("rotor_mode", reinterpret_cast(&motors[1].rotor_mode)), - Endpoint::close_tree(), - Endpoint::make_property("error", reinterpret_cast(&motors[1].error)), - Endpoint::make_property("pos_setpoint", &motors[1].pos_setpoint), - Endpoint::make_property("vel_setpoint", &motors[1].vel_setpoint), - Endpoint::make_property("vel_integrator_current", &motors[1].vel_integrator_current), - Endpoint::make_property("current_setpoint", &motors[1].current_setpoint), - Endpoint::make_property("current_meas_phB", const_cast(&motors[1].current_meas.phB)), - Endpoint::make_property("current_meas_phC", const_cast(&motors[1].current_meas.phC)), - Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB), - Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), - Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), - Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain), - Endpoint::make_property("thread_id_valid", &motors[1].thread_id_valid), - Endpoint::make_property("control_deadline", &motors[1].control_deadline), - Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time), - Endpoint::make_property("loop_counter", &motors[1].loop_counter), - Endpoint::make_object("current_control"), - Endpoint::make_object("config"), - Endpoint::make_property("current_lim", &motors[1].current_control.current_lim), - Endpoint::close_tree(), - Endpoint::make_property("p_gain", &motors[1].current_control.p_gain), - Endpoint::make_property("i_gain", &motors[1].current_control.i_gain), - Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d), - Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_setpoint", &motors[1].current_control.Iq_setpoint), - Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured), - Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), - Endpoint::close_tree(), - Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_fault", reinterpret_cast(&motors[1].drv_fault)), - Endpoint::make_property("status_reg_1", (&motors[1].gate_driver_regs.Stat_Reg_1_Value)), - Endpoint::make_property("status_reg_2", (&motors[1].gate_driver_regs.Stat_Reg_2_Value)), - Endpoint::make_property("ctrl_reg_1", (&motors[1].gate_driver_regs.Ctrl_Reg_1_Value)), - Endpoint::make_property("ctrl_reg_2", (&motors[1].gate_driver_regs.Ctrl_Reg_2_Value)), - Endpoint::close_tree(), - Endpoint::make_object("encoder"), - Endpoint::make_object("config"), - Endpoint::make_property("use_index", &motors[1].encoder.use_index), - Endpoint::make_property("calibrated", &motors[1].encoder.calibrated), - Endpoint::make_property("idx_search_speed", &motors[1].encoder.idx_search_speed), - Endpoint::make_property("cpr", &motors[1].encoder.encoder_cpr), - Endpoint::make_property("offset", &motors[1].encoder.encoder_offset), - Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_property("phase", const_cast(&motors[1].encoder.phase)), - Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos), - Endpoint::make_property("pll_vel", &motors[1].encoder.pll_vel), - Endpoint::make_property("pll_kp", &motors[1].encoder.pll_kp), - Endpoint::make_property("pll_ki", &motors[1].encoder.pll_ki), - Endpoint::make_property("encoder_offset", &motors[1].encoder.encoder_offset), - Endpoint::make_property("encoder_state", &motors[1].encoder.encoder_state), - Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_function("set_pos_setpoint", &motors_1_set_pos_setpoint_func), - Endpoint::make_property("pos_setpoint", &motors[1].set_pos_setpoint_args.pos_setpoint), - Endpoint::make_property("vel_feed_forward", &motors[1].set_pos_setpoint_args.vel_feed_forward), - Endpoint::make_property("current_feed_forward", &motors[1].set_pos_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_vel_setpoint", &motors_1_set_vel_setpoint_func), - Endpoint::make_property("vel_setpoint", &motors[1].set_vel_setpoint_args.vel_setpoint), - Endpoint::make_property("current_feed_forward", &motors[1].set_vel_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_current_setpoint", &motors_1_set_current_setpoint_func), - Endpoint::make_property("current_setpoint", &motors[1].set_current_setpoint_args.current_setpoint), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor1 - Endpoint::make_function("save_configuration", &save_configuration), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("erase_configuration", &erase_configuration), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("reboot", &NVIC_SystemReset), - // no arguments - Endpoint::close_tree() -}; -// clang-format on - -constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); - - -#if defined(USB_PROTOCOL_NATIVE) - -class USBSender : public PacketSink { -public: - int process_packet(const uint8_t* buffer, size_t length) { - // cannot send partial packets - if (length > USB_TX_DATA_SIZE) - return -1; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit packet - uint8_t status = CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length); - return (status == USBD_OK) ? 0 : -1; - } -} usb_sender; - -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender); - -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - -class USBSender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - if (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, chunk) != USBD_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -} usb_sender; - -PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_channel); - -#endif - -#if defined(UART_PROTOCOL_NATIVE) -class UART4Sender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_sender; - -PacketToStreamConverter uart4_packet_sender(uart4_sender); -BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_channel); -#endif - -/* Private function prototypes -----------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -void init_communication(void) { - switch (gpio_mode) { - case GPIO_MODE_NONE: - break; //do nothing - case GPIO_MODE_UART: { -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 - SetGPIO12toUART(); -#endif - } break; - case GPIO_MODE_STEP_DIR: { - SetGPIO12toStepDir(); - } break; - default: - //TODO: report error unexpected mode - break; - } -} - -// Thread to handle deffered processing of USB interrupt, and -// read commands out of the UART DMA circular buffer -void communication_task(void const * argument) { - (void) argument; - - -#if !defined(UART_PROTOCOL_NONE) - //DMA open loop continous circular buffer - //1ms delay periodic, chase DMA ptr around - - #define UART_RX_BUFFER_SIZE 64 - static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; - - // DMA is set up to recieve in a circular buffer forever. - // We dont use interrupts to fetch the data, instead we periodically read - // data out of the circular buffer into a parse buffer, controlled by a state machine - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; -#endif - - // Re-run state-machine forever - for (;;) { -#if !defined(UART_PROTOCOL_NONE) - // Check for UART errors and restart recieve DMA transfer if required - if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { - HAL_UART_AbortReceive(&huart4); - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - } - // Fetch the circular buffer "write pointer", where it would write next - uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(UART_PROTOCOL_NATIVE) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#elif defined(UART_PROTOCOL_LEGACY) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#endif -#endif - -#if !defined(USB_PROTOCOL_NONE) - // When we reach here, we are out of immediate characters to fetch out of UART buffer - // Now we check if there is any USB processing to do: we wait for up to 1 ms, - // before going back to checking UART again. - const uint32_t usb_check_timeout = 1; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); - if (sem_stat == osOK) { - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(USB_PROTOCOL_NATIVE) - usb_channel.process_packet(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_stream_sink.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_LEGACY) - legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); -#endif - USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet - } -#endif - } - - // If we get here, then this task is done - vTaskDelete(osThreadGetId()); -} - -// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the -// incoming USB data -void set_cmd_buffer(uint8_t *buf, uint32_t len) { - usb_buf = buf; - usb_len = len; -} - -void usb_update_thread() { - for (;;) { - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); - if (semaphore_status == osOK) { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } - } - - vTaskDelete(osThreadGetId()); -} -#endif \ No newline at end of file diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp new file mode 100644 index 00000000..2a082459 --- /dev/null +++ b/Firmware/MotorControl/communication.cpp @@ -0,0 +1,293 @@ + +/* Includes ------------------------------------------------------------------*/ + +// TODO: remove this option +// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py +// todo: make clean switches for protocol +#define ENABLE_LEGACY_PROTOCOL + +#include "communication.h" +//#include "low_level.h" +#include "odrive_main.hpp" +#include "protocol.hpp" +#include "freertos_vars.h" +#include "utils.h" + +#ifdef ENABLE_LEGACY_PROTOCOL +#include "legacy_commands.h" +#endif + +#include +#include +#include +#include +#include +#include + +#define UART_TX_BUFFER_SIZE 64 + +/* Private defines -----------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ + +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; +extern USBD_HandleTypeDef hUsbDeviceFS; + +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ + +static uint8_t* usb_buf; +static uint32_t usb_len; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + + +#if defined(USB_PROTOCOL_NATIVE) + +class USBSender : public PacketSink { +public: + int process_packet(const uint8_t* buffer, size_t length) { + // cannot send partial packets + if (length > USB_TX_DATA_SIZE) + return -1; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit packet + uint8_t status = CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, length); + return (status == USBD_OK) ? 0 : -1; + } +} usb_sender; + +BidirectionalPacketBasedChannel usb_channel(usb_sender); + +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + +class USBSender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + if (CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, chunk) != USBD_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +} usb_sender; + +PacketToStreamConverter usb_packet_sender(usb_sender); +BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); +StreamToPacketConverter usb_stream_sink(usb_channel); + +#endif + +#if defined(UART_PROTOCOL_NATIVE) +class UART4Sender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart4_sender; + +PacketToStreamConverter uart4_packet_sender(uart4_sender); +BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); +StreamToPacketConverter UART4_stream_sink(uart4_channel); +#endif + + +class test_class { +public: + uint32_t property1; + float property2; + + float set_both(uint32_t arg1, float arg2) { + printf("set_both called with %u and %.3f\n", (unsigned int)arg1, arg2); + property1 = arg1; + property2 = arg2; + return arg1 + arg2; + } +}; + +float bla; + +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void init_communication(void) { + printf("hi!\r\n"); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 4*512); + thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); +} + + + +static auto make_obj_tree() { + return make_protocol_member_list( + make_protocol_property("bla2", &bla), + make_protocol_object("axis0", axes[0]->make_protocol_definitions()), + make_protocol_object("axis1", axes[1]->make_protocol_definitions()) + ); +} + +using tree_type = decltype(make_obj_tree()); +uint8_t tree_buffer[sizeof(tree_type)]; + +// the protocol has one additional built-in endpoint +constexpr size_t MAX_ENDPOINTS = decltype(make_obj_tree())::endpoint_count + 1; +Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; +const size_t max_endpoints_ = MAX_ENDPOINTS; +size_t n_endpoints_ = 0; + +// Thread to handle deffered processing of USB interrupt, and +// read commands out of the UART DMA circular buffer +void communication_task(void * ctx) { + (void) ctx; // unused parameter + + auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); + auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); + set_application_endpoints(&endpoint_provider); + +#if !defined(UART_PROTOCOL_NONE) + //DMA open loop continous circular buffer + //1ms delay periodic, chase DMA ptr around + + #define UART_RX_BUFFER_SIZE 64 + static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; + + // DMA is set up to recieve in a circular buffer forever. + // We dont use interrupts to fetch the data, instead we periodically read + // data out of the circular buffer into a parse buffer, controlled by a state machine + HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); + uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; +#endif + + // Re-run state-machine forever + for (;;) { +#if !defined(UART_PROTOCOL_NONE) + // Check for UART errors and restart recieve DMA transfer if required + if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { + HAL_UART_AbortReceive(&huart4); + HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); + } + // Fetch the circular buffer "write pointer", where it would write next + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(UART_PROTOCOL_NATIVE) + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { + UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx); + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { + UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); + last_rcv_idx = new_rcv_idx; + } +#elif defined(UART_PROTOCOL_LEGACY) + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { + legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx); + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { + legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); + last_rcv_idx = new_rcv_idx; + } +#endif +#endif + +#if !defined(USB_PROTOCOL_NONE) + // When we reach here, we are out of immediate characters to fetch out of UART buffer + // Now we check if there is any USB processing to do: we wait for up to 1 ms, + // before going back to checking UART again. + const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); + if (sem_stat == osOK) { + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(USB_PROTOCOL_NATIVE) + usb_channel.process_packet(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + usb_stream_sink.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_LEGACY) + legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); +#endif + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet + } +#endif + +#if defined(USB_PROTOCOL_NONE) && defined(UART_PROTOCOL_NONE) + osDelay(1); // don't starve other threads +#endif + } + + // If we get here, then this task is done + vTaskDelete(osThreadGetId()); +} + +// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the +// incoming USB data +void set_cmd_buffer(uint8_t *buf, uint32_t len) { + usb_buf = buf; + usb_len = len; +} + +void usb_update_thread(void * ctx) { + (void) ctx; // unused parameter + + for (;;) { + // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) + osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); + if (semaphore_status == osOK) { + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } + + vTaskDelete(osThreadGetId()); +} diff --git a/Firmware/MotorControl/commands.h b/Firmware/MotorControl/communication.h similarity index 81% rename from Firmware/MotorControl/commands.h rename to Firmware/MotorControl/communication.h index 2c82ad88..f02d1ae8 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/communication.h @@ -21,16 +21,6 @@ // #define UART_PROTOCOL_LEGACY #define UART_PROTOCOL_NONE -// Use GPIO 1/2 for step/dir input instead of UART -// #define USE_GPIO_MODE_STEP_DIR - - -typedef enum { - GPIO_MODE_NONE, - GPIO_MODE_UART, - GPIO_MODE_STEP_DIR, -} GpioMode_t; - extern "C" { #endif diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index b3ccd70c..c20cf504 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -3,7 +3,7 @@ Controller::Controller(ControllerConfig_t& config) : - config(config) + config_(config) {} //-------------------------------- @@ -11,32 +11,39 @@ Controller::Controller(ControllerConfig_t& config) : //-------------------------------- void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - pos_setpoint = pos_setpoint; - vel_setpoint = vel_feed_forward; - current_setpoint = current_feed_forward; - config.control_mode = CTRL_MODE_POSITION_CONTROL; + pos_setpoint_ = pos_setpoint; + vel_setpoint_ = vel_feed_forward; + current_setpoint_ = current_feed_forward; + config_.control_mode = CTRL_MODE_POSITION_CONTROL; #ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); + printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_); #endif } void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) { - vel_setpoint = vel_setpoint; - current_setpoint = current_feed_forward; - config.control_mode = CTRL_MODE_VELOCITY_CONTROL; + vel_setpoint_ = vel_setpoint; + current_setpoint_ = current_feed_forward; + config_.control_mode = CTRL_MODE_VELOCITY_CONTROL; #ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); + printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_); #endif } void Controller::set_current_setpoint(float current_setpoint) { - current_setpoint = current_setpoint; - config.control_mode = CTRL_MODE_CURRENT_CONTROL; + current_setpoint_ = current_setpoint; + config_.control_mode = CTRL_MODE_CURRENT_CONTROL; #ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); + printf("CURRENT_CONTROL %3.3f\n", current_setpoint_); #endif } +void Controller::start_anticogging_calibration() { + // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating + if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) { + anticogging_.calib_anticogging = true; + } +} + /* * This anti-cogging implementation iterates through each encoder position, * waits for zero velocity & position error, @@ -44,21 +51,21 @@ void Controller::set_current_setpoint(float current_setpoint) { * * This holding current is added as a feedforward term in the control loop. */ -bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate) { - if (anticogging.calib_anticogging && anticogging.cogging_map != NULL) { - float pos_err = anticogging.index - pos_estimate; - if (fabsf(pos_err) <= anticogging.calib_pos_threshold && - fabsf(vel_estimate) < anticogging.calib_vel_threshold) { - anticogging.cogging_map[anticogging.index++] = vel_integrator_current; +bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) { + if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) { + float pos_err = anticogging_.index - pos_estimate; + if (fabsf(pos_err) <= anticogging_.calib_pos_threshold && + fabsf(vel_estimate) < anticogging_.calib_vel_threshold) { + anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_; } - if (anticogging.index < axis->encoder.config.cpr) { // TODO: remove the dependency on encoder CPR - set_pos_setpoint(anticogging.index, 0.0f, 0.0f); + if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR + set_pos_setpoint(anticogging_.index, 0.0f, 0.0f); return false; } else { - anticogging.index = 0; + anticogging_.index = 0; set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home - anticogging.use_anticogging = true; // We're good to go, enable anti-cogging - anticogging.calib_anticogging = false; + anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging + anticogging_.calib_anticogging = false; return true; } } @@ -66,42 +73,42 @@ bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate } bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { - // Only runs if anticogging.calib_anticogging is true; non-blocking - anti_cogging_calibration(pos_estimate, vel_estimate); + // Only runs if anticogging_.calib_anticogging is true; non-blocking + anticogging_calibration(pos_estimate, vel_estimate); // Position control // TODO Decide if we want to use encoder or pll position here - float vel_des = vel_setpoint; - if (config.control_mode >= CTRL_MODE_POSITION_CONTROL) { - float pos_err = pos_setpoint - pos_estimate; - vel_des += config.pos_gain * pos_err; + float vel_des = vel_setpoint_; + if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) { + float pos_err = pos_setpoint_ - pos_estimate; + vel_des += config_.pos_gain * pos_err; } // Velocity limiting - float vel_lim = config.vel_limit; + float vel_lim = config_.vel_limit; if (vel_des > vel_lim) vel_des = vel_lim; if (vel_des < -vel_lim) vel_des = -vel_lim; // Velocity control - float Iq = current_setpoint; + float Iq = current_setpoint_; // Anti-cogging is enabled after calibration // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (anticogging.use_anticogging) { - Iq += anticogging.cogging_map[mod(pos_estimate, axis->encoder.config.cpr)]; + if (anticogging_.use_anticogging) { + Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; - if (config.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += config.vel_gain * v_err; + if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + Iq += config_.vel_gain * v_err; } // Velocity integral action before limiting - Iq += vel_integrator_current; + Iq += vel_integrator_current_; // Current limiting - float Ilim = std::min(axis->motor.config.current_lim, axis->motor.current_control.max_allowed_current); + float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current); bool limited = false; if (Iq > Ilim) { limited = true; @@ -113,15 +120,15 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s } // Velocity integrator (behaviour dependent on limiting) - if (config.control_mode < CTRL_MODE_VELOCITY_CONTROL) { + if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) { // reset integral if not in use - vel_integrator_current = 0.0f; + vel_integrator_current_ = 0.0f; } else { if (limited) { // TODO make decayfactor configurable - vel_integrator_current *= 0.99f; + vel_integrator_current_ *= 0.99f; } else { - vel_integrator_current += (config.vel_integrator_gain * current_meas_period) * v_err; + vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err; } } diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index df6e22e9..b5767b6f 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -30,14 +30,15 @@ public: void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); void set_current_setpoint(float current_setpoint); - + // TODO: make this more similar to other calibration loops - bool anti_cogging_calibration(float pos_estimate, float vel_estimate); + void start_anticogging_calibration(); + bool anticogging_calibration(float pos_estimate, float vel_estimate); bool update(float pos_estimate, float vel_estimate, float* current_setpoint); - ControllerConfig_t& config; - Axis* axis = nullptr; // set by Axis constructor + ControllerConfig_t& config_; + Axis* axis_ = nullptr; // set by Axis constructor // TODO: anticogging overhaul: // - expose selected (all?) variables on protocol @@ -53,7 +54,7 @@ public: float calib_pos_threshold; float calib_vel_threshold; } Anticogging_t; - Anticogging_t anticogging = { + Anticogging_t anticogging_ = { .index = 0, .cogging_map = nullptr, .use_anticogging = false, @@ -63,25 +64,38 @@ public: }; // variables exposed on protocol - float pos_setpoint = 0.0f; - float vel_setpoint = 0.0f; + float pos_setpoint_ = 0.0f; + float vel_setpoint_ = 0.0f; // float vel_setpoint = 800.0f; - float vel_integrator_current = 0.0f; // [A] - float current_setpoint = 0.0f; // [A] + float vel_integrator_current_ = 0.0f; // [A] + float current_setpoint_ = 0.0f; // [A] - // Cache for remote procedure calls arguments TODO: remove - struct { - float pos_setpoint; - float vel_feed_forward; - float current_feed_forward; - } set_pos_setpoint_args; - struct { - float vel_setpoint; - float current_feed_forward; - } set_vel_setpoint_args; - struct { - float current_setpoint; - } set_current_setpoint_args; + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_property("pos_setpoint", &pos_setpoint_), + make_protocol_property("vel_setpoint", &vel_setpoint_), + make_protocol_property("vel_integrator_current", &vel_integrator_current_), + make_protocol_property("current_setpoint", ¤t_setpoint_), + make_protocol_object("config", + make_protocol_property("control_mode", &config_.control_mode), + make_protocol_property("pos_gain", &config_.pos_gain), + make_protocol_property("vel_gain", &config_.vel_gain), + make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), + make_protocol_property("vel_limit", &config_.vel_limit) + ), + make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, + "pos_setpoint", + "vel_feed_forward", + "current_feed_forward"), + make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, + "vel_setpoint", + "current_feed_forward"), + make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, + "current_setpoint"), + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) + ); + } }; #endif // __CONTROLLER_HPP diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index f4d81da7..7bfd7123 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -5,16 +5,16 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, EncoderConfig_t& config) : - hw_config(hw_config), - config(config) + hw_config_(hw_config), + config_(config) { // Calculate encoder pll gains // This calculation is currently identical to the PLL in SensorlessEstimator float pll_bandwidth = 1000.0f; // [rad/s] - pll_kp = 2.0f * pll_bandwidth; + pll_kp_ = 2.0f * pll_bandwidth; // Critically damped - pll_ki = 0.25f * (pll_kp * pll_kp); + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); } static void enc_index_cb_wrapper(void* ctx) { @@ -22,8 +22,8 @@ static void enc_index_cb_wrapper(void* ctx) { } void Encoder::setup() { - HAL_TIM_Encoder_Start(hw_config.timer, TIM_CHANNEL_ALL); - GPIO_subscribe(hw_config.index_port, hw_config.index_pin, GPIO_NOPULL, + HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL); + GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL, enc_index_cb_wrapper, this); } @@ -33,22 +33,22 @@ void Encoder::setup() { // Triggered when an encoder passes over the "Index" pin // TODO: only arm index edge interrupt when we know encoder has powered up -// TODO: disarm interrupt once we found the index +// TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { - if (!index_found) { + if (!index_found_) { set_count(0); - index_found = true; + index_found_ = true; } } // Function that sets the current encoder count to a desired 32-bit value. -void Encoder::set_count(uint32_t count) { +void Encoder::set_count(int32_t count) { // Disable interrupts to make a critical section to avoid race condition uint32_t prim = __get_PRIMASK(); __disable_irq(); - state = count; - hw_config.timer->Instance->CNT = count; - pll_pos = (float)count; + state_ = count; + hw_config_.timer->Instance->CNT = count; + pll_pos_ = (float)count; __set_PRIMASK(prim); } @@ -59,109 +59,109 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { static const float start_lock_duration = 1.0f; static const float scan_omega = 4.0f * M_PI; static const float scan_distance = 16.0f * M_PI; - static const size_t num_steps = scan_distance / scan_omega * current_meas_hz; + static const int num_steps = scan_distance / scan_omega * current_meas_hz; // go to motor zero phase for start_lock_duration to get ready to scan - size_t i = 0; - axis->run_control_loop([&](){ - axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f); + int i = 0; + axis_->run_control_loop([&](){ + axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f); return ++i < start_lock_duration * current_meas_hz; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; - int32_t init_enc_val = (int16_t)hw_config.timer->Instance->CNT; + int32_t init_enc_val = (int16_t)hw_config_.timer->Instance->CNT; int64_t encvaluesum = 0; // scan forward i = 0; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; return ++i < num_steps; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float expected_encoder_delta = scan_distance / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range) + float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { - error = ERROR_CPR_OUT_OF_RANGE; + error_ = ERROR_CPR_OUT_OF_RANGE; return false; } // check direction - if ((int16_t)hw_config.timer->Instance->CNT > init_enc_val + 8) { + if ((int16_t)hw_config_.timer->Instance->CNT > init_enc_val + 8) { // motor same dir as encoder - axis->motor.config.direction = 1; - } else if ((int16_t)hw_config.timer->Instance->CNT < init_enc_val - 8) { + axis_->motor_.config_.direction = 1; + } else if ((int16_t)hw_config_.timer->Instance->CNT < init_enc_val - 8) { // motor opposite dir as encoder - axis->motor.config.direction = -1; + axis_->motor_.config_.direction = -1; } else { // Encoder response error - error = ERROR_RESPONSE; + error_ = ERROR_RESPONSE; return false; } // scan backwards i = 0; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; return ++i < num_steps; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; int offset = encvaluesum / (num_steps * 2); - config.offset = offset; - config.calibrated = true; + config_.offset = offset; + is_calibrated_ = true; return true; } bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { - index_found = false; + index_found_ = false; float phase = 0.0f; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ phase = wrap_pm_pi(phase + omega * current_meas_period); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); // continue until the index is found - return !index_found; + return !index_found_; }); - return axis->error == Axis::ERROR_NO_ERROR; + return axis_->error_ == Axis::ERROR_NO_ERROR; } bool Encoder::run_calibration() { float enc_calibration_voltage; - if (axis->motor.config.motor_type == MOTOR_TYPE_HIGH_CURRENT) - enc_calibration_voltage = axis->motor.config.calibration_current * axis->motor.config.phase_resistance; - else if (axis->motor.config.motor_type == MOTOR_TYPE_GIMBAL) - enc_calibration_voltage = axis->motor.config.calibration_current; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + enc_calibration_voltage = axis_->motor_.config_.calibration_current; else return false; - if (config.use_index && !index_found) + if (config_.use_index && !index_found_) if (!scan_for_enc_idx( - /*(float)(axis->motor.config.direction) * */ config.idx_search_speed, + (float)(axis_->motor_.config_.direction) * config_.idx_search_speed, enc_calibration_voltage)) return false; - if (!config.calibrated) + if (!config_.hand_calibrated) // if (!calib_enc_offset(enc_calibration_voltage)) return false; return true; @@ -169,38 +169,38 @@ bool Encoder::run_calibration() { bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp < 1.0f)) { - error = ERROR_NUMERICAL; + if (!(current_meas_period * pll_kp_ < 1.0f)) { + error_ = ERROR_NUMERICAL; return false; } // update internal encoder state - int16_t delta_enc = (int16_t)hw_config.timer->Instance->CNT - (int16_t)state; - state += (int32_t)delta_enc; + int16_t delta_enc = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)state_; + state_ += (int32_t)delta_enc; // compute electrical phase - int corrected_enc = state % config.cpr; - corrected_enc -= config.offset; - //corrected_enc *= axis->motor.config.direction; TODO: verify if this still works + int corrected_enc = state_ % config_.cpr; + corrected_enc -= config_.offset; + //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float ph = elec_rad_per_enc * (float)corrected_enc; // ph = fmodf(ph, 2*M_PI); - phase = wrap_pm_pi(ph); + phase_ = wrap_pm_pi(ph); // run pll (for now pll is in units of encoder counts) // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? // Predict current pos - pll_pos += current_meas_period * pll_vel; + pll_pos_ += current_meas_period * pll_vel_; // discrete phase detector - float delta_pos = (float)(state - (int32_t)floorf(pll_pos)); + float delta_pos = (float)(state_ - (int32_t)floorf(pll_pos_)); // pll feedback - pll_pos += current_meas_period * pll_kp * delta_pos; - pll_vel += current_meas_period * pll_ki * delta_pos; + pll_pos_ += current_meas_period * pll_kp_ * delta_pos; + pll_vel_ += current_meas_period * pll_ki_ * delta_pos; // Assign output arguments - if (*pos_estimate) *pos_estimate = pll_pos; - if (*vel_estimate) *vel_estimate = pll_vel; - if (*phase_output) *phase_output = phase; + if (pos_estimate) *pos_estimate = pll_pos_; + if (vel_estimate) *vel_estimate = pll_vel_; + if (phase_output) *phase_output = phase_; return true; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 6ab419b8..1082594e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -7,7 +7,7 @@ struct EncoderConfig_t { bool use_index = false; - bool calibrated = false; + bool hand_calibrated = false; float idx_search_speed = 10.0f; // [rad/s electrical] int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, int32_t offset = 0; @@ -30,25 +30,48 @@ public: void enc_index_cb(); - void set_count(uint32_t count); + void set_count(int32_t count); bool calib_enc_offset(float voltage_magnitude); bool scan_for_enc_idx(float omega, float voltage_magnitude); bool update(float* pos_estimate, float* vel_estimate, float* phase); bool run_calibration(); - const EncoderHardwareConfig_t& hw_config; - EncoderConfig_t& config; - Axis* axis = nullptr; // set by Axis constructor + const EncoderHardwareConfig_t& hw_config_; + EncoderConfig_t& config_; + Axis* axis_ = nullptr; // set by Axis constructor - Error_t error = ERROR_NONE; - volatile bool index_found = false; - int32_t state = 0; - float phase = 0.0f; // [rad] - float pll_pos = 0.0f; // [rad] - float pll_vel = 0.0f; // [rad/s] - float pll_kp = 0.0f; // [rad/s / rad] - float pll_ki = 0.0f; // [(rad/s^2) / rad] + Error_t error_ = ERROR_NONE; + bool index_found_ = false; + bool is_calibrated_ = config_.hand_calibrated; + int32_t state_ = 0; + float phase_ = 0.0f; // [rad] + float pll_pos_ = 0.0f; // [rad] + float pll_vel_ = 0.0f; // [rad/s] + float pll_kp_ = 0.0f; // [rad/s / rad] + float pll_ki_ = 0.0f; // [(rad/s^2) / rad] + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_object("config", + make_protocol_property("use_index", &config_.use_index), + make_protocol_property("hand_calibrated", &config_.hand_calibrated), + make_protocol_property("idx_search_speed", &config_.idx_search_speed), + make_protocol_property("cpr", &config_.cpr), + make_protocol_property("offset", &config_.offset), + make_protocol_property("calib_range", &config_.calib_range) + ), + make_protocol_property("error", &error_), + make_protocol_ro_property("index_found", const_cast(&index_found_)), + make_protocol_property("state", &state_), + make_protocol_property("phase", &phase_), + make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("pll_kp", &pll_kp_), + make_protocol_property("pll_ki", &pll_ki_) + ); + } }; #endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 1dc791e9..5b1b44a5 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -76,20 +76,6 @@ void start_adc_pwm() { HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); } -void halt_motors(Motor::Error_t error) { - // Disable motors NOW! - for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->motor.disarm(); - } - // Set fault codes, etc. - for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->motor.error = error; - axes[i]->error = Axis::ERROR_MOTOR_FAILED; - } - // disable brake resistor - set_brake_current(0.0f); -} - void start_pwm(TIM_HandleTypeDef* htim) { // Init PWM int half_load = TIM_1_8_PERIOD_CLOCKS / 2; @@ -155,6 +141,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } +// @brief Floats ALL phases immediately and sets the brake current to 0. +void disable_all_pwms(Motor::Error_t error) { + // Disable all motors NOW! + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->motor_.disarm(); + axes[i]->motor_.error_ = error; + } +} + //-------------------------------- // IRQ Callbacks //-------------------------------- @@ -175,7 +170,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Ensure ADCs are expected ones to simplify the logic below if (!(hadc == &hadc2 || hadc == &hadc3)) { - halt_motors(Motor::ERROR_ADC_FAILED); + disable_all_pwms(Motor::ERROR_ADC_FAILED); return; }; @@ -185,27 +180,35 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // If we are counting down, we just sampled in SVM vector 7, with zero current Axis& axis = injected ? *axes[0] : *axes[1]; Axis& other_axis = injected ? *axes[1] : *axes[0]; - bool counting_down = axis.motor.hw_config.timer->Instance->CR1 & TIM_CR1_DIR; + bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR; bool current_meas_not_DC_CAL = !counting_down; - if (&axis == axes[1] && counting_down) { - // Load next timings for M0 (only once is sufficient) - if (hadc == &hadc2) { - other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; - other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; - other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; - } - } else if (&axis == axes[0] && !counting_down) { - // Load next timings for M1 (only once is sufficient) - if (hadc == &hadc2) { - other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; - other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; - other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; + bool update_timings = false; + if (hadc == &hadc2) { + if (&axis == axes[1] && counting_down) + update_timings = true; // update timings of M0 + else if (&axis == axes[0] && !counting_down) + update_timings = true; // update timings of M1 + } + + // Load next timings for the motor that we're not currently sampling + if (update_timings) { + if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) { + other_axis.motor_.next_timings_valid_ = false; + other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0]; + other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1]; + other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2]; + __HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer); // enable pwm outputs + update_brake_current(); + } else { + // the motor control loop failed to update the timings in time + // we must assume that it died and therefore float all phases + other_axis.motor_.disarm(); } } // Check the timing of the sequencing - axis.motor.check_timing(); + axis.motor_.log_timing(); uint32_t ADCValue; if (injected) { @@ -213,7 +216,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { } else { ADCValue = HAL_ADC_GetValue(hadc); } - float current = axis.motor.phase_current_from_adcval(ADCValue); + float current = axis.motor_.phase_current_from_adcval(ADCValue); if (current_meas_not_DC_CAL) { // ADC2 and ADC3 record the phB and phC currents concurrently, @@ -224,33 +227,32 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // return or continue if (hadc == &hadc2) { - axis.motor.current_meas.phB = current - axis.motor.DC_calib.phB; + axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB; return; } else { - axis.motor.current_meas.phC = current - axis.motor.DC_calib.phC; + axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC; } // Trigger axis thread - axis.signal_thread(Axis::thread_signals::M_SIGNAL_PH_CURRENT_MEAS); + axis.signal_current_meas(); } else { // DC_CAL measurement if (hadc == &hadc2) { - axis.motor.DC_calib.phB += (current - axis.motor.DC_calib.phB) * calib_filter_k; + axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k; } else { - axis.motor.DC_calib.phC += (current - axis.motor.DC_calib.phC) * calib_filter_k; + axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k; } } } +// @brief Sums up the Ibus contribution of each motor and updates the +// brake resistor PWM accordingly. void update_brake_current() { float Ibus_sum = 0.0f; for (size_t i = 0; i < AXIS_COUNT; ++i) { - Ibus_sum += axes[i]->motor.current_control.Ibus; + Ibus_sum += axes[i]->motor_.current_control_.Ibus; } - // Note: set_brake_current will clip negative values to 0.0f - set_brake_current(-Ibus_sum); -} - -void set_brake_current(float brake_current) { + float brake_current = -Ibus_sum; + // Clip negative values to 0.0f if (brake_current < 0.0f) brake_current = 0.0f; float brake_duty = brake_current * brake_resistance / vbus_voltage; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 7654dfaa..3105bae0 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -12,12 +12,6 @@ extern "C" { #include /* Exported types ------------------------------------------------------------*/ - -typedef struct{ - int type; - int index; -} monitoring_slot; - /* Exported constants --------------------------------------------------------*/ /* Exported variables --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ @@ -35,7 +29,6 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); void update_brake_current(); -void set_brake_current(float brake_current); #ifdef __cplusplus } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 6dde3afd..e834b075 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,7 +1,7 @@ #include "odrive_main.hpp" -#include - +#include "nvm_config.hpp" +#include "communication.h" EncoderConfig_t encoder_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; @@ -67,7 +67,7 @@ int odrive_main(void) { // TODO: make dynamically reconfigurable #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (enable_uart) { - axes[0]->config.enable_step_dir = false; + axes[0]->config_.enable_step_dir = false; axes[0]->set_step_dir_enabled(false); SetGPIO12toUART(); } diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 044f4264..1ef1e9ce 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -9,33 +9,61 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_t& config) : - hw_config(hw_config), - gate_driver_config(gate_driver_config), - config(config), - gate_driver({ - .spiHandle = gate_driver_config.spi, - .EngpioHandle = gate_driver_config.enable_port, - .EngpioNumber = gate_driver_config.enable_pin, - .nCSgpioHandle = gate_driver_config.nCS_port, - .nCSgpioNumber = gate_driver_config.nCS_pin, + hw_config_(hw_config), + gate_driver_config_(gate_driver_config), + config_(config), + gate_driver_({ + .spiHandle = gate_driver_config_.spi, + .EngpioHandle = gate_driver_config_.enable_port, + .EngpioNumber = gate_driver_config_.enable_pin, + .nCSgpioHandle = gate_driver_config_.nCS_port, + .nCSgpioNumber = gate_driver_config_.nCS_pin, }) { } -void Motor::arm() { - __HAL_TIM_MOE_ENABLE(hw_config.timer); // enable pwm outputs +// @brief Arms the PWM outputs that belong to this motor. +// +// Note that this does not yet activate the PWM outputs, it just unlocks them. +// +// While the motor is armed, the control loop must set new modulation timings +// between any two interrupts (that is, enqueue_modulation_timings must be executed). +// If the control loop fails to do so, the next interrupt handler floats the +// phases. Once this happens, missed_control_deadline is set to true and +// the motor can be considered disarmed. +// +// @returns: True on success, false otherwise +bool Motor::arm() { + // Wait until the interrupt handler triggers twice. After the first wait there is an + // undefined period until the next trigger. After the second wait we know for sure + // that we have exactly one full interrupt period until the third trigger. This gives + // the control loop the correct time quota to set up modulation timings. + if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) + return false; + next_timings_valid_ = false; + axis_->missed_control_deadline_ = false; + return true; } +// @brief Floats the phases of this motor immediately and updates +// the brake current accordingly. void Motor::disarm() { - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config.timer); // disables pwm outputs + // disable pwm + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer); + // set this motor's contribution to 0 + current_control_.Ibus = 0.0f; + update_brake_current(); + // ensure the PWM is not re-enabled without the state machine explicitly + // calling motor.arm() + axis_->missed_control_deadline_ = true; } -// Set up the gate drivers +// @brief Set up the gate drivers void Motor::DRV8301_setup() { - DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; - DRV8301_enable(&gate_driver); - DRV8301_setupSpi(&gate_driver, local_regs); + DRV8301_enable(&gate_driver_); + DRV8301_setupSpi(&gate_driver_, local_regs); // TODO we can use reporting only if we actually wire up the nOCTW pin local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; @@ -50,41 +78,42 @@ void Motor::DRV8301_setup() { switch (local_regs->Ctrl_Reg_2.GAIN) { case DRV8301_ShuntAmpGain_10VpV: - phase_current_rev_gain = 1.0f / 10.0f; + phase_current_rev_gain_ = 1.0f / 10.0f; break; case DRV8301_ShuntAmpGain_20VpV: - phase_current_rev_gain = 1.0f / 20.0f; + phase_current_rev_gain_ = 1.0f / 20.0f; break; case DRV8301_ShuntAmpGain_40VpV: - phase_current_rev_gain = 1.0f / 40.0f; + phase_current_rev_gain_ = 1.0f / 40.0f; break; case DRV8301_ShuntAmpGain_80VpV: - phase_current_rev_gain = 1.0f / 80.0f; + phase_current_rev_gain_ = 1.0f / 80.0f; break; } float margin = 0.90f; - float max_input = margin * 0.3f * hw_config.shunt_conductance; - float max_swing = margin * 1.6f * hw_config.shunt_conductance * phase_current_rev_gain; - current_control.max_allowed_current = std::min(max_input, max_swing); + float max_input = margin * 0.3f * hw_config_.shunt_conductance; + float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_; + current_control_.max_allowed_current = std::min(max_input, max_swing); local_regs->SndCmd = true; - DRV8301_writeData(&gate_driver, local_regs); + DRV8301_writeData(&gate_driver_, local_regs); local_regs->RcvCmd = true; - DRV8301_readData(&gate_driver, local_regs); + DRV8301_readData(&gate_driver_, local_regs); } -//Returns true if everything is OK (no fault) +// @brief Checks if the gate driver is in operational state. +// @returns: true if the gate driver is OK (no fault), false otherwise bool Motor::check_DRV_fault() { //TODO: make this pin configurable per motor ch - GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config.nFAULT_port, gate_driver_config.nFAULT_pin); + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin); if (nFAULT_state == GPIO_PIN_RESET) { // Update DRV Fault Code - drv_fault = DRV8301_getFaultType(&gate_driver); + drv_fault_ = DRV8301_getFaultType(&gate_driver_); // Update/Cache all SPI device registers - DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; local_regs->RcvCmd = true; - DRV8301_readData(&gate_driver, local_regs); + DRV8301_readData(&gate_driver_, local_regs); return false; }; return true; @@ -92,14 +121,14 @@ bool Motor::check_DRV_fault() { bool Motor::do_checks() { if (!check_DRV_fault()) { - error = ERROR_DRV_FAULT; + error_ = ERROR_DRV_FAULT; return false; } return true; } -uint16_t Motor::check_timing() { - TIM_HandleTypeDef* htim = hw_config.timer; +void Motor::log_timing() { + TIM_HandleTypeDef* htim = hw_config_.timer; uint16_t timing = htim->Instance->CNT; bool down = htim->Instance->CR1 & TIM_CR1_DIR; if (down) { @@ -107,19 +136,17 @@ uint16_t Motor::check_timing() { timing = TIM_1_8_PERIOD_CLOCKS + delta; } - if (++(timing_log_index) == TIMING_LOG_SIZE) { - timing_log_index = 0; + if (++(timing_log_index_) == TIMING_LOG_SIZE) { + timing_log_index_ = 0; } - timing_log[timing_log_index] = timing; - - return timing; + timing_log_[timing_log_index_] = timing; } float Motor::phase_current_from_adcval(uint32_t ADCValue) { int adcval_bal = (int)ADCValue - (1 << 11); float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; - float shunt_volt = amp_out_volt * phase_current_rev_gain; - float current = shunt_volt * hw_config.shunt_conductance; + float shunt_volt = amp_out_volt * phase_current_rev_gain_; + float current = shunt_volt * hw_config_.shunt_conductance; return current; } @@ -134,25 +161,25 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { float test_voltage = 0.0f; size_t i = 0; - axis->run_control_loop([&](){ - float Ialpha = -(current_meas.phB + current_meas.phC); + axis_->run_control_loop([&](){ + float Ialpha = -(current_meas_.phB + current_meas_.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage || test_voltage < -max_voltage) - return error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; + return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A enqueue_voltage_timings(test_voltage, 0.0f); return ++i < num_test_cycles; }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; //// De-energize motor //enqueue_voltage_timings(motor, 0.0f, 0.0f); float R = test_voltage / test_current; - config.phase_resistance = R; + config_.phase_resistance = R; return true; // if we ran to completion that means success } @@ -162,16 +189,16 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { static const int num_cycles = 5000; size_t t = 0; - axis->run_control_loop([&](){ + axis_->run_control_loop([&](){ int i = t & 1; - Ialphas[i] += -current_meas.phB - current_meas.phC; + Ialphas[i] += -current_meas_.phB - current_meas_.phC; // Test voltage along phase A enqueue_voltage_timings(test_voltages[i], 0.0f); return ++t < (num_cycles << 1); }); - if (axis->error != Axis::ERROR_NO_ERROR) + if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; //// De-energize motor @@ -183,24 +210,24 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); float L = v_L / dI_by_dt; - config.phase_inductance = L; + config_.phase_inductance = L; // TODO arbitrary values set for now if (L < 1e-6f || L > 500e-6f) - return error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; + return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; return true; } bool Motor::run_calibration() { - error = ERROR_NO_ERROR; + error_ = ERROR_NO_ERROR; - float R_calib_max_voltage = config.resistance_calib_max_voltage; - if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if (!measure_phase_resistance(config.calibration_current, R_calib_max_voltage)) + float R_calib_max_voltage = config_.resistance_calib_max_voltage; + if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) return false; if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage)) return false; - } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { // no calibration needed } else { return false; @@ -208,19 +235,21 @@ bool Motor::run_calibration() { // Calculate current control gains float current_control_bandwidth = 1000.0f; // [rad/s] - current_control.p_gain = current_control_bandwidth * config.phase_inductance; - float plant_pole = config.phase_resistance / config.phase_inductance; - current_control.i_gain = plant_pole * current_control.p_gain; - + current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; + float plant_pole = config_.phase_resistance / config_.phase_inductance; + current_control_.i_gain = plant_pole * current_control_.p_gain; + + is_calibrated_ = true; return true; } void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; SVM(mod_alpha, mod_beta, &tA, &tB, &tC); - next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); - next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); - next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings_valid_ = true; } void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { @@ -242,14 +271,14 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { - Current_control_t* ictrl = ¤t_control; + Current_control_t* ictrl = ¤t_control_; // For Reporting ictrl->Iq_setpoint = Iq_des; // Clarke transform - float Ialpha = -current_meas.phB - current_meas.phC; - float Ibeta = one_by_sqrt3 * (current_meas.phB - current_meas.phC); + float Ialpha = -current_meas_.phB - current_meas_.phC; + float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); // Park transform float c = arm_cos_f32(phase); @@ -306,21 +335,21 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { bool Motor::update(float current_setpoint, float phase) { - current_setpoint *= config.direction; - phase *= config.direction; + current_setpoint *= config_.direction; + phase *= config_.direction; // Execute current command // TODO: move this into the mot - if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { if(!FOC_current(0.0f, current_setpoint, phase)){ return false; } - } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + } else if (config_.motor_type == MOTOR_TYPE_GIMBAL) { //In gimbal motor mode, current is reinterptreted as voltage. if(!FOC_voltage(0.0f, current_setpoint, phase)) return false; } else { - error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; return false; } return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 877921f9..2566a28d 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -36,6 +36,7 @@ typedef struct { // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. typedef struct { + bool hand_calibrated = true; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. float calibration_current = 10.0f; // [A] float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. @@ -67,7 +68,7 @@ public: const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_t& config); - void arm(); + bool arm(); void disarm(); void setup() { DRV8301_setup(); @@ -75,7 +76,7 @@ public: void DRV8301_setup(); bool check_DRV_fault(); bool do_checks(); - uint16_t check_timing(); + void log_timing(); float phase_current_from_adcval(uint32_t ADCValue); bool measure_phase_resistance(float test_current, float max_voltage); bool measure_phase_inductance(float voltage_low, float voltage_high); @@ -86,30 +87,32 @@ public: bool FOC_current(float Id_des, float Iq_des, float phase); bool update(float current_setpoint, float phase); - const MotorHardwareConfig_t& hw_config; - const GateDriverHardwareConfig_t gate_driver_config; - MotorConfig_t& config; - Axis* axis = nullptr; // set by Axis constructor + const MotorHardwareConfig_t& hw_config_; + const GateDriverHardwareConfig_t gate_driver_config_; + MotorConfig_t& config_; + Axis* axis_ = nullptr; // set by Axis constructor //private: - DRV8301_Obj gate_driver; // initialized in constructor - uint16_t next_timings[3] = { + DRV8301_Obj gate_driver_; // initialized in constructor + uint16_t next_timings_[3] = { TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2 }; - uint16_t last_cpu_time = 0; - int timing_log_index = 0; - uint16_t timing_log[TIMING_LOG_SIZE] = { 0 }; + bool next_timings_valid_ = false; + uint16_t last_cpu_time_ = 0; + int timing_log_index_ = 0; + uint16_t timing_log_[TIMING_LOG_SIZE] = { 0 }; // variables exposed on protocol - Error_t error = ERROR_NO_ERROR; - Iph_BC_t current_meas = {0.0f, 0.0f}; - Iph_BC_t DC_calib = {0.0f, 0.0f}; - const float shunt_conductance = 1.0f / SHUNT_RESISTANCE; //[S] - float phase_current_rev_gain = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) - Current_control_t current_control = { + Error_t error_ = ERROR_NO_ERROR; + bool is_calibrated_ = config_.hand_calibrated; + Iph_BC_t current_meas_ = {0.0f, 0.0f}; + Iph_BC_t DC_calib_ = {0.0f, 0.0f}; + const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S] + float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) + Current_control_t current_control_ = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, @@ -121,47 +124,47 @@ public: .Iq_measured = 0.0f, .max_allowed_current = 0.0f, }; - DRV8301_FaultType_e drv_fault = DRV8301_FaultType_NoFault; - DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup) + DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault; + DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup) // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( - make_protocol_property("error", reinterpret_cast(&this->error)), - make_protocol_ro_property("current_meas.phB", &this->current_meas.phB), - make_protocol_ro_property("current_meas.phC", &this->current_meas.phC), - make_protocol_property("DC_calib.phB", &this->DC_calib.phB), - make_protocol_property("DC_calib.phC", &this->DC_calib.phC), - make_protocol_property("shunt_conductance", &this->shunt_conductance), - make_protocol_property("phase_current_rev_gain", &this->phase_current_rev_gain), + make_protocol_property("error", &error_), + make_protocol_ro_property("current_meas_phB", ¤t_meas_.phB), + make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), + make_protocol_property("DC_calib_phB", &DC_calib_.phB), + make_protocol_property("DC_calib_phC", &DC_calib_.phC), + make_protocol_property("shunt_conductance", &shunt_conductance_), + make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_), make_protocol_object("current_control", - make_protocol_property("p_gain", &this->current_control.p_gain), - make_protocol_property("i_gain", &this->current_control.i_gain), - make_protocol_property("v_current_control_integral_d", &this->current_control.v_current_control_integral_d), - make_protocol_property("v_current_control_integral_q", &this->current_control.v_current_control_integral_q), - make_protocol_property("Ibus", &this->current_control.Ibus), - make_protocol_property("final_v_alpha", &this->current_control.final_v_alpha), - make_protocol_property("final_v_beta", &this->current_control.final_v_beta), - make_protocol_property("Iq_setpoint", &this->current_control.Iq_setpoint), - make_protocol_property("Iq_measured", &this->current_control.Iq_measured), - make_protocol_property("max_allowed_current", &this->current_control.max_allowed_current) + make_protocol_property("p_gain", ¤t_control_.p_gain), + make_protocol_property("i_gain", ¤t_control_.i_gain), + make_protocol_property("v_current_control_integral_d", ¤t_control_.v_current_control_integral_d), + make_protocol_property("v_current_control_integral_q", ¤t_control_.v_current_control_integral_q), + make_protocol_property("Ibus", ¤t_control_.Ibus), + make_protocol_property("final_v_alpha", ¤t_control_.final_v_alpha), + make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), + make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), + make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), + make_protocol_property("max_allowed_current", ¤t_control_.max_allowed_current) ), make_protocol_object("gate_driver", - make_protocol_ro_property("drv_fault", reinterpret_cast(&this->drv_fault)), - make_protocol_ro_property("status_reg_1", &this->gate_driver_regs.Stat_Reg_1_Value), - make_protocol_ro_property("status_reg_2", &this->gate_driver_regs.Stat_Reg_2_Value), - make_protocol_ro_property("ctrl_reg_1", &this->gate_driver_regs.Ctrl_Reg_1_Value), - make_protocol_ro_property("ctrl_reg_2", &this->gate_driver_regs.Ctrl_Reg_2_Value) + make_protocol_ro_property("drv_fault", &drv_fault_), + make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), + make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), + make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), + make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) ), make_protocol_object("config", - make_protocol_property("pole_pairs", &this->config.pole_pairs), - make_protocol_property("calibration_current", &this->config.calibration_current), - make_protocol_property("resistance_calib_max_voltage", &this->config.resistance_calib_max_voltage), - make_protocol_property("phase_inductance", &this->config.phase_inductance), - make_protocol_property("phase_resistance", &this->config.phase_resistance), - make_protocol_property("direction", &this->config.direction), - make_protocol_property("motor_type", reinterpret_cast(&this->config.motor_type)), - make_protocol_property("current_lim", &this->config.current_lim) + make_protocol_property("pole_pairs", &config_.pole_pairs), + make_protocol_property("calibration_current", &config_.calibration_current), + make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage), + make_protocol_property("phase_inductance", &config_.phase_inductance), + make_protocol_property("phase_resistance", &config_.phase_resistance), + make_protocol_property("direction", &config_.direction), + make_protocol_property("motor_type", &config_.motor_type), + make_protocol_property("current_lim", &config_.current_lim) ) ); } diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index e9bb285d..ef9e3df8 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -44,8 +44,6 @@ extern Axis *axes[AXIS_COUNT]; #include #include -#include // TODO: remove - // defined in main.cpp void save_configuration(void); void erase_configuration(void); diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index 3e0336fa..c0cd07f0 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -118,10 +118,6 @@ JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint(); EndpointProvider* application_endpoints; uint16_t json_crc_; -Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; -size_t n_endpoints_ = 0; -EndpointProvider* endpoint_provider_ = nullptr; - void JSONDescriptorEndpoint::write_json(size_t id, StreamSink* output) { write_string("{\"name\":\"\",", output); @@ -162,9 +158,9 @@ void set_application_endpoints(EndpointProvider* endpoints) { application_endpoints = endpoints; n_endpoints_ = 0; - json_file_endpoint.register_endpoints(endpoints_, 0, MAX_ENDPOINTS); + json_file_endpoint.register_endpoints(endpoints_, 0, max_endpoints_); n_endpoints_ += decltype(json_file_endpoint)::endpoint_count; - application_endpoints->register_endpoints(endpoints_, n_endpoints_, MAX_ENDPOINTS); + application_endpoints->register_endpoints(endpoints_, n_endpoints_, max_endpoints_); n_endpoints_ += application_endpoints->get_endpoint_count(); // Calculates the CRC16 of the JSON file. @@ -174,7 +170,6 @@ void set_application_endpoints(EndpointProvider* endpoints) { json_file_endpoint.handle(offset, sizeof(offset), &crc16_calculator); json_crc_ = crc16_calculator.get_crc16(); - CRC16Calculator crc16_calculator2(PROTOCOL_VERSION); endpoints_[0]->handle(offset, sizeof(offset), &crc16_calculator2); json_crc_ = crc16_calculator2.get_crc16(); diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index c4e1652f..b3606211 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -369,8 +369,6 @@ inline constexpr const char* get_default_json_modifier() { return "\"type\":\"bool\",\"access\":\"rw\""; } -constexpr size_t MAX_ENDPOINTS = 100; - class Endpoint { public: //const char* const name_; @@ -417,7 +415,6 @@ template<> struct MemberList<> { public: static constexpr size_t endpoint_count = 0; - size_t get_endpoint_count() { return endpoint_count; } static constexpr bool is_empty = true; void write_json(size_t id, StreamSink* output) { // no action @@ -432,7 +429,6 @@ template struct MemberList { public: static constexpr size_t endpoint_count = TMember::endpoint_count + MemberList::endpoint_count; - size_t get_endpoint_count() { return endpoint_count; } static constexpr bool is_empty = false; MemberList(TMember&& this_member, TMembers&&... subsequent_members) : @@ -441,7 +437,7 @@ public: MemberList(TMember&& this_member, MemberList&& subsequent_members) : this_member_(std::forward(this_member)), - subsequent_members_(std::forward(subsequent_members)) {} + subsequent_members_(std::forward>(subsequent_members)) {} // @brief Move constructor /* MemberList(MemberList&& other) : @@ -572,16 +568,30 @@ public: TProperty* property_; }; -template +// Non-const non-enum types +template::value>> ProtocolProperty make_protocol_property(const char * name, TProperty* property) { return ProtocolProperty(name, property); }; -template +// Const non-enum types +template::value>> ProtocolProperty make_protocol_ro_property(const char * name, const TProperty* property) { return ProtocolProperty(name, property); }; +// Non-const enum types +template::value>> +ProtocolProperty> make_protocol_property(const char * name, TProperty* property) { + return ProtocolProperty>(name, reinterpret_cast*>(property)); +}; + +// Const enum types +template::value>> +ProtocolProperty> make_protocol_ro_property(const char * name, const TProperty* property) { + return ProtocolProperty>(name, reinterpret_cast*>(property)); +}; + template @@ -636,7 +646,7 @@ struct PropertyListFactory { static MemberList, ProtocolProperty...> make_property_list(std::array names, std::tuple& values) { return MemberList, ProtocolProperty...>( - make_protocol_property(std::get(names), std::get(values)), + make_protocol_property(std::get(names), &std::get(values)), PropertyListFactory::template make_property_list(names, values) ); } @@ -715,7 +725,7 @@ class EndpointProvider_from_MemberList : public EndpointProvider { public: EndpointProvider_from_MemberList(T& member_list) : member_list_(member_list) {} size_t get_endpoint_count() final { - return member_list_.get_endpoint_count(); + return T::endpoint_count; } void write_json(size_t id, StreamSink* output) final { return member_list_.write_json(id, output); @@ -728,4 +738,10 @@ public: void set_application_endpoints(EndpointProvider* endpoints); + +// defined in communication.cpp +extern Endpoint* endpoints_[]; +extern size_t n_endpoints_; +extern const size_t max_endpoints_; + #endif diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 1ba30015..c1358150 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -7,10 +7,10 @@ SensorlessEstimator::SensorlessEstimator() // Calculate pll gains // This calculation is currently identical to the PLL in Encoder float pll_bandwidth = 1000.0f; // [rad/s] - pll_kp = 2.0f * pll_bandwidth; + pll_kp_ = 2.0f * pll_bandwidth; // Critically damped - pll_ki = 0.25f * (pll_kp * pll_kp); + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); } bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) { @@ -23,35 +23,35 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp < 1.0f)) { - error = ERROR_NUMERICAL; + if (!(current_meas_period * pll_kp_ < 1.0f)) { + error_ = ERROR_NUMERICAL; return false; } // Clarke transform float I_alpha_beta[2] = { - -axis->motor.current_meas.phB - axis->motor.current_meas.phC, - one_by_sqrt3 * (axis->motor.current_meas.phB - axis->motor.current_meas.phC)}; + -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC, + one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)}; // alpha-beta vector operations float eta[2]; for (int i = 0; i <= 1; ++i) { // y is the total flux-driving voltage (see paper eqn 4) - float y = -axis->motor.config.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory[i]; + float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i]; // flux dynamics (prediction) float x_dot = y; // integrate prediction to current timestep - flux_state[i] += x_dot * current_meas_period; + flux_state_[i] += x_dot * current_meas_period; // eta is the estimated permanent magnet flux (see paper eqn 6) - eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i]; } // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = pm_flux_linkage * pm_flux_linkage; + float pm_flux_sqr = pm_flux_linkage_ * pm_flux_linkage_; float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; - float bandwidth_factor = 1.0f / (pm_flux_linkage * pm_flux_linkage); - float eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); + float bandwidth_factor = 1.0f / pm_flux_sqr; + float eta_factor = 0.5f * (observer_gain_ * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); static float eta_factor_avg_test = 0.0f; eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); @@ -61,25 +61,25 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // add observer action to flux estimate dynamics float x_dot = eta_factor * eta[i]; // convert action to discrete-time - flux_state[i] += x_dot * current_meas_period; + flux_state_[i] += x_dot * current_meas_period; // update new eta - eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i]; } // Flux state estimation done, store V_alpha_beta for next timestep - V_alpha_beta_memory[0] = axis->motor.current_control.final_v_alpha; - V_alpha_beta_memory[1] = axis->motor.current_control.final_v_beta; + V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha; + V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta; // PLL // TODO: the PLL part has some code duplication with the encoder PLL // predict PLL phase with velocity - pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_vel); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_vel_); // update PLL phase with observer permanent magnet phase - phase = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(phase - pll_pos); - pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_kp * delta_phase); + phase_ = fast_atan2(eta[1], eta[0]); + float delta_phase = wrap_pm_pi(phase_ - pll_pos_); + pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp_ * delta_phase); // update PLL velocity - pll_vel += current_meas_period * pll_ki * delta_phase; + pll_vel_ += current_meas_period * pll_ki_ * delta_phase; //TODO TEMP TEST HACK // static int trigger_ctr = 0; @@ -94,8 +94,8 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // motor->rotor_mode = ROTOR_MODE_SENSORLESS; // } - if (pos_estimate) *pos_estimate = pll_pos; - if (vel_estimate) *vel_estimate = pll_vel; - if (phase_output) *phase_output = phase; + if (pos_estimate) *pos_estimate = pll_pos_; + if (vel_estimate) *vel_estimate = pll_vel_; + if (phase_output) *phase_output = phase_; return true; }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 18b291ca..569c9a09 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -12,20 +12,20 @@ public: bool update(float* pos_estimate, float* vel_estimate, float* phase); - Axis* axis = nullptr; // set by Axis constructor + Axis* axis_ = nullptr; // set by Axis constructor // TODO: expose on protocol - Error_t error = ERROR_NONE; - float phase = 0.0f; // [rad] - float pll_pos = 0.0f; // [rad] - float pll_vel = 0.0f; // [rad/s] - float pll_kp = 0.0f; // [rad/s / rad] - float pll_ki = 0.0f; // [(rad/s^2) / rad] - float observer_gain = 1000.0f; // [rad/s] - float flux_state[2] = {0.0f, 0.0f}; // [Vs] - float V_alpha_beta_memory[2] = {0.0f, 0.0f}; // [V] - float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } - bool estimator_good = false; + Error_t error_ = ERROR_NONE; + float phase_ = 0.0f; // [rad] + float pll_pos_ = 0.0f; // [rad] + float pll_vel_ = 0.0f; // [rad/s] + float pll_kp_ = 0.0f; // [rad/s / rad] + float pll_ki_ = 0.0f; // [(rad/s^2) / rad] + float observer_gain_ = 1000.0f; // [rad/s] + float flux_state_[2] = {0.0f, 0.0f}; // [Vs] + float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] + float pm_flux_linkage_ = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } + bool estimator_good_ = false; }; #endif /* __SENSORLESS_ESTIMATOR_HPP */ diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index e3908cde..b7cb2838 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -68,7 +68,7 @@ build{ 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', - 'MotorControl/commands.cpp', + 'MotorControl/communication.cpp', 'MotorControl/protocol.cpp', 'MotorControl/motor.cpp', 'MotorControl/encoder.cpp', From 9d85854de381b72d031f746c6beb1541a4abe007 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 15:03:55 -0800 Subject: [PATCH 006/112] add toplevel functions back to protocol --- Firmware/MotorControl/communication.cpp | 27 ++- Firmware/MotorControl/config.cpp | 247 ------------------------ Firmware/MotorControl/low_level.cpp | 6 +- Firmware/MotorControl/odrive_main.hpp | 8 +- 4 files changed, 30 insertions(+), 258 deletions(-) delete mode 100644 Firmware/MotorControl/config.cpp diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 2a082459..0e243911 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -142,8 +142,6 @@ public: } }; -float bla; - /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -159,13 +157,32 @@ void init_communication(void) { thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); } - +// Helper class because the protocol library doesn't yet +// support non-member functions +// TODO: make this go away +class StaticFunctions { +public: + void save_configuration_helper() { save_configuration(); } + void erase_configuration_helper() { erase_configuration(); } + void NVIC_SystemReset_helper() { NVIC_SystemReset(); } +} static_functions; static auto make_obj_tree() { return make_protocol_member_list( - make_protocol_property("bla2", &bla), + make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("UUID_0", (const uint32_t*)(ID_UNIQUE_ADDRESS + 0*4)), + make_protocol_ro_property("UUID_1", (const uint32_t*)(ID_UNIQUE_ADDRESS + 1*4)), + make_protocol_ro_property("UUID_2", (const uint32_t*)(ID_UNIQUE_ADDRESS + 2*4)), + make_protocol_object("config", + make_protocol_property("brake_resistance", &board_config.brake_resistance), + // TODO: changing this currently requires a reboot - fix this + make_protocol_property("enable_uart", &board_config.enable_uart) + ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), - make_protocol_object("axis1", axes[1]->make_protocol_definitions()) + make_protocol_object("axis1", axes[1]->make_protocol_definitions()), + make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), + make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), + make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper) ); } diff --git a/Firmware/MotorControl/config.cpp b/Firmware/MotorControl/config.cpp deleted file mode 100644 index 847a8938..00000000 --- a/Firmware/MotorControl/config.cpp +++ /dev/null @@ -1,247 +0,0 @@ - -/* Includes ------------------------------------------------------------------*/ - -#include "config.h" - -#include -#include -#include - -#include "nvm.h" -#include "crc.hpp" -#include "low_level.h" -#include "axis.h" - -// IMPORTANT: if you change, reorder or otherwise modify any of the fields in -// the config structs, make sure to increment this number: -uint16_t config_version = 0x0001; - -/* Private defines -----------------------------------------------------------*/ -#define CRC16_INIT 0xabcd - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ - -typedef struct { - Motor_control_mode_t control_mode; - float counts_per_step; - int32_t pole_pairs; - float pos_gain; - float vel_gain; - float vel_integrator_gain; - float vel_limit; - float calibration_current; - float resistance_calib_max_voltage; - float phase_inductance; - float phase_resistance; - Motor_type_t motor_type; - Rotor_mode_t rotor_mode; - float current_control_current_lim; - bool encoder_use_index; - bool encoder_calibrated; - float encoder_idx_search_speed; - int32_t encoder_cpr; - int32_t encoder_offset; - int32_t encoder_motor_dir; -} MotorConfig_t; - -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -/* Private constant data -----------------------------------------------------*/ -/* Private variables ---------------------------------------------------------*/ -/* Private function prototypes -----------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -// @brief Manages configuration load and store operations from and to NVM -// -// The NVM stores consecutive one-to-one copies of arbitrary objects. -// The types of these objects are passed as template arguments to Config. -// -// Config has two template specializations to implement template recursion: -// - Config handles loading/storing of the first object (type T) and leaves -// the rest of the objects to an "inner" class Config. -// - Config<> represents the leaf of the recursion. -template -struct Config; - -template<> -struct Config<> { - static size_t get_size() { - return 0; - } - static int load_config(size_t offset, uint16_t* crc16) { - return 0; - } - static int store_config(size_t offset, uint16_t* crc16) { - return 0; - } -}; - -template -struct Config { - static size_t get_size() { - return sizeof(T) + Config::get_size(); - } - - // @brief Loads one or more consecutive objects from the NVM. - // During loading this function also calculates the CRC over the loaded data. - // @param offset: 0 means that the function should start reading at the beginning - // of the last comitted NVM block - // @param crc16: the result of the CRC calculation is written to this address - // @param val0, vals: the values to be loaded - static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) { - size_t size = sizeof(T); - // save current CRC (in case val0 and crc16 point to the same address) - size_t previous_crc16 = *crc16; - if (NVM_read(offset, (uint8_t *)val0, size)) - return -1; - *crc16 = calc_crc16(previous_crc16, (uint8_t *)val0, size); - if (Config::load_config(offset + size, crc16, vals...)) - return -1; - return 0; - } - - // @brief Stores one or more consecutive objects to the NVM. - // During storing this function also calculates the CRC over the stored data. - // @param offset: 0 means that the function should start writing at the beginning - // of the currently active NVM write block - // @param crc16: the result of the CRC calculation is written to this address - // @param val0, vals: the values to be stored - static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) { - size_t size = sizeof(T); - if (NVM_write(offset, (uint8_t *)val0, size)) - return -1; - // update CRC _after_ writing (in case val0 and crc16 point to the same address) - if (crc16) - *crc16 = calc_crc16(*crc16, (uint8_t *)val0, size); - if (Config::store_config(offset + size, crc16, vals...)) - return -1; - return 0; - } - - // @brief Loads one or more consecutive objects from the NVM. The loaded data - // is validated using a CRC value that is stored at the beginning of the data. - static int load_config(T* val0, Ts* ... vals) { - //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5); - if (Config::get_size() > NVM_get_max_read_length()) - return -1; - uint16_t crc16 = CRC16_INIT ^ config_version; - if (Config::load_config(0, &crc16, val0, vals..., &crc16)) - return -1; - if (crc16) - return -1; - return 0; - } - - // @brief Stores one or more consecutive objects to the NVM. In addition to the - // provided objects, a CRC of the data is stored. - // - // The CRC includes a version number and thus adds some protection against - // changes of the config structs during firmware update. Note that if the total - // config data length changes, the CRC validation will fail even if the developer - // forgets to update the config version number. - static int store_config(const T* val0, const Ts* ... vals) { - size_t size = Config::get_size() + 2; - //printf("config is %d bytes\r\n", size); osDelay(5); - if (size > NVM_get_max_write_length()) - return -1; - if (NVM_start_write(size)) - return -1; - uint16_t crc16 = CRC16_INIT ^ config_version; - if (Config::store_config(0, &crc16, val0, vals...)) - return -1; - if (Config::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16)) - return -1; - if (NVM_commit()) - return -1; - return 0; - } -}; - -// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t) -// TODO: make this go away as part of the C++ refactoring -void set_motor_config(const MotorConfig_t* config, Motor_t* motor) { - motor->control_mode = config->control_mode; - motor->counts_per_step = config->counts_per_step; - motor->pole_pairs = config->pole_pairs; - motor->pos_gain = config->pos_gain; - motor->vel_gain = config->vel_gain; - motor->vel_integrator_gain = config->vel_integrator_gain; - motor->vel_limit = config->vel_limit; - motor->calibration_current = config->calibration_current; - motor->resistance_calib_max_voltage = config->resistance_calib_max_voltage; - motor->phase_inductance = config->phase_inductance; - motor->phase_resistance = config->phase_resistance; - motor->motor_type = config->motor_type; - motor->rotor_mode = config->rotor_mode; - - motor->current_control.current_lim = config->current_control_current_lim; - - motor->encoder.use_index = config->encoder_use_index; - motor->encoder.calibrated = config->encoder_calibrated; - motor->encoder.idx_search_speed = config->encoder_idx_search_speed; - motor->encoder.encoder_cpr = config->encoder_cpr; - motor->encoder.encoder_offset = config->encoder_offset; - motor->encoder.motor_dir = config->encoder_motor_dir; -} - -// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t) -// TODO: make this go away as part of the C++ refactoring -void get_motor_config(const Motor_t* motor, MotorConfig_t* config) { - config->control_mode = motor->control_mode; - config->counts_per_step = motor->counts_per_step; - config->pole_pairs = motor->pole_pairs; - config->pos_gain = motor->pos_gain; - config->vel_gain = motor->vel_gain; - config->vel_integrator_gain = motor->vel_integrator_gain; - config->vel_limit = motor->vel_limit; - config->calibration_current = motor->calibration_current; - config->resistance_calib_max_voltage = motor->resistance_calib_max_voltage; - config->phase_inductance = motor->phase_inductance; - config->phase_resistance = motor->phase_resistance; - config->motor_type = motor->motor_type; - config->rotor_mode = motor->rotor_mode; - - config->current_control_current_lim = motor->current_control.current_lim; - - config->encoder_use_index = motor->encoder.use_index; - config->encoder_calibrated = motor->encoder.calibrated; - config->encoder_idx_search_speed = motor->encoder.idx_search_speed; - config->encoder_cpr = motor->encoder.encoder_cpr; - config->encoder_offset = motor->encoder.encoder_offset; - config->encoder_motor_dir = motor->encoder.motor_dir; -} - - -void init_configuration(void) { - MotorConfig_t motor_config[2]; - //TODO: we really shouldn't be hardcoding like this - if (NVM_init() || Config::load_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) { - //printf("no config found\r\n"); osDelay(5); - // load default config - // motor_config[0] = MotorConfig_t(); - // motor_config[1] = MotorConfig_t(); - - // Default config coming from flashed Motor_t - return; - } else { - //printf("load config successful\r\n"); osDelay(5); - } - - set_motor_config(&motor_config[0], &motors[0]); - set_motor_config(&motor_config[1], &motors[1]); -} - -void save_configuration(void) { - MotorConfig_t motor_config[2]; - get_motor_config(&motors[0], &motor_config[0]); - get_motor_config(&motors[1], &motor_config[1]); - //TODO: we really shouldn't be hardcoding like this - if (Config::store_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) { - //printf("saving configuration failed\r\n"); osDelay(5); - } -} - -void erase_configuration(void) { - NVM_erase(); -} diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 5b1b44a5..6f9022bb 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -35,10 +35,6 @@ // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; -// TODO: Migrate to C++, clearly we are actually doing object oriented code here... - -float brake_resistance = 0.47f; // [ohm] - /* Private constant data -----------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -254,7 +250,7 @@ void update_brake_current() { float brake_current = -Ibus_sum; // Clip negative values to 0.0f if (brake_current < 0.0f) brake_current = 0.0f; - float brake_duty = brake_current * brake_resistance / vbus_voltage; + float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; // Duty limit at 90% to allow bootstrap caps to charge if (brake_duty > 0.9f) brake_duty = 0.9f; diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index ef9e3df8..730f040d 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -19,6 +19,12 @@ #error "unknown board version" #endif +// @brief general user configurable board configuration +struct BoardConfig_t { + bool enable_uart = true; + float brake_resistance = 0.47f; // [ohm] +}; + class Axis; //default timeout waiting for phase measurement signals @@ -27,8 +33,8 @@ class Axis; static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; extern float vbus_voltage; -extern float brake_resistance; // [ohm] extern const float elec_rad_per_enc; +extern BoardConfig_t board_config; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; From ef5a2bc38f70e2c7cb9243f71e5f7c7eb7101ac1 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 15:08:40 -0800 Subject: [PATCH 007/112] amend changelog --- Firmware/CHANGELOG.md | 3 +++ Firmware/README.md | 1 - 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 7c9738c1..b4a39e4b 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -11,6 +11,9 @@ Please add a note of your changes below this heading if you make a PR ### Changed * Build system is now tup instead of make +* Most code from `lowlevel.c` moved to `axis.cpp`, `encoder.cpp`, `controller.cpp`, `sensorless_estimator.cpp`, `motor.cpp` and the corresponding header files +* Refactoring of the developer-facing communication protocol interface. See e.g. `axis.hpp` or `controller.hpp` for examples on how to add your own fields and functions +* Change of the user-facing field paths. E.g. `my_odrive.motor0.pos_setpoint` is now at `my_odrive.axis0.controller.pos_setpoint`. Names are mostly unchanged. ## [0.3.4] - 2018-02-13 diff --git a/Firmware/README.md b/Firmware/README.md index be43a816..8c6d2952 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -61,7 +61,6 @@ Note that UART is only supported on ODrive v3.3 and higher. - `UART_PROTOCOL_LEGACY`: Use the human-readable legacy protocol Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol. - `UART_PROTOCOL_NONE`: Ignore UART communication - - `USE_GPIO_MODE_STEP_DIR`: Step/direction control mode (use in conjunction with `UART_PROTOCOL_NONE`)

## Compiling and downloading firmware From 74d0d7e14db80406a501825dc380f2e196069a21 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 16:45:21 -0800 Subject: [PATCH 008/112] minor doc updates --- Firmware/CHANGELOG.md | 2 +- Firmware/MotorControl/board_config_v3.h | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index f0a8c1df..0af2dc6b 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -5,7 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * **Storing of configuration parameters to Non Volatile Memory** ### Changed -* Most code from `lowlevel.c` moved to `axis.cpp`, `encoder.cpp`, `controller.cpp`, `sensorless_estimator.cpp`, `motor.cpp` and the corresponding header files +* Most of the code from `lowlevel.c` moved to `axis.cpp`, `encoder.cpp`, `controller.cpp`, `sensorless_estimator.cpp`, `motor.cpp` and the corresponding header files * Refactoring of the developer-facing communication protocol interface. See e.g. `axis.hpp` or `controller.hpp` for examples on how to add your own fields and functions * Change of the user-facing field paths. E.g. `my_odrive.motor0.pos_setpoint` is now at `my_odrive.axis0.controller.pos_setpoint`. Names are mostly unchanged. diff --git a/Firmware/MotorControl/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h index 9ef2d2d6..3e2d965d 100644 --- a/Firmware/MotorControl/board_config_v3.h +++ b/Firmware/MotorControl/board_config_v3.h @@ -1,3 +1,6 @@ +/* +* @brief Contains board specific configuration for ODrive v3.x +*/ #ifndef __BOARD_CONFIG_H #define __BOARD_CONFIG_H From 6131519b258d6d5630a1b1c82581b0d1e4a6fb2e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 16:45:38 -0800 Subject: [PATCH 009/112] increase axis thread stack size to 2k --- Firmware/MotorControl/axis.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index dd14249f..7fe7d1ad 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -38,7 +38,7 @@ static void run_state_machine_loop_wrapper(void* ctx) { // @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { - osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 512); + osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4*512); thread_id_ = osThreadCreate(osThread(thread_def), this); thread_id_valid_ = true; } From 1f732bb0fbb3753d30200247b4ede4876104e5eb Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 20:23:03 -0800 Subject: [PATCH 010/112] fix stack overflow --- Firmware/Board/v3.3/Inc/FreeRTOSConfig.h | 2 ++ Firmware/MotorControl/communication.cpp | 28 +++++++++++------------- Firmware/MotorControl/encoder.hpp | 19 ++++++++-------- Firmware/MotorControl/main.cpp | 1 + Firmware/MotorControl/motor.hpp | 3 ++- Firmware/MotorControl/protocol.hpp | 16 ++++++-------- 6 files changed, 35 insertions(+), 34 deletions(-) diff --git a/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h index 3a55d727..2459c3e9 100644 --- a/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h +++ b/Firmware/Board/v3.3/Inc/FreeRTOSConfig.h @@ -108,6 +108,7 @@ #define configUSE_MUTEXES 1 #define configQUEUE_REGISTRY_SIZE 8 #define configUSE_PORT_OPTIMISED_TASK_SELECTION 1 +#define configCHECK_FOR_STACK_OVERFLOW 1 /* Co-routine definitions. */ #define configUSE_CO_ROUTINES 0 @@ -123,6 +124,7 @@ to exclude the API function. */ #define INCLUDE_vTaskDelayUntil 1 #define INCLUDE_vTaskDelay 1 #define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 /* Cortex-M specific definitions. */ #ifdef __NVIC_PRIO_BITS diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 0e243911..4129e6bb 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -129,19 +129,6 @@ StreamToPacketConverter UART4_stream_sink(uart4_channel); #endif -class test_class { -public: - uint32_t property1; - float property2; - - float set_both(uint32_t arg1, float arg2) { - printf("set_both called with %u and %.3f\n", (unsigned int)arg1, arg2); - property1 = arg1; - property2 = arg2; - return arg1 + arg2; - } -}; - /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -149,7 +136,7 @@ void init_communication(void) { printf("hi!\r\n"); // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 4*512); + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); // Start USB interrupt handler thread @@ -157,6 +144,9 @@ void init_communication(void) { thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); } + +uint32_t comm_stack_info = 0; // for debugging only + // Helper class because the protocol library doesn't yet // support non-member functions // TODO: make this go away @@ -167,9 +157,13 @@ public: void NVIC_SystemReset_helper() { NVIC_SystemReset(); } } static_functions; -static auto make_obj_tree() { +// When adding new functions/variables to the protocol, be careful not to +// blow the communication stack. You can check comm_stack_info to see +// how much headroom you have. +static inline auto make_obj_tree() { return make_protocol_member_list( make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("comm_stack_info", &comm_stack_info), make_protocol_ro_property("UUID_0", (const uint32_t*)(ID_UNIQUE_ADDRESS + 0*4)), make_protocol_ro_property("UUID_1", (const uint32_t*)(ID_UNIQUE_ADDRESS + 1*4)), make_protocol_ro_property("UUID_2", (const uint32_t*)(ID_UNIQUE_ADDRESS + 2*4)), @@ -200,9 +194,13 @@ size_t n_endpoints_ = 0; void communication_task(void * ctx) { (void) ctx; // unused parameter + // TODO: this is supposed to use the move constructor, but currently + // the compiler uses the copy-constructor instead. Thus the make_obj_tree + // ends up with a stupid stack size of around 8000 bytes. Fix this. auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); set_application_endpoints(&endpoint_provider); + comm_stack_info = uxTaskGetStackHighWaterMark(nullptr); #if !defined(UART_PROTOCOL_NONE) //DMA open loop continous circular buffer diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 1082594e..cc1701a0 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -54,6 +54,15 @@ public: // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( + make_protocol_property("error", &error_), + make_protocol_ro_property("is_calibrated", &is_calibrated_), + make_protocol_ro_property("index_found", const_cast(&index_found_)), + make_protocol_property("state", &state_), + make_protocol_property("phase", &phase_), + make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("pll_kp", &pll_kp_), + make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", make_protocol_property("use_index", &config_.use_index), make_protocol_property("hand_calibrated", &config_.hand_calibrated), @@ -61,15 +70,7 @@ public: make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), make_protocol_property("calib_range", &config_.calib_range) - ), - make_protocol_property("error", &error_), - make_protocol_ro_property("index_found", const_cast(&index_found_)), - make_protocol_property("state", &state_), - make_protocol_property("phase", &phase_), - make_protocol_property("pll_pos", &pll_pos_), - make_protocol_property("pll_vel", &pll_vel_), - make_protocol_property("pll_kp", &pll_kp_), - make_protocol_property("pll_ki", &pll_ki_) + ) ); } }; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 1c6b63bb..076d8dac 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -42,6 +42,7 @@ void erase_configuration(void) { extern "C" { int odrive_main(void); +void vApplicationStackOverflowHook(void) { for(;;); } } int odrive_main(void) { diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index f3f71907..caa133e7 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -36,7 +36,7 @@ typedef struct { // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. typedef struct { - bool hand_calibrated = true; // can be set to true to indicate that all values here are valid + bool hand_calibrated = false; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. float calibration_current = 10.0f; // [A] float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. @@ -142,6 +142,7 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), + make_protocol_ro_property("is_calibrated", &is_calibrated_), make_protocol_ro_property("current_meas_phB", ¤t_meas_.phB), make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), make_protocol_property("DC_calib_phB", &DC_calib_.phB), diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index b3606211..bd7f362a 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -496,7 +496,7 @@ ProtocolObject make_protocol_object(const char * name, TMembers&&.. } template -class ProtocolProperty : Endpoint { +class ProtocolProperty : public Endpoint { public: static constexpr const char * json_modifier = get_default_json_modifier(); static constexpr size_t endpoint_count = 1; @@ -505,26 +505,24 @@ public: : name_(name), property_(property) {} -// ProtocolProperty(const ProtocolProperty&) = delete; - +/* TODO: find out why the move constructor is not used when it could be + ProtocolProperty(const ProtocolProperty&) = delete; // @brief Move constructor ProtocolProperty(ProtocolProperty&& other) : Endpoint(std::move(other)), name_(std::move(other.name_)), property_(other.property_) {} - - //constexpr ProtocolProperty& operator=(const ProtocolProperty& other) = delete; - /*constexpr ProtocolProperty& operator=(const ProtocolProperty& other) { + constexpr ProtocolProperty& operator=(const ProtocolProperty& other) = delete; + constexpr ProtocolProperty& operator=(const ProtocolProperty& other) { //Endpoint(std::move(other)), //name_(std::move(other.name_)), //property_(other.property_) name_ = other.name_; property_ = other.property_; return *this; - }*/ - - /*ProtocolProperty& operator=(ProtocolProperty&& other) + } + ProtocolProperty& operator=(ProtocolProperty&& other) : name_(other.name_), property_(other.property_) {} ProtocolProperty& operator=(const ProtocolProperty& other) From ed3832ca7772eefcd37cafa82a66614ab50f1f6e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 9 Mar 2018 20:33:35 -0800 Subject: [PATCH 011/112] make axis error clearable --- Firmware/MotorControl/axis.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 6f401154..94e2be45 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -157,7 +157,7 @@ public: // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( - make_protocol_ro_property("error", &error_), + make_protocol_property("error", &error_), make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_), make_protocol_property("enable_step_dir", &enable_step_dir_), make_protocol_ro_property("current_state", ¤t_state_), From e89569a6b42a005c880435e39d47b8d0d4680e43 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 13 Mar 2018 14:30:07 -0700 Subject: [PATCH 012/112] add back enter_dfu_mode --- Firmware/MotorControl/communication.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 6e0fc30d..8f951fbb 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -182,7 +182,7 @@ static inline auto make_obj_tree() { make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper), - make_protocol_function("enter_dfu_mode", static_functions, &StaticFunctions::enter_dfu_mode_helper), + make_protocol_function("enter_dfu_mode", static_functions, &StaticFunctions::enter_dfu_mode_helper) ); } From cea32ee1bee5b04dd80dfd2ca4d1ac40cecbbbdd Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 13 Mar 2018 14:15:29 -0700 Subject: [PATCH 013/112] fix board voltage version being ignored --- Firmware/Board/v3/Inc/main.h | 6 ++++-- Firmware/Tupfile.lua | 10 +++++----- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index 02aae9cb..50256101 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -167,10 +167,12 @@ #define CURRENT_MEAS_PERIOD ((float)(2*TIM_1_8_PERIOD_CLOCKS)/(float)TIM_1_8_CLOCK_HZ) #define CURRENT_MEAS_HZ (TIM_1_8_CLOCK_HZ/(2*TIM_1_8_PERIOD_CLOCKS)) -#ifdef HW_VERSION_HIGH_VOLTAGE +#if HW_VERSION_VOLTAGE == 48 #define VBUS_S_DIVIDER_RATIO 19.0f -#else +#elif HW_VERSION_VOLTAGE == 24 #define VBUS_S_DIVIDER_RATIO 11.0f +#else +#error "unknown board voltage" #endif /* USER CODE END Private defines */ diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 2301c773..114a3c8c 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -6,23 +6,23 @@ boardversion = tup.getconfig("BOARD_VERSION") if boardversion == "v3.1" then boarddir = 'Board/v3' -- currently all platform code is in the same v3.3 directory FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=1" - FLAGS += "-DHW_VERSION_HIGH_VOLTAGE=false" + FLAGS += "-DHW_VERSION_VOLTAGE=24" elseif boardversion == "v3.2" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=2" - FLAGS += "-DHW_VERSION_HIGH_VOLTAGE=false" + FLAGS += "-DHW_VERSION_VOLTAGE=24" elseif boardversion == "v3.3" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=3" - FLAGS += "-DHW_VERSION_HIGH_VOLTAGE=false" + FLAGS += "-DHW_VERSION_VOLTAGE=24" elseif boardversion == "v3.4-24V" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=4" - FLAGS += "-DHW_VERSION_HIGH_VOLTAGE=false" + FLAGS += "-DHW_VERSION_VOLTAGE=24" elseif boardversion == "v3.4-48V" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=4" - FLAGS += "-DHW_VERSION_HIGH_VOLTAGE=true" + FLAGS += "-DHW_VERSION_VOLTAGE=48" elseif boardversion == "" then error("board version not specified - take a look at tup.config.default") else From 2bf05b303a809a1d59c0c19080ba17fa4b9ed1bb Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 13 Mar 2018 14:30:46 -0700 Subject: [PATCH 014/112] rename Axis::ERROR_BAD_VOLTAGE --- Firmware/MotorControl/axis.cpp | 6 ++---- Firmware/MotorControl/axis.hpp | 3 ++- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7fe7d1ad..e186e864 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -96,9 +96,7 @@ void Axis::set_step_dir_enabled(bool enable) { // @brief Returns true if the power supply is within range bool Axis::check_PSU_brownout() { - if(vbus_voltage < config_.dc_bus_brownout_trip_level) - return error_ = ERROR_BAD_VOLTAGE, false; - return true; + return vbus_voltage >= config_.dc_bus_brownout_trip_level; } // @brief Returns true if everything is ok. @@ -107,7 +105,7 @@ bool Axis::do_checks() { if (!motor_.do_checks()) return error_ = ERROR_MOTOR_FAILED, false; if (!check_PSU_brownout()) - return error_ = ERROR_BAD_VOLTAGE, false; + return error_ = ERROR_DC_BUS_UNDER_VOLTAGE, false; return true; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 94e2be45..d4feadbd 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -42,7 +42,8 @@ public: enum Error_t { ERROR_NO_ERROR, ERROR_INVALID_STATE, // Date: Tue, 13 Mar 2018 14:31:43 -0700 Subject: [PATCH 015/112] improve user experience when trying DFU on old firmware --- tools/dfu.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/tools/dfu.py b/tools/dfu.py index 1c9aeda9..86c13bda 100755 --- a/tools/dfu.py +++ b/tools/dfu.py @@ -173,6 +173,7 @@ def put_odrive_into_dfu_mode_thread(cancellation_token): matching devices into DFU mode until cancellation_token is set. """ + global app_cancellation_token while not cancellation_token.is_set(): constraints = {} if serial_number == None else {'serial_number': serial_number} my_drive = odrive.core.find_any(consider_usb=True, consider_serial=False, @@ -180,6 +181,15 @@ def put_odrive_into_dfu_mode_thread(cancellation_token): **constraints) if cancellation_token.is_set(): return + if not hasattr(my_drive, "enter_dfu_mode"): + print("The firmware on device {} does not support DFU. You need to \n" + "flash the firmware once using STLink (`make flash`), after that \n" + "DFU with this script should work fine." + .format(my_drive.__channel__.usb_device.serial_number)) + # Terminate script, otherwise it would try to reconnect to the same + # incompatible device + app_cancellation_token.set() # TODO: implement a more sensible discorvery mechanism to fix this + return print("Putting device {} into DFU mode...".format(my_drive.__channel__.usb_device.serial_number)) try: my_drive.enter_dfu_mode() @@ -224,6 +234,7 @@ else: serial_number = None +app_cancellation_token = threading.Event() find_odrive_cancellation_token = threading.Event() try: print("Waiting for ODrive...") @@ -232,13 +243,15 @@ try: threading.Thread(target=put_odrive_into_dfu_mode_thread, args=(find_odrive_cancellation_token,)).start() # Poll libUSB until a device in DFU mode is found - while True: + while not app_cancellation_token.is_set(): params = {} if serial_number == None else {'serial_number': serial_number} stm_device = usb.core.find(idVendor=0x0483, idProduct=0xdf11, **params) if stm_device != None: break time.sleep(1) find_odrive_cancellation_token.set() # we don't need this thread anymore + if app_cancellation_token.is_set(): + sys.exit(1) print("Found device {} in DFU mode".format(stm_device.serial_number)) dfudev = dfuse.DfuDevice(stm_device) From a2378dbf79f9ef6ec6ab2760ccdefeb4f017d7da Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 16:53:18 -0700 Subject: [PATCH 016/112] make encoder index search and encoder offset calibration independent These two activities are now separate states of the axis state machine. Each of them can be invoked independently at any time (provided the motor is calibrated). --- Firmware/MotorControl/axis.cpp | 20 ++++-- Firmware/MotorControl/axis.hpp | 46 ++++++++------ Firmware/MotorControl/encoder.cpp | 102 +++++++++++++++++------------- Firmware/MotorControl/encoder.hpp | 20 ++++-- Firmware/MotorControl/motor.hpp | 5 +- 5 files changed, 114 insertions(+), 79 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index e186e864..d6c718e4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -214,8 +214,10 @@ void Axis::run_state_machine_loop() { if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) { if (config_.startup_motor_calibration) task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; - if (config_.startup_encoder_calibration) - task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (config_.startup_encoder_index_search && encoder_.config_.use_index) + task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; + if (config_.startup_encoder_offset_calibration) + task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; if (config_.startup_closed_loop_control) task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; else if (config_.startup_sensorless_control) @@ -223,7 +225,9 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) { task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; - task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (encoder_.config_.use_index) + task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; + task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ != AXIS_STATE_UNDEFINED) { task_chain_[pos++] = requested_state_; @@ -239,7 +243,7 @@ void Axis::run_state_machine_loop() { // Validate the state before running it if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_) current_state_ = AXIS_STATE_UNDEFINED; - if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_) + if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_) current_state_ = AXIS_STATE_UNDEFINED; // Run the specified state @@ -250,8 +254,12 @@ void Axis::run_state_machine_loop() { status = motor_.run_calibration(); break; - case AXIS_STATE_ENCODER_CALIBRATION: - status = encoder_.run_calibration(); + case AXIS_STATE_ENCODER_INDEX_SEARCH: + status = encoder_.run_index_search(); + break; + + case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: + status = encoder_.run_offset_calibration(); break; case AXIS_STATE_SENSORLESS_CONTROL: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index d4feadbd..65ef9d97 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -8,19 +8,22 @@ // Warning: Do not reorder these enum values. // The state machine uses ">" comparision on them. enum AxisState_t { - AXIS_STATE_UNDEFINED, //Instance->CNT = count; pll_pos_ = (float)count; @@ -53,14 +59,59 @@ void Encoder::set_count(int32_t count) { } +// @brief Slowly turns the motor in one direction until the +// encoder index is found. // TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool Encoder::calib_enc_offset(float voltage_magnitude) { +bool Encoder::run_index_search() { + float voltage_magnitude; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + voltage_magnitude = axis_->motor_.config_.calibration_current; + else + return false; + + float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed; + + index_found_ = false; + float phase = 0.0f; + axis_->run_control_loop([&](){ + phase = wrap_pm_pi(phase + omega * current_meas_period); + + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); + + // continue until the index is found + return !index_found_; + }); + return axis_->error_ != Axis::ERROR_NO_ERROR; +} + +// @brief Turns the motor in one direction for a bit and then in the other +// direction in order to find the offset between the electrical phase 0 +// and the encoder state 0. +// TODO: Do the scan with current, not voltage! +bool Encoder::run_offset_calibration() { 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 = scan_distance / scan_omega * current_meas_hz; + // Temporarily disable index search so it doesn't mess + // with the offset calibration + bool old_use_index = config_.use_index; + config_.use_index = true; + + float voltage_magnitude; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + voltage_magnitude = axis_->motor_.config_.calibration_current; + else + return false; + // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ @@ -128,46 +179,9 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; - int offset = encvaluesum / (num_steps * 2); - config_.offset = offset; - is_calibrated_ = true; - return true; -} - -bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { - index_found_ = false; - float phase = 0.0f; - axis_->run_control_loop([&](){ - phase = wrap_pm_pi(phase + omega * current_meas_period); - - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); - - // continue until the index is found - return !index_found_; - }); - return axis_->error_ == Axis::ERROR_NO_ERROR; -} - -bool Encoder::run_calibration() { - float enc_calibration_voltage; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) - enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) - enc_calibration_voltage = axis_->motor_.config_.calibration_current; - else - return false; - - if (config_.use_index && !index_found_) - if (!scan_for_enc_idx( - (float)(axis_->motor_.config_.direction) * config_.idx_search_speed, - enc_calibration_voltage)) - return false; - if (!config_.hand_calibrated) // TODO: discuss what logic we want here - if (!calib_enc_offset(enc_calibration_voltage)) - return false; + offset_ = encvaluesum / (num_steps * 2); + is_ready_ = true; + config_.use_index = old_use_index; return true; } @@ -184,7 +198,7 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // compute electrical phase int corrected_enc = state_ % config_.cpr; - corrected_enc -= config_.offset; + corrected_enc -= offset_; //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index cc1701a0..f3dbc78c 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -7,10 +7,15 @@ struct EncoderConfig_t { bool use_index = false; - bool hand_calibrated = false; + bool pre_calibrated = false; // If true, this means the offset stored in + // configuration is valid and does not need + // be determined by run_offset_calibration. + // In this case the encoder will enter ready + // state as soon as the index is found. float idx_search_speed = 10.0f; // [rad/s electrical] int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, - int32_t offset = 0; + int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once + // index search succeeds float calib_range = 0.02; }; @@ -34,8 +39,9 @@ public: bool calib_enc_offset(float voltage_magnitude); bool scan_for_enc_idx(float omega, float voltage_magnitude); + bool run_index_search(); + bool run_offset_calibration(); bool update(float* pos_estimate, float* vel_estimate, float* phase); - bool run_calibration(); const EncoderHardwareConfig_t& hw_config_; EncoderConfig_t& config_; @@ -43,8 +49,9 @@ public: Error_t error_ = ERROR_NONE; bool index_found_ = false; - bool is_calibrated_ = config_.hand_calibrated; + bool is_ready_ = false; int32_t state_ = 0; + int32_t offset_ = 0; float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] @@ -55,9 +62,10 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("is_calibrated", &is_calibrated_), + make_protocol_ro_property("is_ready", &is_ready_), make_protocol_ro_property("index_found", const_cast(&index_found_)), make_protocol_property("state", &state_), + make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), make_protocol_property("pll_pos", &pll_pos_), make_protocol_property("pll_vel", &pll_vel_), @@ -65,7 +73,7 @@ public: make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", make_protocol_property("use_index", &config_.use_index), - make_protocol_property("hand_calibrated", &config_.hand_calibrated), + make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("idx_search_speed", &config_.idx_search_speed), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index caa133e7..2e771b27 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -36,7 +36,7 @@ typedef struct { // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. typedef struct { - bool hand_calibrated = false; // can be set to true to indicate that all values here are valid + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. float calibration_current = 10.0f; // [A] float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. @@ -118,7 +118,7 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; - bool is_calibrated_ = config_.hand_calibrated; + bool is_calibrated_ = config_.pre_calibrated; Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S] @@ -180,6 +180,7 @@ public: make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]) ), make_protocol_object("config", + make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("pole_pairs", &config_.pole_pairs), make_protocol_property("calibration_current", &config_.calibration_current), make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage), From 3e1d0aaad0e6e4a04154c1a885b372732187a767 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 16:53:44 -0700 Subject: [PATCH 017/112] expose sensorless estimator on protocol --- Firmware/MotorControl/axis.hpp | 3 ++- Firmware/MotorControl/sensorless_estimator.hpp | 12 ++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 65ef9d97..300c7e94 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -184,7 +184,8 @@ public: ), 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("encoder", encoder_.make_protocol_definitions()), + make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()) ); } }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 569c9a09..740ae8c1 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -26,6 +26,18 @@ public: float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] float pm_flux_linkage_ = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } bool estimator_good_ = false; + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_property("error", &error_), + make_protocol_property("phase", &phase_), + make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("pll_kp", &pll_kp_), + make_protocol_property("pll_ki", &pll_ki_) + ); + } }; #endif /* __SENSORLESS_ESTIMATOR_HPP */ From d71aa4ca8e2ff6953062ced1d728e7437afbf22c Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 17:09:04 -0700 Subject: [PATCH 018/112] robustify python USB discovery/communication --- tools/odrive/usbbulk_transport.py | 8 +++++++- tools/odrive/utils.py | 19 +++++++++++++++++-- 2 files changed, 24 insertions(+), 3 deletions(-) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index f513789f..5eecbd24 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -35,6 +35,12 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) return string def init(self): + # Under some conditions, the Linux USB/libusb stack ends up in a corrupt + # state where there are a few packets in a receive queue but a call + # to epr.read() does not return these packet until a new packet arrives. + # This undesirable queue can be cleared by resetting the device. + self.dev.reset() + try: if self.dev.is_kernel_driver_active(1): self.dev.detach_kernel_driver(1) @@ -106,7 +112,7 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) else: # Try resetting halt/stall condition try: - self.epw.clear_halt() + self.epr.clear_halt() except usb.core.USBError: raise odrive.protocol.ChannelBrokenException() # Retry transfer diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index be19adcd..a8c0e680 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -73,11 +73,15 @@ class Event(): """ Alternative to threading.Event(), enhanced by the subscribe() function that the original fails to provide. + @param Trigger: if supplied, the newly created event will be triggered + as soon as the trigger event becomes set """ - def __init__(self): + def __init__(self, trigger=None): self._evt = threading.Event() self._subscribers = [] self._mutex = threading.Lock() + if not trigger is None: + trigger.subscribe(self.set()) def is_set(self): return self._evt.is_set() @@ -120,7 +124,18 @@ class Event(): self._mutex.release() def wait(self, timeout=None): - return self._evt.wait(timeout=timeout) + if not self._evt.wait(timeout=timeout): + raise TimeoutError() + + def trigger_after(self, timeout): + """ + Triggers the event after the specified timeout. + This function returns immediately. + """ + def delayed_trigger(): + if not self.wait(timeout=timeout): + self.set() + threading.Thread(target=delayed_trigger, daemon=True).start() def wait_any(*events, timeout=None): """ From 69e1b5331a059ebdf8a5375c3843ddc456d1728e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 16:53:18 -0700 Subject: [PATCH 019/112] make encoder index search and encoder offset calibration independent These two activities are now separate states of the axis state machine. Each of them can be invoked independently at any time (provided the motor is calibrated). --- Firmware/MotorControl/axis.cpp | 20 ++++-- Firmware/MotorControl/axis.hpp | 46 ++++++++------ Firmware/MotorControl/encoder.cpp | 102 +++++++++++++++++------------- Firmware/MotorControl/encoder.hpp | 20 ++++-- Firmware/MotorControl/motor.hpp | 5 +- 5 files changed, 114 insertions(+), 79 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index e186e864..d6c718e4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -214,8 +214,10 @@ void Axis::run_state_machine_loop() { if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) { if (config_.startup_motor_calibration) task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; - if (config_.startup_encoder_calibration) - task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (config_.startup_encoder_index_search && encoder_.config_.use_index) + task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; + if (config_.startup_encoder_offset_calibration) + task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; if (config_.startup_closed_loop_control) task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL; else if (config_.startup_sensorless_control) @@ -223,7 +225,9 @@ void Axis::run_state_machine_loop() { task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) { task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION; - task_chain_[pos++] = AXIS_STATE_ENCODER_CALIBRATION; + if (encoder_.config_.use_index) + task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH; + task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION; task_chain_[pos++] = AXIS_STATE_IDLE; } else if (requested_state_ != AXIS_STATE_UNDEFINED) { task_chain_[pos++] = requested_state_; @@ -239,7 +243,7 @@ void Axis::run_state_machine_loop() { // Validate the state before running it if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_) current_state_ = AXIS_STATE_UNDEFINED; - if (current_state_ > AXIS_STATE_ENCODER_CALIBRATION && !encoder_.is_calibrated_) + if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_) current_state_ = AXIS_STATE_UNDEFINED; // Run the specified state @@ -250,8 +254,12 @@ void Axis::run_state_machine_loop() { status = motor_.run_calibration(); break; - case AXIS_STATE_ENCODER_CALIBRATION: - status = encoder_.run_calibration(); + case AXIS_STATE_ENCODER_INDEX_SEARCH: + status = encoder_.run_index_search(); + break; + + case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: + status = encoder_.run_offset_calibration(); break; case AXIS_STATE_SENSORLESS_CONTROL: diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index d4feadbd..65ef9d97 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -8,19 +8,22 @@ // Warning: Do not reorder these enum values. // The state machine uses ">" comparision on them. enum AxisState_t { - AXIS_STATE_UNDEFINED, //Instance->CNT = count; pll_pos_ = (float)count; @@ -53,14 +59,59 @@ void Encoder::set_count(int32_t count) { } +// @brief Slowly turns the motor in one direction until the +// encoder index is found. // TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool Encoder::calib_enc_offset(float voltage_magnitude) { +bool Encoder::run_index_search() { + float voltage_magnitude; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + voltage_magnitude = axis_->motor_.config_.calibration_current; + else + return false; + + float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed; + + index_found_ = false; + float phase = 0.0f; + axis_->run_control_loop([&](){ + phase = wrap_pm_pi(phase + omega * current_meas_period); + + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); + + // continue until the index is found + return !index_found_; + }); + return axis_->error_ != Axis::ERROR_NO_ERROR; +} + +// @brief Turns the motor in one direction for a bit and then in the other +// direction in order to find the offset between the electrical phase 0 +// and the encoder state 0. +// TODO: Do the scan with current, not voltage! +bool Encoder::run_offset_calibration() { 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 = scan_distance / scan_omega * current_meas_hz; + // Temporarily disable index search so it doesn't mess + // with the offset calibration + bool old_use_index = config_.use_index; + config_.use_index = true; + + float voltage_magnitude; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + voltage_magnitude = axis_->motor_.config_.calibration_current; + else + return false; + // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ @@ -128,46 +179,9 @@ bool Encoder::calib_enc_offset(float voltage_magnitude) { if (axis_->error_ != Axis::ERROR_NO_ERROR) return false; - int offset = encvaluesum / (num_steps * 2); - config_.offset = offset; - is_calibrated_ = true; - return true; -} - -bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { - index_found_ = false; - float phase = 0.0f; - axis_->run_control_loop([&](){ - phase = wrap_pm_pi(phase + omega * current_meas_period); - - float v_alpha = voltage_magnitude * arm_cos_f32(phase); - float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); - axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); - - // continue until the index is found - return !index_found_; - }); - return axis_->error_ == Axis::ERROR_NO_ERROR; -} - -bool Encoder::run_calibration() { - float enc_calibration_voltage; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) - enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; - else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) - enc_calibration_voltage = axis_->motor_.config_.calibration_current; - else - return false; - - if (config_.use_index && !index_found_) - if (!scan_for_enc_idx( - (float)(axis_->motor_.config_.direction) * config_.idx_search_speed, - enc_calibration_voltage)) - return false; - if (!config_.hand_calibrated) // TODO: discuss what logic we want here - if (!calib_enc_offset(enc_calibration_voltage)) - return false; + offset_ = encvaluesum / (num_steps * 2); + is_ready_ = true; + config_.use_index = old_use_index; return true; } @@ -184,7 +198,7 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // compute electrical phase int corrected_enc = state_ % config_.cpr; - corrected_enc -= config_.offset; + corrected_enc -= offset_; //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index cc1701a0..f3dbc78c 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -7,10 +7,15 @@ struct EncoderConfig_t { bool use_index = false; - bool hand_calibrated = false; + bool pre_calibrated = false; // If true, this means the offset stored in + // configuration is valid and does not need + // be determined by run_offset_calibration. + // In this case the encoder will enter ready + // state as soon as the index is found. float idx_search_speed = 10.0f; // [rad/s electrical] int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, - int32_t offset = 0; + int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once + // index search succeeds float calib_range = 0.02; }; @@ -34,8 +39,9 @@ public: bool calib_enc_offset(float voltage_magnitude); bool scan_for_enc_idx(float omega, float voltage_magnitude); + bool run_index_search(); + bool run_offset_calibration(); bool update(float* pos_estimate, float* vel_estimate, float* phase); - bool run_calibration(); const EncoderHardwareConfig_t& hw_config_; EncoderConfig_t& config_; @@ -43,8 +49,9 @@ public: Error_t error_ = ERROR_NONE; bool index_found_ = false; - bool is_calibrated_ = config_.hand_calibrated; + bool is_ready_ = false; int32_t state_ = 0; + int32_t offset_ = 0; float phase_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] @@ -55,9 +62,10 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("is_calibrated", &is_calibrated_), + make_protocol_ro_property("is_ready", &is_ready_), make_protocol_ro_property("index_found", const_cast(&index_found_)), make_protocol_property("state", &state_), + make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), make_protocol_property("pll_pos", &pll_pos_), make_protocol_property("pll_vel", &pll_vel_), @@ -65,7 +73,7 @@ public: make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", make_protocol_property("use_index", &config_.use_index), - make_protocol_property("hand_calibrated", &config_.hand_calibrated), + make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("idx_search_speed", &config_.idx_search_speed), make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index caa133e7..2e771b27 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -36,7 +36,7 @@ typedef struct { // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. typedef struct { - bool hand_calibrated = false; // can be set to true to indicate that all values here are valid + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. float calibration_current = 10.0f; // [A] float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. @@ -118,7 +118,7 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; - bool is_calibrated_ = config_.hand_calibrated; + bool is_calibrated_ = config_.pre_calibrated; Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S] @@ -180,6 +180,7 @@ public: make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]) ), make_protocol_object("config", + make_protocol_property("pre_calibrated", &config_.pre_calibrated), make_protocol_property("pole_pairs", &config_.pole_pairs), make_protocol_property("calibration_current", &config_.calibration_current), make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage), From 09b8201868858dc0822aa08b5be9e7bb3e062364 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 16:53:44 -0700 Subject: [PATCH 020/112] expose sensorless estimator on protocol --- Firmware/MotorControl/axis.hpp | 3 ++- Firmware/MotorControl/sensorless_estimator.hpp | 12 ++++++++++++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 65ef9d97..300c7e94 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -184,7 +184,8 @@ public: ), 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("encoder", encoder_.make_protocol_definitions()), + make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions()) ); } }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 569c9a09..740ae8c1 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -26,6 +26,18 @@ public: float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] float pm_flux_linkage_ = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } bool estimator_good_ = false; + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_property("error", &error_), + make_protocol_property("phase", &phase_), + make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pll_vel", &pll_vel_), + make_protocol_property("pll_kp", &pll_kp_), + make_protocol_property("pll_ki", &pll_ki_) + ); + } }; #endif /* __SENSORLESS_ESTIMATOR_HPP */ From b5f47f045e926f4d3e828aa5dd4b4ed9b64c4b96 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 19:48:58 -0700 Subject: [PATCH 021/112] add automated test script --- Firmware/Makefile | 6 +- Firmware/test-rig.yaml | 25 +++++ tools/odrive/enums.py | 24 +++++ tools/odrive/tests.py | 235 +++++++++++++++++++++++++++++++++++++++++ tools/odrive/utils.py | 68 ++++++++++++ tools/run_tests.py | 120 +++++++++++++++++++++ 6 files changed, 477 insertions(+), 1 deletion(-) create mode 100644 Firmware/test-rig.yaml create mode 100644 tools/odrive/enums.py create mode 100644 tools/odrive/tests.py create mode 100755 tools/run_tests.py diff --git a/Firmware/Makefile b/Firmware/Makefile index 84a28e30..1fdd4897 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -24,9 +24,13 @@ bmp: all --ex 'attach 1' \ --ex 'load' $(FIRMWARE) +# Erase entire STM32 +erase: + openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ erase_address\ 0x8000000\ 0x100000 -c reset\ run -c exit + # Erase all configuration from the ODrive erase_config: - openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ run -c exit + openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ init -c reset\ run -c exit clean: -rm -fR .dep $(BUILD_DIR) diff --git a/Firmware/test-rig.yaml b/Firmware/test-rig.yaml new file mode 100644 index 00000000..875898b3 --- /dev/null +++ b/Firmware/test-rig.yaml @@ -0,0 +1,25 @@ + +# ODrives +odrives: + - board-version: v3.4-24V + serial-number: "385F324D3037" + brake-resistance: 0.47 + uart: /dev/serial/by-id/... + usb: auto + programmer: /dev... + axes: + - motor-phase-resistance: 0.033 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: 1 + encoder-cpr: 8192 + - motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: -1 + encoder-cpr: 8192 + +# Mechanical couplings +couplings: + #- [ odrive0.axis0, odrive1.axis0 ] + #- [ odrive0.axis1, odrive1.axis1 ] diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py new file mode 100644 index 00000000..14870c2d --- /dev/null +++ b/tools/odrive/enums.py @@ -0,0 +1,24 @@ + +# TODO: transmit enums over protocol + +AXIS_STATE_UNDEFINED = 0 +AXIS_STATE_IDLE = 1 +AXIS_STATE_STARTUP_SEQUENCE = 2 +AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3 +AXIS_STATE_MOTOR_CALIBRATION = 4 +AXIS_STATE_SENSORLESS_CONTROL = 5 +AXIS_STATE_ENCODER_INDEX_SEARCH = 6 +AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 +AXIS_STATE_CLOSED_LOOP_CONTROL = 8 + +AXIS_ERROR_NO_ERROR = 0 +AXIS_ERROR_INVALID_STATE = 1 +AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2 +AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3 +AXIS_ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4 +AXIS_ERROR_CONTROL_LOOP_TIMEOUT = 5 +AXIS_ERROR_MOTOR_FAILED = 6 +AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED = 7 +AXIS_ERROR_ENCODER_FAILED = 8 +AXIS_ERROR_CONTROLLER_FAILED = 9 +AXIS_ERROR_POS_CTRL_DURING_SENSORLESS = 10 diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py new file mode 100644 index 00000000..924dbee8 --- /dev/null +++ b/tools/odrive/tests.py @@ -0,0 +1,235 @@ + +import subprocess +import shlex +import math +import time +import sys +import odrive.discovery +from odrive.enums import * + +import abc +ABC = abc.ABC + +class TestFailed(Exception): + def __init__(self, message): + Exception.__init__(self, message) + +def test_assert_eq(observed, expected, range=None, accuracy=None): + if range is None and accuracy is None and observed != expected: + raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed)) + if not range is None and ((observed < expected - range) or (observed > expected + range)): + raise TestFailed("value out of range: expected {}+-{} but observed {}".format(expected, range, observed)) + elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))): + raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed)) + +def run(command_line, logger, timeout=None): + """ + Runs a shell command in the Firmware directory + """ + logger.debug("invoke: " + command_line) + cmd = shlex.split(command_line) + result = subprocess.run(cmd, cwd='../Firmware', timeout=timeout, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + if result.returncode != 0: + logger.error(result.stdout.decode(sys.stdout.encoding)) + raise TestFailed("command {} failed".format(command_line)) + +def rediscover(odrv_yaml): + """ + Connects to the ODrive indicated by odrv_yaml + """ + odrv = odrive.discovery.find_any(path="usb", serial_number=odrv_yaml['serial-number'], timeout=10) + odrv_yaml['odrv'] = odrv + for axis_idx, axis_yaml in enumerate(odrv_yaml['axes']): + axis_yaml['axis'] = odrv.__dict__['axis{}'.format(axis_idx)] + return odrv + +class ODriveTest(ABC): + """ + Tests inheriting from this class get full ownership of the ODrive + being tested. However no guarantees are made for the mechanical + state of the axes. + """ + @abc.abstractmethod + def run_test(self, odrv, odrv_config, logger): + pass + +class AxisTest(ABC): + """ + Tests inheriting from this class get ownership of one axis of + an ODrive. If the axis is mechanically coupled to another + axis, the other axis is guaranteed to be disabled (high impedance) + during this test. + """ + @abc.abstractmethod + def run_test(self, axis, axis_config, logger): + pass + +class DualAxisTest(ABC): + """ + Tests using this scope get ownership of two axes that are mechanically + coupled. + """ + @abc.abstractmethod + def run_test(self, axis0, axis0_config, axis1, axis1_config, logger): + pass + +class TestFlashAndErase(ODriveTest): + def run_test(self, odrv, odrv_config, logger): + run("make flash PROGRAMMER='" + odrv_config['programmer'] + "'", logger, timeout=20) + # FIXME: device does not reboot correctly after erasing config this way + #run("make erase_config PROGRAMMER='" + test_rig.programmer + "'", timeout=10) + + logger.debug("waiting for ODrive...") + odrv = rediscover(odrv_config) + # ensure the correct odrive is returned + test_assert_eq(format(odrv.serial_number, 'x').upper(), odrv_config['serial-number']) + + # erase configuration and reboot + logger.debug("erasing old configuration...") + odrv.erase_configuration() + #time.sleep(0.1) + try: + # FIXME: sometimes the device does not reappear after this ("no response - probably incompatible") + # this is a firmware issue since it persists when unplugging/replugging + # but goes away when power cycling the device + odrv.reboot() + except odrive.protocol.ChannelBrokenException: + pass # this is expected + time.sleep(0.5) + +class TestSetup(ODriveTest): + """ + Preconditions: ODrive is unconfigured and just rebooted + """ + def run_test(self, odrv, odrv_config, logger): + odrv = rediscover(odrv_config) + + # initial protocol tests and setup + logger.debug("setting up ODrive...") + odrv.config.enable_uart = True + test_assert_eq(odrv.config.enable_uart, True) + odrv.config.enable_uart = False + test_assert_eq(odrv.config.enable_uart, False) + odrv.config.brake_resistance = 1.0 + test_assert_eq(odrv.config.brake_resistance, 1.0) + odrv.config.brake_resistance = odrv_config['brake-resistance'] + test_assert_eq(odrv.config.brake_resistance, odrv_config['brake-resistance'], accuracy=0.01) + + # firmware has 1500ms startup delay + time.sleep(2) + + logger.debug("ensure we're in idle state") + test_assert_eq(odrv.axis0.current_state, AXIS_STATE_IDLE) + test_assert_eq(odrv.axis1.current_state, AXIS_STATE_IDLE) + +def request_state(axis, state, expect_success=True): + axis.requested_state = state + time.sleep(0.001) + if expect_success: + test_assert_eq(axis.current_state, state) + else: + test_assert_eq(axis.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis.error, AXIS_ERROR_INVALID_STATE) + axis.error = AXIS_ERROR_NO_ERROR # reset error + +class TestMotorCalibration(AxisTest): + """ + Tests motor calibration. + The calibration results are compared against well known test rig values. + Preconditions: The motor must be uncalibrated. + Postconditions: The motor will be calibrated after this test. + """ + def run_test(self, axis, axis_config, logger): + logger.debug("try to enter closed loop control (should be rejected)") + request_state(axis, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) + + logger.debug("try to start encoder index search (should be rejected)") + request_state(axis, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False) + + logger.debug("try to start encoder offset calibration (should be rejected)") + request_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False) + + logger.debug("motor calibration (takes about 4.5 seconds)") + axis.motor.config.pole_pairs = axis_config['motor-pole-pairs'] + request_state(axis, AXIS_STATE_MOTOR_CALIBRATION) + time.sleep(6) + test_assert_eq(axis.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis.error, AXIS_ERROR_NO_ERROR) + test_assert_eq(axis.motor.config.phase_resistance, axis_config['motor-phase-resistance'], accuracy=0.1) + test_assert_eq(axis.motor.config.phase_inductance, axis_config['motor-phase-inductance'], accuracy=0.5) + axis.motor.config.pre_calibrated = True + +class TestEncoderOffsetCalibration(AxisTest): + """ + Tests encoder offset calibration. + Preconditions: The encoder must be non-ready. + Postconditions: The encoder will be ready after this test. + """ + def run_test(self, axis, axis_config, logger): + logger.debug("try to enter closed loop control (should be rejected)") + request_state(axis, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) + + logger.debug("encoder offset calibration (takes about 9.5 seconds)") + axis.encoder.config.cpr = axis_config['encoder-cpr'] # TODO: test setting a wrong CPR + request_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) + # TODO: ensure the encoder calibration doesn't do crap + time.sleep(11) + test_assert_eq(axis.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis.error, AXIS_ERROR_NO_ERROR) + test_assert_eq(axis.motor.config.direction, axis_config['motor-direction']) + axis.encoder.config.pre_calibrated = True + +class TestClosedLoopControl(AxisTest): + """ + Tests closed loop position control and velocity control + and verifies that the sensorless estimator works + Precondition: The axis is calibrated and ready for closed loop control + """ + def run_test(self, axis, axis_config, logger): + logger.debug("closed loop control: test tiny position changes") + axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + time.sleep(0.001) + test_assert_eq(axis.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + time.sleep(0.1) # give the PLL some time to settle + test_assert_eq(axis.encoder.pll_pos, 0, range=300) + axis.controller.set_pos_setpoint(1000, 0, 0) + time.sleep(0.5) + test_assert_eq(axis.encoder.pll_pos, 1000, range=200) + axis.controller.set_pos_setpoint(-1000, 0, 0) + time.sleep(0.5) + test_assert_eq(axis.encoder.pll_pos, -1000, range=200) + + logger.debug("closed loop control: test vel_limit") + axis.controller.set_pos_setpoint(50000, 0, 0) + axis.controller.config.vel_limit = 40000 + time.sleep(0.3) + test_assert_eq(axis.encoder.pll_vel, 40000, range=4000) + expected_sensorless_estimation = 40000 * 2 * math.pi / axis_config['encoder-cpr'] * axis_config['motor-pole-pairs'] + test_assert_eq(axis.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50) + time.sleep(3) + test_assert_eq(axis.encoder.pll_vel, 0, range=1000) + +class TestStoreAndReboot(ODriveTest): + """ + Stores the current configuration to NVM and reboots. + """ + def run_test(self, odrv, odrv_config, logger): + logger.debug("storing configuration and rebooting...") + odrv.save_configuration() + try: + odrv.reboot() + except odrive.protocol.ChannelBrokenException: + pass # this is expected + time.sleep(2) + + odrv = rediscover(odrv_config) + + logger.debug("verifying configuration after reboot...") + test_assert_eq(odrv.config.brake_resistance, odrv_config['brake-resistance'], accuracy=0.01) + for axis_config in odrv_config['axes']: + axis = axis_config['axis'] + test_assert_eq(axis.encoder.config.cpr, axis_config['encoder-cpr']) + test_assert_eq(axis.motor.config.phase_resistance, axis_config['motor-phase-resistance'], accuracy=0.1) + test_assert_eq(axis.motor.config.phase_inductance, axis_config['motor-phase-inductance'], accuracy=0.5) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index a8c0e680..4f070b1e 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -154,3 +154,71 @@ def wait_any(*events, timeout=None): if events[i].is_set(): return i raise TimeoutException() + + +def for_all_parallel(objects, get_name, callback): + """ + Executes the specified callback for every object in the objects + list concurrently. This function waits for all callbacks to + finish and throws an exception if any of the callbacks throw + an exception. + """ + tracebacks = [] + + def run_callback(element): + try: + callback(element) + except Exception as ex: + tracebacks.append((get_name(element), ex)) + + # Start a thread for each element in the list + all_threads = [] + for element in objects: + thread = threading.Thread(target=run_callback, args=(element,)) + thread.start() + all_threads.append(thread) + + # Wait for all threads to complete + for thread in all_threads: + thread.join() + + if len(tracebacks) == 1: + msg = "task {} failed.".format(tracebacks[0][0]) + raise Exception(msg) from tracebacks[0][1] + elif len(tracebacks) > 1: + msg = "task {} and {} failed.".format( + tracebacks[0][0], + "one other" if len(tracebacks) == 2 else str(len(tracebacks)-1) + " others" + ) + raise Exception(msg) from tracebacks[0][1] + + +class Logger(): + """ + Logs messages to stdout + """ + + COLOR_GREEN = '\x1b[92;1m' + COLOR_CYAN = '\x1b[96;1m' + COLOR_YELLOW = '\x1b[93;1m' + COLOR_RED = '\x1b[91;1m' + COLOR_RESET = '\x1b[0m' + + def __init__(self): + self._prefix = '' + + def indent(self, prefix=' '): + indented_logger = Logger() + indented_logger._prefix = self._prefix + prefix + return indented_logger + + def debug(self, text): + print(self._prefix + text) + def success(self, text): + print(self._prefix + Logger.COLOR_GREEN + text + Logger.COLOR_RESET) + def info(self, text): + print(self._prefix + Logger.COLOR_CYAN + text + Logger.COLOR_RESET) + def warn(self, text): + print(self._prefix + Logger.COLOR_YELLOW + text + Logger.COLOR_RESET) + def error(self, text): + print(self._prefix + Logger.COLOR_RED + text + Logger.COLOR_RESET) diff --git a/tools/run_tests.py b/tools/run_tests.py new file mode 100755 index 00000000..c070b919 --- /dev/null +++ b/tools/run_tests.py @@ -0,0 +1,120 @@ +#!/bin/env python3 +# +# This script tests various functions of the ODrive firmware and +# the ODrive Python library. +# +# Usage: +# 1. adapt test-rig.yaml for your test rig. +# 2. ./run_tests.py + +import yaml +import os +import sys +import threading +import traceback +from odrive.tests import * +from odrive.utils import Logger, for_all_parallel + + +all_tests = [ + TestFlashAndErase(), + TestSetup(), + TestMotorCalibration(), + # TODO: test encoder index search + TestEncoderOffsetCalibration(), + TestClosedLoopControl(), + TestStoreAndReboot(), + TestEncoderOffsetCalibration(), # need to find offset _or_ index after reboot + TestClosedLoopControl() + # TODO: test step/dir + # TODO: test sensorless + # TODO: test ASCII protocol + # TODO: test protocol over UART +] + + +logger = Logger() + +with open('test-rig.yaml', 'r') as file_stream: + test_rig_yaml = yaml.load(file_stream) + +# Ensure every device has a name +for idx, odrv_yaml in enumerate(test_rig_yaml['odrives']): + if not 'name' in odrv_yaml: + odrv_yaml['name'] = 'odrive{}'.format(idx) + +# Build a dictionary of axes by name (e.g. odrive0.axis0) +# Also ensure every axis has a name and mutex +axes_by_name = {} +for odrv_yaml in test_rig_yaml['odrives']: + for axis_idx, axis_yaml in enumerate(odrv_yaml['axes']): + if not 'name' in axis_yaml: + axis_yaml['name'] = '{}.axis{}'.format(odrv_yaml['name'], axis_idx) + axis_yaml['lock'] = threading.Lock() + axes_by_name[axis_yaml['name']] = axis_yaml + +# Ensure mechanical couplings are valid +if test_rig_yaml['couplings'] is None: + test_rig_yaml['couplings'] = {} +else: + for axis in sum(test_rig_yaml['couplings'], []): + if not axis in axes_by_name: + logger.error('Unknown axis {} in list of mechanical couplings'.format(axis)) + + +try: + for test in all_tests: + if isinstance(test, ODriveTest): + def odrv_test_thread(odrv_yaml): + test_subject_name = odrv_yaml['name'] + logger.info('● running {} on {}...'.format(type(test).__name__, test_subject_name)) + odrv = odrv_yaml['odrv'] if 'odrv' in odrv_yaml else None + test.run_test(odrv, odrv_yaml, + logger.indent(' {}: '.format(test_subject_name))) + + for_all_parallel(test_rig_yaml['odrives'], lambda x: x['name'], odrv_test_thread) + + elif isinstance(test, AxisTest): + def axis_test_thread(axis_name): + # Get all axes that are mechanically coupled with the axis specified by axis_name + conflicting_axes = sum([c for c in test_rig_yaml['couplings'] if (axis_name in c)], []) + # Remove duplicates + conflicting_axes = list(set(conflicting_axes)) + # Acquire lock for all conflicting axes + conflicting_axes.sort() # prevent deadlocks + for conflicting_axis in conflicting_axes: + axes_by_name[conflicting_axis]['lock'].acquire() + try: + # Run test on this axis + logger.info('● running {} on {}...'.format(type(test).__name__, axis_name)) + axis_yaml = axes_by_name[axis_name] + test.run_test(axis_yaml['axis'], axis_yaml, + logger.indent(' {}: '.format(axis_name))) + finally: + # Release all conflicting axes + for conflicting_axis in conflicting_axes: + axes_by_name[conflicting_axis]['lock'].release() + + for_all_parallel(axes_by_name, lambda x: x, axis_test_thread) + + else: + logger.warn("ignoring unknown test type {}".format(type(test))) + +except: + logger.error(traceback.format_exc()) + logger.debug('=> Test failed. Please wait while I secure the test rig...') + try: + dont_secure_after_failure = True # TODO: disable + if not dont_secure_after_failure: + def odrv_reset_thread(odrv_yaml): + run("make erase PROGRAMMER='" + odrv_yaml['programmer'] + "'", logger, timeout=30) + for_all_parallel(test_rig_yaml['odrives'], lambda x: x['name'], odrv_reset_thread) + except: + logger.error('///////////////////////////////////////////') + logger.error('/// CRITICAL: COULD NOT SECURE TEST RIG ///') + logger.error('/// CUT THE POWER IMMEDIATELY! ///') + logger.error('///////////////////////////////////////////') + else: + logger.error('some test failed!') +else: + logger.success('All tests succeeded!') From c547bb09a244e7d5d448f858cd460555835def05 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 19:50:35 -0700 Subject: [PATCH 022/112] more USB and discovery fixes --- tools/odrive/discovery.py | 24 ++++++++++++------------ tools/odrive/usbbulk_transport.py | 6 ++++++ 2 files changed, 18 insertions(+), 12 deletions(-) diff --git a/tools/odrive/discovery.py b/tools/odrive/discovery.py index f31f0a2d..18b5107a 100644 --- a/tools/odrive/discovery.py +++ b/tools/odrive/discovery.py @@ -12,6 +12,7 @@ import odrive.utils import odrive.remote_object import odrive.usbbulk_transport import odrive.serial_transport +from odrive.utils import Event channel_types = { "usb": odrive.usbbulk_transport.discover_channels, @@ -59,7 +60,7 @@ def find_all(path, serial_number, return json_data = {"name": "odrive", "members": json_data} obj = odrive.remote_object.RemoteObject(json_data, None, channel, printer) - device_serial_number = serial_number if hasattr(obj, 'serial_number') else "[unknown serial number]" + device_serial_number = format(obj.serial_number, 'x').upper() if hasattr(obj, 'serial_number') else "[unknown serial number]" if serial_number != None and device_serial_number != serial_number: printer("Ignoring device with serial number {}".format(device_serial_number)) return @@ -78,19 +79,18 @@ def find_all(path, serial_number, raise Exception("Invalid path spec \"{}\"".format(search_spec)) -def find_any(path="usb", serial_number=None, printer=noprint): +def find_any(path="usb", serial_number=None, cancellation_token=None, timeout=None, printer=noprint): """ Blocks until the first matching ODrive is connected and then returns that device """ - cancellation_token = None # TODO: make this a parameter (see todo below) - if cancellation_token is None: - cancellation_token = threading.Event() - done_signal = threading.Event() + result = [ None ] + done_signal = Event(cancellation_token) def did_discover_object(obj): - global result - result = obj + result[0] = obj done_signal.set() - find_all(path, serial_number, did_discover_object, cancellation_token, printer) - done_signal.wait() # TODO: wait on done_signal OR cancellation_token - cancellation_token.set() - return result + find_all(path, serial_number, did_discover_object, done_signal, printer) + try: + done_signal.wait(timeout=timeout) + finally: + done_signal.set() # terminate find_all + return result[0] diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index 5eecbd24..edf057e2 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -87,7 +87,10 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) except usb.core.USBError as ex: if ex.errno == 19: # "no such device" raise odrive.protocol.ChannelBrokenException() + elif ex.errno == 110: # timeout + raise odrive.utils.TimeoutException() else: + self._printer("halt condition: {}".format(ex.errno)) # Try resetting halt/stall condition try: self.epw.clear_halt() @@ -109,7 +112,10 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) except usb.core.USBError as ex: if ex.errno == 19: # "no such device" raise odrive.protocol.ChannelBrokenException() + elif ex.errno == 110: # timeout + raise odrive.utils.TimeoutException() else: + self._printer("halt condition: {}".format(ex.errno)) # Try resetting halt/stall condition try: self.epr.clear_halt() From fe7a87afecf14dfde664d4f614a7e047bee5466b Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 19:51:36 -0700 Subject: [PATCH 023/112] fix sensorless estimator to respect motor direction --- Firmware/MotorControl/sensorless_estimator.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index c1358150..0fa3c7fe 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -33,6 +33,9 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC, one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)}; + // Swap sign of I_beta if motor is reversed + I_alpha_beta[1] *= axis_->motor_.config_.direction; + // alpha-beta vector operations float eta[2]; for (int i = 0; i <= 1; ++i) { @@ -68,7 +71,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // Flux state estimation done, store V_alpha_beta for next timestep V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha; - V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta; + V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta * axis_->motor_.config_.direction; // PLL // TODO: the PLL part has some code duplication with the encoder PLL From 765e63af01a4ce199c69027ae49da2359c6ff7bf Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 19:53:46 -0700 Subject: [PATCH 024/112] set current controller gains based on NVM configration If the motor phase resistance and phase inductance was loaded from NVM (pre_calibrated = true) (as opposed to calibration) the current control P and I gains would not be loaded correctly. This commit fixes this. --- Firmware/MotorControl/motor.cpp | 17 ++++++++++++----- Firmware/MotorControl/motor.hpp | 2 ++ 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 75e81202..5979257c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -58,6 +58,17 @@ void Motor::disarm() { axis_->missed_control_deadline_ = true; } +// @brief Tune the current controller based on phase resistance and inductance +// This should be invoked whenever one of these values changes. +// TODO: allow update on user-request or update automatically via hooks +void Motor::update_current_controller_gains() { + // Calculate current control gains + float current_control_bandwidth = 1000.0f; // [rad/s] + current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; + float plant_pole = config_.phase_resistance / config_.phase_inductance; + current_control_.i_gain = plant_pole * current_control_.p_gain; +} + // @brief Set up the gate drivers void Motor::DRV8301_setup() { DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; @@ -234,11 +245,7 @@ bool Motor::run_calibration() { return false; } - // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] - current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; - float plant_pole = config_.phase_resistance / config_.phase_inductance; - current_control_.i_gain = plant_pole * current_control_.p_gain; + update_current_controller_gains(); is_calibrated_ = true; return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 2e771b27..37647db0 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -82,8 +82,10 @@ public: bool arm(); void disarm(); void setup() { + update_current_controller_gains(); DRV8301_setup(); } + void update_current_controller_gains(); void DRV8301_setup(); bool check_DRV_fault(); bool do_checks(); From 7695828d31f4de860eff4612fdb3b73f41601885 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 19:51:36 -0700 Subject: [PATCH 025/112] fix sensorless estimator to respect motor direction --- Firmware/MotorControl/sensorless_estimator.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index c1358150..0fa3c7fe 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -33,6 +33,9 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC, one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)}; + // Swap sign of I_beta if motor is reversed + I_alpha_beta[1] *= axis_->motor_.config_.direction; + // alpha-beta vector operations float eta[2]; for (int i = 0; i <= 1; ++i) { @@ -68,7 +71,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // Flux state estimation done, store V_alpha_beta for next timestep V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha; - V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta; + V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta * axis_->motor_.config_.direction; // PLL // TODO: the PLL part has some code duplication with the encoder PLL From 0e37ff01e45aefcf8dd681f6dc3bd2b9c3c7d8cc Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 19:53:46 -0700 Subject: [PATCH 026/112] set current controller gains based on NVM configration If the motor phase resistance and phase inductance was loaded from NVM (pre_calibrated = true) (as opposed to calibration) the current control P and I gains would not be loaded correctly. This commit fixes this. --- Firmware/MotorControl/motor.cpp | 17 ++++++++++++----- Firmware/MotorControl/motor.hpp | 2 ++ 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 75e81202..5979257c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -58,6 +58,17 @@ void Motor::disarm() { axis_->missed_control_deadline_ = true; } +// @brief Tune the current controller based on phase resistance and inductance +// This should be invoked whenever one of these values changes. +// TODO: allow update on user-request or update automatically via hooks +void Motor::update_current_controller_gains() { + // Calculate current control gains + float current_control_bandwidth = 1000.0f; // [rad/s] + current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; + float plant_pole = config_.phase_resistance / config_.phase_inductance; + current_control_.i_gain = plant_pole * current_control_.p_gain; +} + // @brief Set up the gate drivers void Motor::DRV8301_setup() { DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; @@ -234,11 +245,7 @@ bool Motor::run_calibration() { return false; } - // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] - current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; - float plant_pole = config_.phase_resistance / config_.phase_inductance; - current_control_.i_gain = plant_pole * current_control_.p_gain; + update_current_controller_gains(); is_calibrated_ = true; return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 2e771b27..37647db0 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -82,8 +82,10 @@ public: bool arm(); void disarm(); void setup() { + update_current_controller_gains(); DRV8301_setup(); } + void update_current_controller_gains(); void DRV8301_setup(); bool check_DRV_fault(); bool do_checks(); From 0784bd341376be5c410ecabe9b38829e0fabf897 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 23 Mar 2018 20:21:25 -0700 Subject: [PATCH 027/112] move test-rig.yaml to tools/ --- tools/odrive/tests.py | 4 ++-- tools/run_tests.py | 5 ++++- {Firmware => tools}/test-rig.yaml | 0 3 files changed, 6 insertions(+), 3 deletions(-) rename {Firmware => tools}/test-rig.yaml (100%) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 924dbee8..4e9b4398 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -24,11 +24,11 @@ def test_assert_eq(observed, expected, range=None, accuracy=None): def run(command_line, logger, timeout=None): """ - Runs a shell command in the Firmware directory + Runs a shell command in the current directory """ logger.debug("invoke: " + command_line) cmd = shlex.split(command_line) - result = subprocess.run(cmd, cwd='../Firmware', timeout=timeout, + result = subprocess.run(cmd, timeout=timeout, stdout=subprocess.PIPE, stderr=subprocess.STDOUT) if result.returncode != 0: diff --git a/tools/run_tests.py b/tools/run_tests.py index c070b919..463d6939 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -35,9 +35,12 @@ all_tests = [ logger = Logger() -with open('test-rig.yaml', 'r') as file_stream: +script_path=os.path.dirname(os.path.realpath(__file__)) +with open(script_path + '/test-rig.yaml', 'r') as file_stream: test_rig_yaml = yaml.load(file_stream) +os.chdir(script_path + '/../Firmware') + # Ensure every device has a name for idx, odrv_yaml in enumerate(test_rig_yaml['odrives']): if not 'name' in odrv_yaml: diff --git a/Firmware/test-rig.yaml b/tools/test-rig.yaml similarity index 100% rename from Firmware/test-rig.yaml rename to tools/test-rig.yaml From ae8acae7ef7e0ea08297bf0ee1072f09ed125f8d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 25 Mar 2018 15:57:50 -0700 Subject: [PATCH 028/112] make fancy terminal features work on Windows Add Windows support for the following terminal features: - colored output - output on the second last line On Unix systems, VT100 escape codes are used to achieve this, functionality however Windows <10 doesn't interpret VT100 escape codes. For normal colored output, we use the colorama module to abstract this away. To print text on the second-last line we call the appropriate Win32 API functions directly (using the win32console module). --- tools/explore_odrive.py | 31 ++------- tools/odrive/serial_transport.py | 2 +- tools/odrive/usbbulk_transport.py | 8 ++- tools/odrive/utils.py | 102 +++++++++++++++++++++++++++--- 4 files changed, 105 insertions(+), 38 deletions(-) diff --git a/tools/explore_odrive.py b/tools/explore_odrive.py index ccc252d4..66884d0f 100755 --- a/tools/explore_odrive.py +++ b/tools/explore_odrive.py @@ -8,7 +8,7 @@ import sys import platform import threading import odrive.discovery -from odrive.utils import start_liveplotter +from odrive.utils import start_liveplotter, Logger # Flush stdout by default import functools @@ -63,27 +63,7 @@ else: ## Interactive console utils ## -COLOR_RED = '\x1b[91;1m' -COLOR_CYAN = '\x1b[96;1m' -COLOR_RESET = '\x1b[0m' - -def print_on_second_last_line(text, **kwargs): - """ - Prints a text on the second last line. - This can be used to print a message above the command - prompt. If the command prompt spans multiple lines, - there will be glitches. - """ - # Escape character sequence: - # ESC 7: store cursor position - # ESC 1A: move cursor up by one - # ESC 1S: scroll entire viewport by one - # ESC 1L: insert 1 line at cursor position - # (print text) - # ESC 8: restore old cursor position - kwargs['end'] = '' - kwargs['flush'] = True - print('\x1b7\x1b[1A\x1b[1S\x1b[1L' + text + '\x1b8', **kwargs) +logger = Logger() def print_banner(): print('ODrive control utility v0.4') @@ -135,7 +115,7 @@ def did_discover_device(odrive): # Publish new ODrive to interactive console interactive_variables[interactive_name] = odrive globals()[interactive_name] = odrive # Add to globals so tab complete works - print_on_second_last_line(COLOR_CYAN + "{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name) + COLOR_RESET) + logger.info("{} to ODrive {:012X} as {}".format(verb, serial_number, interactive_name)) # Subscribe to disappearance of the device odrive.__channel__._channel_broken.subscribe(lambda: did_lose_device(interactive_name)) @@ -146,7 +126,7 @@ def did_lose_device(interactive_name): a message. """ if not app_shutdown_token.is_set(): - print_on_second_last_line(COLOR_RED + "Oh no {} disappeared".format(interactive_name) + COLOR_RESET) + logger.warn("Oh no {} disappeared".format(interactive_name)) # Connect to device printer("Waiting for device...") @@ -183,7 +163,7 @@ else: # Enable tab complete if possible try: import rlcompleter - import readline + import readline # Works only on Unix readline.parse_and_bind("tab: complete") except: sudo_prefix = "" if platform.system() == "Windows" else "sudo " @@ -206,5 +186,6 @@ console.runcode('sys.excepthook=newexcepthook') # Launch shell print_banner() +logger._skip_bottom_line = True interact() app_shutdown_token.set() diff --git a/tools/odrive/serial_transport.py b/tools/odrive/serial_transport.py index 53ca6850..dd595c6d 100644 --- a/tools/odrive/serial_transport.py +++ b/tools/odrive/serial_transport.py @@ -89,7 +89,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer input_stream, output_stream, printer) channel.serial_device = serial_device except serial.serialutil.SerialException: - printer("Serial device init failed. Ignoring this port") + printer("Serial device init failed. Ignoring this port. More info: " + traceback.format_exc()) known_devices.append(port_name) else: known_devices.append(port_name) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index edf057e2..e1004ba8 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -6,6 +6,8 @@ import time import usb.core import usb.util import odrive.protocol +import traceback +import platform ODRIVE_VID_PID_PAIRS = [ (0x1209, 0x0D31), @@ -39,7 +41,9 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) # state where there are a few packets in a receive queue but a call # to epr.read() does not return these packet until a new packet arrives. # This undesirable queue can be cleared by resetting the device. - self.dev.reset() + # On windows this would cause file-not-found errors in subsequent dev calls + if platform.system() != 'Windows': + self.dev.reset() try: if self.dev.is_kernel_driver_active(1): @@ -185,7 +189,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer usb_device.reset() continue else: - printer("USB device init failed. Ignoring this device") + printer("USB device init failed. Ignoring this device. More info: " + traceback.format_exc()) known_devices.append((usb_device.bus, usb_device.address)) else: known_devices.append((usb_device.bus, usb_device.address)) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 4f070b1e..7762494b 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -6,6 +6,16 @@ Liveplotter import sys import time import threading +import platform + +try: + if platform.system() == 'Windows': + import win32console + import colorama + colorama.init() +except ModuleNotFoundError: + print("Could not init terminal colors") + pass data_rate = 100 plot_rate = 10 @@ -198,27 +208,99 @@ class Logger(): Logs messages to stdout """ - COLOR_GREEN = '\x1b[92;1m' - COLOR_CYAN = '\x1b[96;1m' - COLOR_YELLOW = '\x1b[93;1m' - COLOR_RED = '\x1b[91;1m' - COLOR_RESET = '\x1b[0m' + COLOR_DEFAULT = 0 + COLOR_GREEN = 1 + COLOR_CYAN = 2 + COLOR_YELLOW = 3 + COLOR_RED = 4 + + _VT100Colors = { + COLOR_GREEN: '\x1b[92;1m', + COLOR_CYAN: '\x1b[96;1m', + COLOR_YELLOW: '\x1b[93;1m', + COLOR_RED: '\x1b[91;1m', + COLOR_DEFAULT: '\x1b[0m' + } + + _Win32Colors = { + COLOR_GREEN: 0x0A, + COLOR_CYAN: 0x0B, + COLOR_YELLOW: 0x0E, + COLOR_RED: 0x0C, + COLOR_DEFAULT: 0x07 + } def __init__(self): self._prefix = '' + self._skip_bottom_line = False # If true, messages are printed one line above the cursor + if platform.system() == 'Windows': + self._stdout_buf = win32console.GetStdHandle(win32console.STD_OUTPUT_HANDLE) def indent(self, prefix=' '): indented_logger = Logger() indented_logger._prefix = self._prefix + prefix return indented_logger + def print_on_second_last_line(self, text, color): + """ + Prints a text on the second last line. + This can be used to print a message above the command + prompt. If the command prompt spans multiple lines + there will be glitches. + If the printed text spans multiple lines there will also + be glitches (though this could be fixed). + """ + + if platform.system() == 'Windows': + # Windows <10 doesn't understand VT100 escape codes and the colorama + # also doesn't support the specific escape codes we need so we use the + # native Win32 API. + info = self._stdout_buf.GetConsoleScreenBufferInfo() + cursor_pos = info['CursorPosition'] + scroll_rect=win32console.PySMALL_RECTType( + Left=0, Top=1, + Right=info['Window'].Right, + Bottom=cursor_pos.Y-1) + scroll_dest = win32console.PyCOORDType(scroll_rect.Left, scroll_rect.Top-1) + self._stdout_buf.ScrollConsoleScreenBuffer( + scroll_rect, scroll_rect, scroll_dest, # clipping rect is same as scroll rect + u' ', Logger._Win32Colors[color]) # fill with empty cells with the desired color attributes + line_start = win32console.PyCOORDType(0, cursor_pos.Y-1) + self._stdout_buf.WriteConsoleOutputCharacter(text, line_start) + + else: + # Assume we're in a terminal that interprets VT100 escape codes. + # TODO: test on macOS + + # Escape character sequence: + # ESC 7: store cursor position + # ESC 1A: move cursor up by one + # ESC 1S: scroll entire viewport by one + # ESC 1L: insert 1 line at cursor position + # (print text) + # ESC 8: restore old cursor position + + sys.stdout.write('\x1b7\x1b[1A\x1b[1S\x1b[1L', end='', flush=True) + sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT]) + sys.stdout.write('\x1b8', end='', flush=True) + sys.stdout.flush() + + def print_colored(self, text, color): + if self._skip_bottom_line: + self.print_on_second_last_line(text, color) + else: + # On Windows, colorama does the job of interpreting the VT100 escape sequences + sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT] + '\n') + sys.stdout.flush() + def debug(self, text): - print(self._prefix + text) + self.print_colored(self._prefix + text, Logger.COLOR_DEFAULT) def success(self, text): - print(self._prefix + Logger.COLOR_GREEN + text + Logger.COLOR_RESET) + self.print_colored(self._prefix + text, Logger.COLOR_GREEN) def info(self, text): - print(self._prefix + Logger.COLOR_CYAN + text + Logger.COLOR_RESET) + self.print_colored(self._prefix + text, Logger.COLOR_CYAN) def warn(self, text): - print(self._prefix + Logger.COLOR_YELLOW + text + Logger.COLOR_RESET) + self.print_colored(self._prefix + text, Logger.COLOR_YELLOW) def error(self, text): - print(self._prefix + Logger.COLOR_RED + text + Logger.COLOR_RESET) + # TODO: write to stderr + self.print_colored(self._prefix + text, Logger.COLOR_RED) From 0790ed8959ee901c7a875536f7fc1055d51be314 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 25 Mar 2018 16:13:46 -0700 Subject: [PATCH 029/112] don't fail if build dir doesn't exist --- tools/build.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/build.sh b/tools/build.sh index 7591b0e5..a953d480 100755 --- a/tools/build.sh +++ b/tools/build.sh @@ -9,8 +9,8 @@ THIS_DIR="$(dirname "$0")" cd "$THIS_DIR/../Firmware" # Write all environment variables that start with "CONFIG_" to tup.config +rm -rdf build mkdir -p build -rm build/* env | grep ^CONFIG > tup.config tup generate ./tup_build.sh bash -xe ./tup_build.sh From ac309d371b3c50944f3ee03951eea82140a90605 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 26 Mar 2018 15:14:56 -0700 Subject: [PATCH 030/112] update CubeMX settings --- .../0001-expose-correct-serial-number-on-USB.patch | 14 +++----------- Firmware/Board/v3/Inc/FreeRTOSConfig.h | 2 +- .../FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 2 +- Firmware/Board/v3/Odrive.ioc | 6 ++++-- Firmware/Board/v3/Src/freertos.c | 10 ++++++++++ Firmware/Board/v3/Src/usbd_desc.c | 3 +-- 6 files changed, 20 insertions(+), 17 deletions(-) diff --git a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch index da4b9707..a2e510b0 100644 --- a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch +++ b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch @@ -4,22 +4,14 @@ Date: Mon, 12 Mar 2018 23:49:32 -0700 Subject: [PATCH] expose correct serial number on USB --- - Firmware/Board/v3/Src/usbd_desc.c | 16 +++++++++------- - 1 file changed, 9 insertions(+), 7 deletions(-) + Firmware/Board/v3/Src/usbd_desc.c | 15 +++++++++------- + 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index b9c7bd0..94dc49b 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c -@@ -51,6 +51,7 @@ - #include "usbd_core.h" - #include "usbd_desc.h" - #include "usbd_conf.h" -+#include "commands.h" - - /* USER CODE BEGIN INCLUDE */ - -@@ -327,14 +328,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l +@@ -327,14 +327,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l */ uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { diff --git a/Firmware/Board/v3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3/Inc/FreeRTOSConfig.h index cf2d6f0e..fd592cbe 100644 --- a/Firmware/Board/v3/Inc/FreeRTOSConfig.h +++ b/Firmware/Board/v3/Inc/FreeRTOSConfig.h @@ -107,8 +107,8 @@ #define configUSE_16_BIT_TICKS 0 #define configUSE_MUTEXES 1 #define configQUEUE_REGISTRY_SIZE 8 -#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1 #define configCHECK_FOR_STACK_OVERFLOW 1 +#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1 /* Co-routine definitions. */ #define configUSE_CO_ROUTINES 0 diff --git a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index 2e1c9e0b..754be245 100644 --- a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t; /// Thread Definition structure contains startup information of a thread. /// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS. typedef struct os_thread_def { - const char *name; ///< Thread name + const char *name; ///< Thread name os_pthread pthread; ///< start address of thread function osPriority tpriority; ///< initial thread priority uint32_t instances; ///< maximum number of instances of that thread function diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index aea71bef..130005f8 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -102,9 +102,11 @@ Dma.UART4_TX.1.PeriphInc=DMA_PINC_DISABLE Dma.UART4_TX.1.Priority=DMA_PRIORITY_LOW Dma.UART4_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode FREERTOS.FootprintOK=true +FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark=1 FREERTOS.INCLUDE_vTaskDelayUntil=1 -FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK -FREERTOS.Tasks01=defaultTask,-3,256,StartDefaultTask,Default +FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,INCLUDE_uxTaskGetStackHighWaterMark +FREERTOS.Tasks01=defaultTask,0,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL +FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1 FREERTOS.configTOTAL_HEAP_SIZE=65536 File.Version=6 KeepUserPlacement=true diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index 002ac979..b247994a 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -85,6 +85,16 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ /* USER CODE END FunctionPrototypes */ /* Hook prototypes */ +void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName); + +/* USER CODE BEGIN 4 */ +__weak void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName) +{ + /* Run time stack overflow checking is performed if + configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is + called if a stack overflow is detected. */ +} +/* USER CODE END 4 */ /* Init FreeRTOS */ diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index 71b353a8..9974e643 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -51,10 +51,9 @@ #include "usbd_core.h" #include "usbd_desc.h" #include "usbd_conf.h" -#include "communication.h" /* USER CODE BEGIN INCLUDE */ - +#include "communication.h" /* USER CODE END INCLUDE */ /* Private typedef -----------------------------------------------------------*/ From 3387da80304b7b73ad29d957264242e55181dc88 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 26 Mar 2018 15:35:37 -0700 Subject: [PATCH 031/112] add patch for FreeRTOS const fixes --- .../v3/0002-FreeRTOS-constness-fixes.patch | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch diff --git a/Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch b/Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch new file mode 100644 index 00000000..afe0326e --- /dev/null +++ b/Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch @@ -0,0 +1,64 @@ +From 510ead2b159e1d8116e5241066c54a7bf8b7bfbe Mon Sep 17 00:00:00 2001 +From: Samuel Sadok +Date: Mon, 26 Mar 2018 15:29:44 -0700 +Subject: [PATCH] FreeRTOS constness fixes + + - make thread names const char * + - make thread argument non-const void* +--- + .../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 6 +++--- + Firmware/Board/v3/Src/freertos.c | 4 ++-- + 2 files changed, 5 insertions(+), 5 deletions(-) + +diff --git a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +index 09cdf27..754be24 100644 +--- a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h ++++ b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +@@ -270,11 +270,11 @@ typedef enum { + + /// Entry point of a thread. + /// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS. +-typedef void (*os_pthread) (void const *argument); ++typedef void (*os_pthread) (void *argument); + + /// Entry point of a timer call back function. + /// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS. +-typedef void (*os_ptimer) (void const *argument); ++typedef void (*os_ptimer) (void *argument); + + // >>> the following data type definitions may shall adapted towards a specific RTOS + +@@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t; + /// Thread Definition structure contains startup information of a thread. + /// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS. + typedef struct os_thread_def { +- char *name; ///< Thread name ++ const char *name; ///< Thread name + os_pthread pthread; ///< start address of thread function + osPriority tpriority; ///< initial thread priority + uint32_t instances; ///< maximum number of instances of that thread function +diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c +index 6eaea82..b247994 100644 +--- a/Firmware/Board/v3/Src/freertos.c ++++ b/Firmware/Board/v3/Src/freertos.c +@@ -75,7 +75,7 @@ uint8_t ucHeap[configTOTAL_HEAP_SIZE]; + /* USER CODE END Variables */ + + /* Function prototypes -------------------------------------------------------*/ +-void StartDefaultTask(void const * argument); ++void StartDefaultTask(void * argument); + + extern void MX_USB_DEVICE_Init(void); + void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ +@@ -147,7 +147,7 @@ void MX_FREERTOS_Init(void) { + } + + /* StartDefaultTask function */ +-void StartDefaultTask(void const * argument) ++void StartDefaultTask(void * argument) + { + /* init code for USB_DEVICE */ + MX_USB_DEVICE_Init(); +-- +2.16.2 + From b8b483c9d6d298c0c5185601987438f8cc9210fb Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 30 Mar 2018 22:32:07 -0700 Subject: [PATCH 032/112] Firmware hardening - check return code of enqueue_modulation_timings - wrap all hardware access to safety critical hardware timer registers in safety_critical_ sections. These functions like the armed-ness of each PWM output to a corresponding armed state that can only be set at start or by the user - ensure NaN in any variables doesn't cause unpredictable behavior on the safety critical PWM outputs --- Firmware/Board/v3/Src/stm32f4xx_it.c | 6 +- Firmware/MotorControl/axis.cpp | 4 +- Firmware/MotorControl/axis.hpp | 16 +- Firmware/MotorControl/communication.cpp | 1 + Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/encoder.cpp | 12 +- Firmware/MotorControl/encoder.hpp | 2 +- Firmware/MotorControl/legacy_commands.h | 4 +- Firmware/MotorControl/low_level.cpp | 217 +++++++++++++++++++++--- Firmware/MotorControl/low_level.h | 14 +- Firmware/MotorControl/motor.cpp | 49 +++--- Firmware/MotorControl/motor.hpp | 17 +- Firmware/MotorControl/odrive_main.hpp | 2 + Firmware/MotorControl/protocol.cpp | 2 +- Firmware/MotorControl/utils.c | 16 +- Firmware/Tupfile.lua | 7 +- Firmware/build.sh | 3 + 17 files changed, 285 insertions(+), 89 deletions(-) diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 0a940b15..cd96644a 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -38,11 +38,15 @@ /* USER CODE BEGIN 0 */ #include "freertos_vars.h" -#include "low_level.h" +#include typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); +// TODO: move somewhere else +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); + /* USER CODE END 0 */ /* External variables --------------------------------------------------------*/ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index d6c718e4..f9696284 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -53,9 +53,7 @@ void Axis::signal_current_meas() { // @brief Blocks until a current measurement is completed // @returns True on success, false otherwise bool Axis::wait_for_current_meas() { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) - return error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; - return true; + return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal; } static void step_cb_wrapper(void* ctx) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 300c7e94..c7f909e0 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -106,7 +106,8 @@ public: error_ = ERROR_MOTOR_FAILED; break; } - if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) { + if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { + // motor got disarmed in something other than the idle loop error_ = ERROR_CONTROL_LOOP_TIMEOUT; break; } @@ -121,8 +122,12 @@ public: ++loop_counter_; // Wait until the current measurement interrupt fires - if (!wait_for_current_meas()) { // error set by function call - motor_.disarm(); // maybe the interrupt handler is dead, let's be safe and float all phases + if (!wait_for_current_meas()) { + // maybe the interrupt handler is dead, let's be + // safe and float the phases + safety_critical_disarm_motor_pwm(motor_); + update_brake_current(); + error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } } @@ -148,10 +153,6 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; - bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler - // whenever there's no active control loop that - // sets the timings. The flag must be explicitly - // cleared by a call to motors.arm(). bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; @@ -162,7 +163,6 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), - make_protocol_ro_property("missed_control_deadline", &missed_control_deadline_), make_protocol_property("enable_step_dir", &enable_step_dir_), make_protocol_ro_property("current_state", ¤t_state_), make_protocol_property("requested_state", &requested_state_), diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 8f951fbb..b8a0ed65 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -172,6 +172,7 @@ static inline auto make_obj_tree() { make_protocol_ro_property("vbus_voltage", &vbus_voltage), make_protocol_ro_property("comm_stack_info", &comm_stack_info), make_protocol_ro_property("serial_number", &serial_number), + make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_), make_protocol_object("config", make_protocol_property("brake_resistance", &board_config.brake_resistance), // TODO: changing this currently requires a reboot - fix this diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c20cf504..3e1b5661 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -96,7 +96,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_.use_anticogging) { - Iq += anticogging_.cogging_map[mod(pos_estimate, axis_->encoder_.config_.cpr)]; + Iq += anticogging_.cogging_map[mod(static_cast(pos_estimate), axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 37a40b1c..8fae0d95 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -80,7 +80,8 @@ bool Encoder::run_index_search() { float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + 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); // continue until the index is found @@ -115,7 +116,8 @@ bool Encoder::run_offset_calibration() { // go to motor zero phase for start_lock_duration to get ready to scan int i = 0; axis_->run_control_loop([&](){ - axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f); + if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f)) + return false; // error set inside enqueue_voltage_timings axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); return ++i < start_lock_duration * current_meas_hz; }); @@ -131,7 +133,8 @@ bool Encoder::run_offset_calibration() { float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + 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_ENC_CALIB); encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; @@ -169,7 +172,8 @@ bool Encoder::run_offset_calibration() { float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); float v_alpha = voltage_magnitude * arm_cos_f32(phase); float v_beta = voltage_magnitude * arm_sin_f32(phase); - axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + 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_ENC_CALIB); encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index f3dbc78c..2edf5be0 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -16,7 +16,7 @@ struct EncoderConfig_t { int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once // index search succeeds - float calib_range = 0.02; + float calib_range = 0.02f; }; class Encoder { diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/legacy_commands.h index bb945dab..11ee7203 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/MotorControl/legacy_commands.h @@ -6,7 +6,9 @@ extern "C" { #endif /* Includes ------------------------------------------------------------------*/ -#include "low_level.h" +#include +#include +#include /* Exported types ------------------------------------------------------------*/ typedef enum { diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index b6f36d39..01f8a392 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -7,8 +7,6 @@ #define ARM_MATH_CM4 #include -#include - #include #include #include @@ -34,9 +32,166 @@ // This value is updated by the DC-bus reading ADC. // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; +bool brake_resistor_armed_ = false; /* Private constant data -----------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ + +/* CPU critical section helpers ----------------------------------------------*/ + +static inline uint8_t cpu_enter_critical() { + uint8_t status_register; + asm ( + "MRS R0, PRIMASK\n\t" + "CPSID I\n\t" + "STRB R0, %[output]" + : [output] "=m" (status_register) :: "r0" + ); + return status_register; +} + +static inline void cpu_exit_critical(uint8_t status_register) { + asm ( + "ldrb r0, %[input]\n\t" + "msr PRIMASK,r0;\n\t" + ::[input] "m" (status_register) : "r0" + ); +} + +/* Safety critical functions -------------------------------------------------*/ + +/* +* This section contains all accesses to safety critical hardware registers. +* Specifically, these registers: +* Motor0 PWMs: +* Timer1.MOE (master output enabled) +* Timer1.CCR1 (counter compare register 1) +* Timer1.CCR2 (counter compare register 2) +* Timer1.CCR3 (counter compare register 3) +* Motor1 PWMs: +* Timer8.MOE (master output enabled) +* Timer8.CCR1 (counter compare register 1) +* Timer8.CCR2 (counter compare register 2) +* Timer8.CCR3 (counter compare register 3) +* Brake resistor PWM: +* Timer2.CCR3 (counter compare register 3) +* Timer2.CCR4 (counter compare register 4) +* +* The following assumptions are made: +* - The hardware operates as described in the datasheet: +* http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf +* This assumption also requires for instance that there are no radiation +* caused hardware errors. +* - After startup, all variables used in this section are exclusively modified +* by the code in this section (this excludes function parameters) +* This assumption also requires that there is no memory corruption. +* - This code is compiled by a C standard compliant compiler. +* +* Furthermore: +* - Between calls to safety_critical_arm_motor_pwm and +* safety_critical_disarm_motor_pwm the motor's Ibus current is +* set to the correct value and update_brake_resistor is called +* at a high rate. +*/ + + +// @brief Kicks off the arming process of the motor. +// All calls to this function must clearly originate +// from user input. +void safety_critical_arm_motor_pwm(Motor& motor) { + uint8_t sr = cpu_enter_critical(); + if (brake_resistor_armed_) { + motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS; + } + cpu_exit_critical(sr); +} + +// @brief Disarms the motor PWM. +// After calling this function, it is guaranteed that all three +// motor phases are floating and will not be enabled again until +// safety_critical_arm_motor_phases is called. +void safety_critical_disarm_motor_pwm(Motor& motor) { + uint8_t sr = cpu_enter_critical(); + motor.armed_state_ = Motor::ARMED_STATE_DISARMED; + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer); + cpu_exit_critical(sr); +} + +// @brief Updates the phase timings unless the motor is disarmed. +// +// If this is called at a rate higher than the motor's timer period, +// the actual PMW timings on the pins can be undefined for up to one +// timer period. +void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) { + uint8_t sr = cpu_enter_critical(); + if (!brake_resistor_armed_) { + motor.armed_state_ = Motor::ARMED_STATE_ARMED; + } + + motor.hw_config_.timer->Instance->CCR1 = timings[0]; + motor.hw_config_.timer->Instance->CCR2 = timings[1]; + motor.hw_config_.timer->Instance->CCR3 = timings[2]; + + if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) { + // timings were just loaded into the timer registers + // the timer register are buffered, so they won't have an effect + // on the output just yet so we need to wait until the next + // interrupt before we actually enable the output + motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE; + } else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) { + // now we waited long enough. Enter armed state and + // enable the actual PWM outputs. + motor.armed_state_ = Motor::ARMED_STATE_ARMED; + __HAL_TIM_MOE_ENABLE(motor.hw_config_.timer); // enable pwm outputs + } else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) { + // nothing to do, PWM is running, all good + } else { + // unknown state oh no + safety_critical_disarm_motor_pwm(motor); + } + cpu_exit_critical(sr); +} + +// @brief Arms the brake resistor +void safety_critical_arm_brake_resistor() { + uint8_t sr = cpu_enter_critical(); + brake_resistor_armed_ = true; + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + cpu_exit_critical(sr); +} + +// @brief Disarms the brake resistor and by extension +// all motor PWM outputs. +// After calling this, the brake resistor can only be armed again +// by calling safety_critical_arm_brake_resistor(). +void safety_critical_disarm_brake_resistor() { + uint8_t sr = cpu_enter_critical(); + brake_resistor_armed_ = false; + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + for (size_t i = 0; i < AXIS_COUNT; ++i) { + safety_critical_disarm_motor_pwm(axes[i]->motor_); + } + cpu_exit_critical(sr); +} + +// @brief Updates the brake resistor PWM timings unless +// the brake resistor is disarmed. +void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) { + uint8_t sr = cpu_enter_critical(); + if (brake_resistor_armed_) { + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; + } + cpu_exit_critical(sr); +} + /* Function implementations --------------------------------------------------*/ void start_adc_pwm() { @@ -70,6 +225,12 @@ void start_adc_pwm() { htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); + + // Disarm motors and arm brake resistor + for (size_t i = 0; i < AXIS_COUNT; ++i) { + safety_critical_disarm_motor_pwm(axes[i]->motor_); + } + safety_critical_arm_brake_resistor(); } void start_pwm(TIM_HandleTypeDef* htim) { @@ -137,13 +298,15 @@ void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, htim_b->Instance->BDTR |= MOE_store_b; } -// @brief Floats ALL phases immediately and sets the brake current to 0. -void disable_all_pwms(Motor::Error_t error) { +// @brief Floats ALL phases immediately and disarms both motors and the brake resistor. +void low_level_fault(Motor::Error_t error) { // Disable all motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->motor_.disarm(); + safety_critical_disarm_motor_pwm(axes[i]->motor_); axes[i]->motor_.error_ = error; } + + safety_critical_disarm_brake_resistor(); } //-------------------------------- @@ -166,7 +329,7 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Ensure ADCs are expected ones to simplify the logic below if (!(hadc == &hadc2 || hadc == &hadc3)) { - disable_all_pwms(Motor::ERROR_ADC_FAILED); + low_level_fault(Motor::ERROR_ADC_FAILED); return; }; @@ -189,18 +352,17 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Load next timings for the motor that we're not currently sampling if (update_timings) { - if (other_axis.motor_.next_timings_valid_ && !other_axis.missed_control_deadline_) { - other_axis.motor_.next_timings_valid_ = false; - other_axis.motor_.hw_config_.timer->Instance->CCR1 = other_axis.motor_.next_timings_[0]; - other_axis.motor_.hw_config_.timer->Instance->CCR2 = other_axis.motor_.next_timings_[1]; - other_axis.motor_.hw_config_.timer->Instance->CCR3 = other_axis.motor_.next_timings_[2]; - __HAL_TIM_MOE_ENABLE(other_axis.motor_.hw_config_.timer); // enable pwm outputs - update_brake_current(); - } else { + if (!other_axis.motor_.next_timings_valid_) { // the motor control loop failed to update the timings in time // we must assume that it died and therefore float all phases - other_axis.motor_.disarm(); + safety_critical_disarm_motor_pwm(other_axis.motor_); + } else { + other_axis.motor_.next_timings_valid_ = false; + safety_critical_apply_motor_pwm_timings( + other_axis.motor_, other_axis.motor_.next_timings_ + ); } + update_brake_current(); } // Check the timing of the sequencing @@ -248,7 +410,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { void update_brake_current() { float Ibus_sum = 0.0f; for (size_t i = 0; i < AXIS_COUNT; ++i) { - Ibus_sum += axes[i]->motor_.current_control_.Ibus; + if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { + Ibus_sum += axes[i]->motor_.current_control_.Ibus; + } } float brake_current = -Ibus_sum; // Clip negative values to 0.0f @@ -256,16 +420,13 @@ void update_brake_current() { float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; // Duty limit at 90% to allow bootstrap caps to charge - if (brake_duty > 0.9f) brake_duty = 0.9f; - int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - - // Safe update of low and high side timings - // To avoid race condition, first reset timings to safe state - // ch3 is low side, ch4 is high side - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - htim2.Instance->CCR3 = low_off; - htim2.Instance->CCR4 = high_on; + // If brake_duty is NaN, this expression will also evaluate to true + if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) { + int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + safety_critical_apply_brake_resistor_timings(low_off, high_on); + } else { + safety_critical_disarm_brake_resistor(); + } } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 3105bae0..5f4a7cde 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -2,6 +2,10 @@ #ifndef __LOW_LEVEL_H #define __LOW_LEVEL_H +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif + #ifdef __cplusplus extern "C" { #endif @@ -17,10 +21,18 @@ extern "C" { /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -//Note: to control without feed forward, set feed forward terms to 0.0f. +void safety_critical_arm_motor_pwm(Motor& motor); +void safety_critical_disarm_motor_pwm(Motor& motor); +void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]); +void safety_critical_arm_brake_resistor(); +void safety_critical_disarm_brake_resistor(); +void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on); +// called from STM platform code +extern "C" { void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +} // Initalisation void start_adc_pwm(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 5979257c..cf0e80f9 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -39,25 +39,12 @@ bool Motor::arm() { // that we have exactly one full interrupt period until the third trigger. This gives // the control loop the correct time quota to set up modulation timings. if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) - return false; + return axis_->error_ = Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; next_timings_valid_ = false; - axis_->missed_control_deadline_ = false; + safety_critical_arm_motor_pwm(*this); return true; } -// @brief Floats the phases of this motor immediately and updates -// the brake current accordingly. -void Motor::disarm() { - // disable pwm - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer); - // set this motor's contribution to 0 - current_control_.Ibus = 0.0f; - update_brake_current(); - // ensure the PWM is not re-enabled without the state machine explicitly - // calling motor.arm() - axis_->missed_control_deadline_ = true; -} - // @brief Tune the current controller based on phase resistance and inductance // This should be invoked whenever one of these values changes. // TODO: allow update on user-request or update automatically via hooks @@ -167,7 +154,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) { // TODO check Ibeta balance to verify good motor connection bool Motor::measure_phase_resistance(float test_current, float max_voltage) { static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + static const int num_test_cycles = static_cast(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s float test_voltage = 0.0f; size_t i = 0; @@ -178,7 +165,8 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A - enqueue_voltage_timings(test_voltage, 0.0f); + if (!enqueue_voltage_timings(test_voltage, 0.0f)) + return false; // error set inside enqueue_voltage_timings log_timing(TIMING_LOG_MEAS_R); return ++i < num_test_cycles; @@ -187,7 +175,8 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { return false; //// De-energize motor - //enqueue_voltage_timings(motor, 0.0f, 0.0f); + //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f)) + // return false; // error set inside enqueue_voltage_timings float R = test_voltage / test_current; config_.phase_resistance = R; @@ -205,7 +194,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { Ialphas[i] += -current_meas_.phB - current_meas_.phC; // Test voltage along phase A - enqueue_voltage_timings(test_voltages[i], 0.0f); + if (!enqueue_voltage_timings(test_voltages[i], 0.0f)) + return false; // error set inside enqueue_voltage_timings log_timing(TIMING_LOG_MEAS_L); return ++t < (num_cycles << 1); @@ -214,7 +204,8 @@ bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { return false; //// De-energize motor - //enqueue_voltage_timings(motor, 0.0f, 0.0f); + //if (!enqueue_voltage_timings(motor, 0.0f, 0.0f)) + // return false; // error set inside enqueue_voltage_timings float v_L = 0.5f * (voltage_high - voltage_low); // Note: A more correct formula would also take into account that there is a finite timestep. @@ -251,21 +242,25 @@ bool Motor::run_calibration() { return true; } -void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { +bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; - SVM(mod_alpha, mod_beta, &tA, &tB, &tC); + if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0) + return error_ = ERROR_NUMERICAL, false; next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_valid_ = true; + return true; } -void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { +bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); float mod_alpha = vfactor * v_alpha; float mod_beta = vfactor * v_beta; - enqueue_modulation_timings(mod_alpha, mod_beta); + if (!enqueue_modulation_timings(mod_alpha, mod_beta)) + return false; log_timing(TIMING_LOG_FOC_VOLTAGE); + return true; } // TODO: This doesn't update brake current @@ -275,8 +270,7 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { float s = arm_sin_f32(phase); float v_alpha = c*v_d - s*v_q; float v_beta = c*v_q + s*v_d; - enqueue_voltage_timings(v_alpha, v_beta); - return true; + return enqueue_voltage_timings(v_alpha, v_beta); } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { @@ -336,7 +330,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { ictrl->final_v_beta = mod_to_V * mod_beta; // Apply SVM - enqueue_modulation_timings(mod_alpha, mod_beta); + if (!enqueue_modulation_timings(mod_alpha, mod_beta)) + return false; // error set inside enqueue_modulation_timings log_timing(TIMING_LOG_FOC_CURRENT); return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 37647db0..503b8756 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -60,6 +60,8 @@ public: ERROR_ADC_FAILED, ERROR_DRV_FAULT, ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE, + ERROR_NUMERICAL }; enum TimingLog_t { @@ -75,6 +77,13 @@ public: TIMING_LOG_NUM_SLOTS }; + enum ArmedState_t { + ARMED_STATE_DISARMED, + ARMED_STATE_WAITING_FOR_TIMINGS, + ARMED_STATE_WAITING_FOR_UPDATE, + ARMED_STATE_ARMED, + }; + Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, MotorConfig_t& config); @@ -94,8 +103,8 @@ public: bool measure_phase_resistance(float test_current, float max_voltage); bool measure_phase_inductance(float voltage_low, float voltage_high); bool run_calibration(); - void enqueue_modulation_timings(float mod_alpha, float mod_beta); - void enqueue_voltage_timings(float v_alpha, float v_beta); + 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); @@ -120,6 +129,9 @@ public: // variables exposed on protocol Error_t error_ = ERROR_NO_ERROR; + // Do not write to this variable directly! + // It is for exclusive use by the safety_critical_... functions. + ArmedState_t armed_state_ = ARMED_STATE_DISARMED; bool is_calibrated_ = config_.pre_calibrated; Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; @@ -144,6 +156,7 @@ public: auto make_protocol_definitions() { return make_protocol_member_list( make_protocol_property("error", &error_), + make_protocol_ro_property("armed_state", &armed_state_), make_protocol_ro_property("is_calibrated", &is_calibrated_), make_protocol_ro_property("current_meas_phB", ¤t_meas_.phB), make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 730f040d..50bf69c8 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -26,6 +26,7 @@ struct BoardConfig_t { }; class Axis; +class Motor; //default timeout waiting for phase measurement signals #define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] @@ -33,6 +34,7 @@ class Axis; static const float current_meas_period = CURRENT_MEAS_PERIOD; static const int current_meas_hz = CURRENT_MEAS_HZ; extern float vbus_voltage; +extern bool brake_resistor_armed_; extern const float elec_rad_per_enc; extern BoardConfig_t board_config; diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/MotorControl/protocol.cpp index c0cd07f0..50e38fab 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/MotorControl/protocol.cpp @@ -1,7 +1,7 @@ /* Includes ------------------------------------------------------------------*/ -#include "low_level.h" +//#include "low_level.h" #include "protocol.hpp" #include diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 269d1e7e..0c80088e 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -120,16 +120,12 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { } } - int retval = 0; - if ( - *tA < 0.0f - || *tA > 1.0f - || *tB < 0.0f - || *tB > 1.0f - || *tC < 0.0f - || *tC > 1.0f - ) retval = -1; - return retval; + // if any of the results becomes NaN, result_valid will evaluate to false + int result_valid = + *tA >= 0.0f && *tA <= 1.0f + && *tB >= 0.0f && *tB <= 1.0f + && *tC >= 0.0f && *tC <= 1.0f; + return result_valid ? 0 : -1; } //beware of inserting large angles! diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index d71b2238..51a35e76 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -63,6 +63,11 @@ if tup.getconfig("STEP_DIR") == "y" then end end +-- Compiler settings +if tup.getconfig("STRICT") == "true" then + FLAGS += '-Werror' +end + -- C-specific flags FLAGS += '-D__weak="__attribute__((weak))"' @@ -74,7 +79,7 @@ FLAGS += '-mthumb' FLAGS += '-mcpu=cortex-m4' FLAGS += '-mfpu=fpv4-sp-d16' FLAGS += '-mfloat-abi=hard' -FLAGS += { '-Wall', '-fdata-sections', '-ffunction-sections'} +FLAGS += { '-Wall', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} -- debug build FLAGS += '-g -gdwarf-2' diff --git a/Firmware/build.sh b/Firmware/build.sh index 811a4ab7..1622fa69 100755 --- a/Firmware/build.sh +++ b/Firmware/build.sh @@ -8,6 +8,9 @@ set -euo pipefail THIS_DIR="$(dirname "$0")" cd "$THIS_DIR" +# Treat warnings as errors +export CONFIG_STRICT=true + # Write all environment variables that start with "CONFIG_" to tup.config rm -rdf build mkdir -p build From 99c56491055a944349ef7a6a072f3354a749c8c2 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 3 Apr 2018 16:52:58 -0700 Subject: [PATCH 033/112] set board version in vscode settings --- Firmware/.vscode/c_cpp_properties.json | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index de6eb463..3e925aa9 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -26,6 +26,7 @@ "defines": [ "STM32F405xx", "USE_HAL_DRIVER", + "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" @@ -61,6 +62,7 @@ "defines": [ "STM32F405xx", "USE_HAL_DRIVER", + "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" @@ -103,6 +105,7 @@ "defines": [ "STM32F405xx", "USE_HAL_DRIVER", + "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" From ab698a9dcef8007b1ff06a5aa3deb8e6ac7ec60f Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 5 Apr 2018 22:27:20 -0700 Subject: [PATCH 034/112] turn error enums into flags --- Firmware/MotorControl/axis.cpp | 25 +++++++------ Firmware/MotorControl/axis.hpp | 37 ++++++++++--------- Firmware/MotorControl/encoder.cpp | 6 +-- Firmware/MotorControl/encoder.hpp | 10 +++-- Firmware/MotorControl/low_level.cpp | 12 ++++-- Firmware/MotorControl/low_level.h | 2 +- Firmware/MotorControl/motor.cpp | 14 +++---- Firmware/MotorControl/motor.hpp | 19 ++++++---- Firmware/MotorControl/odrive_main.hpp | 11 ++++++ .../MotorControl/sensorless_estimator.cpp | 2 +- .../MotorControl/sensorless_estimator.hpp | 6 ++- 11 files changed, 85 insertions(+), 59 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f9696284..fe3760aa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -101,9 +101,9 @@ bool Axis::check_PSU_brownout() { // Sets error and returns false otherwise. bool Axis::do_checks() { if (!motor_.do_checks()) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; if (!check_PSU_brownout()) - return error_ = ERROR_DC_BUS_UNDER_VOLTAGE, false; + return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false; return true; } @@ -115,7 +115,7 @@ bool Axis::run_sensorless_spin_up() { float I_mag = config_.spin_up_current * x; x += current_meas_period / config_.ramp_up_time; if (!motor_.update(I_mag, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return x < 1.0f; }); if (error_ != ERROR_NO_ERROR) @@ -129,7 +129,7 @@ bool Axis::run_sensorless_spin_up() { 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 error_ |= ERROR_MOTOR_FAILED, false; return vel < config_.spin_up_target_vel; }); return error_ == ERROR_NO_ERROR; @@ -142,16 +142,16 @@ bool Axis::run_sensorless_control_loop() { float pos_estimate, vel_estimate, phase, current_setpoint; if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) - return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false; + return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; // We update the encoder just in case someone needs the output for testing encoder_.update(nullptr, nullptr, nullptr); if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase)) - return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; + return error_ |= ERROR_SENSORLESS_ESTIMATOR_FAILED, false; if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error_ = ERROR_CONTROLLER_FAILED, false; + return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return true; }); set_step_dir_enabled(false); @@ -166,11 +166,11 @@ bool Axis::run_closed_loop_control_loop() { // We update the sensorless estimator just in case someone needs the output for testing sensorless_estimator_.update(nullptr, nullptr, nullptr); if (!encoder_.update(&pos_estimate, &vel_estimate, &phase)) - return error_ = ERROR_ENCODER_FAILED, false; + return error_ |= ERROR_ENCODER_FAILED, false; if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error_ = ERROR_CONTROLLER_FAILED, false; + return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return true; }); set_step_dir_enabled(false); @@ -180,6 +180,7 @@ bool Axis::run_closed_loop_control_loop() { bool Axis::run_idle_loop() { // run_control_loop ignores missed modulation timing updates // if and only if we're in AXIS_STATE_IDLE + safety_critical_disarm_motor_pwm(motor_); run_control_loop([this](){ sensorless_estimator_.update(nullptr, nullptr, nullptr); encoder_.update(nullptr, nullptr, nullptr); @@ -276,7 +277,7 @@ void Axis::run_state_machine_loop() { break; default: - error_ = ERROR_INVALID_STATE; + error_ |= ERROR_INVALID_STATE; status = false; // this will set the state to idle break; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c7f909e0..fe5ff15d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -43,17 +43,17 @@ struct AxisConfig_t { class Axis { public: enum Error_t { - ERROR_NO_ERROR = 0, - ERROR_INVALID_STATE = 1, // void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { - if (motor_.error_ != Motor::ERROR_NO_ERROR) { - error_ = ERROR_MOTOR_FAILED; - break; - } if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { // motor got disarmed in something other than the idle loop - error_ = ERROR_CONTROL_LOOP_TIMEOUT; + error_ |= ERROR_MOTOR_DISARMED; + break; + } + if (motor_.error_ != Motor::ERROR_NO_ERROR) { + error_ |= ERROR_MOTOR_FAILED; break; } @@ -127,7 +127,7 @@ public: // safe and float the phases safety_critical_disarm_motor_pwm(motor_); update_brake_current(); - error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT; + error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } } @@ -190,4 +190,7 @@ public: } }; + +DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t) + #endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 8fae0d95..19a39792 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -150,7 +150,7 @@ bool Encoder::run_offset_calibration() { float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { - error_ = ERROR_CPR_OUT_OF_RANGE; + error_ |= ERROR_CPR_OUT_OF_RANGE; return false; } // check direction @@ -162,7 +162,7 @@ bool Encoder::run_offset_calibration() { axis_->motor_.config_.direction = -1; } else { // Encoder response error - error_ = ERROR_RESPONSE; + error_ |= ERROR_RESPONSE; return false; } @@ -192,7 +192,7 @@ bool Encoder::run_offset_calibration() { bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ = ERROR_NUMERICAL; + error_ |= ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 2edf5be0..ae4b3c0e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -22,10 +22,10 @@ struct EncoderConfig_t { class Encoder { public: enum Error_t { - ERROR_NONE, - ERROR_NUMERICAL, - ERROR_CPR_OUT_OF_RANGE, - ERROR_RESPONSE, + ERROR_NONE = 0, + ERROR_NUMERICAL = 0x01, + ERROR_CPR_OUT_OF_RANGE = 0x02, + ERROR_RESPONSE = 0x04, }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -83,4 +83,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t) + #endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 01f8a392..98ac5620 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -110,11 +110,14 @@ void safety_critical_arm_motor_pwm(Motor& motor) { // After calling this function, it is guaranteed that all three // motor phases are floating and will not be enabled again until // safety_critical_arm_motor_phases is called. -void safety_critical_disarm_motor_pwm(Motor& motor) { +// @returns true if the motor was in a state other than disarmed before +bool safety_critical_disarm_motor_pwm(Motor& motor) { uint8_t sr = cpu_enter_critical(); + bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED; motor.armed_state_ = Motor::ARMED_STATE_DISARMED; __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer); cpu_exit_critical(sr); + return was_armed; } // @brief Updates the phase timings unless the motor is disarmed. @@ -303,7 +306,7 @@ void low_level_fault(Motor::Error_t error) { // Disable all motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { safety_critical_disarm_motor_pwm(axes[i]->motor_); - axes[i]->motor_.error_ = error; + axes[i]->motor_.error_ |= error; } safety_critical_disarm_brake_resistor(); @@ -355,7 +358,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { if (!other_axis.motor_.next_timings_valid_) { // the motor control loop failed to update the timings in time // we must assume that it died and therefore float all phases - safety_critical_disarm_motor_pwm(other_axis.motor_); + bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_); + if (was_armed) { + other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED; + } } else { other_axis.motor_.next_timings_valid_ = false; safety_critical_apply_motor_pwm_timings( diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 5f4a7cde..2bd9fe04 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -22,7 +22,7 @@ extern "C" { /* Exported functions --------------------------------------------------------*/ void safety_critical_arm_motor_pwm(Motor& motor); -void safety_critical_disarm_motor_pwm(Motor& motor); +bool safety_critical_disarm_motor_pwm(Motor& motor); void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]); void safety_critical_arm_brake_resistor(); void safety_critical_disarm_brake_resistor(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index cf0e80f9..0229e697 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -39,7 +39,7 @@ bool Motor::arm() { // that we have exactly one full interrupt period until the third trigger. This gives // the control loop the correct time quota to set up modulation timings. if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) - return axis_->error_ = Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; + return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; next_timings_valid_ = false; safety_critical_arm_motor_pwm(*this); return true; @@ -119,7 +119,7 @@ bool Motor::check_DRV_fault() { bool Motor::do_checks() { if (!check_DRV_fault()) { - error_ = ERROR_DRV_FAULT; + error_ |= ERROR_DRV_FAULT; return false; } return true; @@ -162,7 +162,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { float Ialpha = -(current_meas_.phB + current_meas_.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage || test_voltage < -max_voltage) - return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; + return error_ |= ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A if (!enqueue_voltage_timings(test_voltage, 0.0f)) @@ -216,14 +216,12 @@ 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 > 500e-6f) - return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; + return error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; return true; } bool Motor::run_calibration() { - error_ = ERROR_NO_ERROR; - float R_calib_max_voltage = config_.resistance_calib_max_voltage; if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) @@ -245,7 +243,7 @@ bool Motor::run_calibration() { bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0) - return error_ = ERROR_NUMERICAL, false; + return error_ |= ERROR_NUMERICAL, false; next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); @@ -353,7 +351,7 @@ bool Motor::update(float current_setpoint, float phase) { if(!FOC_voltage(0.0f, current_setpoint, phase)) return false; } else { - error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + error_ |= ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; return false; } return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 503b8756..b2a79f88 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -54,14 +54,15 @@ typedef struct { class Motor { public: enum Error_t { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ADC_FAILED, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_BRAKE_CURRENT_OUT_OF_RANGE, - ERROR_NUMERICAL + ERROR_NO_ERROR = 0, + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02, + ERROR_ADC_FAILED = 0x04, + ERROR_DRV_FAULT = 0x08, + ERROR_CONTROL_DEADLINE_MISSED = 0x10, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x20, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x40, + ERROR_NUMERICAL = 0x80 }; enum TimingLog_t { @@ -209,4 +210,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t) + #endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 50bf69c8..1b4b8850 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -41,6 +41,17 @@ extern BoardConfig_t board_config; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; +// TODO: move +// this is technically not thread-safe but practically it might be +#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \ +inline ENUMTYPE operator | (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) | static_cast>(b)); } \ +inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) & static_cast>(b)); } \ +inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) ^ static_cast>(b)); } \ +inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) |= static_cast>(b)); } \ +inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) &= static_cast>(b)); } \ +inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) ^= static_cast>(b)); } \ +inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_cast>(a)); } + // ODrive specific includes #include diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 0fa3c7fe..4ae081f4 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -24,7 +24,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ = ERROR_NUMERICAL; + error_ |= ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 740ae8c1..910bc05a 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -4,8 +4,8 @@ class SensorlessEstimator { public: enum Error_t { - ERROR_NONE, - ERROR_NUMERICAL, + ERROR_NONE = 0, + ERROR_NUMERICAL = 0x01, }; SensorlessEstimator(); @@ -40,4 +40,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(SensorlessEstimator::Error_t) + #endif /* __SENSORLESS_ESTIMATOR_HPP */ From 6719a2dfe054ff2607cd447bbfada9d72dd8c4c6 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 12:52:56 -0700 Subject: [PATCH 035/112] add PROGRAMMER=... parameter to makefile --- Firmware/Makefile | 18 +++++++++++++----- Firmware/find_programmer.sh | 2 ++ 2 files changed, 15 insertions(+), 5 deletions(-) create mode 100755 Firmware/find_programmer.sh diff --git a/Firmware/Makefile b/Firmware/Makefile index 899f043c..921a9e13 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -5,12 +5,21 @@ BUILD_DIR = build FIRMWARE = $(BUILD_DIR)/ODriveFirmware.elf FIRMWARE_HEX = $(BUILD_DIR)/ODriveFirmware.hex +PROGRAMMER_HEX := $(shell echo $(PROGRAMMER) | sed -e 's/.\{2\}/\\x&/g') +OPENOCD := openocd -f interface/stlink-v2.cfg \ + $(if $(value PROGRAMMER),-c 'hla_serial $(PROGRAMMER_HEX)',) \ + -f target/stm32f4x.cfg + all: @tup --quiet --no-environ-check flash: all - openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ write_image\ erase\ $(FIRMWARE) -c reset\ run -c exit + $(OPENOCD) -c init \ + -c 'reset halt' \ + -c 'flash write_image erase $(FIRMWARE)' \ + -c 'reset run' \ + -c exit gdb: all arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit @@ -26,11 +35,11 @@ bmp: all # Erase entire STM32 erase: - openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ erase_address\ 0x8000000\ 0x100000 -c reset\ run -c exit + $(OPENOCD) -c init -c reset\ halt -c flash\ erase_address\ 0x8000000\ 0x100000 -c reset\ run -c exit # Erase all configuration from the ODrive erase_config: - openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ init -c reset\ run -c exit + $(OPENOCD) -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ init -c reset\ run -c exit # The one-time programmable memory stores the board version # has the following format: @@ -51,8 +60,7 @@ erase_config: # [write OTP] write_otp: ifeq ($(ODRV_FACTORY),TRUE) - # Data: - openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg \ + $(OPENOCD) \ -c init \ -c 'reset halt' \ -c 'mww 0x40023C04 0x45670123' \ diff --git a/Firmware/find_programmer.sh b/Firmware/find_programmer.sh new file mode 100755 index 00000000..97a94928 --- /dev/null +++ b/Firmware/find_programmer.sh @@ -0,0 +1,2 @@ +#!/bin/bash +openocd -d3 -f board/stm32f4discovery.cfg -c "hla_serial wrong_serial" 2>&1 | xxd -p | tr -d '\n' | sed -n 's/^.*6e756d6265722027\([0-9a-f]*\)2720646f65736e27.*$/\1/p'; echo From 0b4ecb0581054cbc5a17c5964c25be73937bcd70 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 12:59:14 -0700 Subject: [PATCH 036/112] add motor argument to odrive.utils.print_drv_regs --- tools/odrive/utils.py | 20 ++++++++++---------- tools/odrvtool | 3 ++- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 79c69b01..2acb9430 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -72,21 +72,21 @@ def start_liveplotter(get_var_callback): threading.Thread(target=plot_data).start() #plot_data() -def print_drv_regs(device): +def print_drv_regs(name, motor): """ - Dumps the current gate driver regisers for Motor 0 + Dumps the current gate driver regisers for the specified motor """ - fault = device.motor0.gate_driver.drv_fault - status_reg_1 = device.motor0.gate_driver.status_reg_1 - status_reg_2 = device.motor0.gate_driver.status_reg_2 - ctrl_reg_1 = device.motor0.gate_driver.ctrl_reg_1 - ctrl_reg_2 = device.motor0.gate_driver.ctrl_reg_2 - + fault = motor.gate_driver.drv_fault + status_reg_1 = motor.gate_driver.status_reg_1 + status_reg_2 = motor.gate_driver.status_reg_2 + ctrl_reg_1 = motor.gate_driver.ctrl_reg_1 + ctrl_reg_2 = motor.gate_driver.ctrl_reg_2 + print(name + ": " + str(fault)) print("DRV Fault Code: " + str(fault)) print("Status Reg 1: " + str(status_reg_1) + " (" + format(status_reg_1, '#010b') + ")") print("Status Reg 2: " + str(status_reg_2) + " (" + format(status_reg_2, '#010b') + ")") - print("Control Reg 1: " + str(ctrl_reg_1) + " (" + format(ctrl_reg_1, '#010b') + ")") - print("Control Reg 2: " + str(ctrl_reg_2) + " (" + format(ctrl_reg_2, '#010b') + ")") + print("Control Reg 1: " + str(ctrl_reg_1) + " (" + format(ctrl_reg_1, '#013b') + ")") + print("Control Reg 2: " + str(ctrl_reg_2) + " (" + format(ctrl_reg_2, '#09b') + ")") def rate_test(device): """ diff --git a/tools/odrvtool b/tools/odrvtool index 3e8211d7..0bcc39b7 100755 --- a/tools/odrvtool +++ b/tools/odrvtool @@ -111,7 +111,8 @@ try: from odrive.utils import print_drv_regs print("Waiting for ODrive...") my_odrive = odrive.discovery.find_any(path=args.path, serial_number=args.serial_number) - print_drv_regs(my_odrive) + print_drv_regs("Motor 0", my_odrive.axis0.motor) + print_drv_regs("Motor 1", my_odrive.axis1.motor) elif args.command == 'rate-test': from odrive.utils import rate_test From 1aba0cb51318942fe5007eb0e24085422bb942cb Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 13:00:02 -0700 Subject: [PATCH 037/112] add Axis::ERROR_BRAKE_RESISTOR_DISARMED --- Firmware/MotorControl/axis.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index fe5ff15d..f6415422 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -48,12 +48,13 @@ public: ERROR_DC_BUS_UNDER_VOLTAGE = 0x02, ERROR_DC_BUS_OVER_VOLTAGE = 0x04, ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08, - ERROR_MOTOR_DISARMED = 0x10, // void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { + if (!brake_resistor_armed_) { + error_ |= ERROR_BRAKE_RESISTOR_DISARMED; + break; + } if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { // motor got disarmed in something other than the idle loop error_ |= ERROR_MOTOR_DISARMED; From 359f0c656a15c28344e432e019e1dac6dd026e3d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 13:03:35 -0700 Subject: [PATCH 038/112] make tests work on back-to-back rig, check test preconditions --- tools/odrive/enums.py | 18 +-- tools/odrive/tests.py | 326 +++++++++++++++++++++++++++++++----------- tools/run_tests.py | 121 ++++++++++------ tools/test-rig.yaml | 47 ++++-- 4 files changed, 374 insertions(+), 138 deletions(-) diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index da762bbb..52b1e610 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -13,15 +13,15 @@ AXIS_STATE_CLOSED_LOOP_CONTROL = 8 AXIS_ERROR_NO_ERROR = 0 AXIS_ERROR_INVALID_STATE = 1 -AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2 -AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3 -AXIS_ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4 -AXIS_ERROR_CONTROL_LOOP_TIMEOUT = 5 -AXIS_ERROR_MOTOR_FAILED = 6 -AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED = 7 -AXIS_ERROR_ENCODER_FAILED = 8 -AXIS_ERROR_CONTROLLER_FAILED = 9 -AXIS_ERROR_POS_CTRL_DURING_SENSORLESS = 10 +#AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2 +#AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3 +#AXIS_ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4 +#AXIS_ERROR_CONTROL_LOOP_TIMEOUT = 5 +#AXIS_ERROR_MOTOR_FAILED = 6 +#AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED = 7 +#AXIS_ERROR_ENCODER_FAILED = 8 +#AXIS_ERROR_CONTROLLER_FAILED = 9 +#AXIS_ERROR_POS_CTRL_DURING_SENSORLESS = 10 MOTOR_TYPE_HIGH_CURRENT = 0 #MOTOR_TYPE_LOW_CURRENT = 1 diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 4e9b4398..0cdc121f 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -4,8 +4,10 @@ import shlex import math import time import sys +import threading import odrive.discovery from odrive.enums import * +import odrive.utils import abc ABC = abc.ABC @@ -14,6 +16,36 @@ class TestFailed(Exception): def __init__(self, message): Exception.__init__(self, message) +class PreconditionsNotMet(Exception): + pass + +class ODriveTestContext(): + def __init__(self, name: str, yaml: dict): + self.handle = None + self.yaml = yaml + self.name = name + self.axes = [] + for axis_idx, axis_yaml in enumerate(yaml['axes']): + axis_name = axis_yaml['name'] if 'name' in axis_yaml else '{}.axis{}'.format(name, axis_idx) + self.axes.append(AxisTestContext(axis_name, axis_yaml, self)) + + def rediscover(self): + """ + Reconnects to the ODrive + """ + self.handle = odrive.discovery.find_any( + path="usb", serial_number=self.yaml['serial-number'], timeout=15) + for axis_idx, axis_ctx in enumerate(self.axes): + axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)] + +class AxisTestContext(): + def __init__(self, name: str, yaml: dict, odrv_ctx: ODriveTestContext): + self.handle = None + self.yaml = yaml + self.name = name + self.lock = threading.Lock() + self.odrv_ctx = odrv_ctx + def test_assert_eq(observed, expected, range=None, accuracy=None): if range is None and accuracy is None and observed != expected: raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed)) @@ -22,6 +54,19 @@ def test_assert_eq(observed, expected, range=None, accuracy=None): elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))): raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed)) +def test_assert_no_error(axis_ctx: AxisTestContext): + errors = [] + if axis_ctx.handle.motor.error != 0: + errors.append("motor failed with error {:04X}".format(axis_ctx.handle.motor.error)) + if axis_ctx.handle.encoder.error != 0: + errors.append("encoder failed with error {:04X}".format(axis_ctx.handle.encoder.error)) + if axis_ctx.handle.sensorless_estimator.error != 0: + errors.append("sensorless_estimator failed with error {:04X}".format(axis_ctx.handle.sensorless_estimator.error)) + if axis_ctx.handle.error != 0: + errors.append("axis failed with error {:04X}".format(axis_ctx.handle.error)) + if len(errors) > 0: + raise TestFailed("\n".join(errors)) + def run(command_line, logger, timeout=None): """ Runs a shell command in the current directory @@ -35,24 +80,51 @@ def run(command_line, logger, timeout=None): logger.error(result.stdout.decode(sys.stdout.encoding)) raise TestFailed("command {} failed".format(command_line)) -def rediscover(odrv_yaml): +def request_state(axis_ctx: AxisTestContext, state, expect_success=True): + axis_ctx.handle.requested_state = state + time.sleep(0.001) + if expect_success: + test_assert_eq(axis_ctx.handle.current_state, state) + else: + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_INVALID_STATE) + axis_ctx.handle.error = AXIS_ERROR_NO_ERROR # reset error + +def set_limits(axis_ctx: AxisTestContext, logger, vel_limit=20000, current_limit=10): """ - Connects to the ODrive indicated by odrv_yaml + Sets the velocity and current limits for the axis, subject to the following constraints: + - the arguments given to this function are not exceeded + - max motor current is not exceeded + - max brake resistor power divided by two is not exceeded (here velocity takes precedence over current) """ - odrv = odrive.discovery.find_any(path="usb", serial_number=odrv_yaml['serial-number'], timeout=10) - odrv_yaml['odrv'] = odrv - for axis_idx, axis_yaml in enumerate(odrv_yaml['axes']): - axis_yaml['axis'] = odrv.__dict__['axis{}'.format(axis_idx)] - return odrv + max_rpm = vel_limit / axis_ctx.yaml['encoder-cpr'] * 60 + max_emf_voltage = max_rpm / axis_ctx.yaml['motor-kv'] + max_brake_power = axis_ctx.odrv_ctx.yaml['max-brake-power'] / 2 * 0.8 # 20% safety margin + max_motor_current = max_brake_power / max_emf_voltage + logger.debug("velocity limit = {} => V_emf = {:.3}V, I_lim = {:.3}A".format(vel_limit, max_emf_voltage, max_motor_current)) + + # Bound current limit based on the motor's current limit and the brake resistor current limit + current_limit = min(current_limit, axis_ctx.yaml['motor-max-current'], max_motor_current) + # TODO: set as an atomic operation + axis_ctx.handle.motor.config.current_lim = current_limit + axis_ctx.handle.controller.config.vel_limit = vel_limit + class ODriveTest(ABC): """ Tests inheriting from this class get full ownership of the ODrive being tested. However no guarantees are made for the mechanical state of the axes. + The test can demand exclusive run time which means that the host will + not run any other test at the same time. This can be used if the test + invokes a command that's so lame that it can't run twice concurrently. """ + def __init__(self, exclusive=False): + self._exclusive = exclusive + def check_preconditions(self, odrv_ctx: ODriveTestContext, logger): + pass @abc.abstractmethod - def run_test(self, odrv, odrv_config, logger): + def run_test(self, odrv_ctx: ODriveTestContext, logger): pass class AxisTest(ABC): @@ -62,8 +134,16 @@ class AxisTest(ABC): axis, the other axis is guaranteed to be disabled (high impedance) during this test. """ + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + if (abs(axis_ctx.handle.encoder.pll_vel) > 500): + logger.warn("axis still in motion, delaying 2 sec...") + time.sleep(2) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) + @abc.abstractmethod - def run_test(self, axis, axis_config, logger): + def run_test(self, axis_ctx: AxisTestContext, logger): pass class DualAxisTest(ABC): @@ -71,30 +151,48 @@ class DualAxisTest(ABC): Tests using this scope get ownership of two axes that are mechanically coupled. """ + def check_preconditions(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + test_assert_no_error(axis0_ctx) + test_assert_no_error(axis1_ctx) + test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=1000) + test_assert_eq(axis1_ctx.handle.encoder.pll_vel, 0, range=1000) + @abc.abstractmethod - def run_test(self, axis0, axis0_config, axis1, axis1_config, logger): + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): pass +class TestDiscoverAndGotoIdle(ODriveTest): + def run_test(self, odrv_ctx: ODriveTestContext, logger): + odrv_ctx.rediscover() + odrv_ctx.axes[0].handle.error = 0 + odrv_ctx.axes[1].handle.error = 0 + request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE) + request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE) + class TestFlashAndErase(ODriveTest): - def run_test(self, odrv, odrv_config, logger): - run("make flash PROGRAMMER='" + odrv_config['programmer'] + "'", logger, timeout=20) + def __init__(self): + ODriveTest.__init__(self, exclusive=True) + def run_test(self, odrv_ctx: ODriveTestContext, logger): + run("make flash PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=20) # FIXME: device does not reboot correctly after erasing config this way #run("make erase_config PROGRAMMER='" + test_rig.programmer + "'", timeout=10) logger.debug("waiting for ODrive...") - odrv = rediscover(odrv_config) + odrv_ctx.rediscover() # ensure the correct odrive is returned - test_assert_eq(format(odrv.serial_number, 'x').upper(), odrv_config['serial-number']) + test_assert_eq(format(odrv_ctx.handle.serial_number, 'x').upper(), odrv_ctx.yaml['serial-number']) # erase configuration and reboot logger.debug("erasing old configuration...") - odrv.erase_configuration() + odrv_ctx.handle.erase_configuration() #time.sleep(0.1) try: # FIXME: sometimes the device does not reappear after this ("no response - probably incompatible") # this is a firmware issue since it persists when unplugging/replugging # but goes away when power cycling the device - odrv.reboot() + odrv_ctx.handle.reboot() except odrive.protocol.ChannelBrokenException: pass # this is expected time.sleep(0.5) @@ -103,36 +201,26 @@ class TestSetup(ODriveTest): """ Preconditions: ODrive is unconfigured and just rebooted """ - def run_test(self, odrv, odrv_config, logger): - odrv = rediscover(odrv_config) + def run_test(self, odrv_ctx: ODriveTestContext, logger): + odrv_ctx.rediscover() # initial protocol tests and setup logger.debug("setting up ODrive...") - odrv.config.enable_uart = True - test_assert_eq(odrv.config.enable_uart, True) - odrv.config.enable_uart = False - test_assert_eq(odrv.config.enable_uart, False) - odrv.config.brake_resistance = 1.0 - test_assert_eq(odrv.config.brake_resistance, 1.0) - odrv.config.brake_resistance = odrv_config['brake-resistance'] - test_assert_eq(odrv.config.brake_resistance, odrv_config['brake-resistance'], accuracy=0.01) + odrv_ctx.handle.config.enable_uart = True + test_assert_eq(odrv_ctx.handle.config.enable_uart, True) + odrv_ctx.handle.config.enable_uart = False + test_assert_eq(odrv_ctx.handle.config.enable_uart, False) + odrv_ctx.handle.config.brake_resistance = 1.0 + test_assert_eq(odrv_ctx.handle.config.brake_resistance, 1.0) + odrv_ctx.handle.config.brake_resistance = odrv_ctx.yaml['brake-resistance'] + test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) # firmware has 1500ms startup delay time.sleep(2) logger.debug("ensure we're in idle state") - test_assert_eq(odrv.axis0.current_state, AXIS_STATE_IDLE) - test_assert_eq(odrv.axis1.current_state, AXIS_STATE_IDLE) - -def request_state(axis, state, expect_success=True): - axis.requested_state = state - time.sleep(0.001) - if expect_success: - test_assert_eq(axis.current_state, state) - else: - test_assert_eq(axis.current_state, AXIS_STATE_IDLE) - test_assert_eq(axis.error, AXIS_ERROR_INVALID_STATE) - axis.error = AXIS_ERROR_NO_ERROR # reset error + test_assert_eq(odrv_ctx.handle.axis0.current_state, AXIS_STATE_IDLE) + test_assert_eq(odrv_ctx.handle.axis1.current_state, AXIS_STATE_IDLE) class TestMotorCalibration(AxisTest): """ @@ -141,25 +229,29 @@ class TestMotorCalibration(AxisTest): Preconditions: The motor must be uncalibrated. Postconditions: The motor will be calibrated after this test. """ - def run_test(self, axis, axis_config, logger): + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestMotorCalibration, self).check_preconditions(axis_ctx, logger) + test_assert_eq(axis_ctx.handle.motor.is_calibrated, False) + + def run_test(self, axis_ctx: AxisTestContext, logger): logger.debug("try to enter closed loop control (should be rejected)") - request_state(axis, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) logger.debug("try to start encoder index search (should be rejected)") - request_state(axis, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False) + request_state(axis_ctx, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False) logger.debug("try to start encoder offset calibration (should be rejected)") - request_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False) + request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False) logger.debug("motor calibration (takes about 4.5 seconds)") - axis.motor.config.pole_pairs = axis_config['motor-pole-pairs'] - request_state(axis, AXIS_STATE_MOTOR_CALIBRATION) + axis_ctx.handle.motor.config.pole_pairs = axis_ctx.yaml['motor-pole-pairs'] + request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION) time.sleep(6) - test_assert_eq(axis.current_state, AXIS_STATE_IDLE) - test_assert_eq(axis.error, AXIS_ERROR_NO_ERROR) - test_assert_eq(axis.motor.config.phase_resistance, axis_config['motor-phase-resistance'], accuracy=0.1) - test_assert_eq(axis.motor.config.phase_inductance, axis_config['motor-phase-inductance'], accuracy=0.5) - axis.motor.config.pre_calibrated = True + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_NO_ERROR) + test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2) + test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) + axis_ctx.handle.motor.config.pre_calibrated = True class TestEncoderOffsetCalibration(AxisTest): """ @@ -167,19 +259,32 @@ class TestEncoderOffsetCalibration(AxisTest): Preconditions: The encoder must be non-ready. Postconditions: The encoder will be ready after this test. """ - def run_test(self, axis, axis_config, logger): + def __init__(self, pass_if_ready=False): + AxisTest.__init__(self) + self._pass_if_ready = pass_if_ready + + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestEncoderOffsetCalibration, self).check_preconditions(axis_ctx, logger) + if not self._pass_if_ready: + test_assert_eq(axis_ctx.handle.encoder.is_ready, False) + + def run_test(self, axis_ctx: AxisTestContext, logger): + if (self._pass_if_ready and axis_ctx.handle.encoder.is_ready): + logger.debug("encoder already ready, skipping this test") + return + logger.debug("try to enter closed loop control (should be rejected)") - request_state(axis, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) logger.debug("encoder offset calibration (takes about 9.5 seconds)") - axis.encoder.config.cpr = axis_config['encoder-cpr'] # TODO: test setting a wrong CPR - request_state(axis, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) + axis_ctx.handle.encoder.config.cpr = axis_ctx.yaml['encoder-cpr'] # TODO: test setting a wrong CPR + request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) # TODO: ensure the encoder calibration doesn't do crap time.sleep(11) - test_assert_eq(axis.current_state, AXIS_STATE_IDLE) - test_assert_eq(axis.error, AXIS_ERROR_NO_ERROR) - test_assert_eq(axis.motor.config.direction, axis_config['motor-direction']) - axis.encoder.config.pre_calibrated = True + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_NO_ERROR) + test_assert_eq(axis_ctx.handle.motor.config.direction, axis_ctx.yaml['motor-direction']) + axis_ctx.handle.encoder.config.pre_calibrated = True class TestClosedLoopControl(AxisTest): """ @@ -187,49 +292,108 @@ class TestClosedLoopControl(AxisTest): and verifies that the sensorless estimator works Precondition: The axis is calibrated and ready for closed loop control """ - def run_test(self, axis, axis_config, logger): + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestClosedLoopControl, self).check_preconditions(axis_ctx, logger) + test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) + test_assert_eq(axis_ctx.handle.encoder.is_ready, True) + + def run_test(self, axis_ctx: AxisTestContext, logger): logger.debug("closed loop control: test tiny position changes") - axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + axis_ctx.handle.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL time.sleep(0.001) - test_assert_eq(axis.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) time.sleep(0.1) # give the PLL some time to settle - test_assert_eq(axis.encoder.pll_pos, 0, range=300) - axis.controller.set_pos_setpoint(1000, 0, 0) + init_pos = axis_ctx.handle.encoder.pll_pos + axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0) time.sleep(0.5) - test_assert_eq(axis.encoder.pll_pos, 1000, range=200) - axis.controller.set_pos_setpoint(-1000, 0, 0) + test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos+1000, range=200) + axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0) time.sleep(0.5) - test_assert_eq(axis.encoder.pll_pos, -1000, range=200) + test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos-1000, range=400) logger.debug("closed loop control: test vel_limit") - axis.controller.set_pos_setpoint(50000, 0, 0) - axis.controller.config.vel_limit = 40000 + axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0) + axis_ctx.handle.controller.config.vel_limit = 40000 time.sleep(0.3) - test_assert_eq(axis.encoder.pll_vel, 40000, range=4000) - expected_sensorless_estimation = 40000 * 2 * math.pi / axis_config['encoder-cpr'] * axis_config['motor-pole-pairs'] - test_assert_eq(axis.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 40000, range=4000) + expected_sensorless_estimation = 40000 * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs'] + test_assert_eq(axis_ctx.handle.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50) time.sleep(3) - test_assert_eq(axis.encoder.pll_vel, 0, range=1000) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=1000) + time.sleep(0.5) + request_state(axis_ctx, AXIS_STATE_IDLE) class TestStoreAndReboot(ODriveTest): """ Stores the current configuration to NVM and reboots. """ - def run_test(self, odrv, odrv_config, logger): + def run_test(self, odrv_ctx: ODriveTestContext, logger): logger.debug("storing configuration and rebooting...") - odrv.save_configuration() + odrv_ctx.handle.save_configuration() try: - odrv.reboot() + odrv_ctx.handle.reboot() except odrive.protocol.ChannelBrokenException: pass # this is expected time.sleep(2) - odrv = rediscover(odrv_config) + odrv_ctx.rediscover() logger.debug("verifying configuration after reboot...") - test_assert_eq(odrv.config.brake_resistance, odrv_config['brake-resistance'], accuracy=0.01) - for axis_config in odrv_config['axes']: - axis = axis_config['axis'] - test_assert_eq(axis.encoder.config.cpr, axis_config['encoder-cpr']) - test_assert_eq(axis.motor.config.phase_resistance, axis_config['motor-phase-resistance'], accuracy=0.1) - test_assert_eq(axis.motor.config.phase_inductance, axis_config['motor-phase-inductance'], accuracy=0.5) + test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) + for axis_ctx in odrv_ctx.axes: + test_assert_eq(axis_ctx.handle.encoder.config.cpr, axis_ctx.yaml['encoder-cpr']) + test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.15) + test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) + +class TestVelCtrlVsPosCtrl(DualAxisTest): + """ + Uses one ODrive as a load operating in velocity control mode. + The other ODrive tries to "fight" against the load in position mode. + """ + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + load_ctx = axis0_ctx + driver_ctx = axis1_ctx + + # Set up viscous fluid load + logger.debug("activating load on {}...".format(load_ctx.name)) + load_ctx.handle.controller.config.vel_integrator_gain = 0 + load_ctx.handle.controller.vel_integrator_current = 0 + set_limits(load_ctx, logger, vel_limit=100000, current_limit=50) + load_ctx.handle.controller.set_vel_setpoint(0, 0) + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Turn to some position + logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name)) + set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50) + init_pos = driver_ctx.handle.encoder.pll_pos + driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) + request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + for _ in range(int(4000/5)): + logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint)) + time.sleep(0.005) + + test_assert_no_error(load_ctx) + test_assert_no_error(driver_ctx) + + logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name)) + set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50) + init_pos = driver_ctx.handle.encoder.pll_pos + driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) + request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + #for _ in range(int(5*4000/5)): + # logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint)) + # time.sleep(0.005) + time.sleep(7) + + odrive.utils.print_drv_regs("load motor ({})".format(load_ctx.name), load_ctx.handle.motor) + odrive.utils.print_drv_regs("driver motor ({})".format(driver_ctx.name), driver_ctx.handle.motor) + + test_assert_no_error(load_ctx) + test_assert_no_error(driver_ctx) + + ## Turn to another position + #logger.debug("controlling against load, vel=40000...") + #set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20) + #init_pos = axis1_ctx.handle.encoder.pll_pos + #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) + #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/run_tests.py b/tools/run_tests.py index 463d6939..968000e6 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -17,15 +17,19 @@ from odrive.utils import Logger, for_all_parallel all_tests = [ - TestFlashAndErase(), - TestSetup(), - TestMotorCalibration(), - # TODO: test encoder index search - TestEncoderOffsetCalibration(), - TestClosedLoopControl(), - TestStoreAndReboot(), - TestEncoderOffsetCalibration(), # need to find offset _or_ index after reboot - TestClosedLoopControl() +# TestFlashAndErase(), +# TestSetup(), +# TestMotorCalibration(), +# # TODO: test encoder index search +# TestEncoderOffsetCalibration(), +# # TODO: hold down one motor while the other one does an index search (should fail) +# TestClosedLoopControl(), +# TestStoreAndReboot(), +# TestEncoderOffsetCalibration(), # need to find offset _or_ index after reboot +# TestClosedLoopControl(), + TestDiscoverAndGotoIdle(), # for testing + TestEncoderOffsetCalibration(pass_if_ready=True), + TestVelCtrlVsPosCtrl() # TODO: test step/dir # TODO: test sensorless # TODO: test ASCII protocol @@ -41,65 +45,101 @@ with open(script_path + '/test-rig.yaml', 'r') as file_stream: os.chdir(script_path + '/../Firmware') -# Ensure every device has a name -for idx, odrv_yaml in enumerate(test_rig_yaml['odrives']): - if not 'name' in odrv_yaml: - odrv_yaml['name'] = 'odrive{}'.format(idx) +# Build a dictionary of odrive test contexts by name +odrives_by_name = {} +for odrv_idx, odrv_yaml in enumerate(test_rig_yaml['odrives']): + name = odrv_yaml['name'] if 'name' in odrv_yaml else 'odrive{}'.format(odrv_idx) + odrives_by_name[name] = ODriveTestContext(name, odrv_yaml) -# Build a dictionary of axes by name (e.g. odrive0.axis0) -# Also ensure every axis has a name and mutex +# Build a dictionary of axis test contexts by name (e.g. odrive0.axis0) axes_by_name = {} -for odrv_yaml in test_rig_yaml['odrives']: - for axis_idx, axis_yaml in enumerate(odrv_yaml['axes']): - if not 'name' in axis_yaml: - axis_yaml['name'] = '{}.axis{}'.format(odrv_yaml['name'], axis_idx) - axis_yaml['lock'] = threading.Lock() - axes_by_name[axis_yaml['name']] = axis_yaml +for odrv_ctx in odrives_by_name.values(): + for axis_idx, axis_ctx in enumerate(odrv_ctx.axes): + axes_by_name[axis_ctx.name] = axis_ctx # Ensure mechanical couplings are valid +couplings = [] if test_rig_yaml['couplings'] is None: test_rig_yaml['couplings'] = {} else: - for axis in sum(test_rig_yaml['couplings'], []): - if not axis in axes_by_name: - logger.error('Unknown axis {} in list of mechanical couplings'.format(axis)) + for coupling in test_rig_yaml['couplings']: + couplings.append([axes_by_name[axis_name] for axis_name in coupling]) try: for test in all_tests: if isinstance(test, ODriveTest): - def odrv_test_thread(odrv_yaml): - test_subject_name = odrv_yaml['name'] - logger.info('● running {} on {}...'.format(type(test).__name__, test_subject_name)) - odrv = odrv_yaml['odrv'] if 'odrv' in odrv_yaml else None - test.run_test(odrv, odrv_yaml, - logger.indent(' {}: '.format(test_subject_name))) + def odrv_test_thread(odrv_name): + odrv_ctx = odrives_by_name[odrv_name] + logger.info('● running {} on {}...'.format(type(test).__name__, odrv_name)) + try: + test.check_preconditions(odrv_ctx, + logger.indent(' {}: '.format(odrv_name))) + except: + raise PreconditionsNotMet() + test.run_test(odrv_ctx, + logger.indent(' {}: '.format(odrv_name))) - for_all_parallel(test_rig_yaml['odrives'], lambda x: x['name'], odrv_test_thread) + if test._exclusive: + for odrv in odrives_by_name: + odrv_test_thread(odrv) + else: + for_all_parallel(odrives_by_name, lambda x: x, odrv_test_thread) elif isinstance(test, AxisTest): def axis_test_thread(axis_name): # Get all axes that are mechanically coupled with the axis specified by axis_name - conflicting_axes = sum([c for c in test_rig_yaml['couplings'] if (axis_name in c)], []) + conflicting_axes = sum([c for c in couplings if (axis_name in [a.name for a in c])], []) # Remove duplicates conflicting_axes = list(set(conflicting_axes)) # Acquire lock for all conflicting axes - conflicting_axes.sort() # prevent deadlocks + conflicting_axes.sort(key=lambda x: x.name) # prevent deadlocks + axis_ctx = axes_by_name[axis_name] for conflicting_axis in conflicting_axes: - axes_by_name[conflicting_axis]['lock'].acquire() + conflicting_axis.lock.acquire() try: # Run test on this axis logger.info('● running {} on {}...'.format(type(test).__name__, axis_name)) - axis_yaml = axes_by_name[axis_name] - test.run_test(axis_yaml['axis'], axis_yaml, + try: + test.check_preconditions(axis_ctx, + logger.indent(' {}: '.format(axis_name))) + except: + raise PreconditionsNotMet() + test.run_test(axis_ctx, logger.indent(' {}: '.format(axis_name))) finally: # Release all conflicting axes for conflicting_axis in conflicting_axes: - axes_by_name[conflicting_axis]['lock'].release() + conflicting_axis.lock.release() for_all_parallel(axes_by_name, lambda x: x, axis_test_thread) + elif isinstance(test, DualAxisTest): + def dual_axis_test_thread(coupling): + coupling_name = "...".join([a.name for a in coupling]) + # Remove duplicates + coupled_axes = list(set(coupling)) + # Acquire lock for all conflicting axes + coupled_axes.sort(key=lambda x: x.name) # prevent deadlocks + for axis_ctx in coupled_axes: + axis_ctx.lock.acquire() + try: + # Run test on this axis + logger.info('● running {} on {}...'.format(type(test).__name__, coupling_name)) + try: + test.check_preconditions(coupled_axes[0], coupled_axes[1], + logger.indent(' {}: '.format(coupling_name))) + except: + raise PreconditionsNotMet() + test.run_test(coupled_axes[0], coupled_axes[1], + logger.indent(' {}: '.format(coupling_name))) + finally: + # Release all conflicting axes + for axis_ctx in coupled_axes: + axis_ctx.lock.release() + + for_all_parallel(couplings, lambda x: "..".join([a.name for a in x]), dual_axis_test_thread) + else: logger.warn("ignoring unknown test type {}".format(type(test))) @@ -109,9 +149,10 @@ except: try: dont_secure_after_failure = True # TODO: disable if not dont_secure_after_failure: - def odrv_reset_thread(odrv_yaml): - run("make erase PROGRAMMER='" + odrv_yaml['programmer'] + "'", logger, timeout=30) - for_all_parallel(test_rig_yaml['odrives'], lambda x: x['name'], odrv_reset_thread) + def odrv_reset_thread(odrv_name): + odrv_ctx = odrives_by_name[odrv_name] + run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30) + for_all_parallel(odrives_by_name, lambda x: x['name'], odrv_reset_thread) except: logger.error('///////////////////////////////////////////') logger.error('/// CRITICAL: COULD NOT SECURE TEST RIG ///') diff --git a/tools/test-rig.yaml b/tools/test-rig.yaml index 875898b3..05e26696 100644 --- a/tools/test-rig.yaml +++ b/tools/test-rig.yaml @@ -1,25 +1,56 @@ # ODrives odrives: - - board-version: v3.4-24V + - name: top-odrive + board-version: v3.4-24V serial-number: "385F324D3037" brake-resistance: 0.47 - uart: /dev/serial/by-id/... + uart: /dev/serial/by-id/[not-yet-used] usb: auto - programmer: /dev... + programmer: '533f7506493f49514454193f' + vbus-voltage: 12 # [V] + max-brake-power: 150 # [W] axes: - - motor-phase-resistance: 0.033 - motor-phase-inductance: 1.6e-05 + - motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 motor-pole-pairs: 7 - motor-direction: 1 + motor-direction: -1 + motor-kv: 190 + motor-max-current: 50 encoder-cpr: 8192 - motor-phase-resistance: 0.028 motor-phase-inductance: 1.6e-05 motor-pole-pairs: 7 motor-direction: -1 + motor-kv: 270 + motor-max-current: 50 + encoder-cpr: 8192 + - name: bottom-odrive + board-version: v3.4-48V + serial-number: "306A396A3235" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '493f6f06493f56540929113f' + vbus-voltage: 12 # [V] + max-brake-power: 150 # [W] + axes: + - motor-phase-resistance: 0.0253 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 270 + motor-max-current: 50 + encoder-cpr: 8192 + - motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 190 + motor-max-current: 50 encoder-cpr: 8192 # Mechanical couplings couplings: - #- [ odrive0.axis0, odrive1.axis0 ] - #- [ odrive0.axis1, odrive1.axis1 ] + - [ top-odrive.axis0, bottom-odrive.axis1 ] + - [ top-odrive.axis1, bottom-odrive.axis0 ] From ae9a208f1fe4a84d5389d075df9b3158bca6ae43 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Thu, 5 Apr 2018 22:27:20 -0700 Subject: [PATCH 039/112] turn error enums into flags --- Firmware/MotorControl/axis.cpp | 25 +++++++------ Firmware/MotorControl/axis.hpp | 37 ++++++++++--------- Firmware/MotorControl/encoder.cpp | 6 +-- Firmware/MotorControl/encoder.hpp | 10 +++-- Firmware/MotorControl/low_level.cpp | 12 ++++-- Firmware/MotorControl/low_level.h | 2 +- Firmware/MotorControl/motor.cpp | 14 +++---- Firmware/MotorControl/motor.hpp | 19 ++++++---- Firmware/MotorControl/odrive_main.hpp | 11 ++++++ .../MotorControl/sensorless_estimator.cpp | 2 +- .../MotorControl/sensorless_estimator.hpp | 6 ++- 11 files changed, 85 insertions(+), 59 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index f9696284..fe3760aa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -101,9 +101,9 @@ bool Axis::check_PSU_brownout() { // Sets error and returns false otherwise. bool Axis::do_checks() { if (!motor_.do_checks()) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; if (!check_PSU_brownout()) - return error_ = ERROR_DC_BUS_UNDER_VOLTAGE, false; + return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false; return true; } @@ -115,7 +115,7 @@ bool Axis::run_sensorless_spin_up() { float I_mag = config_.spin_up_current * x; x += current_meas_period / config_.ramp_up_time; if (!motor_.update(I_mag, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return x < 1.0f; }); if (error_ != ERROR_NO_ERROR) @@ -129,7 +129,7 @@ bool Axis::run_sensorless_spin_up() { 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 error_ |= ERROR_MOTOR_FAILED, false; return vel < config_.spin_up_target_vel; }); return error_ == ERROR_NO_ERROR; @@ -142,16 +142,16 @@ bool Axis::run_sensorless_control_loop() { float pos_estimate, vel_estimate, phase, current_setpoint; if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) - return error_ = ERROR_POS_CTRL_DURING_SENSORLESS, false; + return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; // We update the encoder just in case someone needs the output for testing encoder_.update(nullptr, nullptr, nullptr); if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase)) - return error_ = ERROR_SENSORLESS_ESTIMATOR_FAILED, false; + return error_ |= ERROR_SENSORLESS_ESTIMATOR_FAILED, false; if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error_ = ERROR_CONTROLLER_FAILED, false; + return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return true; }); set_step_dir_enabled(false); @@ -166,11 +166,11 @@ bool Axis::run_closed_loop_control_loop() { // We update the sensorless estimator just in case someone needs the output for testing sensorless_estimator_.update(nullptr, nullptr, nullptr); if (!encoder_.update(&pos_estimate, &vel_estimate, &phase)) - return error_ = ERROR_ENCODER_FAILED, false; + return error_ |= ERROR_ENCODER_FAILED, false; if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) - return error_ = ERROR_CONTROLLER_FAILED, false; + return error_ |= ERROR_CONTROLLER_FAILED, false; if (!motor_.update(current_setpoint, phase)) - return error_ = ERROR_MOTOR_FAILED, false; + return error_ |= ERROR_MOTOR_FAILED, false; return true; }); set_step_dir_enabled(false); @@ -180,6 +180,7 @@ bool Axis::run_closed_loop_control_loop() { bool Axis::run_idle_loop() { // run_control_loop ignores missed modulation timing updates // if and only if we're in AXIS_STATE_IDLE + safety_critical_disarm_motor_pwm(motor_); run_control_loop([this](){ sensorless_estimator_.update(nullptr, nullptr, nullptr); encoder_.update(nullptr, nullptr, nullptr); @@ -276,7 +277,7 @@ void Axis::run_state_machine_loop() { break; default: - error_ = ERROR_INVALID_STATE; + error_ |= ERROR_INVALID_STATE; status = false; // this will set the state to idle break; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c7f909e0..fe5ff15d 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -43,17 +43,17 @@ struct AxisConfig_t { class Axis { public: enum Error_t { - ERROR_NO_ERROR = 0, - ERROR_INVALID_STATE = 1, // void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { - if (motor_.error_ != Motor::ERROR_NO_ERROR) { - error_ = ERROR_MOTOR_FAILED; - break; - } if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { // motor got disarmed in something other than the idle loop - error_ = ERROR_CONTROL_LOOP_TIMEOUT; + error_ |= ERROR_MOTOR_DISARMED; + break; + } + if (motor_.error_ != Motor::ERROR_NO_ERROR) { + error_ |= ERROR_MOTOR_FAILED; break; } @@ -127,7 +127,7 @@ public: // safe and float the phases safety_critical_disarm_motor_pwm(motor_); update_brake_current(); - error_ = ERROR_CURRENT_MEASUREMENT_TIMEOUT; + error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } } @@ -190,4 +190,7 @@ public: } }; + +DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t) + #endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 8fae0d95..19a39792 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -150,7 +150,7 @@ bool Encoder::run_offset_calibration() { float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val); if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range) { - error_ = ERROR_CPR_OUT_OF_RANGE; + error_ |= ERROR_CPR_OUT_OF_RANGE; return false; } // check direction @@ -162,7 +162,7 @@ bool Encoder::run_offset_calibration() { axis_->motor_.config_.direction = -1; } else { // Encoder response error - error_ = ERROR_RESPONSE; + error_ |= ERROR_RESPONSE; return false; } @@ -192,7 +192,7 @@ bool Encoder::run_offset_calibration() { bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ = ERROR_NUMERICAL; + error_ |= ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 2edf5be0..ae4b3c0e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -22,10 +22,10 @@ struct EncoderConfig_t { class Encoder { public: enum Error_t { - ERROR_NONE, - ERROR_NUMERICAL, - ERROR_CPR_OUT_OF_RANGE, - ERROR_RESPONSE, + ERROR_NONE = 0, + ERROR_NUMERICAL = 0x01, + ERROR_CPR_OUT_OF_RANGE = 0x02, + ERROR_RESPONSE = 0x04, }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -83,4 +83,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t) + #endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 01f8a392..98ac5620 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -110,11 +110,14 @@ void safety_critical_arm_motor_pwm(Motor& motor) { // After calling this function, it is guaranteed that all three // motor phases are floating and will not be enabled again until // safety_critical_arm_motor_phases is called. -void safety_critical_disarm_motor_pwm(Motor& motor) { +// @returns true if the motor was in a state other than disarmed before +bool safety_critical_disarm_motor_pwm(Motor& motor) { uint8_t sr = cpu_enter_critical(); + bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED; motor.armed_state_ = Motor::ARMED_STATE_DISARMED; __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer); cpu_exit_critical(sr); + return was_armed; } // @brief Updates the phase timings unless the motor is disarmed. @@ -303,7 +306,7 @@ void low_level_fault(Motor::Error_t error) { // Disable all motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { safety_critical_disarm_motor_pwm(axes[i]->motor_); - axes[i]->motor_.error_ = error; + axes[i]->motor_.error_ |= error; } safety_critical_disarm_brake_resistor(); @@ -355,7 +358,10 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { if (!other_axis.motor_.next_timings_valid_) { // the motor control loop failed to update the timings in time // we must assume that it died and therefore float all phases - safety_critical_disarm_motor_pwm(other_axis.motor_); + bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_); + if (was_armed) { + other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED; + } } else { other_axis.motor_.next_timings_valid_ = false; safety_critical_apply_motor_pwm_timings( diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 5f4a7cde..2bd9fe04 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -22,7 +22,7 @@ extern "C" { /* Exported functions --------------------------------------------------------*/ void safety_critical_arm_motor_pwm(Motor& motor); -void safety_critical_disarm_motor_pwm(Motor& motor); +bool safety_critical_disarm_motor_pwm(Motor& motor); void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]); void safety_critical_arm_brake_resistor(); void safety_critical_disarm_brake_resistor(); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index cf0e80f9..0229e697 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -39,7 +39,7 @@ bool Motor::arm() { // that we have exactly one full interrupt period until the third trigger. This gives // the control loop the correct time quota to set up modulation timings. if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) - return axis_->error_ = Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; + return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; next_timings_valid_ = false; safety_critical_arm_motor_pwm(*this); return true; @@ -119,7 +119,7 @@ bool Motor::check_DRV_fault() { bool Motor::do_checks() { if (!check_DRV_fault()) { - error_ = ERROR_DRV_FAULT; + error_ |= ERROR_DRV_FAULT; return false; } return true; @@ -162,7 +162,7 @@ bool Motor::measure_phase_resistance(float test_current, float max_voltage) { float Ialpha = -(current_meas_.phB + current_meas_.phC); test_voltage += (kI * current_meas_period) * (test_current - Ialpha); if (test_voltage > max_voltage || test_voltage < -max_voltage) - return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; + return error_ |= ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; // Test voltage along phase A if (!enqueue_voltage_timings(test_voltage, 0.0f)) @@ -216,14 +216,12 @@ 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 > 500e-6f) - return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; + return error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; return true; } bool Motor::run_calibration() { - error_ = ERROR_NO_ERROR; - float R_calib_max_voltage = config_.resistance_calib_max_voltage; if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) @@ -245,7 +243,7 @@ bool Motor::run_calibration() { bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { float tA, tB, tC; if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0) - return error_ = ERROR_NUMERICAL, false; + return error_ |= ERROR_NUMERICAL, false; next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); @@ -353,7 +351,7 @@ bool Motor::update(float current_setpoint, float phase) { if(!FOC_voltage(0.0f, current_setpoint, phase)) return false; } else { - error_ = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + error_ |= ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; return false; } return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 503b8756..b2a79f88 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -54,14 +54,15 @@ typedef struct { class Motor { public: enum Error_t { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ADC_FAILED, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_BRAKE_CURRENT_OUT_OF_RANGE, - ERROR_NUMERICAL + ERROR_NO_ERROR = 0, + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02, + ERROR_ADC_FAILED = 0x04, + ERROR_DRV_FAULT = 0x08, + ERROR_CONTROL_DEADLINE_MISSED = 0x10, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x20, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x40, + ERROR_NUMERICAL = 0x80 }; enum TimingLog_t { @@ -209,4 +210,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t) + #endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 50bf69c8..1b4b8850 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -41,6 +41,17 @@ extern BoardConfig_t board_config; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; +// TODO: move +// this is technically not thread-safe but practically it might be +#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \ +inline ENUMTYPE operator | (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) | static_cast>(b)); } \ +inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) & static_cast>(b)); } \ +inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) ^ static_cast>(b)); } \ +inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) |= static_cast>(b)); } \ +inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) &= static_cast>(b)); } \ +inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) ^= static_cast>(b)); } \ +inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_cast>(a)); } + // ODrive specific includes #include diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 0fa3c7fe..4ae081f4 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -24,7 +24,7 @@ bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float // Check that we don't get problems with discrete time approximation if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ = ERROR_NUMERICAL; + error_ |= ERROR_NUMERICAL; return false; } diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 740ae8c1..910bc05a 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -4,8 +4,8 @@ class SensorlessEstimator { public: enum Error_t { - ERROR_NONE, - ERROR_NUMERICAL, + ERROR_NONE = 0, + ERROR_NUMERICAL = 0x01, }; SensorlessEstimator(); @@ -40,4 +40,6 @@ public: } }; +DEFINE_ENUM_FLAG_OPERATORS(SensorlessEstimator::Error_t) + #endif /* __SENSORLESS_ESTIMATOR_HPP */ From 98e24f3eb63dcc9d627d264d25fb4a2809bbc1d3 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 13:00:02 -0700 Subject: [PATCH 040/112] add Axis::ERROR_BRAKE_RESISTOR_DISARMED --- Firmware/MotorControl/axis.hpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index fe5ff15d..f6415422 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -48,12 +48,13 @@ public: ERROR_DC_BUS_UNDER_VOLTAGE = 0x02, ERROR_DC_BUS_OVER_VOLTAGE = 0x04, ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08, - ERROR_MOTOR_DISARMED = 0x10, // void run_control_loop(const T& update_handler) { while (requested_state_ == AXIS_STATE_UNDEFINED) { + if (!brake_resistor_armed_) { + error_ |= ERROR_BRAKE_RESISTOR_DISARMED; + break; + } if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) { // motor got disarmed in something other than the idle loop error_ |= ERROR_MOTOR_DISARMED; From bd2bcd401b524523a9c1e139f551e0c5a3265337 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 21:05:48 -0700 Subject: [PATCH 041/112] add usb_burn_in_test --- tools/odrive/utils.py | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 2acb9430..9852a3f2 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -109,6 +109,27 @@ def rate_test(device): FramePerSec = loopsPerSec/loopsPerFrame print("Frames per second: " + str(FramePerSec)) +def usb_burn_in_test(get_var_callback, cancellation_token): + """ + Starts background threads that read a values form the USB device in a spin-loop + """ + + def fetch_data(): + global vals + i = 0 + while not cancellation_token.is_set(): + try: + get_var_callback() + i += 1 + except Exception as ex: + print(str(ex)) + time.sleep(1) + i = 0 + continue + if i % 1000 == 0: + print("read {} values".format(i)) + threading.Thread(target=fetch_data).start() + ## Exceptions ## From f286bd5a3db0924313f0bfd1f5f777ac11ddfe73 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 21:17:13 -0700 Subject: [PATCH 042/112] if USB fails, call set_configuration instead of clear_halt set_configuration acts as a sort of soft reset, clearing halt conditions along the way. More details: http://libusb.sourceforge.net/api-1.0/group__dev.html#ga186593ecae576dad6cd9679f45a2aa43 --- tools/odrive/usbbulk_transport.py | 31 +++++++++++++++++-------------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index e1004ba8..fbfcfcee 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -19,6 +19,7 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) def __init__(self, dev, printer): self._printer = printer self.dev = dev + self.intf = None self._name = "USB device {}:{}".format(dev.idVendor, dev.idProduct) self._was_damaged = False @@ -45,18 +46,17 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) if platform.system() != 'Windows': self.dev.reset() + interface_number = 1 try: - if self.dev.is_kernel_driver_active(1): - self.dev.detach_kernel_driver(1) - self._printer("Detached Kernel Driver\n") + if self.dev.is_kernel_driver_active(interface_number): + self.dev.detach_kernel_driver(interface_number) + self._printer("Detached Kernel Driver") except NotImplementedError: pass #is_kernel_driver_active not implemented on Windows - # set the active configuration. With no arguments, the first - # configuration will be the active one - self.dev.set_configuration() - # get an endpoint instance + + self.dev.set_configuration() # no args: set first configuration self.cfg = self.dev.get_active_configuration() - self.intf = self.cfg[(1,0)] + self.intf = self.cfg[(1,0)] # this implicitly claims the interface # write endpoint self.epw = usb.util.find_descriptor(self.intf, # match the first OUT endpoint @@ -66,7 +66,7 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) usb.util.ENDPOINT_OUT ) assert self.epw is not None - self._printer("EndpointAddress for writing {}\n".format(self.epw.bEndpointAddress)) + self._printer("EndpointAddress for writing {}".format(self.epw.bEndpointAddress)) # read endpoint self.epr = usb.util.find_descriptor(self.intf, # match the first IN endpoint @@ -76,10 +76,11 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) usb.util.ENDPOINT_IN ) assert self.epr is not None - self._printer("EndpointAddress for reading {}\n".format(self.epr.bEndpointAddress)) + self._printer("EndpointAddress for reading {}".format(self.epr.bEndpointAddress)) - def shutdown(self): - return 0 + def deinit(self): + if not self.intf is None: + usb.util.release_interface(self.dev, self.intf) def process_packet(self, usbBuffer): try: @@ -97,7 +98,8 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) self._printer("halt condition: {}".format(ex.errno)) # Try resetting halt/stall condition try: - self.epw.clear_halt() + self.deinit() + self.init() except usb.core.USBError: raise odrive.protocol.ChannelBrokenException() # Retry transfer @@ -122,7 +124,8 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) self._printer("halt condition: {}".format(ex.errno)) # Try resetting halt/stall condition try: - self.epr.clear_halt() + self.deinit() + self.init() except usb.core.USBError: raise odrive.protocol.ChannelBrokenException() # Retry transfer From 21f74379de413d434a777b488e8a7eb6eba307f2 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 21:19:24 -0700 Subject: [PATCH 043/112] set usb task pump priority to osPriorityAboveNormal osPriorityNormal is the same priority as the communication task. If the USB pump task runs on the same priority, it sometimes fails to respond to the host in time, causing spurious halt conditions. --- Firmware/MotorControl/communication.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 07515e9a..ce51ded3 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -169,7 +169,7 @@ void init_communication(void) { thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + osThreadDef(task_usb_pump, usb_update_thread, osPriorityAboveNormal, 0, 512); thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); } From 814454da009a879d609298afde2c92683a3241cf Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 6 Apr 2018 21:19:24 -0700 Subject: [PATCH 044/112] set usb task pump priority to osPriorityAboveNormal osPriorityNormal is the same priority as the communication task. If the USB pump task runs on the same priority, it sometimes fails to respond to the host in time, causing spurious halt conditions. --- Firmware/MotorControl/communication.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index b8a0ed65..1c2c1efa 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -146,7 +146,7 @@ void init_communication(void) { thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + osThreadDef(task_usb_pump, usb_update_thread, osPriorityAboveNormal, 0, 512); thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); } From 61983a10da444f6f3e77ad9bd76660b0d436fc59 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 7 Apr 2018 12:30:37 -0700 Subject: [PATCH 045/112] store encoder_configs and controller_configs in NVM --- Firmware/MotorControl/main.cpp | 31 +++++++++++++++++++++---------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 076d8dac..cd360578 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -11,13 +11,20 @@ AxisConfig_t axis_configs[AXIS_COUNT]; Axis *axes[AXIS_COUNT]; -typedef Config ConfigFormat; +typedef Config< + BoardConfig_t, + EncoderConfig_t[AXIS_COUNT], + ControllerConfig_t[AXIS_COUNT], + MotorConfig_t[AXIS_COUNT], + AxisConfig_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( - &board_config, - &axis_configs, - &motor_configs)) { + &board_config, + &encoder_configs, + &controller_configs, + &motor_configs, + &axis_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } } @@ -26,13 +33,17 @@ void load_configuration(void) { if (NVM_init() || ConfigFormat::safe_load_config( &board_config, - &axis_configs, - &motor_configs)) { - for (size_t i = 0; i < AXIS_COUNT; ++i) { - axis_configs[i] = AxisConfig_t(); - motor_configs[i] = MotorConfig_t(); - } + &encoder_configs, + &controller_configs, + &motor_configs, + &axis_configs)) { board_config = BoardConfig_t(); + for (size_t i = 0; i < AXIS_COUNT; ++i) { + encoder_configs[i] = EncoderConfig_t(); + controller_configs[i] = ControllerConfig_t(); + motor_configs[i] = MotorConfig_t(); + axis_configs[i] = AxisConfig_t(); + } } } From e264e1c3269cebdcdb1a83d2a7213deff8669695 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 7 Apr 2018 13:25:19 -0700 Subject: [PATCH 046/112] rename legacy protocol to ASCII protocol --- Firmware/Board/v3/Src/syscalls.c | 2 +- .../{legacy_commands.c => ascii_protocol.c} | 10 +++++----- .../{legacy_commands.h => ascii_protocol.h} | 10 +++++----- Firmware/MotorControl/communication.cpp | 16 ++++++++-------- Firmware/README.md | 2 +- Firmware/Tupfile.lua | 6 +++--- Firmware/legacy-protocol.md | 2 +- tools/odrive/protocol.py | 2 +- 8 files changed, 25 insertions(+), 25 deletions(-) rename Firmware/MotorControl/{legacy_commands.c => ascii_protocol.c} (96%) rename Firmware/MotorControl/{legacy_commands.h => ascii_protocol.h} (79%) diff --git a/Firmware/Board/v3/Src/syscalls.c b/Firmware/Board/v3/Src/syscalls.c index da7674fc..2ef5f8de 100644 --- a/Firmware/Board/v3/Src/syscalls.c +++ b/Firmware/Board/v3/Src/syscalls.c @@ -10,7 +10,7 @@ #include #include #include -#include // TODO: make serial_printf_select constant +#include // TODO: make serial_printf_select constant //int _read(int file, char *data, int len) {} diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/ascii_protocol.c similarity index 96% rename from Firmware/MotorControl/legacy_commands.c rename to Firmware/MotorControl/ascii_protocol.c index 29a6c33a..60fd9d57 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/ascii_protocol.c @@ -1,5 +1,5 @@ /* Includes ------------------------------------------------------------------*/ -#include "legacy_commands.h" +#include "ascii_protocol.h" #include /* Private macros ------------------------------------------------------------*/ @@ -113,12 +113,12 @@ static void print_monitoring(int limit); /* Function implementations --------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) { +void ASCII_protocol_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) { // Set response interface serial_printf_select = response_interface; // Cast away const and write beyond the array bounds. Because we can. - // (TODO: yeah maybe not, but this should be gone once we disable legacy commands) + // (TODO: yeah maybe not, but this should be gone once we disable ASCII commands) ((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0; // check incoming packet type @@ -234,7 +234,7 @@ void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, } } -void legacy_parse_stream(const uint8_t* buffer, size_t len) { +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len) { #define PARSE_BUFFER_SIZE 64 static uint8_t parse_buffer[PARSE_BUFFER_SIZE]; static bool read_active = false; @@ -253,7 +253,7 @@ void legacy_parse_stream(const uint8_t* buffer, size_t len) { parse_buffer[parse_buffer_idx++] = c; if (c == '\r' || c == '\n' || c == '!') { // End of command string - legacy_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART); + ASCII_protocol_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART); // Reset receieve state machine read_active = false; parse_buffer_idx = 0; diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/MotorControl/ascii_protocol.h similarity index 79% rename from Firmware/MotorControl/legacy_commands.h rename to Firmware/MotorControl/ascii_protocol.h index 11ee7203..74e2dff8 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/MotorControl/ascii_protocol.h @@ -1,5 +1,5 @@ -#ifndef LEGACY_COMMANDS_H -#define LEGACY_COMMANDS_H +#ifndef ASCII_PROTOCOL_H +#define ASCII_PROTOCOL_H #ifdef __cplusplus extern "C" { @@ -29,11 +29,11 @@ extern uint16_t* exposed_uint16[]; /* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface); -void legacy_parse_stream(const uint8_t* buffer, size_t len); +void ASCII_protocol_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface); +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len); #ifdef __cplusplus } #endif -#endif /* LEGACY_COMMANDS_H */ +#endif /* ASCII_PROTOCOL_H */ diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 1c2c1efa..f48232e1 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -4,7 +4,7 @@ // TODO: remove this option // and once the legacy protocol is phased out, remove the seq-no hack in protocol.py // todo: make clean switches for protocol -#define ENABLE_LEGACY_PROTOCOL +#define ENABLE_ASCII_PROTOCOL #include "communication.h" //#include "low_level.h" @@ -13,8 +13,8 @@ #include "freertos_vars.h" #include "utils.h" -#ifdef ENABLE_LEGACY_PROTOCOL -#include "legacy_commands.h" +#ifdef ENABLE_ASCII_PROTOCOL +#include "ascii_protocol.h" #endif #include @@ -247,15 +247,15 @@ void communication_task(void * ctx) { new_rcv_idx - last_rcv_idx); last_rcv_idx = new_rcv_idx; } -#elif defined(UART_PROTOCOL_LEGACY) +#elif defined(UART_PROTOCOL_ASCII) // Process bytes in one or two chunks (two in case there was a wrap) if (new_rcv_idx < last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, UART_RX_BUFFER_SIZE - last_rcv_idx); last_rcv_idx = 0; } if (new_rcv_idx > last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, + ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, new_rcv_idx - last_rcv_idx); last_rcv_idx = new_rcv_idx; } @@ -274,8 +274,8 @@ void communication_task(void * ctx) { usb_channel.process_packet(usb_buf, usb_len); #elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) usb_stream_sink.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_LEGACY) - legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); +#elif defined(USB_PROTOCOL_ASCII) + ASCII_protocol_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); #endif USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet } diff --git a/Firmware/README.md b/Firmware/README.md index 8470efea..0346a9bf 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -171,7 +171,7 @@ pip install pyusb pyserial [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) ### Other platforms -See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). +See the [protocol specification](protocol.md) or the [ASCII protocol specification](ascii-protocol.md).

## Configuring parameters diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index fc69849b..3b71baf5 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -36,7 +36,7 @@ if tup.getconfig("USB_PROTOCOL") == "native" or tup.getconfig("USB_PROTOCOL") == elseif tup.getconfig("USB_PROTOCOL") == "native-stream" then FLAGS += "-DUSB_PROTOCOL_NATIVE_STREAM_BASED" elseif tup.getconfig("USB_PROTOCOL") == "ascii" then - FLAGS += "-DUSB_PROTOCOL_LEGACY" + FLAGS += "-DUSB_PROTOCOL_ASCII" elseif tup.getconfig("USB_PROTOCOL") == "none" then FLAGS += "-DUSB_PROTOCOL_NONE" else @@ -47,7 +47,7 @@ end if tup.getconfig("UART_PROTOCOL") == "native" then FLAGS += "-DUART_PROTOCOL_NATIVE" elseif tup.getconfig("UART_PROTOCOL") == "ascii" or tup.getconfig("UART_PROTOCOL") == "" then - FLAGS += "-DUART_PROTOCOL_LEGACY" + FLAGS += "-DUART_PROTOCOL_ASCII" elseif tup.getconfig("UART_PROTOCOL") == "none" then FLAGS += "-DUART_PROTOCOL_NONE" else @@ -133,7 +133,7 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', - 'MotorControl/legacy_commands.c', + 'MotorControl/ascii_protocol.c', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', diff --git a/Firmware/legacy-protocol.md b/Firmware/legacy-protocol.md index 2898432f..9f0a54c5 100644 --- a/Firmware/legacy-protocol.md +++ b/Firmware/legacy-protocol.md @@ -52,7 +52,7 @@ s type index value ** `0` is float ** `1` is int ** `2` is bool -* `index` is the index in the corresponding [exposed variable table](MotorControl/legacy_commands.c). +* `index` is the index in the corresponding [exposed variable table](MotorControl/ascii_protocol.c). For example * `g 0 12` will return the phase resistance of M0 diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 18f44a41..c62fd353 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -224,7 +224,7 @@ class Channel(PacketSink): endpoint_id |= 0x8000 self._outbound_seq_no = ((self._outbound_seq_no + 1) & 0x7fff) - self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol + self._outbound_seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ascii protocol seq_no = self._outbound_seq_no packet = struct.pack(' Date: Sat, 7 Apr 2018 19:20:48 -0700 Subject: [PATCH 047/112] reenable ASCII protocol (formerly "legacy" protocol) --- .travis.yml | 1 + Firmware/Board/v3/Src/main.c | 7 + Firmware/Board/v3/Src/syscalls.c | 43 +--- Firmware/Board/v3/Src/usbd_desc.c | 10 +- Firmware/MotorControl/ascii_protocol.c | 292 ----------------------- Firmware/MotorControl/ascii_protocol.cpp | 172 +++++++++++++ Firmware/MotorControl/ascii_protocol.h | 7 +- Firmware/MotorControl/communication.cpp | 121 ++++++---- Firmware/MotorControl/communication.h | 1 + Firmware/Tupfile.lua | 6 +- Firmware/ascii-protocol.md | 52 ++++ Firmware/legacy-protocol.md | 68 ------ 12 files changed, 317 insertions(+), 463 deletions(-) delete mode 100644 Firmware/MotorControl/ascii_protocol.c create mode 100644 Firmware/MotorControl/ascii_protocol.cpp create mode 100644 Firmware/ascii-protocol.md delete mode 100644 Firmware/legacy-protocol.md diff --git a/.travis.yml b/.travis.yml index d530f0a1..7260e431 100644 --- a/.travis.yml +++ b/.travis.yml @@ -39,6 +39,7 @@ env: # Various protocol combinations - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=native-stream CONFIG_UART_PROTOCOL=native + - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=stdout CONFIG_UART_PROTOCOL=ascii - CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=none CONFIG_UART_PROTOCOL=none script: diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 777a5f20..64810aac 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -125,6 +125,13 @@ int main(void) uint32_t uuid_mixed_part = uuid0 + uuid2; serial_number = ((uint64_t)uuid_mixed_part << 16) | (uint64_t)(uuid1 >> 16); + uint64_t val = serial_number; + for (size_t i = 0; i < 12; ++i) { + serial_number_str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf]; + val <<= 4; + } + serial_number_str[12] = 0; + /* USER CODE END 1 */ /* MCU Configuration----------------------------------------------------------*/ diff --git a/Firmware/Board/v3/Src/syscalls.c b/Firmware/Board/v3/Src/syscalls.c index 2ef5f8de..eff893dc 100644 --- a/Firmware/Board/v3/Src/syscalls.c +++ b/Firmware/Board/v3/Src/syscalls.c @@ -10,7 +10,6 @@ #include #include #include -#include // TODO: make serial_printf_select constant //int _read(int file, char *data, int len) {} @@ -57,46 +56,6 @@ intptr_t _sbrk(size_t size) { return ptr; } -#define UART_TX_BUFFER_SIZE 64 -static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; +// _write is defined in communication.cpp -int _write(int file, char* data, int len) { - //number of bytes written - int written = 0; - switch (serial_printf_select) { - case SERIAL_PRINTF_IS_USB: { - // Wait on semaphore for the interface to be available - // Note that the USB driver will release the interface again when the TX completes - const uint32_t usb_tx_timeout = 100; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_tx, usb_tx_timeout); - if (sem_stat == osOK) { - uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC - written = (status == USBD_OK) ? len : 0; - } // If the semaphore times out, we simply leave "written" as 0 - } break; - case SERIAL_PRINTF_IS_UART: { - //Check length - if (len > UART_TX_BUFFER_SIZE) - return 0; - // Wait on semaphore for the interface to be available - // Note that HAL_UART_TxCpltCallback will release the interface again when the TX completes - const uint32_t uart_tx_timeout = 100; // ms - osStatus sem_stat = osSemaphoreWait(sem_uart_dma, uart_tx_timeout); - if (sem_stat == osOK) { - memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf - HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer - } // If the semaphore times out, we simply leave "written" as 0 - } break; - - default: { - written = 0; - } break; - } - - return written; -} - -void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { - osSemaphoreRelease(sem_uart_dma); -} diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index e7083b3a..ea8584d8 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -329,15 +329,7 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l */ uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { - uint8_t str[13]; // 12 digits + null termination - uint64_t val = serial_number; - for (size_t i = 0; i < 12; ++i) { - str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf]; - val <<= 4; - } - str[12] = 0; - - USBD_GetString ((uint8_t *)str, USBD_StrDesc, length); + USBD_GetString ((uint8_t *)serial_number_str, USBD_StrDesc, length); return USBD_StrDesc; } diff --git a/Firmware/MotorControl/ascii_protocol.c b/Firmware/MotorControl/ascii_protocol.c deleted file mode 100644 index 60fd9d57..00000000 --- a/Firmware/MotorControl/ascii_protocol.c +++ /dev/null @@ -1,292 +0,0 @@ -/* Includes ------------------------------------------------------------------*/ -#include "ascii_protocol.h" -#include - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -// This automatically updates to the interface that most -// recently recieved a command. In the future we may want to separate -// debug printf and the main serial comms. -SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART; -#if 0 -/* Private constant data -----------------------------------------------------*/ - -// variables exposed to usb/serial interface via set/get/monitor -// Note: this will be depricated soon -float* exposed_floats[] = { - &vbus_voltage, // ro - NULL, //&elec_rad_per_enc, // ro - &motors[0].pos_setpoint, // rw - &motors[0].pos_gain, // rw - &motors[0].vel_setpoint, // rw - &motors[0].vel_gain, // rw - &motors[0].vel_integrator_gain, // rw - &motors[0].vel_integrator_current, // rw - &motors[0].vel_limit, // rw - &motors[0].current_setpoint, // rw - &motors[0].calibration_current, // rw - &motors[0].phase_inductance, // ro - &motors[0].phase_resistance, // ro - &motors[0].current_meas.phB, // ro - &motors[0].current_meas.phC, // ro - &motors[0].DC_calib.phB, // rw - &motors[0].DC_calib.phC, // rw - &motors[0].shunt_conductance, // rw - &motors[0].phase_current_rev_gain, // rw - &motors[0].current_control.current_lim, // rw - &motors[0].current_control.p_gain, // rw - &motors[0].current_control.i_gain, // rw - &motors[0].current_control.v_current_control_integral_d, // rw - &motors[0].current_control.v_current_control_integral_q, // rw - &motors[0].current_control.Ibus, // ro - &motors[0].encoder.phase, // ro - &motors[0].encoder.pll_pos, // rw - &motors[0].encoder.pll_vel, // rw - &motors[0].encoder.pll_kp, // rw - &motors[0].encoder.pll_ki, // rw - &motors[1].pos_setpoint, // rw - &motors[1].pos_gain, // rw - &motors[1].vel_setpoint, // rw - &motors[1].vel_gain, // rw - &motors[1].vel_integrator_gain, // rw - &motors[1].vel_integrator_current, // rw - &motors[1].vel_limit, // rw - &motors[1].current_setpoint, // rw - &motors[1].calibration_current, // rw - &motors[1].phase_inductance, // ro - &motors[1].phase_resistance, // ro - &motors[1].current_meas.phB, // ro - &motors[1].current_meas.phC, // ro - &motors[1].DC_calib.phB, // rw - &motors[1].DC_calib.phC, // rw - &motors[1].shunt_conductance, // rw - &motors[1].phase_current_rev_gain, // rw - &motors[1].current_control.current_lim, // rw - &motors[1].current_control.p_gain, // rw - &motors[1].current_control.i_gain, // rw - &motors[1].current_control.v_current_control_integral_d, // rw - &motors[1].current_control.v_current_control_integral_q, // rw - &motors[1].current_control.Ibus, // ro - &motors[1].encoder.phase, // ro - &motors[1].encoder.pll_pos, // rw - &motors[1].encoder.pll_vel, // rw - &motors[1].encoder.pll_kp, // rw - &motors[1].encoder.pll_ki, // rw -}; - -int* exposed_ints[] = { - (int*)&motors[0].control_mode, // rw - (int*)&motors[0].encoder.encoder_offset, // rw - (int*)&motors[0].encoder.encoder_state, // ro - (int*)&motors[0].error, // rw - (int*)&motors[1].control_mode, // rw - (int*)&motors[1].encoder.encoder_offset, // rw - (int*)&motors[1].encoder.encoder_state, // ro - (int*)&motors[1].error, // rw -}; - -bool* exposed_bools[] = { - &motors[0].thread_id_valid, // ro - //For now these are written by Axis::SetupLegacyMappings - &axis[0].enable_control, // rw - &axis[0].do_calibration, // rw - NULL, // &motors[0].calibration_ok, // ro - &motors[1].thread_id_valid, // ro - &axis[1].enable_control, // rw - &axis[1].do_calibration, // rw - NULL, // &motors[1].calibration_ok, // ro -}; - -uint16_t* exposed_uint16[] = { - &motors[0].control_deadline, // rw - &motors[0].last_cpu_time, // ro - &motors[1].control_deadline, // rw - &motors[1].last_cpu_time, // ro -}; - -/* Private variables ---------------------------------------------------------*/ -monitoring_slot monitoring_slots[20] = {0}; -/* Private function prototypes -----------------------------------------------*/ -static void print_monitoring(int limit); - -/* Function implementations --------------------------------------------------*/ - -void ASCII_protocol_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) { - // Set response interface - serial_printf_select = response_interface; - - // Cast away const and write beyond the array bounds. Because we can. - // (TODO: yeah maybe not, but this should be gone once we disable ASCII commands) - ((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0; - - // check incoming packet type - if (buffer[0] == 'p') { - // position control - unsigned motor_number; - float pos_setpoint, vel_feed_forward, current_feed_forward; - int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); - if (numscan == 4 && motor_number < num_motors) { - set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward); - } - } else if (buffer[0] == 'v') { - // velocity control - unsigned motor_number; - float vel_feed_forward, current_feed_forward; - int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, ¤t_feed_forward); - if (numscan == 3 && motor_number < num_motors) { - set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward); - } - } else if (buffer[0] == 'c') { - // current control - unsigned motor_number; - float current_feed_forward; - int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, ¤t_feed_forward); - if (numscan == 2 && motor_number < num_motors) { - set_current_setpoint(&motors[motor_number], current_feed_forward); - } - } else if(buffer[0] == 'i'){ // Dump device info - // Retrieves the device signature, revision, flash size, and UUID - printf("Signature: %#x\n", STM_ID_GetSignature()); - printf("Revision: %#x\n", STM_ID_GetRevision()); - printf("Flash Size: %#x KiB\n", STM_ID_GetFlashSize()); - printf("UUID: 0x%lx%lx%lx\n", STM_ID_GetUUID(2), STM_ID_GetUUID(1), STM_ID_GetUUID(0)); - } else if (buffer[0] == 'g') { // GET - // g <0:float,1:int,2:bool,3:uint16> index - int type = 0; - int index = 0; - int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index); - if (numscan == 2) { - switch(type){ - case 0: { - printf("%f\n",*exposed_floats[index]); - break; - }; - case 1: { - printf("%d\n",*exposed_ints[index]); - break; - }; - case 2: { - printf("%d\n",*exposed_bools[index]); - break; - }; - case 3: { - printf("%hu\n",*exposed_uint16[index]); - break; - }; - } - } - } else if (buffer[0] == 'h'){ // HALT - for(int i = 0; i < num_motors; i++){ - set_vel_setpoint(&motors[i], 0.0f, 0.0f); - } - } else if (buffer[0] == 's') { // SET - // s <0:float,1:int,2:bool,3:uint16> index value - int type = 0; - int index = 0; - int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index); - if (numscan == 2) { - switch(type) { - case 0: { - sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]); - break; - }; - case 1: { - sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]); - break; - }; - case 2: { - int btmp = 0; - sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp); - *exposed_bools[index] = btmp ? true : false; - break; - }; - case 3: { - sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]); - break; - }; - } - } - } else if (buffer[0] == 'm') { // Setup Monitor - // m <0:float,1:int,2:bool,3:uint16> index monitoring_slot - int type = 0; - int index = 0; - int slot = 0; - int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot); - if (numscan == 3) { - monitoring_slots[slot].type = type; - monitoring_slots[slot].index = index; - } - } else if (buffer[0] == 'o') { // Output Monitor - int limit = 0; - int numscan = sscanf((const char*)buffer, "o %u", &limit); - if (numscan == 1) { - print_monitoring(limit); - } - } else if (buffer[0] == 't') { // Run Anti-Cogging Calibration - for (int i = 0; i < num_motors; i++) { - // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) { - motors[i].anticogging.calib_anticogging = true; - } - } - } -} - -void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len) { - #define PARSE_BUFFER_SIZE 64 - static uint8_t parse_buffer[PARSE_BUFFER_SIZE]; - static bool read_active = false; - static uint32_t parse_buffer_idx = 0; - - while (len--) { - // Fetch the next char - uint8_t c = *(buffer++); - // Look for start character - if (c == '$') { - read_active = true; - continue; // do not record start char - } - // Record into parse buffer when actively reading - if (read_active) { - parse_buffer[parse_buffer_idx++] = c; - if (c == '\r' || c == '\n' || c == '!') { - // End of command string - ASCII_protocol_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART); - // Reset receieve state machine - read_active = false; - parse_buffer_idx = 0; - } else if (parse_buffer_idx == PARSE_BUFFER_SIZE - 1) { - // We are not at end of command, and receiving another character after this - // would go into the last slot, which is reserved for terminating null. - // We have effectively overflowed parse buffer: abort. - read_active = false; - parse_buffer_idx = 0; - } - } - } -} - -static void print_monitoring(int limit) { - for (int i=0;i + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ + +#define MAX_LINE_LENGTH 64 + +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +// @brief Sends a line on the specified output. +template +void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) { + char response[64]; + size_t len = snprintf(response, sizeof(response), fmt, std::forward(args)...); + output.process_bytes((uint8_t*)response, len); + if (include_checksum) { + uint8_t checksum = 0; + for (size_t i = 0; i < len; ++i) + checksum ^= response[i]; + len = snprintf(response, sizeof(response), "*%u", checksum); + output.process_bytes((uint8_t*)response, len); + } + output.process_bytes((const uint8_t*)"\r\n", 2); +} + + +// @brief Executes an ASCII protocol command +// @param buffer buffer of ASCII encoded characters +// @param len size of the buffer +void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) { + static_assert(sizeof(char) == sizeof(uint8_t)); + + // scan line to find beginning of checksum and prune comment + uint8_t checksum = 0; + size_t checksum_start = SIZE_MAX; + for (size_t i = 0; i < len; ++i) { + if (buffer[i] == ';') { // ';' is the comment start char + len = i; + break; + } + if (checksum_start > i) { + if (buffer[i] == '*') { + checksum_start = i + 1; + } else { + checksum ^= buffer[i]; + } + } + } + + // copy everything into a local buffer so we can insert null-termination + char cmd[MAX_LINE_LENGTH + 1]; + if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH; + memcpy(cmd, buffer, len); + + // optional checksum validation + bool use_checksum = (checksum_start < len); + if (use_checksum) { + unsigned int received_checksum; + sscanf((const char *)cmd + checksum_start, "%u", &received_checksum); + if (received_checksum != checksum) + return; + len = checksum_start - 1; // prune checksum and asterisk + } + + cmd[len] = 0; // null-terminate + + // check incoming packet type + if (cmd[0] == 'p') { // position control + unsigned motor_number; + float pos_setpoint, vel_feed_forward, current_feed_forward; + int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); + 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 { + if (numscan < 3) + 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); + } + + } else if (cmd[0] == 'v') { // velocity control + unsigned motor_number; + float vel_setpoint, current_feed_forward; + int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_feed_forward); + 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 { + if (numscan < 3) + current_feed_forward = 0.0f; + axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + } + + } else if (cmd[0] == 'c') { // current control + unsigned motor_number; + float current_setpoint; + int numscan = sscanf(cmd, "c %u %f", &motor_number, ¤t_setpoint); + 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 { + axes[motor_number]->controller_.set_current_setpoint(current_setpoint); + respond(response_channel, use_checksum, "ok", motor_number); + } + + } else if (cmd[0] == 'i'){ // Dump device info + respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); + respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision()); + respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); + respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); + +// } else if (cmd[0] == 'r') { // read property +// } else if (cmd[0] == 'w') { // write property + + } else if (cmd[0] == 'h') { // HALT + for(size_t i = 0; i < AXIS_COUNT; i++){ + axes[i]->controller_.set_vel_setpoint(0.0f, 0.0f); + } + } else if (cmd[0] != 0) { + respond(response_channel, use_checksum, "unknown command"); + } +} + +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) { + static uint8_t parse_buffer[MAX_LINE_LENGTH]; + static bool read_active = true; + static uint32_t parse_buffer_idx = 0; + + while (len--) { + // if the line becomes too long, reset buffer and wait for the next line + if (parse_buffer_idx >= MAX_LINE_LENGTH) { + read_active = false; + parse_buffer_idx = 0; + } + + // Fetch the next char + uint8_t c = *(buffer++); + bool is_end_of_line = (c == '\r' || c == '\n' || c == '!'); + if (is_end_of_line) { + if (read_active) + ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel); + parse_buffer_idx = 0; + read_active = true; + } else { + if (read_active) { + parse_buffer[parse_buffer_idx++] = c; + } + } + } +} diff --git a/Firmware/MotorControl/ascii_protocol.h b/Firmware/MotorControl/ascii_protocol.h index 74e2dff8..680dd9f3 100644 --- a/Firmware/MotorControl/ascii_protocol.h +++ b/Firmware/MotorControl/ascii_protocol.h @@ -1,6 +1,10 @@ #ifndef ASCII_PROTOCOL_H #define ASCII_PROTOCOL_H +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif + #ifdef __cplusplus extern "C" { #endif @@ -29,8 +33,7 @@ extern uint16_t* exposed_uint16[]; /* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -void ASCII_protocol_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface); -void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len); +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel); #ifdef __cplusplus } diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index f48232e1..6d84b3bc 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -35,6 +35,7 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern USBD_HandleTypeDef hUsbDeviceFS; uint64_t serial_number; +char serial_number_str[13]; // 12 digits + null termination /* Private constant data -----------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ @@ -46,7 +47,7 @@ static uint32_t usb_len; static thread_local uint32_t deadline_ms = 0; -#if defined(USB_PROTOCOL_NATIVE) +#if !defined(USB_PROTOCOL_NONE) class USBSender : public PacketSink { public: @@ -63,42 +64,44 @@ public: well... it's not actually. Stupid STM. */, length); return (status == USBD_OK) ? 0 : -1; } -} usb_sender; +} usb_packet_output; -BidirectionalPacketBasedChannel usb_channel(usb_sender); - -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - -class USBSender : public StreamSink { +#if !defined(USB_PROTOCOL_NATIVE) +class TreatPacketSinkAsStreamSink : public StreamSink { public: + TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} int process_bytes(const uint8_t* buffer, size_t length) { // Loop to ensure all bytes get sent while (length) { size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - if (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, chunk) != USBD_OK) + if (output_.process_packet(buffer, length) != 0) return -1; buffer += chunk; length -= chunk; } return 0; } - size_t get_free_space() { return SIZE_MAX; } -} usb_sender; - -PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_channel); - +private: + PacketSink& output_; +} usb_stream_output(usb_packet_output); #endif -#if defined(UART_PROTOCOL_NATIVE) +#if defined(USB_PROTOCOL_NATIVE) +BidirectionalPacketBasedChannel usb_channel(usb_packet_output); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +PacketToStreamConverter usb_packetized_output(usb_stream_output); +BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +StreamToPacketConverter usb_native_stream_input(usb_channel); +#endif + +#endif // !defined(USB_PROTOCOL_NONE) + + +#if !defined(UART_PROTOCOL_NONE) class UART4Sender : public StreamSink { public: int process_bytes(const uint8_t* buffer, size_t length) { @@ -122,13 +125,16 @@ public: size_t get_free_space() { return SIZE_MAX; } private: uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_sender; +} uart4_stream_output; -PacketToStreamConverter uart4_packet_sender(uart4_sender); +#if defined(UART_PROTOCOL_NATIVE) +PacketToStreamConverter uart4_packet_sender(uart4_stream_output); BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_channel); +StreamToPacketConverter uart4_stream_input(uart4_channel); #endif +#endif // !defined(UART_PROTOCOL_NONE) + /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -235,31 +241,29 @@ void communication_task(void * ctx) { uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < last_rcv_idx) { #if defined(UART_PROTOCOL_NATIVE) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, + uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#elif defined(UART_PROTOCOL_ASCII) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } #endif +#if defined(UART_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, + UART_RX_BUFFER_SIZE - last_rcv_idx, uart4_stream_output); +#endif + last_rcv_idx = 0; + } + if (new_rcv_idx > last_rcv_idx) { +#if defined(UART_PROTOCOL_NATIVE) + uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx); +#endif +#if defined(UART_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, + new_rcv_idx - last_rcv_idx, uart4_stream_output); +#endif + last_rcv_idx = new_rcv_idx; + } #endif #if !defined(USB_PROTOCOL_NONE) @@ -273,9 +277,9 @@ void communication_task(void * ctx) { #if defined(USB_PROTOCOL_NATIVE) usb_channel.process_packet(usb_buf, usb_len); #elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_stream_sink.process_bytes(usb_buf, usb_len); + usb_native_stream_input.process_bytes(usb_buf, usb_len); #elif defined(USB_PROTOCOL_ASCII) - ASCII_protocol_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); + ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); #endif USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet } @@ -313,3 +317,22 @@ void usb_update_thread(void * ctx) { vTaskDelete(osThreadGetId()); } + +extern "C" { +int _write(int file, const char* data, int len); +} + +// @brief This is what printf calls internally +int _write(int file, const char* data, int len) { +#ifdef USB_PROTOCOL_STDOUT + usb_stream_output.process_bytes((const uint8_t *)data, len); +#endif +#ifdef UART_PROTOCOL_STDOUT + uart4_stream_output.process_bytes((const uint8_t *)data, len); +#endif + return len; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); +} diff --git a/Firmware/MotorControl/communication.h b/Firmware/MotorControl/communication.h index 2b0f8543..88e37921 100644 --- a/Firmware/MotorControl/communication.h +++ b/Firmware/MotorControl/communication.h @@ -20,6 +20,7 @@ void usb_update_thread(void * ctx); void USB_receive_packet(const uint8_t *buffer, size_t length); extern uint64_t serial_number; +extern char serial_number_str[13]; #ifdef __cplusplus } diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 3b71baf5..aa122ca5 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -37,6 +37,8 @@ elseif tup.getconfig("USB_PROTOCOL") == "native-stream" then FLAGS += "-DUSB_PROTOCOL_NATIVE_STREAM_BASED" elseif tup.getconfig("USB_PROTOCOL") == "ascii" then FLAGS += "-DUSB_PROTOCOL_ASCII" +elseif tup.getconfig("USB_PROTOCOL") == "stdout" then + FLAGS += "-DUSB_PROTOCOL_STDOUT" elseif tup.getconfig("USB_PROTOCOL") == "none" then FLAGS += "-DUSB_PROTOCOL_NONE" else @@ -48,6 +50,8 @@ if tup.getconfig("UART_PROTOCOL") == "native" then FLAGS += "-DUART_PROTOCOL_NATIVE" elseif tup.getconfig("UART_PROTOCOL") == "ascii" or tup.getconfig("UART_PROTOCOL") == "" then FLAGS += "-DUART_PROTOCOL_ASCII" +elseif tup.getconfig("UART_PROTOCOL") == "stdout" then + FLAGS += "-DUART_PROTOCOL_STDOUT" elseif tup.getconfig("UART_PROTOCOL") == "none" then FLAGS += "-DUART_PROTOCOL_NONE" else @@ -133,7 +137,7 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', - 'MotorControl/ascii_protocol.c', + 'MotorControl/ascii_protocol.cpp', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', diff --git a/Firmware/ascii-protocol.md b/Firmware/ascii-protocol.md new file mode 100644 index 00000000..f7d89235 --- /dev/null +++ b/Firmware/ascii-protocol.md @@ -0,0 +1,52 @@ + +## How to send commands + + * **Via USB:** + * **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. + * **Windows/Linux/macOS:** You can use an FTDI USB-UART cable to connect to the ODrive. + + +## Command Reference + +#### Motor Position command +``` +p motor position velocity_ff current_ff +``` +* `p` for position +* `motor` is the motor number, `0` or `1`. +* `position` is the desired position, in encoder counts. +* `velocity_ff` is the velocity feed-forward term, in counts/s (optional). +* `current_ff` is the current feed-forward term, in A (optional). + +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. + + +#### Motor Velocity command +``` +v motor velocity current_ff +``` +* `v` for velocity +* `motor` is the motor number, `0` or `1`. +* `velocity` is the desired velocity in counts/s. +* `current_ff` is the current feed-forward term, in A (optional). + +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. + +#### Motor Current command +``` +c motor current +``` +* `c` for current +* `motor` is the motor number, `0` or `1`. +* `current` is the desired current in A. + +#### Parameter reading/writing + +This is currently not supported. Use the native protocol. diff --git a/Firmware/legacy-protocol.md b/Firmware/legacy-protocol.md deleted file mode 100644 index 9f0a54c5..00000000 --- a/Firmware/legacy-protocol.md +++ /dev/null @@ -1,68 +0,0 @@ - -Warning: this protocol has [been replaced](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md). -It's still operational but for new applications it's recommended to use the new protocol. - -### Command set -The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART. - -#### UART framing -USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command: -``` -$p 0 10000 0 0! -``` - -#### Motor Position command -``` -p motor position velocity_ff current_ff -``` -* `p` for position -* `motor` is the motor number, `0` or `1`. -* `position` is the desired position, in encoder counts. -* `velocity_ff` is the velocity feed-forward term, in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Velocity command -``` -v motor velocity current_ff -``` -* `v` for velocity -* `motor` is the motor number, `0` or `1`. -* `velocity` is the desired velocity in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Current command -``` -c motor current -``` -* `c` for current -* `motor` is the motor number, `0` or `1`. -* `current` is the desired current in A. - -#### Variable getting and setting -``` -g type index -s type index value -``` -* `g` for get, `s` for set -* `type` is the data type as follows: -** `0` is float -** `1` is int -** `2` is bool -* `index` is the index in the corresponding [exposed variable table](MotorControl/ascii_protocol.c). - -For example -* `g 0 12` will return the phase resistance of M0 -* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s -* `g 1 3` will return the error status of M0 -* `g 1 7` will return the error status of M1 - -The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h). - -Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values. - -#### Continous monitoring of variables -You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this. From 8f02ddc05213d48cbf745ed4f802b43a111e3438 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 18:17:57 -0700 Subject: [PATCH 048/112] fix USB patch file --- ...01-expose-correct-serial-number-on-USB.patch | 17 +++++------------ 1 file changed, 5 insertions(+), 12 deletions(-) diff --git a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch index a2e510b0..b9fef648 100644 --- a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch +++ b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch @@ -4,14 +4,14 @@ Date: Mon, 12 Mar 2018 23:49:32 -0700 Subject: [PATCH] expose correct serial number on USB --- - Firmware/Board/v3/Src/usbd_desc.c | 15 +++++++++------- - 1 file changed, 8 insertions(+), 7 deletions(-) + Firmware/Board/v3/Src/usbd_desc.c | 9 +++++++++------- + 1 file changed, 1 insertions(+), 8 deletions(-) diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index b9c7bd0..94dc49b 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c -@@ -327,14 +327,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l +@@ -327,14 +327,7 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l */ uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length) { @@ -22,15 +22,8 @@ index b9c7bd0..94dc49b 100644 - else - { - USBD_GetString((uint8_t *)USBD_SERIALNUMBER_STRING_FS, USBD_StrDesc, length); -+ uint8_t str[13]; // 12 digits + null termination -+ uint64_t val = serial_number; -+ for (size_t i = 0; i < 12; ++i) { -+ str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf]; -+ val <<= 4; - } -+ str[12] = 0; -+ -+ USBD_GetString ((uint8_t *)str, USBD_StrDesc, length); +- } ++ USBD_GetString ((uint8_t *)serial_number_str, USBD_StrDesc, length); return USBD_StrDesc; } From 68ffd112805200c543a7cfbe93842eb8b301448a Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 8 Apr 2018 20:38:18 -0700 Subject: [PATCH 049/112] change pin names and modes in Cube --- Firmware/Board/v3/Odrive.ioc | 24 +++++++++++------------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index ce8e1e2f..f13657b1 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -238,7 +238,7 @@ PA13.Signal=SYS_JTMS-SWDIO PA14.Mode=Serial_Wire PA14.Signal=SYS_JTCK-SWCLK PA15.GPIOParameters=GPIO_Label -PA15.GPIO_Label=M0_ENC_Z +PA15.GPIO_Label=GPIO_7 PA15.Locked=true PA15.Signal=GPIO_Input PA2.GPIOParameters=GPIO_PuPd,GPIO_Label @@ -256,7 +256,7 @@ PA4.GPIO_Label=M1_TEMP PA4.Locked=true PA4.Signal=ADCx_IN4 PA5.GPIOParameters=GPIO_Label -PA5.GPIO_Label=AUX_I +PA5.GPIO_Label=AUX_TEMP PA5.Locked=true PA5.Signal=ADCx_IN5 PA6.GPIOParameters=GPIO_Label @@ -314,11 +314,11 @@ PB15.Locked=true PB15.Mode=PWM Generation3 CH3 CH3N PB15.Signal=TIM1_CH3N PB2.GPIOParameters=GPIO_Label -PB2.GPIO_Label=GPIO_5 +PB2.GPIO_Label=GPIO_6 PB2.Locked=true PB2.Signal=GPIO_Input PB3.GPIOParameters=GPIO_Label -PB3.GPIO_Label=M1_ENC_Z +PB3.GPIO_Label=GPIO_8 PB3.Locked=true PB3.Signal=GPIO_Input PB4.GPIOParameters=GPIO_Label @@ -360,9 +360,9 @@ PC14-OSC32_IN.Locked=true PC14-OSC32_IN.PinState=GPIO_PIN_SET PC14-OSC32_IN.Signal=GPIO_Output PC15-OSC32_OUT.GPIOParameters=GPIO_Label -PC15-OSC32_OUT.GPIO_Label=M1_DC_CAL +PC15-OSC32_OUT.GPIO_Label=M1_ENC_Z PC15-OSC32_OUT.Locked=true -PC15-OSC32_OUT.Signal=GPIO_Output +PC15-OSC32_OUT.Signal=GPIO_Input PC2.GPIOParameters=GPIO_Label PC2.GPIO_Label=M1_IC PC2.Signal=ADCx_IN12 @@ -370,8 +370,9 @@ PC3.GPIOParameters=GPIO_Label PC3.GPIO_Label=M1_IB PC3.Signal=ADCx_IN13 PC4.GPIOParameters=GPIO_Label -PC4.GPIO_Label=AUX_TEMP -PC4.Signal=ADCx_IN14 +PC4.GPIO_Label=GPIO_5 +PC4.Locked=true +PC4.Signal=GPIO_Input PC5.GPIOParameters=GPIO_Label PC5.GPIO_Label=M0_TEMP PC5.Signal=ADCx_IN15 @@ -388,9 +389,9 @@ PC8.GPIO_Label=M1_CH PC8.Locked=true PC8.Signal=S_TIM8_CH3 PC9.GPIOParameters=GPIO_Label -PC9.GPIO_Label=M0_DC_CAL +PC9.GPIO_Label=M0_ENC_Z PC9.Locked=true -PC9.Signal=GPIO_Output +PC9.Signal=GPIO_Input PCC.Checker=false PCC.Line=STM32F405/415 PCC.MCU=STM32F405RGTx @@ -484,9 +485,6 @@ SH.ADCx_IN13.0=ADC1_IN13,IN13 SH.ADCx_IN13.1=ADC2_IN13,IN13 SH.ADCx_IN13.2=ADC3_IN13,IN13 SH.ADCx_IN13.ConfNb=3 -SH.ADCx_IN14.0=ADC1_IN14,IN14 -SH.ADCx_IN14.1=ADC2_IN14,IN14 -SH.ADCx_IN14.ConfNb=2 SH.ADCx_IN15.0=ADC1_IN15,IN15 SH.ADCx_IN15.1=ADC2_IN15,IN15 SH.ADCx_IN15.ConfNb=2 From 80470547839f83cfb215d8b8e9e958e09bb7d087 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 17:23:23 -0700 Subject: [PATCH 050/112] generate CubeMX files for board version v3.5 The files main.h, adc.c and gpio.c change from version v3.4 to v3.5 The old versions are moved into the prev_board_version/ directories. --- Firmware/Board/v3/Inc/main.h | 31 +- .../Board/v3/Inc/prev_board_ver/main_V3_2.h | 1 + .../Board/v3/Inc/prev_board_ver/main_V3_4.h | 91 +++++ Firmware/Board/v3/Src/adc.c | 23 +- Firmware/Board/v3/Src/gpio.c | 20 +- .../Board/v3/Src/prev_board_ver/adc_V3_4.c | 375 ++++++++++++++++++ .../Board/v3/Src/prev_board_ver/gpio_V3_4.c | 71 ++++ 7 files changed, 579 insertions(+), 33 deletions(-) create mode 100644 Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h create mode 100644 Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c create mode 100644 Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index ac9bd857..1ff706df 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -58,6 +58,9 @@ #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 #include "prev_board_ver/main_V3_2.h" +#elif HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 3 \ +|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 4 +#include "prev_board_ver/main_V3_4.h" #else /* USER CODE END Includes */ @@ -74,8 +77,8 @@ #define M0_nCS_GPIO_Port GPIOC #define M1_nCS_Pin GPIO_PIN_14 #define M1_nCS_GPIO_Port GPIOC -#define M1_DC_CAL_Pin GPIO_PIN_15 -#define M1_DC_CAL_GPIO_Port GPIOC +#define M1_ENC_Z_Pin GPIO_PIN_15 +#define M1_ENC_Z_GPIO_Port GPIOC #define M0_IB_Pin GPIO_PIN_0 #define M0_IB_GPIO_Port GPIOC #define M0_IC_Pin GPIO_PIN_1 @@ -95,22 +98,22 @@ #define GPIO_4_GPIO_Port GPIOA #define M1_TEMP_Pin GPIO_PIN_4 #define M1_TEMP_GPIO_Port GPIOA -#define AUX_I_Pin GPIO_PIN_5 -#define AUX_I_GPIO_Port GPIOA +#define AUX_TEMP_Pin GPIO_PIN_5 +#define AUX_TEMP_GPIO_Port GPIOA #define VBUS_S_Pin GPIO_PIN_6 #define VBUS_S_GPIO_Port GPIOA #define M1_AL_Pin GPIO_PIN_7 #define M1_AL_GPIO_Port GPIOA -#define AUX_TEMP_Pin GPIO_PIN_4 -#define AUX_TEMP_GPIO_Port GPIOC +#define GPIO_5_Pin GPIO_PIN_4 +#define GPIO_5_GPIO_Port GPIOC #define M0_TEMP_Pin GPIO_PIN_5 #define M0_TEMP_GPIO_Port GPIOC #define M1_BL_Pin GPIO_PIN_0 #define M1_BL_GPIO_Port GPIOB #define M1_CL_Pin GPIO_PIN_1 #define M1_CL_GPIO_Port GPIOB -#define GPIO_5_Pin GPIO_PIN_2 -#define GPIO_5_GPIO_Port GPIOB +#define GPIO_6_Pin GPIO_PIN_2 +#define GPIO_6_GPIO_Port GPIOB #define AUX_L_Pin GPIO_PIN_10 #define AUX_L_GPIO_Port GPIOB #define AUX_H_Pin GPIO_PIN_11 @@ -129,20 +132,20 @@ #define M1_BH_GPIO_Port GPIOC #define M1_CH_Pin GPIO_PIN_8 #define M1_CH_GPIO_Port GPIOC -#define M0_DC_CAL_Pin GPIO_PIN_9 -#define M0_DC_CAL_GPIO_Port GPIOC +#define M0_ENC_Z_Pin GPIO_PIN_9 +#define M0_ENC_Z_GPIO_Port GPIOC #define M0_AH_Pin GPIO_PIN_8 #define M0_AH_GPIO_Port GPIOA #define M0_BH_Pin GPIO_PIN_9 #define M0_BH_GPIO_Port GPIOA #define M0_CH_Pin GPIO_PIN_10 #define M0_CH_GPIO_Port GPIOA -#define M0_ENC_Z_Pin GPIO_PIN_15 -#define M0_ENC_Z_GPIO_Port GPIOA +#define GPIO_7_Pin GPIO_PIN_15 +#define GPIO_7_GPIO_Port GPIOA #define nFAULT_Pin GPIO_PIN_2 #define nFAULT_GPIO_Port GPIOD -#define M1_ENC_Z_Pin GPIO_PIN_3 -#define M1_ENC_Z_GPIO_Port GPIOB +#define GPIO_8_Pin GPIO_PIN_3 +#define GPIO_8_GPIO_Port GPIOB #define M0_ENC_A_Pin GPIO_PIN_4 #define M0_ENC_A_GPIO_Port GPIOB #define M0_ENC_B_Pin GPIO_PIN_5 diff --git a/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h index 8c8eff81..bd3f6305 100644 --- a/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h +++ b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h @@ -6,6 +6,7 @@ #define TIM_APB1_CLOCK_HZ 84000000 #define TIM_APB1_PERIOD_CLOCKS 4096 #define TIM_APB1_DEADTIME_CLOCKS 40 +#define configAPPLICATION_ALLOCATED_HEAP 1 #define M0_nCS_Pin GPIO_PIN_13 #define M0_nCS_GPIO_Port GPIOC diff --git a/Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h new file mode 100644 index 00000000..19428406 --- /dev/null +++ b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h @@ -0,0 +1,91 @@ + +/* Private define ------------------------------------------------------------*/ +#define TIM_1_8_CLOCK_HZ 168000000 +#define TIM_1_8_PERIOD_CLOCKS 10192 +#define TIM_1_8_DEADTIME_CLOCKS 20 +#define TIM_APB1_CLOCK_HZ 84000000 +#define TIM_APB1_PERIOD_CLOCKS 4096 +#define TIM_APB1_DEADTIME_CLOCKS 40 +#define configAPPLICATION_ALLOCATED_HEAP 1 + +#define M0_nCS_Pin GPIO_PIN_13 +#define M0_nCS_GPIO_Port GPIOC +#define M1_nCS_Pin GPIO_PIN_14 +#define M1_nCS_GPIO_Port GPIOC +#define M1_DC_CAL_Pin GPIO_PIN_15 +#define M1_DC_CAL_GPIO_Port GPIOC +#define M0_IB_Pin GPIO_PIN_0 +#define M0_IB_GPIO_Port GPIOC +#define M0_IC_Pin GPIO_PIN_1 +#define M0_IC_GPIO_Port GPIOC +#define M1_IC_Pin GPIO_PIN_2 +#define M1_IC_GPIO_Port GPIOC +#define M1_IB_Pin GPIO_PIN_3 +#define M1_IB_GPIO_Port GPIOC +#define GPIO_1_Pin GPIO_PIN_0 +#define GPIO_1_GPIO_Port GPIOA +#define GPIO_2_Pin GPIO_PIN_1 +#define GPIO_2_GPIO_Port GPIOA +#define GPIO_3_Pin GPIO_PIN_2 +#define GPIO_3_GPIO_Port GPIOA +#define GPIO_3_EXTI_IRQn EXTI2_IRQn +#define GPIO_4_Pin GPIO_PIN_3 +#define GPIO_4_GPIO_Port GPIOA +#define M1_TEMP_Pin GPIO_PIN_4 +#define M1_TEMP_GPIO_Port GPIOA +#define AUX_I_Pin GPIO_PIN_5 +#define AUX_I_GPIO_Port GPIOA +#define VBUS_S_Pin GPIO_PIN_6 +#define VBUS_S_GPIO_Port GPIOA +#define M1_AL_Pin GPIO_PIN_7 +#define M1_AL_GPIO_Port GPIOA +#define AUX_TEMP_Pin GPIO_PIN_4 +#define AUX_TEMP_GPIO_Port GPIOC +#define M0_TEMP_Pin GPIO_PIN_5 +#define M0_TEMP_GPIO_Port GPIOC +#define M1_BL_Pin GPIO_PIN_0 +#define M1_BL_GPIO_Port GPIOB +#define M1_CL_Pin GPIO_PIN_1 +#define M1_CL_GPIO_Port GPIOB +#define GPIO_5_Pin GPIO_PIN_2 +#define GPIO_5_GPIO_Port GPIOB +#define AUX_L_Pin GPIO_PIN_10 +#define AUX_L_GPIO_Port GPIOB +#define AUX_H_Pin GPIO_PIN_11 +#define AUX_H_GPIO_Port GPIOB +#define EN_GATE_Pin GPIO_PIN_12 +#define EN_GATE_GPIO_Port GPIOB +#define M0_AL_Pin GPIO_PIN_13 +#define M0_AL_GPIO_Port GPIOB +#define M0_BL_Pin GPIO_PIN_14 +#define M0_BL_GPIO_Port GPIOB +#define M0_CL_Pin GPIO_PIN_15 +#define M0_CL_GPIO_Port GPIOB +#define M1_AH_Pin GPIO_PIN_6 +#define M1_AH_GPIO_Port GPIOC +#define M1_BH_Pin GPIO_PIN_7 +#define M1_BH_GPIO_Port GPIOC +#define M1_CH_Pin GPIO_PIN_8 +#define M1_CH_GPIO_Port GPIOC +#define M0_DC_CAL_Pin GPIO_PIN_9 +#define M0_DC_CAL_GPIO_Port GPIOC +#define M0_AH_Pin GPIO_PIN_8 +#define M0_AH_GPIO_Port GPIOA +#define M0_BH_Pin GPIO_PIN_9 +#define M0_BH_GPIO_Port GPIOA +#define M0_CH_Pin GPIO_PIN_10 +#define M0_CH_GPIO_Port GPIOA +#define M0_ENC_Z_Pin GPIO_PIN_15 +#define M0_ENC_Z_GPIO_Port GPIOA +#define nFAULT_Pin GPIO_PIN_2 +#define nFAULT_GPIO_Port GPIOD +#define M1_ENC_Z_Pin GPIO_PIN_3 +#define M1_ENC_Z_GPIO_Port GPIOB +#define M0_ENC_A_Pin GPIO_PIN_4 +#define M0_ENC_A_GPIO_Port GPIOB +#define M0_ENC_B_Pin GPIO_PIN_5 +#define M0_ENC_B_GPIO_Port GPIOB +#define M1_ENC_A_Pin GPIO_PIN_6 +#define M1_ENC_A_GPIO_Port GPIOB +#define M1_ENC_B_Pin GPIO_PIN_7 +#define M1_ENC_B_GPIO_Port GPIOB diff --git a/Firmware/Board/v3/Src/adc.c b/Firmware/Board/v3/Src/adc.c index 6d0db380..bb536024 100644 --- a/Firmware/Board/v3/Src/adc.c +++ b/Firmware/Board/v3/Src/adc.c @@ -57,6 +57,9 @@ #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 #include "prev_board_ver/adc_V3_2.c" +#elif HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 3 \ +|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 4 +#include "prev_board_ver/adc_V3_4.c" #else /* USER CODE END 0 */ @@ -241,16 +244,15 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) PA4 ------> ADC1_IN4 PA5 ------> ADC1_IN5 PA6 ------> ADC1_IN6 - PC4 ------> ADC1_IN14 PC5 ------> ADC1_IN15 */ GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin - |AUX_TEMP_Pin|M0_TEMP_Pin; + |M0_TEMP_Pin; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin; + GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -278,16 +280,15 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) PA4 ------> ADC2_IN4 PA5 ------> ADC2_IN5 PA6 ------> ADC2_IN6 - PC4 ------> ADC2_IN14 PC5 ------> ADC2_IN15 */ GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin - |AUX_TEMP_Pin|M0_TEMP_Pin; + |M0_TEMP_Pin; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin; + GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin; GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -346,13 +347,12 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) PA4 ------> ADC1_IN4 PA5 ------> ADC1_IN5 PA6 ------> ADC1_IN6 - PC4 ------> ADC1_IN14 PC5 ------> ADC1_IN15 */ HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin - |AUX_TEMP_Pin|M0_TEMP_Pin); + |M0_TEMP_Pin); - HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin); + HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin); /* ADC1 interrupt Deinit */ /* USER CODE BEGIN ADC1:ADC_IRQn disable */ @@ -383,13 +383,12 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) PA4 ------> ADC2_IN4 PA5 ------> ADC2_IN5 PA6 ------> ADC2_IN6 - PC4 ------> ADC2_IN14 PC5 ------> ADC2_IN15 */ HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin - |AUX_TEMP_Pin|M0_TEMP_Pin); + |M0_TEMP_Pin); - HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin); + HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin); /* ADC2 interrupt Deinit */ /* USER CODE BEGIN ADC2:ADC_IRQn disable */ diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 9585749f..91b3f3cd 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -55,6 +55,9 @@ #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 #include "prev_board_ver/gpio_V3_2.c" +#elif HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 3 \ +|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 4 +#include "prev_board_ver/gpio_V3_4.c" #else /* USER CODE END 0 */ @@ -87,19 +90,22 @@ void MX_GPIO_Init(void) /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(GPIOC, M0_nCS_Pin|M1_nCS_Pin, GPIO_PIN_SET); - /*Configure GPIO pin Output Level */ - HAL_GPIO_WritePin(GPIOC, M1_DC_CAL_Pin|M0_DC_CAL_Pin, GPIO_PIN_RESET); - /*Configure GPIO pin Output Level */ HAL_GPIO_WritePin(EN_GATE_GPIO_Port, EN_GATE_Pin, GPIO_PIN_RESET); - /*Configure GPIO pins : PCPin PCPin PCPin PCPin */ - GPIO_InitStruct.Pin = M0_nCS_Pin|M1_nCS_Pin|M1_DC_CAL_Pin|M0_DC_CAL_Pin; + /*Configure GPIO pins : PCPin PCPin */ + GPIO_InitStruct.Pin = M0_nCS_Pin|M1_nCS_Pin; GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; GPIO_InitStruct.Pull = GPIO_NOPULL; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + /*Configure GPIO pins : PCPin PCPin PCPin */ + GPIO_InitStruct.Pin = M1_ENC_Z_Pin|GPIO_5_Pin|M0_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + /*Configure GPIO pin : PtPin */ GPIO_InitStruct.Pin = GPIO_3_Pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; @@ -107,13 +113,13 @@ void MX_GPIO_Init(void) HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct); /*Configure GPIO pins : PAPin PAPin */ - GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin; + GPIO_InitStruct.Pin = GPIO_4_Pin|GPIO_7_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); /*Configure GPIO pins : PBPin PBPin */ - GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin; + GPIO_InitStruct.Pin = GPIO_6_Pin|GPIO_8_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); diff --git a/Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c b/Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c new file mode 100644 index 00000000..49862c97 --- /dev/null +++ b/Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c @@ -0,0 +1,375 @@ + +ADC_HandleTypeDef hadc1; +ADC_HandleTypeDef hadc2; +ADC_HandleTypeDef hadc3; + +/* ADC1 init function */ +void MX_ADC1_Init(void) +{ + ADC_ChannelConfTypeDef sConfig; + ADC_InjectionConfTypeDef sConfigInjected; + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc1.Instance = ADC1; + hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc1.Init.Resolution = ADC_RESOLUTION_12B; + hadc1.Init.ScanConvMode = DISABLE; + hadc1.Init.ContinuousConvMode = DISABLE; + hadc1.Init.DiscontinuousConvMode = DISABLE; + hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START; + hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc1.Init.NbrOfConversion = 1; + hadc1.Init.DMAContinuousRequests = DISABLE; + hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if (HAL_ADC_Init(&hadc1) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. + */ + sConfig.Channel = ADC_CHANNEL_6; + sConfig.Rank = 1; + sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; + if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedChannel = ADC_CHANNEL_6; + sConfigInjected.InjectedRank = 1; + sConfigInjected.InjectedNbrOfConversion = 1; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + +} +/* ADC2 init function */ +void MX_ADC2_Init(void) +{ + ADC_ChannelConfTypeDef sConfig; + ADC_InjectionConfTypeDef sConfigInjected; + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc2.Instance = ADC2; + hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc2.Init.Resolution = ADC_RESOLUTION_12B; + hadc2.Init.ScanConvMode = DISABLE; + hadc2.Init.ContinuousConvMode = DISABLE; + hadc2.Init.DiscontinuousConvMode = DISABLE; + hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; + hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc2.Init.NbrOfConversion = 1; + hadc2.Init.DMAContinuousRequests = DISABLE; + hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if (HAL_ADC_Init(&hadc2) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. + */ + sConfig.Channel = ADC_CHANNEL_13; + sConfig.Rank = 1; + sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; + if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedChannel = ADC_CHANNEL_10; + sConfigInjected.InjectedRank = 1; + sConfigInjected.InjectedNbrOfConversion = 1; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + if (HAL_ADCEx_InjectedConfigChannel(&hadc2, &sConfigInjected) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + +} +/* ADC3 init function */ +void MX_ADC3_Init(void) +{ + ADC_ChannelConfTypeDef sConfig; + ADC_InjectionConfTypeDef sConfigInjected; + + /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) + */ + hadc3.Instance = ADC3; + hadc3.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + hadc3.Init.Resolution = ADC_RESOLUTION_12B; + hadc3.Init.ScanConvMode = DISABLE; + hadc3.Init.ContinuousConvMode = DISABLE; + hadc3.Init.DiscontinuousConvMode = DISABLE; + hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; + hadc3.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO; + hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT; + hadc3.Init.NbrOfConversion = 1; + hadc3.Init.DMAContinuousRequests = DISABLE; + hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV; + if (HAL_ADC_Init(&hadc3) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. + */ + sConfig.Channel = ADC_CHANNEL_12; + sConfig.Rank = 1; + sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES; + if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + /**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time + */ + sConfigInjected.InjectedChannel = ADC_CHANNEL_11; + sConfigInjected.InjectedRank = 1; + sConfigInjected.InjectedNbrOfConversion = 1; + sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES; + sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING; + sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO; + sConfigInjected.AutoInjectedConv = DISABLE; + sConfigInjected.InjectedDiscontinuousConvMode = DISABLE; + sConfigInjected.InjectedOffset = 0; + if (HAL_ADCEx_InjectedConfigChannel(&hadc3, &sConfigInjected) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + +} + +void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct; + if(adcHandle->Instance==ADC1) + { + /* USER CODE BEGIN ADC1_MspInit 0 */ + + /* USER CODE END ADC1_MspInit 0 */ + /* ADC1 clock enable */ + __HAL_RCC_ADC1_CLK_ENABLE(); + + /**ADC1 GPIO Configuration + PC0 ------> ADC1_IN10 + PC1 ------> ADC1_IN11 + PC2 ------> ADC1_IN12 + PC3 ------> ADC1_IN13 + PA4 ------> ADC1_IN4 + PA5 ------> ADC1_IN5 + PA6 ------> ADC1_IN6 + PC4 ------> ADC1_IN14 + PC5 ------> ADC1_IN15 + */ + GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin + |AUX_TEMP_Pin|M0_TEMP_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* ADC1 interrupt Init */ + HAL_NVIC_SetPriority(ADC_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + /* USER CODE BEGIN ADC1_MspInit 1 */ + + /* USER CODE END ADC1_MspInit 1 */ + } + else if(adcHandle->Instance==ADC2) + { + /* USER CODE BEGIN ADC2_MspInit 0 */ + + /* USER CODE END ADC2_MspInit 0 */ + /* ADC2 clock enable */ + __HAL_RCC_ADC2_CLK_ENABLE(); + + /**ADC2 GPIO Configuration + PC0 ------> ADC2_IN10 + PC1 ------> ADC2_IN11 + PC2 ------> ADC2_IN12 + PC3 ------> ADC2_IN13 + PA4 ------> ADC2_IN4 + PA5 ------> ADC2_IN5 + PA6 ------> ADC2_IN6 + PC4 ------> ADC2_IN14 + PC5 ------> ADC2_IN15 + */ + GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin + |AUX_TEMP_Pin|M0_TEMP_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* ADC2 interrupt Init */ + HAL_NVIC_SetPriority(ADC_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + /* USER CODE BEGIN ADC2_MspInit 1 */ + + /* USER CODE END ADC2_MspInit 1 */ + } + else if(adcHandle->Instance==ADC3) + { + /* USER CODE BEGIN ADC3_MspInit 0 */ + + /* USER CODE END ADC3_MspInit 0 */ + /* ADC3 clock enable */ + __HAL_RCC_ADC3_CLK_ENABLE(); + + /**ADC3 GPIO Configuration + PC0 ------> ADC3_IN10 + PC1 ------> ADC3_IN11 + PC2 ------> ADC3_IN12 + PC3 ------> ADC3_IN13 + */ + GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + /* ADC3 interrupt Init */ + HAL_NVIC_SetPriority(ADC_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(ADC_IRQn); + /* USER CODE BEGIN ADC3_MspInit 1 */ + + /* USER CODE END ADC3_MspInit 1 */ + } +} + +void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle) +{ + + if(adcHandle->Instance==ADC1) + { + /* USER CODE BEGIN ADC1_MspDeInit 0 */ + + /* USER CODE END ADC1_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_ADC1_CLK_DISABLE(); + + /**ADC1 GPIO Configuration + PC0 ------> ADC1_IN10 + PC1 ------> ADC1_IN11 + PC2 ------> ADC1_IN12 + PC3 ------> ADC1_IN13 + PA4 ------> ADC1_IN4 + PA5 ------> ADC1_IN5 + PA6 ------> ADC1_IN6 + PC4 ------> ADC1_IN14 + PC5 ------> ADC1_IN15 + */ + HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin + |AUX_TEMP_Pin|M0_TEMP_Pin); + + HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin); + + /* ADC1 interrupt Deinit */ + /* USER CODE BEGIN ADC1:ADC_IRQn disable */ + /** + * Uncomment the line below to disable the "ADC_IRQn" interrupt + * Be aware, disabling shared interrupt may affect other IPs + */ + /* HAL_NVIC_DisableIRQ(ADC_IRQn); */ + /* USER CODE END ADC1:ADC_IRQn disable */ + + /* USER CODE BEGIN ADC1_MspDeInit 1 */ + + /* USER CODE END ADC1_MspDeInit 1 */ + } + else if(adcHandle->Instance==ADC2) + { + /* USER CODE BEGIN ADC2_MspDeInit 0 */ + + /* USER CODE END ADC2_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_ADC2_CLK_DISABLE(); + + /**ADC2 GPIO Configuration + PC0 ------> ADC2_IN10 + PC1 ------> ADC2_IN11 + PC2 ------> ADC2_IN12 + PC3 ------> ADC2_IN13 + PA4 ------> ADC2_IN4 + PA5 ------> ADC2_IN5 + PA6 ------> ADC2_IN6 + PC4 ------> ADC2_IN14 + PC5 ------> ADC2_IN15 + */ + HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin + |AUX_TEMP_Pin|M0_TEMP_Pin); + + HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin); + + /* ADC2 interrupt Deinit */ + /* USER CODE BEGIN ADC2:ADC_IRQn disable */ + /** + * Uncomment the line below to disable the "ADC_IRQn" interrupt + * Be aware, disabling shared interrupt may affect other IPs + */ + /* HAL_NVIC_DisableIRQ(ADC_IRQn); */ + /* USER CODE END ADC2:ADC_IRQn disable */ + + /* USER CODE BEGIN ADC2_MspDeInit 1 */ + + /* USER CODE END ADC2_MspDeInit 1 */ + } + else if(adcHandle->Instance==ADC3) + { + /* USER CODE BEGIN ADC3_MspDeInit 0 */ + + /* USER CODE END ADC3_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_ADC3_CLK_DISABLE(); + + /**ADC3 GPIO Configuration + PC0 ------> ADC3_IN10 + PC1 ------> ADC3_IN11 + PC2 ------> ADC3_IN12 + PC3 ------> ADC3_IN13 + */ + HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin); + + /* ADC3 interrupt Deinit */ + /* USER CODE BEGIN ADC3:ADC_IRQn disable */ + /** + * Uncomment the line below to disable the "ADC_IRQn" interrupt + * Be aware, disabling shared interrupt may affect other IPs + */ + /* HAL_NVIC_DisableIRQ(ADC_IRQn); */ + /* USER CODE END ADC3:ADC_IRQn disable */ + + /* USER CODE BEGIN ADC3_MspDeInit 1 */ + + /* USER CODE END ADC3_MspDeInit 1 */ + } +} diff --git a/Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c b/Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c new file mode 100644 index 00000000..075be197 --- /dev/null +++ b/Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c @@ -0,0 +1,71 @@ +/** Configure pins as + * Analog + * Input + * Output + * EVENT_OUT + * EXTI +*/ +void MX_GPIO_Init(void) +{ + + GPIO_InitTypeDef GPIO_InitStruct; + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOH_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOC, M0_nCS_Pin|M1_nCS_Pin, GPIO_PIN_SET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(GPIOC, M1_DC_CAL_Pin|M0_DC_CAL_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(EN_GATE_GPIO_Port, EN_GATE_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pins : PCPin PCPin PCPin PCPin */ + GPIO_InitStruct.Pin = M0_nCS_Pin|M1_nCS_Pin|M1_DC_CAL_Pin|M0_DC_CAL_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + /*Configure GPIO pin : PtPin */ + GPIO_InitStruct.Pin = GPIO_3_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; + HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pins : PAPin PAPin */ + GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /*Configure GPIO pins : PBPin PBPin */ + GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /*Configure GPIO pin : PtPin */ + GPIO_InitStruct.Pin = EN_GATE_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(EN_GATE_GPIO_Port, &GPIO_InitStruct); + + /*Configure GPIO pin : PtPin */ + GPIO_InitStruct.Pin = nFAULT_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_PULLUP; + HAL_GPIO_Init(nFAULT_GPIO_Port, &GPIO_InitStruct); + + /* EXTI interrupt init*/ + HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0); + HAL_NVIC_EnableIRQ(EXTI2_IRQn); + +} From ab27552c0ebf7e08d626b5b4c001ebabaddcfc01 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 17:26:51 -0700 Subject: [PATCH 051/112] add v3.5 support to build system --- Firmware/README.md | 2 +- Firmware/Tupfile.lua | 8 ++++++++ Firmware/tup.config.default | 2 +- 3 files changed, 10 insertions(+), 2 deletions(-) diff --git a/Firmware/README.md b/Firmware/README.md index 0346a9bf..39f6b948 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -30,7 +30,7 @@ In this section we will set the compile-time parameters, later we will also set To customize the compile time parameters, copy or rename the file `Firmware/tup.config.default` to `Firmware/tup.config` and edit the parameters in that file: -__CONFIG_BOARD_VERSION__: The board version you're using. Can be `v3.1`, `v3.2`, `v3.3`, `v3.4-24V` or `v3.4-48V`. Check for a label on the upper side of the ODrive to find out which version you have. +__CONFIG_BOARD_VERSION__: The board version you're using. Can be `v3.1`, `v3.2`, `v3.3`, `v3.4-24V`, `v3.4-48V`, `v3.5-24V` or `v3.5-48V`. Check for a label on the upper side of the ODrive to find out which version you have. __CONFIG_USB_PROTOCOL__: Defines which protocol the ODrive should use on the USB interface. * `native`: The native ODrive protocol. Use this if you want to use the python tools in this repo. diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index aa122ca5..b57acb42 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -23,6 +23,14 @@ elseif boardversion == "v3.4-48V" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=4" FLAGS += "-DHW_VERSION_VOLTAGE=48" +elseif boardversion == "v3.5-24V" then + boarddir = 'Board/v3' + FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=5" + FLAGS += "-DHW_VERSION_VOLTAGE=24" +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 == "" then error("board version not specified - take a look at tup.config.default") else diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default index be0515fc..5cd89434 100644 --- a/Firmware/tup.config.default +++ b/Firmware/tup.config.default @@ -1,6 +1,6 @@ # Copy this file to tup.config and adapt it to your needs # make sure this fits your board -#CONFIG_BOARD_VERSION=v3.4-24V +#CONFIG_BOARD_VERSION=v3.5-24V CONFIG_USB_PROTOCOL=native CONFIG_UART_PROTOCOL=ascii CONFIG_STEP_DIR=n From 09f361de61d4670e5f94c52df52e1672a5c478cd Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 19:32:36 -0700 Subject: [PATCH 052/112] fix encoder offset calibration and error handling --- Firmware/MotorControl/axis.cpp | 6 ++++++ Firmware/MotorControl/encoder.cpp | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index fe3760aa..c0488dfa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -251,14 +251,20 @@ void Axis::run_state_machine_loop() { switch (current_state_) { case AXIS_STATE_MOTOR_CALIBRATION: status = motor_.run_calibration(); + if (!status) + error_ |= ERROR_MOTOR_FAILED; break; case AXIS_STATE_ENCODER_INDEX_SEARCH: status = encoder_.run_index_search(); + if (!status) + error_ |= ERROR_ENCODER_FAILED; break; case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: status = encoder_.run_offset_calibration(); + if (!status) + error_ |= ERROR_ENCODER_FAILED; break; case AXIS_STATE_SENSORLESS_CONTROL: diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 19a39792..fad27267 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -103,7 +103,7 @@ bool Encoder::run_offset_calibration() { // Temporarily disable index search so it doesn't mess // with the offset calibration bool old_use_index = config_.use_index; - config_.use_index = true; + config_.use_index = false; float voltage_magnitude; if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) From 282d824545a7f6960817f36c6d32b665405ef895 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 19:35:56 -0700 Subject: [PATCH 053/112] improve error detection --- tools/odrive/tests.py | 12 ++++++++++-- tools/run_tests.py | 6 +++--- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 0cdc121f..3454b56a 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -64,6 +64,8 @@ def test_assert_no_error(axis_ctx: AxisTestContext): errors.append("sensorless_estimator failed with error {:04X}".format(axis_ctx.handle.sensorless_estimator.error)) if axis_ctx.handle.error != 0: errors.append("axis failed with error {:04X}".format(axis_ctx.handle.error)) + elif len(errors) > 0: + errors.append("and by the way: axis reports no error even though there is one") if len(errors) > 0: raise TestFailed("\n".join(errors)) @@ -175,6 +177,12 @@ class TestFlashAndErase(ODriveTest): def __init__(self): ODriveTest.__init__(self, exclusive=True) def run_test(self, odrv_ctx: ODriveTestContext, logger): + # Set board-version and compile + with open("tup.config", mode="w") as tup_config: + tup_config.write("CONFIG_STRICT=true\n") + tup_config.write("CONFIG_BOARD_VERSION={}\n".format(odrv_ctx.yaml['board-version'])) + #exit(1) + run("make", logger, timeout=10) run("make flash PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=20) # FIXME: device does not reboot correctly after erasing config this way #run("make erase_config PROGRAMMER='" + test_rig.programmer + "'", timeout=10) @@ -248,7 +256,7 @@ class TestMotorCalibration(AxisTest): request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION) time.sleep(6) test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) - test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_NO_ERROR) + test_assert_no_error(axis_ctx) test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2) test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) axis_ctx.handle.motor.config.pre_calibrated = True @@ -282,7 +290,7 @@ class TestEncoderOffsetCalibration(AxisTest): # TODO: ensure the encoder calibration doesn't do crap time.sleep(11) test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) - test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_NO_ERROR) + test_assert_no_error(axis_ctx) test_assert_eq(axis_ctx.handle.motor.config.direction, axis_ctx.yaml['motor-direction']) axis_ctx.handle.encoder.config.pre_calibrated = True diff --git a/tools/run_tests.py b/tools/run_tests.py index 968000e6..394201d0 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -84,7 +84,7 @@ try: for odrv in odrives_by_name: odrv_test_thread(odrv) else: - for_all_parallel(odrives_by_name, lambda x: x, odrv_test_thread) + for_all_parallel(odrives_by_name, lambda x: type(test).__name__ + " on " + x, odrv_test_thread) elif isinstance(test, AxisTest): def axis_test_thread(axis_name): @@ -112,7 +112,7 @@ try: for conflicting_axis in conflicting_axes: conflicting_axis.lock.release() - for_all_parallel(axes_by_name, lambda x: x, axis_test_thread) + for_all_parallel(axes_by_name, lambda x: type(test).__name__ + " on " + x, axis_test_thread) elif isinstance(test, DualAxisTest): def dual_axis_test_thread(coupling): @@ -138,7 +138,7 @@ try: for axis_ctx in coupled_axes: axis_ctx.lock.release() - for_all_parallel(couplings, lambda x: "..".join([a.name for a in x]), dual_axis_test_thread) + for_all_parallel(couplings, lambda x: type(test).__name__ + " on " + "..".join([a.name for a in x]), dual_axis_test_thread) else: logger.warn("ignoring unknown test type {}".format(type(test))) From ab5e68697559794b95fc5b8484d26d4655870b07 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 19:38:19 -0700 Subject: [PATCH 054/112] upgrade test-rig.yaml to v3.5 --- tools/test-rig.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tools/test-rig.yaml b/tools/test-rig.yaml index 05e26696..2c05cc9d 100644 --- a/tools/test-rig.yaml +++ b/tools/test-rig.yaml @@ -2,8 +2,8 @@ # ODrives odrives: - name: top-odrive - board-version: v3.4-24V - serial-number: "385F324D3037" + board-version: v3.5-48V + serial-number: "3660335E3037" brake-resistance: 0.47 uart: /dev/serial/by-id/[not-yet-used] usb: auto @@ -26,8 +26,8 @@ odrives: motor-max-current: 50 encoder-cpr: 8192 - name: bottom-odrive - board-version: v3.4-48V - serial-number: "306A396A3235" + board-version: v3.5-24V + serial-number: "3661335E3037" brake-resistance: 0.47 uart: /dev/serial/by-id/[not-yet-used] usb: auto From fa8f4e99151cd04bac8a88e6a2e9039fd5a27b75 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 21:48:55 -0700 Subject: [PATCH 055/112] improve response time for auto-securing the test rig when something goes wrong --- tools/run_tests.py | 53 +++++++++++++++++++++++++++++----------------- 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/tools/run_tests.py b/tools/run_tests.py index 394201d0..dca6b39d 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -13,7 +13,7 @@ import sys import threading import traceback from odrive.tests import * -from odrive.utils import Logger, for_all_parallel +from odrive.utils import Logger, for_all_parallel, Event all_tests = [ @@ -65,6 +65,7 @@ else: for coupling in test_rig_yaml['couplings']: couplings.append([axes_by_name[axis_name] for axis_name in coupling]) +app_shutdown_token = Event() try: for test in all_tests: @@ -98,15 +99,21 @@ try: for conflicting_axis in conflicting_axes: conflicting_axis.lock.acquire() try: - # Run test on this axis - logger.info('● running {} on {}...'.format(type(test).__name__, axis_name)) - try: - test.check_preconditions(axis_ctx, + if not app_shutdown_token.is_set(): + # Run test on this axis + logger.info('● running {} on {}...'.format(type(test).__name__, axis_name)) + try: + test.check_preconditions(axis_ctx, + logger.indent(' {}: '.format(axis_name))) + except: + raise PreconditionsNotMet() + test.run_test(axis_ctx, logger.indent(' {}: '.format(axis_name))) - except: - raise PreconditionsNotMet() - test.run_test(axis_ctx, - logger.indent(' {}: '.format(axis_name))) + else: + logger.warn('⬛ skipping {} on {}'.format(type(test).__name__, axis_name)) + except: + app_shutdown_token.set() + raise finally: # Release all conflicting axes for conflicting_axis in conflicting_axes: @@ -124,15 +131,21 @@ try: for axis_ctx in coupled_axes: axis_ctx.lock.acquire() try: - # Run test on this axis - logger.info('● running {} on {}...'.format(type(test).__name__, coupling_name)) - try: - test.check_preconditions(coupled_axes[0], coupled_axes[1], + if not app_shutdown_token.is_set(): + # Run test on this axis + logger.info('● running {} on {}...'.format(type(test).__name__, coupling_name)) + try: + test.check_preconditions(coupled_axes[0], coupled_axes[1], + logger.indent(' {}: '.format(coupling_name))) + except: + raise PreconditionsNotMet() + test.run_test(coupled_axes[0], coupled_axes[1], logger.indent(' {}: '.format(coupling_name))) - except: - raise PreconditionsNotMet() - test.run_test(coupled_axes[0], coupled_axes[1], - logger.indent(' {}: '.format(coupling_name))) + else: + logger.warn('⬛ skipping {} on {}...'.format(type(test).__name__, coupling_name)) + except: + app_shutdown_token.set() + raise finally: # Release all conflicting axes for axis_ctx in coupled_axes: @@ -147,11 +160,13 @@ except: logger.error(traceback.format_exc()) logger.debug('=> Test failed. Please wait while I secure the test rig...') try: - dont_secure_after_failure = True # TODO: disable + dont_secure_after_failure = False # TODO: disable if not dont_secure_after_failure: def odrv_reset_thread(odrv_name): odrv_ctx = odrives_by_name[odrv_name] - run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30) + #run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30) + odrv_ctx.handle.axis0.requested_state = AXIS_STATE_IDLE + odrv_ctx.handle.axis1.requested_state = AXIS_STATE_IDLE for_all_parallel(odrives_by_name, lambda x: x['name'], odrv_reset_thread) except: logger.error('///////////////////////////////////////////') From 69b06dde89f2f0058bd3e9b45e464a61856b7bad Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 21:50:52 -0700 Subject: [PATCH 056/112] implement high velocity test --- tools/odrive/tests.py | 78 +++++++++++++++++++++++++++++++++++++++++++ tools/run_tests.py | 3 +- 2 files changed, 80 insertions(+), 1 deletion(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 3454b56a..f13c5d71 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -353,6 +353,84 @@ class TestStoreAndReboot(ODriveTest): test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.15) test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) + +class TestHighVelocityCtrl(AxisTest): + """ + Spins the motor up to it's max speed during a period of 10s. + The commanded max speed is based on the motor's KV rating and nominal V_bus, + however due to several factors the theoretical limit is about 72% of that. + The test passes if the motor follows the commanded ramp closely up to 90% of + the theoretical limit (and if no errors occur along the way). + """ + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestHighVelocityCtrl, self).check_preconditions(axis_ctx, logger) + test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) + test_assert_eq(axis_ctx.handle.encoder.is_ready, True) + + def run_test(self, axis_ctx: AxisTestContext, logger): + # Calculate theoretical max velocity in encoder counts per second based on the nominal + # V_bus and motor KV rating + max_rpm = axis_ctx.odrv_ctx.yaml['vbus-voltage'] * axis_ctx.yaml['motor-kv'] + rated_limit = max_rpm / 60 * axis_ctx.yaml['encoder-cpr'] + expected_limit = rated_limit + + # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) + # whereas the ODrive modulates the space vector around a circular trajectory. + # See Fig 4.28 here: http://krex.k-state.edu/dspace/bitstream/handle/2097/1507/JamesMevey2009.pdf + expected_limit *= (2/math.sqrt(3)) / (4/math.pi) # roughtly 90% + + # The ODrive only goes to 80% modulation depth in order to save some time for the ADC measurements. + # See FOC_current in motor.cpp. + expected_limit *= 0.8 + + # Add a 10% margin to account for + expected_limit *= 0.9 + + logger.debug("rated max speed: {}, expected max speed: >= {}".format(rated_limit, expected_limit)) + #theoretical_limit = 100000 + + # Set the current limit accordingly so we don't burn the brake resistor while slowing down + set_limits(axis_ctx, logger, vel_limit=rated_limit, current_limit=50) + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + + ramp_up_time = 20.0 + t_0 = time.monotonic() + last_print = t_0 + max_true_vel = 0.0 + while True: + ratio = (time.monotonic() - t_0) / ramp_up_time + if ratio >= 1: + break + + # While ramping up we want to remain within +-5% of the setpoint. + # However we accept if we can only approach 80% of the theoretical limit. + vel_setpoint = ratio * rated_limit + vel_range = max(0.05*vel_setpoint, 5000) + if vel_setpoint - vel_range > expected_limit: + vel_range = vel_setpoint - expected_limit + + # log progress + if time.monotonic() - last_print > 1: + last_print = time.monotonic() + logger.debug("ramping up: now at " + str(vel_setpoint)) + + # set and measure velocity + axis_ctx.handle.controller.set_vel_setpoint(vel_setpoint, 0) + true_vel = axis_ctx.handle.encoder.pll_vel + max_true_vel = max(true_vel, max_true_vel) + test_assert_eq(true_vel, vel_setpoint, range=vel_range) + test_assert_no_error(axis_ctx) + + time.sleep(0.001) + + axis_ctx.handle.controller.set_vel_setpoint(0, 0) + time.sleep(0.5) + # TODO: this is not a good bound, but the encoder float resolution results in a bad velocity estimate after this many turns + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=2000) + request_state(axis_ctx, AXIS_STATE_IDLE) + + class TestVelCtrlVsPosCtrl(DualAxisTest): """ Uses one ODrive as a load operating in velocity control mode. diff --git a/tools/run_tests.py b/tools/run_tests.py index dca6b39d..f0ee3bb1 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -29,7 +29,8 @@ all_tests = [ # TestClosedLoopControl(), TestDiscoverAndGotoIdle(), # for testing TestEncoderOffsetCalibration(pass_if_ready=True), - TestVelCtrlVsPosCtrl() + TestHighVelocityCtrl(), +# TestVelCtrlVsPosCtrl() # TODO: test step/dir # TODO: test sensorless # TODO: test ASCII protocol From 23009d1e933aed6c22386395c8b9aeaf27f25d84 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 10 Apr 2018 00:25:10 -0700 Subject: [PATCH 057/112] add overvoltage protection The motor phases will go into floating state as soon as an overvoltage condition is detected. --- Firmware/MotorControl/axis.cpp | 9 +++------ Firmware/MotorControl/axis.hpp | 2 -- Firmware/MotorControl/communication.cpp | 4 +++- Firmware/MotorControl/odrive_main.hpp | 5 +++++ 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index c0488dfa..bde389d4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -92,18 +92,15 @@ void Axis::set_step_dir_enabled(bool enable) { } } -// @brief Returns true if the power supply is within range -bool Axis::check_PSU_brownout() { - return vbus_voltage >= config_.dc_bus_brownout_trip_level; -} - // @brief Returns true if everything is ok. // Sets error and returns false otherwise. bool Axis::do_checks() { if (!motor_.do_checks()) return error_ |= ERROR_MOTOR_FAILED, false; - if (!check_PSU_brownout()) + if (!(vbus_voltage >= board_config.dc_bus_undervoltage_trip_level)) return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false; + if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level)) + return error_ |= ERROR_DC_BUS_OVER_VOLTAGE, false; return true; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index f6415422..72dac9b3 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -30,7 +30,6 @@ struct AxisConfig_t { // For M0 this has no effect if enable_uart is true float counts_per_step = 2.0f; - float dc_bus_brownout_trip_level = 8.0f; //make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 3ee774f3..43a55a99 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -23,6 +23,11 @@ struct BoardConfig_t { bool enable_uart = true; float brake_resistance = 0.47f; // [ohm] + float dc_bus_undervoltage_trip_level = 8.0f; // Date: Tue, 10 Apr 2018 00:25:56 -0700 Subject: [PATCH 058/112] add high velocity test with load --- tools/odrive/tests.py | 98 ++++++++++++++++++++++++++++++++++--------- tools/run_tests.py | 3 +- 2 files changed, 80 insertions(+), 21 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index f13c5d71..64237768 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -143,6 +143,8 @@ class AxisTest(ABC): logger.warn("axis still in motion, delaying 2 sec...") time.sleep(2) test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) @abc.abstractmethod def run_test(self, axis_ctx: AxisTestContext, logger): @@ -222,6 +224,10 @@ class TestSetup(ODriveTest): test_assert_eq(odrv_ctx.handle.config.brake_resistance, 1.0) odrv_ctx.handle.config.brake_resistance = odrv_ctx.yaml['brake-resistance'] test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) + odrv_ctx.handle.config.dc_bus_undervoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 0.92 + odrv_ctx.handle.config.dc_bus_overvoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 1.08 + test_assert_eq(odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) + test_assert_eq(odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) # firmware has 1500ms startup delay time.sleep(2) @@ -354,7 +360,7 @@ class TestStoreAndReboot(ODriveTest): test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) -class TestHighVelocityCtrl(AxisTest): +class TestHighVelocity(AxisTest): """ Spins the motor up to it's max speed during a period of 10s. The commanded max speed is based on the motor's KV rating and nominal V_bus, @@ -362,8 +368,18 @@ class TestHighVelocityCtrl(AxisTest): The test passes if the motor follows the commanded ramp closely up to 90% of the theoretical limit (and if no errors occur along the way). """ + def __init__(self, override_current_limit=None, load_current=0, brake=True): + """ + param override_current_limit: If None, the test selects a current limit that is guaranteed + not to fry the brake resistor. If you override the limit, you're + on your own. + """ + self._override_current_limit = override_current_limit + self._load_current = load_current + self._brake = brake + def check_preconditions(self, axis_ctx: AxisTestContext, logger): - super(TestHighVelocityCtrl, self).check_preconditions(axis_ctx, logger) + super(TestHighVelocity, self).check_preconditions(axis_ctx, logger) test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) test_assert_eq(axis_ctx.handle.encoder.is_ready, True) @@ -372,6 +388,7 @@ class TestHighVelocityCtrl(AxisTest): # V_bus and motor KV rating max_rpm = axis_ctx.odrv_ctx.yaml['vbus-voltage'] * axis_ctx.yaml['motor-kv'] rated_limit = max_rpm / 60 * axis_ctx.yaml['encoder-cpr'] + rated_limit *= 0.5 # TODO: remove this later, but for now we want to stay away from the modulation depth limit expected_limit = rated_limit # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) @@ -390,14 +407,18 @@ class TestHighVelocityCtrl(AxisTest): #theoretical_limit = 100000 # Set the current limit accordingly so we don't burn the brake resistor while slowing down - set_limits(axis_ctx, logger, vel_limit=rated_limit, current_limit=50) + if self._override_current_limit is None: + set_limits(axis_ctx, logger, vel_limit=rated_limit, current_limit=50) + else: + axis_ctx.handle.motor.config.current_lim = self._override_current_limit + axis_ctx.handle.controller.config.vel_limit = rated_limit request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) ramp_up_time = 20.0 t_0 = time.monotonic() last_print = t_0 - max_true_vel = 0.0 + max_measured_vel = 0.0 while True: ratio = (time.monotonic() - t_0) / ramp_up_time if ratio >= 1: @@ -406,29 +427,66 @@ class TestHighVelocityCtrl(AxisTest): # While ramping up we want to remain within +-5% of the setpoint. # However we accept if we can only approach 80% of the theoretical limit. vel_setpoint = ratio * rated_limit - vel_range = max(0.05*vel_setpoint, 5000) - if vel_setpoint - vel_range > expected_limit: - vel_range = vel_setpoint - expected_limit + expected_velocity = max(vel_setpoint - rated_limit / ramp_up_time * self._load_current / 20, 0) + vel_range = max(0.05*expected_velocity, 50000) + if expected_velocity - vel_range > expected_limit: + vel_range = expected_velocity - expected_limit + + # set and measure velocity + axis_ctx.handle.controller.set_vel_setpoint(vel_setpoint, 0) + measured_vel = axis_ctx.handle.encoder.pll_vel + max_measured_vel = max(measured_vel, max_measured_vel) + test_assert_eq(measured_vel, expected_velocity, range=vel_range) + test_assert_no_error(axis_ctx) # log progress if time.monotonic() - last_print > 1: last_print = time.monotonic() - logger.debug("ramping up: now at " + str(vel_setpoint)) - - # set and measure velocity - axis_ctx.handle.controller.set_vel_setpoint(vel_setpoint, 0) - true_vel = axis_ctx.handle.encoder.pll_vel - max_true_vel = max(true_vel, max_true_vel) - test_assert_eq(true_vel, vel_setpoint, range=vel_range) - test_assert_no_error(axis_ctx) + logger.debug("ramping up: commanded {}, expected {}, measured {} ".format(vel_setpoint, expected_velocity, measured_vel)) time.sleep(0.001) - axis_ctx.handle.controller.set_vel_setpoint(0, 0) - time.sleep(0.5) - # TODO: this is not a good bound, but the encoder float resolution results in a bad velocity estimate after this many turns - test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=2000) - request_state(axis_ctx, AXIS_STATE_IDLE) + logger.debug("reached top speed of {} counts/sec".format(max_measured_vel)) + + if self._brake: + axis_ctx.handle.controller.set_vel_setpoint(0, 0) + time.sleep(0.5) + # If the velocity integrator at work, it may now work against slowing down. + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=rated_limit*0.3) + # TODO: this is not a good bound, but the encoder float resolution results in a bad velocity estimate after this many turns + time.sleep(0.5) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=2000) + request_state(axis_ctx, AXIS_STATE_IDLE) + test_assert_no_error(axis_ctx) + + +class TestHighVelocityInViscousFluid(DualAxisTest): + """ + Runs TestHighVelocity on one motor while using the other motor as a load. + The load is created by running velocity control with setpoint 0. + """ + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + load_ctx = axis0_ctx + driver_ctx = axis1_ctx + + # Set up viscous fluid load + logger.debug("activating load on {}...".format(load_ctx.name)) + load_ctx.handle.controller.config.vel_integrator_gain = 0 + load_ctx.handle.controller.vel_integrator_current = 0 + load_ctx.handle.controller.config.vel_limit = 20000 # this is not really relevant + load_ctx.handle.motor.config.current_lim = 20 + load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus + load_ctx.handle.controller.set_vel_setpoint(0, 0) + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + driver_test = TestHighVelocity(override_current_limit=40, load_current=20, brake=False) + driver_test.check_preconditions(driver_ctx, logger) + driver_test.run_test(driver_ctx, logger) + + # put load to idle as quickly as possible, otherwise, because the brake resistor is disabled, + # it will try to put the braking power into the power rail where it has nowhere to go. + request_state(load_ctx, AXIS_STATE_IDLE) + request_state(driver_ctx, AXIS_STATE_IDLE) class TestVelCtrlVsPosCtrl(DualAxisTest): diff --git a/tools/run_tests.py b/tools/run_tests.py index f0ee3bb1..14185cf0 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -29,7 +29,8 @@ all_tests = [ # TestClosedLoopControl(), TestDiscoverAndGotoIdle(), # for testing TestEncoderOffsetCalibration(pass_if_ready=True), - TestHighVelocityCtrl(), +# TestHighVelocity(), + TestHighVelocityInViscousFluid(), # TestVelCtrlVsPosCtrl() # TODO: test step/dir # TODO: test sensorless From d202491e9a201355995c7ba48517ae58cf7f66f1 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 10 Apr 2018 15:41:34 -0700 Subject: [PATCH 059/112] make tests more flexible --- docs/testing.md | 23 ++++++++++++++ tools/odrive/tests.py | 17 ++++++++--- tools/run_tests.py | 71 +++++++++++++++++++++++++++---------------- tools/test-rig.yaml | 16 ++++++---- 4 files changed, 90 insertions(+), 37 deletions(-) create mode 100644 docs/testing.md diff --git a/docs/testing.md b/docs/testing.md new file mode 100644 index 00000000..347db2b1 --- /dev/null +++ b/docs/testing.md @@ -0,0 +1,23 @@ +# Automated Testing + +This section describes how to use the automated testing facilities. +You don't have to do this as an end user. + +They test the following aspects: + - System functions (communication interfaces, configuration storage) + - Functionality of the motor controller and state machine + - High speed and high load conditions + +The testing facility consists of the following components: + * **Test rig:** In the simplest case this can be a single ODrive with a single motor and encoder pair. Can also be multiple ODrives with multiple axes, some of which may be mechanically coupled. + * **Test host:** The PC on which the test script runs. All ODrives must be connected to the test host via USB. + * **test-rig.yaml:** Describes your test rig. Make sure all values are correct. Incorrect values may physically break or fry your test setup. + * **run_tests.py:** This is the main script that runs all the tests. + +## How to run + +Example: + +``` +./run_tests.py --skip-boring-tests --ignore top-odrive.yellow bottom-odrive.yellow +``` diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 64237768..6e4d9cd6 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -26,7 +26,7 @@ class ODriveTestContext(): self.name = name self.axes = [] for axis_idx, axis_yaml in enumerate(yaml['axes']): - axis_name = axis_yaml['name'] if 'name' in axis_yaml else '{}.axis{}'.format(name, axis_idx) + axis_name = (name + "." + axis_yaml['name']) if 'name' in axis_yaml else '{}.axis{}'.format(name, axis_idx) self.axes.append(AxisTestContext(axis_name, axis_yaml, self)) def rediscover(self): @@ -388,7 +388,6 @@ class TestHighVelocity(AxisTest): # V_bus and motor KV rating max_rpm = axis_ctx.odrv_ctx.yaml['vbus-voltage'] * axis_ctx.yaml['motor-kv'] rated_limit = max_rpm / 60 * axis_ctx.yaml['encoder-cpr'] - rated_limit *= 0.5 # TODO: remove this later, but for now we want to stay away from the modulation depth limit expected_limit = rated_limit # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) @@ -399,6 +398,10 @@ class TestHighVelocity(AxisTest): # The ODrive only goes to 80% modulation depth in order to save some time for the ADC measurements. # See FOC_current in motor.cpp. expected_limit *= 0.8 + + # TODO: remove the following two lines, but for now we want to stay away from the modulation depth limit + expected_limit *= 0.8 + rated_limit = expected_limit # Add a 10% margin to account for expected_limit *= 0.9 @@ -465,6 +468,10 @@ class TestHighVelocityInViscousFluid(DualAxisTest): Runs TestHighVelocity on one motor while using the other motor as a load. The load is created by running velocity control with setpoint 0. """ + def __init__(self, load_current=10, driver_current=20): + self._load_current = load_current + self._driver_current = driver_current + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): load_ctx = axis0_ctx driver_ctx = axis1_ctx @@ -474,12 +481,14 @@ class TestHighVelocityInViscousFluid(DualAxisTest): load_ctx.handle.controller.config.vel_integrator_gain = 0 load_ctx.handle.controller.vel_integrator_current = 0 load_ctx.handle.controller.config.vel_limit = 20000 # this is not really relevant - load_ctx.handle.motor.config.current_lim = 20 + load_ctx.handle.motor.config.current_lim = self._load_current load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus load_ctx.handle.controller.set_vel_setpoint(0, 0) request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) - driver_test = TestHighVelocity(override_current_limit=40, load_current=20, brake=False) + driver_test = TestHighVelocity( + override_current_limit=self._driver_current, + load_current=self._load_current, brake=False) driver_test.check_preconditions(driver_ctx, logger) driver_test.run_test(driver_ctx, logger) diff --git a/tools/run_tests.py b/tools/run_tests.py index 14185cf0..6931dd49 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -12,52 +12,67 @@ import os import sys import threading import traceback +import argparse from odrive.tests import * from odrive.utils import Logger, for_all_parallel, Event +script_path=os.path.dirname(os.path.realpath(__file__)) -all_tests = [ -# TestFlashAndErase(), -# TestSetup(), -# TestMotorCalibration(), -# # TODO: test encoder index search -# TestEncoderOffsetCalibration(), -# # TODO: hold down one motor while the other one does an index search (should fail) -# TestClosedLoopControl(), -# TestStoreAndReboot(), -# TestEncoderOffsetCalibration(), # need to find offset _or_ index after reboot -# TestClosedLoopControl(), - TestDiscoverAndGotoIdle(), # for testing - TestEncoderOffsetCalibration(pass_if_ready=True), -# TestHighVelocity(), - TestHighVelocityInViscousFluid(), -# TestVelCtrlVsPosCtrl() - # TODO: test step/dir - # TODO: test sensorless - # TODO: test ASCII protocol - # TODO: test protocol over UART -] +parser = argparse.ArgumentParser(description='ODrive automated test tool\n') +parser.add_argument("--skip-boring-tests", action="store_true", + help="Skip the boring tests and go right to the high power tests") +parser.add_argument("--ignore", metavar='DEVICE', action='store', nargs='+', + help="Ignore one or more ODrives or axes") +parser.add_argument("--test-rig-yaml", type=argparse.FileType('r'), + help="test rig YAML file") +parser.set_defaults(test_rig_yaml=script_path + '/test-rig.yaml') +parser.set_defaults(ignore=[]) +args = parser.parse_args() +all_tests = [] +if not args.skip_boring_tests: + all_tests.append(TestFlashAndErase()) + all_tests.append(TestSetup()) + all_tests.append(TestMotorCalibration()) + # # TODO: test encoder index search + all_tests.append(TestEncoderOffsetCalibration()) + # # TODO: hold down one motor while the other one does an index search (should fail) + all_tests.append(TestClosedLoopControl()) + all_tests.append(TestStoreAndReboot()) + all_tests.append(TestEncoderOffsetCalibration()) # need to find offset _or_ index after reboot + all_tests.append(TestClosedLoopControl()) +else: + all_tests.append(TestDiscoverAndGotoIdle()) + all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) +#all_tests.append(TestHighVelocity()) +all_tests.append(TestHighVelocityInViscousFluid(load_current=20, driver_current=40)) +#all_tests.append(TestVelCtrlVsPosCtrl()) +# TODO: test step/dir +# TODO: test sensorless +# TODO: test ASCII protocol +# TODO: test protocol over UART + +print(str(args.ignore)) logger = Logger() -script_path=os.path.dirname(os.path.realpath(__file__)) -with open(script_path + '/test-rig.yaml', 'r') as file_stream: - test_rig_yaml = yaml.load(file_stream) +test_rig_yaml = yaml.load(args.test_rig_yaml) os.chdir(script_path + '/../Firmware') # Build a dictionary of odrive test contexts by name odrives_by_name = {} for odrv_idx, odrv_yaml in enumerate(test_rig_yaml['odrives']): name = odrv_yaml['name'] if 'name' in odrv_yaml else 'odrive{}'.format(odrv_idx) - odrives_by_name[name] = ODriveTestContext(name, odrv_yaml) + if not name in args.ignore: + odrives_by_name[name] = ODriveTestContext(name, odrv_yaml) # Build a dictionary of axis test contexts by name (e.g. odrive0.axis0) axes_by_name = {} for odrv_ctx in odrives_by_name.values(): for axis_idx, axis_ctx in enumerate(odrv_ctx.axes): - axes_by_name[axis_ctx.name] = axis_ctx + if not axis_ctx.name in args.ignore: + axes_by_name[axis_ctx.name] = axis_ctx # Ensure mechanical couplings are valid couplings = [] @@ -65,7 +80,9 @@ if test_rig_yaml['couplings'] is None: test_rig_yaml['couplings'] = {} else: for coupling in test_rig_yaml['couplings']: - couplings.append([axes_by_name[axis_name] for axis_name in coupling]) + c = [axes_by_name[axis_name] for axis_name in coupling if (axis_name in axes_by_name)] + if len(c) > 1: + couplings.append(c) app_shutdown_token = Event() diff --git a/tools/test-rig.yaml b/tools/test-rig.yaml index 2c05cc9d..092bed42 100644 --- a/tools/test-rig.yaml +++ b/tools/test-rig.yaml @@ -11,14 +11,16 @@ odrives: vbus-voltage: 12 # [V] max-brake-power: 150 # [W] axes: - - motor-phase-resistance: 0.0245 + - name: 'yellow' + motor-phase-resistance: 0.0245 motor-phase-inductance: 2.03e-05 motor-pole-pairs: 7 motor-direction: -1 motor-kv: 190 motor-max-current: 50 encoder-cpr: 8192 - - motor-phase-resistance: 0.028 + - name: 'black' + motor-phase-resistance: 0.028 motor-phase-inductance: 1.6e-05 motor-pole-pairs: 7 motor-direction: -1 @@ -35,14 +37,16 @@ odrives: vbus-voltage: 12 # [V] max-brake-power: 150 # [W] axes: - - motor-phase-resistance: 0.0253 + - name: 'black' + motor-phase-resistance: 0.0253 motor-phase-inductance: 1.6e-05 motor-pole-pairs: 7 motor-direction: 1 motor-kv: 270 motor-max-current: 50 encoder-cpr: 8192 - - motor-phase-resistance: 0.0245 + - name: 'yellow' + motor-phase-resistance: 0.0245 motor-phase-inductance: 2.03e-05 motor-pole-pairs: 7 motor-direction: -1 @@ -52,5 +56,5 @@ odrives: # Mechanical couplings couplings: - - [ top-odrive.axis0, bottom-odrive.axis1 ] - - [ top-odrive.axis1, bottom-odrive.axis0 ] + - [ top-odrive.yellow, bottom-odrive.yellow ] + - [ top-odrive.black, bottom-odrive.black ] From b4fa53c84980aff073bfabd38c5baf72c7384ba5 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 9 Apr 2018 19:32:36 -0700 Subject: [PATCH 060/112] fix encoder offset calibration and error handling --- Firmware/MotorControl/axis.cpp | 6 ++++++ Firmware/MotorControl/encoder.cpp | 2 +- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index fe3760aa..c0488dfa 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -251,14 +251,20 @@ void Axis::run_state_machine_loop() { switch (current_state_) { case AXIS_STATE_MOTOR_CALIBRATION: status = motor_.run_calibration(); + if (!status) + error_ |= ERROR_MOTOR_FAILED; break; case AXIS_STATE_ENCODER_INDEX_SEARCH: status = encoder_.run_index_search(); + if (!status) + error_ |= ERROR_ENCODER_FAILED; break; case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: status = encoder_.run_offset_calibration(); + if (!status) + error_ |= ERROR_ENCODER_FAILED; break; case AXIS_STATE_SENSORLESS_CONTROL: diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 19a39792..fad27267 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -103,7 +103,7 @@ bool Encoder::run_offset_calibration() { // Temporarily disable index search so it doesn't mess // with the offset calibration bool old_use_index = config_.use_index; - config_.use_index = true; + config_.use_index = false; float voltage_magnitude; if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) From 44fd8090e190d8bb38afa6d1ce4f3a36637454a9 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 10 Apr 2018 00:25:10 -0700 Subject: [PATCH 061/112] add overvoltage protection The motor phases will go into floating state as soon as an overvoltage condition is detected. --- Firmware/MotorControl/axis.cpp | 9 +++------ Firmware/MotorControl/axis.hpp | 2 -- Firmware/MotorControl/communication.cpp | 4 +++- Firmware/MotorControl/odrive_main.hpp | 5 +++++ 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index c0488dfa..bde389d4 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -92,18 +92,15 @@ void Axis::set_step_dir_enabled(bool enable) { } } -// @brief Returns true if the power supply is within range -bool Axis::check_PSU_brownout() { - return vbus_voltage >= config_.dc_bus_brownout_trip_level; -} - // @brief Returns true if everything is ok. // Sets error and returns false otherwise. bool Axis::do_checks() { if (!motor_.do_checks()) return error_ |= ERROR_MOTOR_FAILED, false; - if (!check_PSU_brownout()) + if (!(vbus_voltage >= board_config.dc_bus_undervoltage_trip_level)) return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false; + if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level)) + return error_ |= ERROR_DC_BUS_OVER_VOLTAGE, false; return true; } diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index f6415422..72dac9b3 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -30,7 +30,6 @@ struct AxisConfig_t { // For M0 this has no effect if enable_uart is true float counts_per_step = 2.0f; - float dc_bus_brownout_trip_level = 8.0f; //make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 1b4b8850..f1d91007 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -23,6 +23,11 @@ struct BoardConfig_t { bool enable_uart = true; float brake_resistance = 0.47f; // [ohm] + float dc_bus_undervoltage_trip_level = 8.0f; // Date: Tue, 10 Apr 2018 23:45:58 -0700 Subject: [PATCH 062/112] [HACK] implement return values --- Firmware/MotorControl/protocol.hpp | 80 ++++++++++++++++++++++++++++++ tools/odrive/remote_object.py | 7 +++ 2 files changed, 87 insertions(+) diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index bd853608..7c2f9d38 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -745,11 +745,91 @@ public: MemberList...> input_properties_; }; +template +class ProtocolFunctionWithRet : Endpoint { +public: + static constexpr size_t endpoint_count = 1 + MemberList>::endpoint_count + MemberList...>::endpoint_count; + template + ProtocolFunctionWithRet(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) : + name_(name), out_arg_names_{"out"}, all_arg_names_{names...}, obj_(obj), func_ptr_(func_ptr), + output_properties_(PropertyListFactory::template make_property_list<0>(out_arg_names_, out_args_)), + input_properties_(PropertyListFactory::template make_property_list<0>(all_arg_names_, in_args_)) + { + LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + } + + ProtocolFunctionWithRet(const ProtocolFunctionWithRet& other) : + name_(other.name_), all_arg_names_(other.all_arg_names_), obj_(other.obj_), func_ptr_(other.func_ptr_), + output_properties_(PropertyListFactory::template make_property_list<0>( + out_arg_names_, out_args_)), + input_properties_(PropertyListFactory::template make_property_list<0>( + all_arg_names_, in_args_)) + { + LOG_PROTO("COPIED! my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + } + + void write_json(size_t id, StreamSink* output) { + // write name + write_string("{\"name\":\"", output); + write_string(name_, output); + + // write endpoint ID + write_string("\",\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + // write arguments + write_string(",\"type\":\"function\",\"inputs\":[", output); + input_properties_.write_json(id + 1, output), + write_string("],\"outputs\":[", output); + output_properties_.write_json(id + 1 + decltype(input_properties_)::endpoint_count, output), + write_string("]}", output); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + input_properties_.register_endpoints(list, id + 1, length); + output_properties_.register_endpoints(list, id + 1 + decltype(input_properties_)::endpoint_count, length); + } + + void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + (void) input; + (void) input_length; + (void) output; + LOG_PROTO("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + LOG_PROTO("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_)); + std::get<0>(out_args_) = invoke_function_with_tuple(obj_, func_ptr_, in_args_); + } + + const char * name_; + std::array out_arg_names_; // TODO: remove + std::array all_arg_names_; // TODO: remove + TObj& obj_; + TRet(TObj::*func_ptr_)(TArgs...); + //TRet ret_val_; + std::tuple out_args_; + std::tuple in_args_; + MemberList> output_properties_; + MemberList...> input_properties_; +}; + +//template> +//ProtocolFunction make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { +// return ProtocolFunction(name, obj, func_ptr, names...); +//} + template> ProtocolFunction make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { return ProtocolFunction(name, obj, func_ptr, names...); } +template> +ProtocolFunctionWithRet make_protocol_function_with_ret(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { + return ProtocolFunctionWithRet(name, obj, func_ptr, names...); +} + template diff --git a/tools/odrive/remote_object.py b/tools/odrive/remote_object.py index 58da44ab..bdd69ae2 100644 --- a/tools/odrive/remote_object.py +++ b/tools/odrive/remote_object.py @@ -101,12 +101,19 @@ class RemoteFunction(object): param_json["mode"] = "r" self._inputs.append(RemoteProperty(param_json, parent)) + self._outputs = [] + for param_json in json_data.get("outputs", []): # TODO: deprecate "arguments" keyword + param_json["mode"] = "r" + self._outputs.append(RemoteProperty(param_json, parent)) + def __call__(self, *args): if (len(self._inputs) != len(args)): raise TypeError("expected {} arguments but have {}".format(len(self._inputs), len(args))) for i in range(len(args)): self._inputs[i].set_value(args[i]) self._parent.__channel__.remote_endpoint_operation(self._trigger_id, None, True, 0) + if len(self._outputs) > 0: + return self._outputs[0].get_value() class RemoteObject(object): """ From 1e14c452765d6865af3726a1b9f1e80c4858ac3d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 10 Apr 2018 23:54:13 -0700 Subject: [PATCH 063/112] [HACK] implement oscilloscope --- Firmware/MotorControl/communication.cpp | 8 ++++++++ Firmware/MotorControl/low_level.cpp | 5 +++++ Firmware/MotorControl/odrive_main.hpp | 4 ++++ tools/odrive/utils.py | 10 ++++++++++ 4 files changed, 27 insertions(+) diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 49b4ee35..e7024c58 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -180,6 +180,12 @@ void init_communication(void) { } +float oscilloscope[OSCILLOSCOPE_SIZE] = { + 0.123f, 0.345f, 0.4576f, 1.543f, -50.0f +}; +size_t oscilloscope_pos = 0; + + uint32_t comm_stack_info = 0; // for debugging only // Helper class because the protocol library doesn't yet @@ -191,6 +197,7 @@ public: void erase_configuration_helper() { erase_configuration(); } void NVIC_SystemReset_helper() { NVIC_SystemReset(); } void enter_dfu_mode_helper() { enter_dfu_mode(); } + float get_oscilloscope_val(uint32_t index) { return oscilloscope[index]; } } static_functions; // When adding new functions/variables to the protocol, be careful not to @@ -219,6 +226,7 @@ static inline auto make_obj_tree() { ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), + make_protocol_function_with_ret("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper), diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 98ac5620..185708db 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -322,6 +322,11 @@ void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; + if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) { + if (oscilloscope_pos >= OSCILLOSCOPE_SIZE) + oscilloscope_pos = 0; + oscilloscope[oscilloscope_pos++] = vbus_voltage; + } } // This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index 43a55a99..3b170736 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -47,6 +47,10 @@ extern bool user_config_loaded_; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; +#define OSCILLOSCOPE_SIZE 18000 +extern float oscilloscope[OSCILLOSCOPE_SIZE]; +extern size_t oscilloscope_pos; + // TODO: move // this is technically not thread-safe but practically it might be #define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \ diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 9852a3f2..db743914 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -88,6 +88,16 @@ def print_drv_regs(name, motor): print("Control Reg 1: " + str(ctrl_reg_1) + " (" + format(ctrl_reg_1, '#013b') + ")") print("Control Reg 2: " + str(ctrl_reg_2) + " (" + format(ctrl_reg_2, '#09b') + ")") +def show_oscilloscope(odrv): + size = 18000 + values = [] + for i in range(size): + values.append(odrv.get_oscilloscope_val(i)) + + import matplotlib.pyplot as plt + plt.plot(values) + plt.show() + def rate_test(device): """ Tests how many integers per second can be transmitted From c49717e38612ae448a78d0d37b3ab3ff0b6e347e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 11 Apr 2018 00:07:12 -0700 Subject: [PATCH 064/112] improve error output --- tools/odrive/tests.py | 30 +++++++++++++++++++++++++----- tools/run_tests.py | 3 +++ 2 files changed, 28 insertions(+), 5 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 6e4d9cd6..10814d92 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -54,7 +54,7 @@ def test_assert_eq(observed, expected, range=None, accuracy=None): elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))): raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed)) -def test_assert_no_error(axis_ctx: AxisTestContext): +def get_errors(axis_ctx: AxisTestContext): errors = [] if axis_ctx.handle.motor.error != 0: errors.append("motor failed with error {:04X}".format(axis_ctx.handle.motor.error)) @@ -66,6 +66,23 @@ def test_assert_no_error(axis_ctx: AxisTestContext): errors.append("axis failed with error {:04X}".format(axis_ctx.handle.error)) elif len(errors) > 0: errors.append("and by the way: axis reports no error even though there is one") + return errors + +def dump_errors(axis_ctx: AxisTestContext, logger): + errors = get_errors(axis_ctx) + if len(errors): + logger.error("errors on " + axis_ctx.name) + for error in errors: + logger.error(error) + +def clear_errors(axis_ctx: AxisTestContext): + axis_ctx.handle.error = 0 + axis_ctx.handle.encoder.error = 0 + axis_ctx.handle.motor.error = 0 + axis_ctx.handle.sensorless_estimator.error = 0 + +def test_assert_no_error(axis_ctx: AxisTestContext): + errors = get_errors(axis_ctx) if len(errors) > 0: raise TestFailed("\n".join(errors)) @@ -160,8 +177,11 @@ class DualAxisTest(ABC): test_assert_no_error(axis1_ctx) test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE) - test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=1000) - test_assert_eq(axis1_ctx.handle.encoder.pll_vel, 0, range=1000) + if (abs(axis0_ctx.handle.encoder.pll_vel) > 500) or (abs(axis1_ctx.handle.encoder.pll_vel) > 500): + logger.warn("some axis still in motion, delaying 2 sec...") + time.sleep(2) + test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=500) + test_assert_eq(axis1_ctx.handle.encoder.pll_vel, 0, range=500) @abc.abstractmethod def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): @@ -170,8 +190,8 @@ class DualAxisTest(ABC): class TestDiscoverAndGotoIdle(ODriveTest): def run_test(self, odrv_ctx: ODriveTestContext, logger): odrv_ctx.rediscover() - odrv_ctx.axes[0].handle.error = 0 - odrv_ctx.axes[1].handle.error = 0 + clear_errors(odrv_ctx.axes[0]) + clear_errors(odrv_ctx.axes[1]) request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE) request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE) diff --git a/tools/run_tests.py b/tools/run_tests.py index 6931dd49..49a9ff32 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -186,6 +186,9 @@ except: #run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30) odrv_ctx.handle.axis0.requested_state = AXIS_STATE_IDLE odrv_ctx.handle.axis1.requested_state = AXIS_STATE_IDLE + dump_errors(odrv_ctx.axes[0], logger) + dump_errors(odrv_ctx.axes[1], logger) + for_all_parallel(odrives_by_name, lambda x: x['name'], odrv_reset_thread) except: logger.error('///////////////////////////////////////////') From ffff42b4f377089a56d144db895a34330d8e83f4 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Wed, 11 Apr 2018 00:07:56 -0700 Subject: [PATCH 065/112] [TEMP] test hacks --- tools/odrive/tests.py | 8 ++++++-- tools/run_tests.py | 2 ++ 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 10814d92..3c0bd0bf 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -160,8 +160,10 @@ class AxisTest(ABC): logger.warn("axis still in motion, delaying 2 sec...") time.sleep(2) test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) - test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) - test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) + #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) + #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) + #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.96, accuracy=0.001) + #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.04, accuracy=0.001) @abc.abstractmethod def run_test(self, axis_ctx: AxisTestContext, logger): @@ -495,6 +497,8 @@ class TestHighVelocityInViscousFluid(DualAxisTest): def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): load_ctx = axis0_ctx driver_ctx = axis1_ctx + if load_ctx.name == 'bottom-odrive.black': + odrive.utils.start_liveplotter(lambda: [load_ctx.odrv_ctx.handle.vbus_voltage]) # Set up viscous fluid load logger.debug("activating load on {}...".format(load_ctx.name)) diff --git a/tools/run_tests.py b/tools/run_tests.py index 49a9ff32..cc4c805d 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -29,6 +29,8 @@ parser.set_defaults(test_rig_yaml=script_path + '/test-rig.yaml') parser.set_defaults(ignore=[]) args = parser.parse_args() +# TODO: add --only option + all_tests = [] if not args.skip_boring_tests: all_tests.append(TestFlashAndErase()) From 18260e64c0a2c176e264abc8e9c685dc48a125ab Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 12 Apr 2018 20:41:23 -0700 Subject: [PATCH 066/112] sed backslashing through make --- Firmware/.vscode/c_cpp_properties.json | 16 ++++++++++++---- Firmware/Makefile | 2 +- tools/odrive/tests.py | 4 ++-- tools/test-rig.yaml | 4 ++-- 4 files changed, 17 insertions(+), 9 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 3e925aa9..5042079e 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -26,7 +26,9 @@ "defines": [ "STM32F405xx", "USE_HAL_DRIVER", - "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24", + "HW_VERSION_MAJOR=3", + "HW_VERSION_MINOR=4", + "HW_VERSION_VOLTAGE=24", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" @@ -38,7 +40,9 @@ "C:/Program Files (x86)/GNU Tools ARM Embedded" ], "limitSymbolsToIncludedHeaders": true - } + }, + "cStandard": "c11", + "cppStandard": "c++14" }, { "name": "Linux", @@ -62,7 +66,9 @@ "defines": [ "STM32F405xx", "USE_HAL_DRIVER", - "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24", + "HW_VERSION_MAJOR=3", + "HW_VERSION_MINOR=4", + "HW_VERSION_VOLTAGE=24", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" @@ -105,7 +111,9 @@ "defines": [ "STM32F405xx", "USE_HAL_DRIVER", - "HW_VERSION_MAJOR=3", "HW_VERSION_MINOR=4", "HW_VERSION_VOLTAGE=24", + "HW_VERSION_MAJOR=3", + "HW_VERSION_MINOR=4", + "HW_VERSION_VOLTAGE=24", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" diff --git a/Firmware/Makefile b/Firmware/Makefile index 921a9e13..9f239b22 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -5,7 +5,7 @@ BUILD_DIR = build FIRMWARE = $(BUILD_DIR)/ODriveFirmware.elf FIRMWARE_HEX = $(BUILD_DIR)/ODriveFirmware.hex -PROGRAMMER_HEX := $(shell echo $(PROGRAMMER) | sed -e 's/.\{2\}/\\x&/g') +PROGRAMMER_HEX := $(shell echo $(PROGRAMMER) | sed -e 's/.\\{2\\}/\\\\x&/g') OPENOCD := openocd -f interface/stlink-v2.cfg \ $(if $(value PROGRAMMER),-c 'hla_serial $(PROGRAMMER_HEX)',) \ -f target/stm32f4x.cfg diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 3c0bd0bf..0fc82c1d 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -160,8 +160,8 @@ class AxisTest(ABC): logger.warn("axis still in motion, delaying 2 sec...") time.sleep(2) test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) - #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) - #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.96, accuracy=0.001) #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.04, accuracy=0.001) diff --git a/tools/test-rig.yaml b/tools/test-rig.yaml index 092bed42..3053070f 100644 --- a/tools/test-rig.yaml +++ b/tools/test-rig.yaml @@ -8,7 +8,7 @@ odrives: uart: /dev/serial/by-id/[not-yet-used] usb: auto programmer: '533f7506493f49514454193f' - vbus-voltage: 12 # [V] + vbus-voltage: 24 # [V] max-brake-power: 150 # [W] axes: - name: 'yellow' @@ -34,7 +34,7 @@ odrives: uart: /dev/serial/by-id/[not-yet-used] usb: auto programmer: '493f6f06493f56540929113f' - vbus-voltage: 12 # [V] + vbus-voltage: 24 # [V] max-brake-power: 150 # [W] axes: - name: 'black' From 2f5f6ead9c8631958255275fc44bedf9afd1172d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 13 Apr 2018 00:25:03 -0700 Subject: [PATCH 067/112] add retry error counter, some fixes for windows --- tools/odrive/protocol.py | 7 ++++++- tools/odrive/tests.py | 9 +++++++-- tools/odrive/usbbulk_transport.py | 4 ++-- tools/odrive/utils.py | 6 ++++++ tools/odrivetool | 6 +----- tools/run_tests.py | 10 +++++----- tools/test-rig.yaml | 4 ++-- 7 files changed, 29 insertions(+), 17 deletions(-) diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index f4bf2b05..a99bc0b1 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -232,8 +232,10 @@ class Channel(PacketSink): The thread quits as soon as the channel enters a broken state. """ def receiver_thread(): + error_ctr = 0 try: - while (not cancellation_token.is_set()) and (not self._channel_broken.is_set()): + while (not cancellation_token.is_set() and not self._channel_broken.is_set() + and error_ctr < 10): # Set an arbitrary deadline because the get_packet function # currently doesn't support a cancellation_token deadline = time.monotonic() + 1.0 @@ -242,7 +244,10 @@ class Channel(PacketSink): except odrive.utils.TimeoutException: continue # try again except ChannelDamagedException: + error_ctr += 1 continue # try again + if (error_ctr > 0): + error_ctr -= 1 # Process response # This should not throw an exception, otherwise the channel breaks self.process_packet(response) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 0fc82c1d..44205006 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -9,6 +9,9 @@ import odrive.discovery from odrive.enums import * import odrive.utils +import functools +print = functools.partial(print, flush=True) + import abc ABC = abc.ABC @@ -34,7 +37,7 @@ class ODriveTestContext(): Reconnects to the ODrive """ self.handle = odrive.discovery.find_any( - path="usb", serial_number=self.yaml['serial-number'], timeout=15) + path="usb", serial_number=self.yaml['serial-number'], timeout=15)#, printer=print) for axis_idx, axis_ctx in enumerate(self.axes): axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)] @@ -378,7 +381,7 @@ class TestStoreAndReboot(ODriveTest): test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) for axis_ctx in odrv_ctx.axes: test_assert_eq(axis_ctx.handle.encoder.config.cpr, axis_ctx.yaml['encoder-cpr']) - test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.15) + test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2) test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) @@ -437,6 +440,8 @@ class TestHighVelocity(AxisTest): else: axis_ctx.handle.motor.config.current_lim = self._override_current_limit axis_ctx.handle.controller.config.vel_limit = rated_limit + axis_ctx.handle.controller.vel_integrator_current = 0 + axis_ctx.handle.controller.set_vel_setpoint(0, 0) request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index fbfcfcee..dc36f334 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -118,7 +118,7 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) except usb.core.USBError as ex: if ex.errno == 19: # "no such device" raise odrive.protocol.ChannelBrokenException() - elif ex.errno == 110: # timeout + elif ex.errno is None or ex.errno == 110: # timeout raise odrive.utils.TimeoutException() else: self._printer("halt condition: {}".format(ex.errno)) @@ -172,7 +172,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, printer return True while not cancellation_token.is_set(): - printer("USB discover loop") + # printer("USB discover loop") devices = usb.core.find(find_all=True, custom_match=device_matcher) for usb_device in devices: try: diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index db743914..6b20cbd3 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -67,6 +67,7 @@ def start_liveplotter(get_var_callback): plt.plot(vals) #time.sleep(1/plot_rate) fig.canvas.flush_events() + plt.pause(1/plot_rate) threading.Thread(target=fetch_data).start() threading.Thread(target=plot_data).start() @@ -303,6 +304,7 @@ class Logger(): self._prefix = '' self._skip_bottom_line = False # If true, messages are printed one line above the cursor self._verbose = verbose + self._print_lock = threading.Lock() if platform.system() == 'Windows': self._stdout_buf = win32console.GetStdHandle(win32console.STD_OUTPUT_HANDLE) @@ -350,18 +352,22 @@ class Logger(): # (print text) # ESC 8: restore old cursor position + self._print_lock.acquire() sys.stdout.write('\x1b7\x1b[1A\x1b[1S\x1b[1L') sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT]) sys.stdout.write('\x1b8') sys.stdout.flush() + self._print_lock.release() def print_colored(self, text, color): if self._skip_bottom_line: self.print_on_second_last_line(text, color) else: # On Windows, colorama does the job of interpreting the VT100 escape sequences + self._print_lock.acquire() sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT] + '\n') sys.stdout.flush() + self._print_lock.release() def debug(self, text): if self._verbose: diff --git a/tools/odrivetool b/tools/odrivetool index 0bcc39b7..4afa2927 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -7,7 +7,7 @@ import argparse import odrive.discovery from odrive.utils import Logger, Event -# Flush stdout by default +# We are interactively printing status messages, so flush by default import functools print = functools.partial(print, flush=True) @@ -68,10 +68,6 @@ if args.command is None: args.command = 'shell' args.no_ipython = False -# We are interactively printing status messages, so flush by default -import functools -print = functools.partial(print, flush=True) - # TODO: deprecate printer - use logger instead if (args.verbose): printer = print diff --git a/tools/run_tests.py b/tools/run_tests.py index cc4c805d..4142dc3c 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -93,7 +93,7 @@ try: if isinstance(test, ODriveTest): def odrv_test_thread(odrv_name): odrv_ctx = odrives_by_name[odrv_name] - logger.info('● running {} on {}...'.format(type(test).__name__, odrv_name)) + logger.info('* running {} on {}...'.format(type(test).__name__, odrv_name)) try: test.check_preconditions(odrv_ctx, logger.indent(' {}: '.format(odrv_name))) @@ -122,7 +122,7 @@ try: try: if not app_shutdown_token.is_set(): # Run test on this axis - logger.info('● running {} on {}...'.format(type(test).__name__, axis_name)) + logger.info('* running {} on {}...'.format(type(test).__name__, axis_name)) try: test.check_preconditions(axis_ctx, logger.indent(' {}: '.format(axis_name))) @@ -131,7 +131,7 @@ try: test.run_test(axis_ctx, logger.indent(' {}: '.format(axis_name))) else: - logger.warn('⬛ skipping {} on {}'.format(type(test).__name__, axis_name)) + logger.warn('- skipping {} on {}'.format(type(test).__name__, axis_name)) except: app_shutdown_token.set() raise @@ -154,7 +154,7 @@ try: try: if not app_shutdown_token.is_set(): # Run test on this axis - logger.info('● running {} on {}...'.format(type(test).__name__, coupling_name)) + logger.info('* running {} on {}...'.format(type(test).__name__, coupling_name)) try: test.check_preconditions(coupled_axes[0], coupled_axes[1], logger.indent(' {}: '.format(coupling_name))) @@ -163,7 +163,7 @@ try: test.run_test(coupled_axes[0], coupled_axes[1], logger.indent(' {}: '.format(coupling_name))) else: - logger.warn('⬛ skipping {} on {}...'.format(type(test).__name__, coupling_name)) + logger.warn('- skipping {} on {}...'.format(type(test).__name__, coupling_name)) except: app_shutdown_token.set() raise diff --git a/tools/test-rig.yaml b/tools/test-rig.yaml index 3053070f..7e84dec8 100644 --- a/tools/test-rig.yaml +++ b/tools/test-rig.yaml @@ -15,7 +15,7 @@ odrives: motor-phase-resistance: 0.0245 motor-phase-inductance: 2.03e-05 motor-pole-pairs: 7 - motor-direction: -1 + motor-direction: 1 motor-kv: 190 motor-max-current: 50 encoder-cpr: 8192 @@ -38,7 +38,7 @@ odrives: max-brake-power: 150 # [W] axes: - name: 'black' - motor-phase-resistance: 0.0253 + motor-phase-resistance: 0.028 motor-phase-inductance: 1.6e-05 motor-pole-pairs: 7 motor-direction: 1 From bd292b4775d5964e82a49287f793943f0fa512b0 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 13 Apr 2018 16:17:05 -0700 Subject: [PATCH 068/112] fix comment --- Firmware/MotorControl/axis.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 72dac9b3..601b2367 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -13,7 +13,7 @@ enum AxisState_t { AXIS_STATE_STARTUP_SEQUENCE = 2, // Date: Fri, 13 Apr 2018 17:17:24 -0700 Subject: [PATCH 069/112] split plotting behaviour across windows and notwindows --- tools/odrive/utils.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 6b20cbd3..af54cf3c 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -65,9 +65,10 @@ def start_liveplotter(get_var_callback): while not cancellation_token.is_set(): plt.clf() plt.plot(vals) - #time.sleep(1/plot_rate) - fig.canvas.flush_events() - plt.pause(1/plot_rate) + if platform.system() == "Windows": + plt.pause(1/plot_rate) + else: + fig.canvas.flush_events() threading.Thread(target=fetch_data).start() threading.Thread(target=plot_data).start() From d864730ecc5927396e8eb10a0b3d8226794b03ea Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 13 Apr 2018 18:37:39 -0700 Subject: [PATCH 070/112] add integrator resets to firmware --- Firmware/MotorControl/controller.cpp | 7 +++++++ Firmware/MotorControl/controller.hpp | 1 + Firmware/MotorControl/motor.cpp | 16 ++++++++++++---- Firmware/MotorControl/motor.hpp | 2 ++ tools/odrive/tests.py | 13 +++++++++++-- 5 files changed, 33 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 3e1b5661..b433efe7 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -6,6 +6,13 @@ Controller::Controller(ControllerConfig_t& config) : config_(config) {} +void Controller::reset() { + pos_setpoint_ = 0.0f; + vel_setpoint_ = 0.0f; + vel_integrator_current_ = 0.0f; + current_setpoint_ = 0.0f; +} + //-------------------------------- // Command Handling //-------------------------------- diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b5767b6f..3a495d18 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -26,6 +26,7 @@ struct ControllerConfig_t { class Controller { public: Controller(ControllerConfig_t& config); + void reset(); void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 0229e697..6f12474d 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -34,17 +34,25 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, // // @returns: True on success, false otherwise bool Motor::arm() { - // Wait until the interrupt handler triggers twice. After the first wait there is an - // undefined period until the next trigger. After the second wait we know for sure - // that we have exactly one full interrupt period until the third trigger. This gives + + // Reset controller states, integrators, setpoints, etc. + axis_->controller_.reset(); + reset_current_control(); + + // Wait until the interrupt handler triggers twice. This gives // the control loop the correct time quota to set up modulation timings. - if (!(axis_->wait_for_current_meas() && axis_->wait_for_current_meas())) + if (!axis_->wait_for_current_meas()) return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false; next_timings_valid_ = false; safety_critical_arm_motor_pwm(*this); return true; } +void Motor::reset_current_control() { + current_control_.v_current_control_integral_d = 0.0f; + current_control_.v_current_control_integral_q = 0.0f; +} + // @brief Tune the current controller based on phase resistance and inductance // This should be invoked whenever one of these values changes. // TODO: allow update on user-request or update automatically via hooks diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index b2a79f88..ebc47b9c 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -95,6 +95,8 @@ public: update_current_controller_gains(); DRV8301_setup(); } + void reset_current_control(); + void update_current_controller_gains(); void DRV8301_setup(); bool check_DRV_fault(); diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 44205006..4a28b449 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -425,7 +425,7 @@ class TestHighVelocity(AxisTest): expected_limit *= 0.8 # TODO: remove the following two lines, but for now we want to stay away from the modulation depth limit - expected_limit *= 0.8 + expected_limit *= 0.5 rated_limit = expected_limit # Add a 10% margin to account for @@ -441,11 +441,15 @@ class TestHighVelocity(AxisTest): axis_ctx.handle.motor.config.current_lim = self._override_current_limit axis_ctx.handle.controller.config.vel_limit = rated_limit axis_ctx.handle.controller.vel_integrator_current = 0 + # logger.debug("Setting {} integrator current to 0".format(axis_ctx.name)) axis_ctx.handle.controller.set_vel_setpoint(0, 0) + # logger.debug("Setting {} vel setpoint to 0".format(axis_ctx.name)) + axis_ctx.handle.motor.current_control.v_current_control_integral_d = 0 + axis_ctx.handle.motor.current_control.v_current_control_integral_q = 0 request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) - ramp_up_time = 20.0 + ramp_up_time = 10.0 t_0 = time.monotonic() last_print = t_0 max_measured_vel = 0.0 @@ -509,10 +513,15 @@ class TestHighVelocityInViscousFluid(DualAxisTest): logger.debug("activating load on {}...".format(load_ctx.name)) load_ctx.handle.controller.config.vel_integrator_gain = 0 load_ctx.handle.controller.vel_integrator_current = 0 + # logger.debug("Setting {} integrator current to 0".format(load_ctx.name)) load_ctx.handle.controller.config.vel_limit = 20000 # this is not really relevant load_ctx.handle.motor.config.current_lim = self._load_current load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus load_ctx.handle.controller.set_vel_setpoint(0, 0) + # logger.debug("Setting {} vel setpoint to 0".format(load_ctx.name)) + load_ctx.handle.motor.current_control.v_current_control_integral_d = 0 + load_ctx.handle.motor.current_control.v_current_control_integral_q = 0 + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) driver_test = TestHighVelocity( From 99e6334089d07762072ae7dcd75dc4ef90e3b813 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 14 Apr 2018 20:28:31 -0700 Subject: [PATCH 071/112] make rotor still check pause more further from error, reduce undervoltage trip level --- tools/odrive/tests.py | 43 +++++++++++++++++++------------------------ tools/run_tests.py | 4 ++-- 2 files changed, 21 insertions(+), 26 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 4a28b449..eb052947 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -159,11 +159,11 @@ class AxisTest(ABC): def check_preconditions(self, axis_ctx: AxisTestContext, logger): test_assert_no_error(axis_ctx) test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) - if (abs(axis_ctx.handle.encoder.pll_vel) > 500): + if (abs(axis_ctx.handle.encoder.pll_vel) > 100): logger.warn("axis still in motion, delaying 2 sec...") time.sleep(2) test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) - test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001) test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.96, accuracy=0.001) #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.04, accuracy=0.001) @@ -182,7 +182,7 @@ class DualAxisTest(ABC): test_assert_no_error(axis1_ctx) test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE) - if (abs(axis0_ctx.handle.encoder.pll_vel) > 500) or (abs(axis1_ctx.handle.encoder.pll_vel) > 500): + if (abs(axis0_ctx.handle.encoder.pll_vel) > 100) or (abs(axis1_ctx.handle.encoder.pll_vel) > 100): logger.warn("some axis still in motion, delaying 2 sec...") time.sleep(2) test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=500) @@ -249,9 +249,9 @@ class TestSetup(ODriveTest): test_assert_eq(odrv_ctx.handle.config.brake_resistance, 1.0) odrv_ctx.handle.config.brake_resistance = odrv_ctx.yaml['brake-resistance'] test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) - odrv_ctx.handle.config.dc_bus_undervoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 0.92 + odrv_ctx.handle.config.dc_bus_undervoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 0.85 odrv_ctx.handle.config.dc_bus_overvoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 1.08 - test_assert_eq(odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 0.92, accuracy=0.001) + test_assert_eq(odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001) test_assert_eq(odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) # firmware has 1500ms startup delay @@ -425,7 +425,7 @@ class TestHighVelocity(AxisTest): expected_limit *= 0.8 # TODO: remove the following two lines, but for now we want to stay away from the modulation depth limit - expected_limit *= 0.5 + expected_limit *= 0.6 rated_limit = expected_limit # Add a 10% margin to account for @@ -440,29 +440,29 @@ class TestHighVelocity(AxisTest): else: axis_ctx.handle.motor.config.current_lim = self._override_current_limit axis_ctx.handle.controller.config.vel_limit = rated_limit - axis_ctx.handle.controller.vel_integrator_current = 0 - # logger.debug("Setting {} integrator current to 0".format(axis_ctx.name)) - axis_ctx.handle.controller.set_vel_setpoint(0, 0) - # logger.debug("Setting {} vel setpoint to 0".format(axis_ctx.name)) - axis_ctx.handle.motor.current_control.v_current_control_integral_d = 0 - axis_ctx.handle.motor.current_control.v_current_control_integral_q = 0 request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + logger.debug("Drive current {}A, Load current {}A".format(axis_ctx.handle.motor.config.current_lim, self._load_current)) - ramp_up_time = 10.0 + ramp_up_time = 15.0 t_0 = time.monotonic() last_print = t_0 max_measured_vel = 0.0 + logger.debug("ramping to {} over {} s".format(rated_limit, ramp_up_time)) while True: ratio = (time.monotonic() - t_0) / ramp_up_time if ratio >= 1: break + #TODO based on integrator gain and torque ramp rate + expected_ramp_lag = 1.0 * (rated_limit / ramp_up_time) + expected_lag = 0 + # While ramping up we want to remain within +-5% of the setpoint. # However we accept if we can only approach 80% of the theoretical limit. vel_setpoint = ratio * rated_limit - expected_velocity = max(vel_setpoint - rated_limit / ramp_up_time * self._load_current / 20, 0) - vel_range = max(0.05*expected_velocity, 50000) + expected_velocity = max(vel_setpoint - expected_lag, 0) + vel_range = max(0.05*expected_velocity, max(expected_lag+expected_ramp_lag, 2000)) if expected_velocity - vel_range > expected_limit: vel_range = expected_velocity - expected_limit @@ -506,21 +506,17 @@ class TestHighVelocityInViscousFluid(DualAxisTest): def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): load_ctx = axis0_ctx driver_ctx = axis1_ctx - if load_ctx.name == 'bottom-odrive.black': - odrive.utils.start_liveplotter(lambda: [load_ctx.odrv_ctx.handle.vbus_voltage]) + if driver_ctx.name == 'top-odrive.black': + # odrive.utils.start_liveplotter(lambda: [driver_ctx.odrv_ctx.handle.vbus_voltage]) + odrive.utils.start_liveplotter(lambda: [driver_ctx.handle.motor.current_control.Iq_measured, + driver_ctx.handle.motor.current_control.Iq_setpoint]) # Set up viscous fluid load logger.debug("activating load on {}...".format(load_ctx.name)) load_ctx.handle.controller.config.vel_integrator_gain = 0 - load_ctx.handle.controller.vel_integrator_current = 0 - # logger.debug("Setting {} integrator current to 0".format(load_ctx.name)) load_ctx.handle.controller.config.vel_limit = 20000 # this is not really relevant load_ctx.handle.motor.config.current_lim = self._load_current load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus - load_ctx.handle.controller.set_vel_setpoint(0, 0) - # logger.debug("Setting {} vel setpoint to 0".format(load_ctx.name)) - load_ctx.handle.motor.current_control.v_current_control_integral_d = 0 - load_ctx.handle.motor.current_control.v_current_control_integral_q = 0 request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) @@ -535,7 +531,6 @@ class TestHighVelocityInViscousFluid(DualAxisTest): request_state(load_ctx, AXIS_STATE_IDLE) request_state(driver_ctx, AXIS_STATE_IDLE) - class TestVelCtrlVsPosCtrl(DualAxisTest): """ Uses one ODrive as a load operating in velocity control mode. diff --git a/tools/run_tests.py b/tools/run_tests.py index 4142dc3c..dc897fde 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -48,8 +48,8 @@ else: all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) #all_tests.append(TestHighVelocity()) -all_tests.append(TestHighVelocityInViscousFluid(load_current=20, driver_current=40)) -#all_tests.append(TestVelCtrlVsPosCtrl()) +all_tests.append(TestHighVelocityInViscousFluid(load_current=60, driver_current=70)) +# all_tests.append(TestVelCtrlVsPosCtrl()) # TODO: test step/dir # TODO: test sensorless # TODO: test ASCII protocol From 339fe59f3011bf1915d951d7d005693fff747365 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 14 Apr 2018 22:47:52 -0700 Subject: [PATCH 072/112] make pos estimate available --- Firmware/MotorControl/axis.hpp | 8 ++++++-- Firmware/MotorControl/encoder.cpp | 21 ++++++++++++--------- Firmware/MotorControl/encoder.hpp | 8 ++++++-- Firmware/MotorControl/utils.c | 7 ------- Firmware/MotorControl/utils.h | 14 ++++++++++++-- 5 files changed, 36 insertions(+), 22 deletions(-) diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 72dac9b3..e7833202 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -119,8 +119,9 @@ public: if (!do_checks()) // error set during function call break; - if (!update_handler()) // error set during function call - break; + // Run main loop function, defer quitting for after wait + // TODO: change arming logic to arm after waiting + bool main_continue = update_handler(); // Check we meet deadlines after queueing ++loop_counter_; @@ -134,6 +135,9 @@ public: error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT; break; } + + if (!main_continue) + break; } } diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index fad27267..b5ae480e 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -51,8 +51,8 @@ void Encoder::set_count(int32_t count) { uint32_t prim = __get_PRIMASK(); __disable_irq(); // Offset and state must be shifted by the same amount - offset_ += count - state_; - state_ = count; + offset_ += count - shadow_count_; + shadow_count_ = count; //TODO FIXME hw_config_.timer->Instance->CNT = count; pll_pos_ = (float)count; __set_PRIMASK(prim); @@ -197,25 +197,28 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp } // update internal encoder state - int16_t delta_enc = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)state_; - state_ += (int32_t)delta_enc; + int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_; + int32_t delta_enc = (int32_t)delta_enc_16; //sign extend + shadow_count_ += delta_enc; + count_in_cpr_ += delta_enc; + count_in_cpr_ = mod(count_in_cpr_, config_.cpr); // compute electrical phase - int corrected_enc = state_ % config_.cpr; - corrected_enc -= offset_; - //corrected_enc *= axis_->motor_.config_.direction; TODO: verify if this still works + int corrected_enc = count_in_cpr_ - offset_; //TODO avoid recomputing elec_rad_per_enc every time float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float ph = elec_rad_per_enc * (float)corrected_enc; // ph = fmodf(ph, 2*M_PI); phase_ = wrap_pm_pi(ph); + // run pll (for now pll is in units of encoder counts) - // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? // Predict current pos + pos_estimate_ += current_meas_period * pll_vel_; pll_pos_ += current_meas_period * pll_vel_; // discrete phase detector - float delta_pos = (float)(state_ - (int32_t)floorf(pll_pos_)); + // float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_)); + float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_)); // pll feedback pll_pos_ += current_meas_period * pll_kp_ * delta_pos; pll_vel_ += current_meas_period * pll_ki_ * delta_pos; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index ae4b3c0e..57194e75 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -50,9 +50,11 @@ public: Error_t error_ = ERROR_NONE; bool index_found_ = false; bool is_ready_ = false; - int32_t state_ = 0; + int32_t shadow_count_ = 0; + int32_t count_in_cpr_ = 0; int32_t offset_ = 0; float phase_ = 0.0f; // [rad] + float pos_estimate_ = 0.0f; // [rad] float pll_pos_ = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] float pll_kp_ = 0.0f; // [rad/s / rad] @@ -64,9 +66,11 @@ public: make_protocol_property("error", &error_), make_protocol_ro_property("is_ready", &is_ready_), make_protocol_ro_property("index_found", const_cast(&index_found_)), - make_protocol_property("state", &state_), + make_protocol_property("shadow_count", &shadow_count_), + make_protocol_property("count_in_cpr", &count_in_cpr_), make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), + make_protocol_property("pos_estimate", &pos_estimate_), make_protocol_property("pll_pos", &pll_pos_), make_protocol_property("pll_vel", &pll_vel_), make_protocol_property("pll_kp", &pll_kp_), diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 0c80088e..4fa8c378 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -128,13 +128,6 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { return result_valid ? 0 : -1; } -//beware of inserting large angles! -float wrap_pm_pi(float theta) { - while (theta >= M_PI) theta -= (2.0f * M_PI); - while (theta < -M_PI) theta += (2.0f * M_PI); - return theta; -} - // based on https://math.stackexchange.com/a/1105038/81278 float fast_atan2(float y, float x) { // a := min (|x|, |y|) / max (|x|, |y|) diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index a4d7a6e0..158065a0 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -82,14 +82,24 @@ static const float one_by_sqrt3 = 0.57735026919f; static const float two_by_sqrt3 = 1.15470053838f; static const float sqrt3_by_2 = 0.86602540378f; +//beware of inserting large values! +static inline float wrap_pm(float x, float pm_range) { + while (x >= pm_range) x -= (2.0f * pm_range); + while (x < -pm_range) x += (2.0f * pm_range); + return x; +} + +//beware of inserting large angles! +static inline float wrap_pm_pi(float theta) { + return wrap_pm(theta, M_PI); +} + // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 // Returns 0 on success, and -1 if the input was out of range int SVM(float alpha, float beta, float* tA, float* tB, float* tC); -//beware of inserting large angles! -float wrap_pm_pi(float theta); float fast_atan2(float y, float x); int mod(int dividend, int divisor); From aee1a7343df853f8bdd79b99bc8530616541cdfe Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 14 Apr 2018 23:03:03 -0700 Subject: [PATCH 073/112] add property read/write functionality to ASCII protocol Command examples: r vbus_voltage w axis0.config.enable_step_dir 1 --- Firmware/MotorControl/ascii_protocol.cpp | 40 ++++++++- Firmware/MotorControl/protocol.hpp | 105 ++++++++++++++++++++++- 2 files changed, 141 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/ascii_protocol.cpp b/Firmware/MotorControl/ascii_protocol.cpp index 08eecc1d..ad24b1b5 100644 --- a/Firmware/MotorControl/ascii_protocol.cpp +++ b/Firmware/MotorControl/ascii_protocol.cpp @@ -18,7 +18,9 @@ /* Global variables ----------------------------------------------------------*/ /* Private constant data -----------------------------------------------------*/ -#define MAX_LINE_LENGTH 64 +#define MAX_LINE_LENGTH 256 +#define TO_STR_INNER(s) #s +#define TO_STR(s) TO_STR_INNER(s) /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ @@ -131,8 +133,40 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); -// } else if (cmd[0] == 'r') { // read property -// } else if (cmd[0] == 'w') { // write property + } else if (cmd[0] == 'r') { // read property + char name[MAX_LINE_LENGTH]; + int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + char response[10]; + bool success = endpoint->get_string(response, sizeof(response)); + if (!success) + respond(response_channel, use_checksum, "not implemented"); + else + respond(response_channel, use_checksum, response); + } + } + } else if (cmd[0] == 'w') { // write property + char name[MAX_LINE_LENGTH]; + char value[MAX_LINE_LENGTH]; + int numscan = sscanf(cmd, "w %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + bool success = endpoint->set_string(value, sizeof(value)); + if (!success) + respond(response_channel, use_checksum, "not implemented"); + } + } } else if (cmd[0] == 'h') { // HALT for(size_t i = 0; i < AXIS_COUNT; i++){ diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp index bd853608..cb0f666b 100644 --- a/Firmware/MotorControl/protocol.hpp +++ b/Firmware/MotorControl/protocol.hpp @@ -407,12 +407,15 @@ class Endpoint { public: //const char* const name_; virtual void handle(const uint8_t* input, size_t input_length, StreamSink* output) = 0; + virtual bool get_string(char * output, size_t length) { return false; }; + virtual bool set_string(char * buffer, size_t length) { return false; } }; class EndpointProvider { public: virtual size_t get_endpoint_count() = 0; virtual void write_json(size_t id, StreamSink* output) = 0; + virtual Endpoint* get_by_name(char * name, size_t length) = 0; virtual void register_endpoints(Endpoint** list, size_t id, size_t length) = 0; }; @@ -456,6 +459,9 @@ public: void register_endpoints(Endpoint** list, size_t id, size_t length) { // no action } + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; + } std::tuple<> get_names_as_tuple() const { return std::tuple<>(); } }; @@ -485,6 +491,12 @@ public: subsequent_members_.write_json(id + TMember::endpoint_count, output); } + Endpoint* get_by_name(const char * name, size_t length) { + Endpoint* result = this_member_.get_by_name(name, length); + if (result) return result; + else return subsequent_members_.get_by_name(name, length); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) /*final*/ { this_member_.register_endpoints(list, id, length); subsequent_members_.register_endpoints(list, id + TMember::endpoint_count, length); @@ -516,6 +528,14 @@ public: write_string("]}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + size_t segment_length = strlen(name); + if (!strncmp(name, name_, length)) + return member_list_.get_by_name(name + segment_length + 1, length - segment_length - 1); + else + return nullptr; + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { member_list_.register_endpoints(list, id, length); } @@ -529,6 +549,11 @@ ProtocolObject make_protocol_object(const char * name, TMembers&&.. return ProtocolObject(name, std::forward(member_list)...); } + +// TODO: move to cpp_utils +#define ENABLE_IF_SAME(a, b, type) \ + template typename std::enable_if_t::value, bool> + template class ProtocolProperty : public Endpoint { public: @@ -585,6 +610,71 @@ public: write_string("}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + if (!strncmp(name, name_, length)) + return this; + else + return nullptr; + } + + + // *** ASCII protocol handlers *** + + ENABLE_IF_SAME(std::decay_t, float, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%f", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, int32_t, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%ld", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, uint32_t, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%lu", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, bool, bool) + get_string_ex(char * buffer, size_t length, int) { + buffer[0] = (*property_) ? '1' : '0'; + buffer[1] = 0; + return true; + } + bool get_string_ex(char * buffer, size_t length, ...) { + return false; + } + bool get_string(char * buffer, size_t length) final { + return get_string_ex(buffer, length, 0); + } + ENABLE_IF_SAME(TProperty, float, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%f", property_) == 1; + } + ENABLE_IF_SAME(TProperty, int32_t, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%ld", property_) == 1; + } + ENABLE_IF_SAME(TProperty, uint32_t, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%lu", property_) == 1; + } + ENABLE_IF_SAME(TProperty, bool, bool) + set_string_ex(char * buffer, size_t length, int) { + int val; + if (sscanf(buffer, "%d", &val) != 1) + return false; + *property_ = val; + return true; + } + bool set_string_ex(char * buffer, size_t length, ...) { + return false; + } + bool set_string(char * buffer, size_t length) final { + //__asm ("bkpt"); + return set_string_ex(buffer, length, 0); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this; @@ -686,7 +776,7 @@ struct PropertyListFactory { template -class ProtocolFunction : Endpoint { +class ProtocolFunction : public Endpoint { public: static constexpr size_t endpoint_count = 1 + MemberList...>::endpoint_count; template @@ -722,6 +812,10 @@ public: write_string("]}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; // can't address functions by name + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this; @@ -765,6 +859,14 @@ public: void register_endpoints(Endpoint** list, size_t id, size_t length) final { return member_list_.register_endpoints(list, id, length); } + Endpoint* get_by_name(char * name, size_t length) final { + for (size_t i = 0; i < length; i++) { + if (name[i] == '.') + name[i] = 0; + } + name[length-1] = 0; + return member_list_.get_by_name(name, length); + } T& member_list_; }; @@ -775,5 +877,6 @@ void set_application_endpoints(EndpointProvider* endpoints); extern Endpoint* endpoints_[]; extern size_t n_endpoints_; extern const size_t max_endpoints_; +extern EndpointProvider* application_endpoints; #endif From e4b199e5c6b69fe99032cf96dd5fa346a1e99388 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 14 Apr 2018 23:43:21 -0700 Subject: [PATCH 074/112] some nan checks require overriding fast math --- Firmware/Tupfile.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 4902285b..6aeb6590 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -107,7 +107,7 @@ LDFLAGS += '-Wl,--undefined=uxTopUsedPriority' -- common flags for ASM, C and C++ OPT += '-Og' -OPT += '-ffast-math' +OPT += '-ffast-math -fno-finite-math-only' tup.append_table(FLAGS, OPT) tup.append_table(LDFLAGS, OPT) From 0b1bf6f278413fe14e4c7dc4ac6795e198f34361 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 00:38:39 -0700 Subject: [PATCH 075/112] implement circular vel pll tracking --- Firmware/MotorControl/encoder.cpp | 28 ++++++++++++++++++++-------- Firmware/MotorControl/encoder.hpp | 4 ++-- Firmware/MotorControl/utils.h | 9 +++++++++ tools/odrive/shell.py | 2 +- tools/odrive/tests.py | 12 ++++++------ tools/odrivetool | 4 ++-- 6 files changed, 40 insertions(+), 19 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index b5ae480e..7f573752 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -50,11 +50,20 @@ void Encoder::set_count(int32_t count) { // Disable interrupts to make a critical section to avoid race condition uint32_t prim = __get_PRIMASK(); __disable_irq(); + + // Update states + shadow_count_ = count; + pos_estimate_ = (float)count; + count_in_cpr_ = mod(count, config_.cpr); + pos_cpr = (float)count_in_cpr_; + // Offset and state must be shifted by the same amount offset_ += count - shadow_count_; - shadow_count_ = count; //TODO FIXME + offset_ = mod(offset_, config_.cpr); + + //Write hardware last hw_config_.timer->Instance->CNT = count; - pll_pos_ = (float)count; + __set_PRIMASK(prim); } @@ -215,16 +224,19 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // run pll (for now pll is in units of encoder counts) // Predict current pos pos_estimate_ += current_meas_period * pll_vel_; - pll_pos_ += current_meas_period * pll_vel_; + pos_cpr += current_meas_period * pll_vel_; // discrete phase detector - // float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_)); - float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pll_pos_)); + float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_)); + float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback - pll_pos_ += current_meas_period * pll_kp_ * delta_pos; - pll_vel_ += current_meas_period * pll_ki_ * delta_pos; + pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; + pos_cpr += current_meas_period * pll_kp_ * delta_pos_cpr; + pos_cpr = fmodf_pos(pos_cpr, (float)(config_.cpr)); + pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr; // Assign output arguments - if (pos_estimate) *pos_estimate = pll_pos_; + if (pos_estimate) *pos_estimate = pos_estimate_; if (vel_estimate) *vel_estimate = pll_vel_; if (phase_output) *phase_output = phase_; return true; diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 57194e75..39139e1f 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -55,7 +55,7 @@ public: int32_t offset_ = 0; float phase_ = 0.0f; // [rad] float pos_estimate_ = 0.0f; // [rad] - float pll_pos_ = 0.0f; // [rad] + float pos_cpr = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] float pll_kp_ = 0.0f; // [rad/s / rad] float pll_ki_ = 0.0f; // [(rad/s^2) / rad] @@ -71,7 +71,7 @@ public: make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), make_protocol_property("pos_estimate", &pos_estimate_), - make_protocol_property("pll_pos", &pll_pos_), + make_protocol_property("pos_cpr", &pos_cpr), make_protocol_property("pll_vel", &pll_vel_), make_protocol_property("pll_kp", &pll_kp_), make_protocol_property("pll_ki", &pll_ki_), diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 158065a0..52e7f98b 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -7,6 +7,7 @@ extern "C" { #endif #include +#include /** * @brief Unique ID register address location @@ -94,6 +95,14 @@ static inline float wrap_pm_pi(float theta) { return wrap_pm(theta, M_PI); } +// like fmodf, but always positive +static inline float fmodf_pos(float x, float y) { + float out = fmodf(x, y); + if (out < 0.0f) + out += y; + return out; +} + // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py index af2e9432..19fee9dd 100644 --- a/tools/odrive/shell.py +++ b/tools/odrive/shell.py @@ -22,7 +22,7 @@ def print_help(args): print('Type "odrv0." and press ') print('This will present you with all the properties that you can reference') print('') - print('For example: "odrv0.motor0.encoder.pll_pos"') + print('For example: "odrv0.motor0.encoder.pos_estimate"') print('will print the current encoder position on motor 0') print('and "odrv0.motor0.pos_setpoint = 10000"') print('will send motor0 to 10000') diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index eb052947..de4c6d8a 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -342,13 +342,13 @@ class TestClosedLoopControl(AxisTest): time.sleep(0.001) test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) time.sleep(0.1) # give the PLL some time to settle - init_pos = axis_ctx.handle.encoder.pll_pos + init_pos = axis_ctx.handle.encoder.pos_estimate axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0) time.sleep(0.5) - test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos+1000, range=200) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos+1000, range=200) axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0) time.sleep(0.5) - test_assert_eq(axis_ctx.handle.encoder.pll_pos, init_pos-1000, range=400) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos-1000, range=400) logger.debug("closed loop control: test vel_limit") axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0) @@ -551,7 +551,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): # Turn to some position logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name)) set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50) - init_pos = driver_ctx.handle.encoder.pll_pos + init_pos = driver_ctx.handle.encoder.pos_estimate driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) for _ in range(int(4000/5)): @@ -563,7 +563,7 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name)) set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50) - init_pos = driver_ctx.handle.encoder.pll_pos + init_pos = driver_ctx.handle.encoder.pos_estimate driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) #for _ in range(int(5*4000/5)): @@ -580,6 +580,6 @@ class TestVelCtrlVsPosCtrl(DualAxisTest): ## Turn to another position #logger.debug("controlling against load, vel=40000...") #set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20) - #init_pos = axis1_ctx.handle.encoder.pll_pos + #init_pos = axis1_ctx.handle.encoder.pos_estimate #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/odrivetool b/tools/odrivetool index 4afa2927..add4c0c6 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -100,8 +100,8 @@ try: # If you want to plot different values, change them here. # You can plot any number of values concurrently. - start_liveplotter(lambda: [my_odrive.motor0.encoder.pll_pos, - my_odrive.motor1.encoder.pll_pos]) + start_liveplotter(lambda: [my_odrive.motor0.encoder.pos_estimate, + my_odrive.motor1.encoder.pos_estimate]) elif args.command == 'drv-status': from odrive.utils import print_drv_regs From be50b6ee7409cc34fbb23688633de0f0d7a976f6 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 01:07:54 -0700 Subject: [PATCH 076/112] split set_count into linear and circular --- Firmware/MotorControl/encoder.cpp | 28 +++++++++++++++++++--------- Firmware/MotorControl/encoder.hpp | 3 ++- 2 files changed, 21 insertions(+), 10 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 7f573752..24585d47 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -36,7 +36,7 @@ void Encoder::setup() { // TODO: disable interrupt once we found the index void Encoder::enc_index_cb() { if (config_.use_index && !index_found_) { - set_count(0); + set_circular_count(0); if (config_.pre_calibrated) { offset_ = config_.offset; is_ready_ = true; @@ -46,7 +46,7 @@ void Encoder::enc_index_cb() { } // Function that sets the current encoder count to a desired 32-bit value. -void Encoder::set_count(int32_t count) { +void Encoder::set_linear_count(int32_t count) { // Disable interrupts to make a critical section to avoid race condition uint32_t prim = __get_PRIMASK(); __disable_irq(); @@ -54,19 +54,29 @@ void Encoder::set_count(int32_t count) { // Update states shadow_count_ = count; pos_estimate_ = (float)count; - count_in_cpr_ = mod(count, config_.cpr); - pos_cpr = (float)count_in_cpr_; - - // Offset and state must be shifted by the same amount - offset_ += count - shadow_count_; - offset_ = mod(offset_, config_.cpr); - //Write hardware last hw_config_.timer->Instance->CNT = count; __set_PRIMASK(prim); } +// Function that sets the CPR circular tracking encoder count to a desired 32-bit value. +// Note that this will get mod'ed down to [0, cpr) +void Encoder::set_circular_count(int32_t count) { + // Disable interrupts to make a critical section to avoid race condition + uint32_t prim = __get_PRIMASK(); + __disable_irq(); + + // Offset and state must be shifted by the same amount + offset_ += count - count_in_cpr_; + offset_ = mod(offset_, config_.cpr); + // Update states + count_in_cpr_ = mod(count, config_.cpr); + pos_cpr = (float)count_in_cpr_; + + __set_PRIMASK(prim); +} + // @brief Slowly turns the motor in one direction until the // encoder index is found. diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 39139e1f..841127f4 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -35,7 +35,8 @@ public: void enc_index_cb(); - void set_count(int32_t count); + void set_linear_count(int32_t count); + void set_circular_count(int32_t count); bool calib_enc_offset(float voltage_magnitude); bool scan_for_enc_idx(float omega, float voltage_magnitude); From 7d99825988e3072b217e6284f0c5950b85528818 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 15:55:36 -0700 Subject: [PATCH 077/112] snap pll_vel to 0 to avoid jitter --- Firmware/MotorControl/encoder.cpp | 2 ++ tools/odrive/tests.py | 1 + tools/run_tests.py | 2 +- 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 24585d47..f2caa9b2 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -244,6 +244,8 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp pos_cpr += current_meas_period * pll_kp_ * delta_pos_cpr; pos_cpr = fmodf_pos(pos_cpr, (float)(config_.cpr)); pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr; + if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_) + pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter // Assign output arguments if (pos_estimate) *pos_estimate = pos_estimate_; diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index de4c6d8a..20ade97c 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -404,6 +404,7 @@ class TestHighVelocity(AxisTest): self._brake = brake def check_preconditions(self, axis_ctx: AxisTestContext, logger): + time.sleep(2.5) #delay in case load needs time to stop moving super(TestHighVelocity, self).check_preconditions(axis_ctx, logger) test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) test_assert_eq(axis_ctx.handle.encoder.is_ready, True) diff --git a/tools/run_tests.py b/tools/run_tests.py index dc897fde..7e178c97 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -48,7 +48,7 @@ else: all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) #all_tests.append(TestHighVelocity()) -all_tests.append(TestHighVelocityInViscousFluid(load_current=60, driver_current=70)) +all_tests.append(TestHighVelocityInViscousFluid(load_current=20, driver_current=40)) # all_tests.append(TestVelCtrlVsPosCtrl()) # TODO: test step/dir # TODO: test sensorless From d558d554f5e8f01b9b4b4a38c96d1e1262366626 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 17:00:51 -0700 Subject: [PATCH 078/112] add speed ratings --- tools/odrive/tests.py | 18 ++++++++++++------ tools/run_tests.py | 2 +- tools/test-rig.yaml | 16 ++++++++++++---- 3 files changed, 25 insertions(+), 11 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 20ade97c..f192c301 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -404,18 +404,22 @@ class TestHighVelocity(AxisTest): self._brake = brake def check_preconditions(self, axis_ctx: AxisTestContext, logger): - time.sleep(2.5) #delay in case load needs time to stop moving + # time.sleep(2.5) #delay in case load needs time to stop moving super(TestHighVelocity, self).check_preconditions(axis_ctx, logger) test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) test_assert_eq(axis_ctx.handle.encoder.is_ready, True) def run_test(self, axis_ctx: AxisTestContext, logger): - # Calculate theoretical max velocity in encoder counts per second based on the nominal - # V_bus and motor KV rating - max_rpm = axis_ctx.odrv_ctx.yaml['vbus-voltage'] * axis_ctx.yaml['motor-kv'] - rated_limit = max_rpm / 60 * axis_ctx.yaml['encoder-cpr'] - expected_limit = rated_limit + # Calculate theoretical max velocity in rpm based on the nominal + # V_bus and motor KV rating. If we are using a higher bus voltage than rated, use rated voltage + voltage_for_speed = min(axis_ctx.odrv_ctx.yaml['vbus-voltage'], axis_ctx.yaml['motor-max-voltage']) + base_speed_rpm = voltage_for_speed * axis_ctx.yaml['motor-kv'] + #but don't go over encoder max rpm + rated_rpm = min(base_speed_rpm, axis_ctx.yaml['encoder-max-rpm']) + #convert to count/s + rated_limit = rated_rpm / 60 * axis_ctx.yaml['encoder-cpr'] + expected_limit = rated_limit # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) # whereas the ODrive modulates the space vector around a circular trajectory. # See Fig 4.28 here: http://krex.k-state.edu/dspace/bitstream/handle/2097/1507/JamesMevey2009.pdf @@ -441,6 +445,7 @@ class TestHighVelocity(AxisTest): else: axis_ctx.handle.motor.config.current_lim = self._override_current_limit axis_ctx.handle.controller.config.vel_limit = rated_limit + axis_ctx.handle.controller.set_vel_setpoint(0, 0) request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) logger.debug("Drive current {}A, Load current {}A".format(axis_ctx.handle.motor.config.current_lim, self._load_current)) @@ -518,6 +523,7 @@ class TestHighVelocityInViscousFluid(DualAxisTest): load_ctx.handle.controller.config.vel_limit = 20000 # this is not really relevant load_ctx.handle.motor.config.current_lim = self._load_current load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus + load_ctx.handle.controller.set_vel_setpoint(0, 0) request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/run_tests.py b/tools/run_tests.py index 7e178c97..c6fe22b7 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -48,7 +48,7 @@ else: all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) #all_tests.append(TestHighVelocity()) -all_tests.append(TestHighVelocityInViscousFluid(load_current=20, driver_current=40)) +all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45)) # all_tests.append(TestVelCtrlVsPosCtrl()) # TODO: test step/dir # TODO: test sensorless diff --git a/tools/test-rig.yaml b/tools/test-rig.yaml index 7e84dec8..c1a16ebf 100644 --- a/tools/test-rig.yaml +++ b/tools/test-rig.yaml @@ -17,16 +17,20 @@ odrives: motor-pole-pairs: 7 motor-direction: 1 motor-kv: 190 - motor-max-current: 50 + motor-max-current: 70 + motor-max-voltage: 40 encoder-cpr: 8192 + encoder-max-rpm: 7000 - name: 'black' motor-phase-resistance: 0.028 motor-phase-inductance: 1.6e-05 motor-pole-pairs: 7 motor-direction: -1 motor-kv: 270 - motor-max-current: 50 + motor-max-current: 70 + motor-max-voltage: 32 encoder-cpr: 8192 + encoder-max-rpm: 7000 - name: bottom-odrive board-version: v3.5-24V serial-number: "3661335E3037" @@ -43,16 +47,20 @@ odrives: motor-pole-pairs: 7 motor-direction: 1 motor-kv: 270 - motor-max-current: 50 + motor-max-current: 70 + motor-max-voltage: 32 encoder-cpr: 8192 + encoder-max-rpm: 7000 - name: 'yellow' motor-phase-resistance: 0.0245 motor-phase-inductance: 2.03e-05 motor-pole-pairs: 7 motor-direction: -1 motor-kv: 190 - motor-max-current: 50 + motor-max-current: 70 + motor-max-voltage: 40 encoder-cpr: 8192 + encoder-max-rpm: 7000 # Mechanical couplings couplings: From fcb88e105e223ae499f29d6b0309f94760048317 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 18:17:17 -0700 Subject: [PATCH 079/112] rename test rig to parallel --- tools/run_tests.py | 2 +- tools/{test-rig.yaml => test-rig-parallel.yaml} | 0 2 files changed, 1 insertion(+), 1 deletion(-) rename tools/{test-rig.yaml => test-rig-parallel.yaml} (100%) diff --git a/tools/run_tests.py b/tools/run_tests.py index c6fe22b7..62d51bf4 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -25,7 +25,7 @@ parser.add_argument("--ignore", metavar='DEVICE', action='store', nargs='+', help="Ignore one or more ODrives or axes") parser.add_argument("--test-rig-yaml", type=argparse.FileType('r'), help="test rig YAML file") -parser.set_defaults(test_rig_yaml=script_path + '/test-rig.yaml') +parser.set_defaults(test_rig_yaml=script_path + '/test-rig-parallel.yaml') parser.set_defaults(ignore=[]) args = parser.parse_args() diff --git a/tools/test-rig.yaml b/tools/test-rig-parallel.yaml similarity index 100% rename from tools/test-rig.yaml rename to tools/test-rig-parallel.yaml From 74cbaee70989608c413e14de667609fddd135bbe Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 21:46:33 -0700 Subject: [PATCH 080/112] add loopback test group --- tools/odrive/tests.py | 3 +++ tools/run_tests.py | 24 ++++++++++++++---------- tools/test-rig-loopback.yaml | 36 ++++++++++++++++++++++++++++++++++++ 3 files changed, 53 insertions(+), 10 deletions(-) create mode 100644 tools/test-rig-loopback.yaml diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index f192c301..bc3a1339 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -538,6 +538,9 @@ class TestHighVelocityInViscousFluid(DualAxisTest): request_state(load_ctx, AXIS_STATE_IDLE) request_state(driver_ctx, AXIS_STATE_IDLE) +# class TestSelfLoadedPosVelDistribution(DualAxisTest): + + class TestVelCtrlVsPosCtrl(DualAxisTest): """ Uses one ODrive as a load operating in velocity control mode. diff --git a/tools/run_tests.py b/tools/run_tests.py index 62d51bf4..96773d86 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -25,12 +25,14 @@ parser.add_argument("--ignore", metavar='DEVICE', action='store', nargs='+', help="Ignore one or more ODrives or axes") parser.add_argument("--test-rig-yaml", type=argparse.FileType('r'), help="test rig YAML file") -parser.set_defaults(test_rig_yaml=script_path + '/test-rig-parallel.yaml') +# parser.set_defaults(test_rig_yaml=script_path + '/test-rig-parallel.yaml') parser.set_defaults(ignore=[]) args = parser.parse_args() +test_rig_yaml = yaml.load(args.test_rig_yaml) # TODO: add --only option + all_tests = [] if not args.skip_boring_tests: all_tests.append(TestFlashAndErase()) @@ -47,19 +49,21 @@ else: all_tests.append(TestDiscoverAndGotoIdle()) all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) -#all_tests.append(TestHighVelocity()) -all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45)) -# all_tests.append(TestVelCtrlVsPosCtrl()) -# TODO: test step/dir -# TODO: test sensorless -# TODO: test ASCII protocol -# TODO: test protocol over UART +if 'test-rig-parallel.yaml' in test_rig_yaml: + #all_tests.append(TestHighVelocity()) + all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45)) + # all_tests.append(TestVelCtrlVsPosCtrl()) + # TODO: test step/dir + # TODO: test sensorless + # TODO: test ASCII protocol + # TODO: test protocol over UART +elif 'test-rig-loopback.yaml' in test_rig_yaml: + pass + print(str(args.ignore)) logger = Logger() - -test_rig_yaml = yaml.load(args.test_rig_yaml) os.chdir(script_path + '/../Firmware') # Build a dictionary of odrive test contexts by name diff --git a/tools/test-rig-loopback.yaml b/tools/test-rig-loopback.yaml new file mode 100644 index 00000000..6cb15c75 --- /dev/null +++ b/tools/test-rig-loopback.yaml @@ -0,0 +1,36 @@ + +odrives: + - name: odrive-48V + board-version: v3.5-48V + serial-number: "3660335E3037" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '533f7506493f49514454193f' + vbus-voltage: 24 # [V] + max-brake-power: 150 # [W] + axes: + - name: 'M0' + motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 190 + motor-max-current: 70 + motor-max-voltage: 40 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: 'M1' + motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 190 + motor-max-current: 70 + motor-max-voltage: 40 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + +# Mechanical couplings +couplings: + - [ odrive-48V.M0, odrive-48V.M1 ] \ No newline at end of file From 6e066b45f3a6a046b913ac7d5e1b210954807880 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 15 Apr 2018 23:18:58 -0700 Subject: [PATCH 081/112] finish spiral test --- tools/odrive/tests.py | 118 ++++++++++++++++++++++++++++------- tools/run_tests.py | 7 ++- tools/test-rig-loopback.yaml | 2 + tools/test-rig-parallel.yaml | 2 + 4 files changed, 105 insertions(+), 24 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index bc3a1339..9e3d47f1 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -8,6 +8,7 @@ import threading import odrive.discovery from odrive.enums import * import odrive.utils +import numpy as np import functools print = functools.partial(print, flush=True) @@ -131,6 +132,27 @@ def set_limits(axis_ctx: AxisTestContext, logger, vel_limit=20000, current_limit axis_ctx.handle.motor.config.current_lim = current_limit axis_ctx.handle.controller.config.vel_limit = vel_limit +def get_max_rpm(axis_ctx: AxisTestContext): + + # Calculate theoretical max velocity in rpm based on the nominal + # V_bus and motor KV rating. + # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) + # whereas the ODrive modulates the space vector around a circular trajectory. + # See Fig 4.28 here: http://krex.k-state.edu/dspace/bitstream/handle/2097/1507/JamesMevey2009.pdf + effective_bus_voltage = axis_ctx.odrv_ctx.yaml['vbus-voltage'] + effective_bus_voltage *= (2/math.sqrt(3)) / (4/math.pi) # roughtly 90% + # The ODrive only goes to 80% modulation depth in order to save some time for the ADC measurements. + # See FOC_current in motor.cpp. + effective_bus_voltage *= 0.8 + + # If we are using a higher bus voltage than rated: use rated voltage, + # since that is an effective speed rating of the motor + voltage_for_speed = min(effective_bus_voltage, axis_ctx.yaml['motor-max-voltage']) + base_speed_rpm = voltage_for_speed * axis_ctx.yaml['motor-kv'] + + #but don't go over encoder max rpm + rated_rpm = min(base_speed_rpm, axis_ctx.yaml['encoder-max-rpm']) + return rated_rpm class ODriveTest(ABC): """ @@ -410,24 +432,8 @@ class TestHighVelocity(AxisTest): test_assert_eq(axis_ctx.handle.encoder.is_ready, True) def run_test(self, axis_ctx: AxisTestContext, logger): - # Calculate theoretical max velocity in rpm based on the nominal - # V_bus and motor KV rating. If we are using a higher bus voltage than rated, use rated voltage - voltage_for_speed = min(axis_ctx.odrv_ctx.yaml['vbus-voltage'], axis_ctx.yaml['motor-max-voltage']) - base_speed_rpm = voltage_for_speed * axis_ctx.yaml['motor-kv'] - #but don't go over encoder max rpm - rated_rpm = min(base_speed_rpm, axis_ctx.yaml['encoder-max-rpm']) - #convert to count/s - rated_limit = rated_rpm / 60 * axis_ctx.yaml['encoder-cpr'] - + rated_limit = get_max_rpm(axis_ctx) / 60 * axis_ctx.yaml['encoder-cpr'] expected_limit = rated_limit - # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) - # whereas the ODrive modulates the space vector around a circular trajectory. - # See Fig 4.28 here: http://krex.k-state.edu/dspace/bitstream/handle/2097/1507/JamesMevey2009.pdf - expected_limit *= (2/math.sqrt(3)) / (4/math.pi) # roughtly 90% - - # The ODrive only goes to 80% modulation depth in order to save some time for the ADC measurements. - # See FOC_current in motor.cpp. - expected_limit *= 0.8 # TODO: remove the following two lines, but for now we want to stay away from the modulation depth limit expected_limit *= 0.6 @@ -451,10 +457,10 @@ class TestHighVelocity(AxisTest): logger.debug("Drive current {}A, Load current {}A".format(axis_ctx.handle.motor.config.current_lim, self._load_current)) ramp_up_time = 15.0 - t_0 = time.monotonic() - last_print = t_0 max_measured_vel = 0.0 logger.debug("ramping to {} over {} s".format(rated_limit, ramp_up_time)) + t_0 = time.monotonic() + last_print = t_0 while True: ratio = (time.monotonic() - t_0) / ramp_up_time if ratio >= 1: @@ -520,7 +526,6 @@ class TestHighVelocityInViscousFluid(DualAxisTest): # Set up viscous fluid load logger.debug("activating load on {}...".format(load_ctx.name)) load_ctx.handle.controller.config.vel_integrator_gain = 0 - load_ctx.handle.controller.config.vel_limit = 20000 # this is not really relevant load_ctx.handle.motor.config.current_lim = self._load_current load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus load_ctx.handle.controller.set_vel_setpoint(0, 0) @@ -538,8 +543,79 @@ class TestHighVelocityInViscousFluid(DualAxisTest): request_state(load_ctx, AXIS_STATE_IDLE) request_state(driver_ctx, AXIS_STATE_IDLE) -# class TestSelfLoadedPosVelDistribution(DualAxisTest): +class TestSelfLoadedPosVelDistribution(DualAxisTest): + """ + Uses an ODrive mechanically connected to itself to test a distribution of + speeds and currents. Since it's connected to itself, we can be a lot less + strict about the brake resistor power use. + """ + def __init__(self, rpm_range=1000, load_current_range=10, driver_current_lim=20): + self._rpm_range = rpm_range + self._load_current_range = load_current_range + self._driver_current_lim = driver_current_lim + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + load_ctx = axis0_ctx + driver_ctx = axis1_ctx + + logger.debug("Iload range: {} A, Idriver: {} A".format(self._load_current_range, self._driver_current_lim)) + + # max speed for rig in counts/s for each encoder (may be different CPR) + max_rpm = min(self._rpm_range, get_max_rpm(driver_ctx), get_max_rpm(load_ctx)) + driver_max_speed = max_rpm / 60 * driver_ctx.yaml['encoder-cpr'] + load_max_speed = max_rpm / 60 * load_ctx.yaml['encoder-cpr'] + logger.debug("RPM range: {} = driver {} = load {}".format(max_rpm, driver_max_speed, load_max_speed)) + + # Set up velocity controlled load + logger.debug("activating load on {}".format(load_ctx.name)) + load_ctx.handle.controller.config.vel_integrator_gain = 0 + load_ctx.handle.controller.config.vel_limit = load_max_speed + load_ctx.handle.motor.config.current_lim = 0 #load current to be set during runtime + load_ctx.handle.controller.set_vel_setpoint(0, 0) # vel sign also set during runtime + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Set up velocity controlled driver + logger.debug("activating driver on {}".format(driver_ctx.name)) + driver_ctx.handle.motor.config.current_lim = self._driver_current_lim + driver_ctx.handle.controller.config.vel_limit = driver_max_speed + driver_ctx.handle.controller.set_vel_setpoint(0, 0) + request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Spiral parameters + command_rate = 500.0 #Hz (nominal, achived rate is less due to time.sleep approx) + test_duration = 15.0 #s + num_cycles = 3.0 # number of spiral "rotations" + + t_0 = time.monotonic() + t_ratio = 0 + last_print = t_0 + while t_ratio < 1: + t_ratio = (time.monotonic() - t_0) / test_duration + phase = 2 * math.pi * num_cycles * t_ratio + driver_speed = t_ratio * driver_max_speed * math.sin(phase) + # print(driver_speed) + driver_ctx.handle.controller.set_vel_setpoint(driver_speed, 0) + load_current = t_ratio * self._load_current_range * math.cos(phase) + Iload_mag = abs(load_current) + Iload_sign = np.sign(load_current) + # print("I: {}, vel {}".format(Iload_mag, Iload_sign * load_max_speed)) + load_ctx.handle.motor.config.current_lim = Iload_mag + load_ctx.handle.controller.set_vel_setpoint(Iload_sign * load_max_speed, 0) + + test_assert_no_error(driver_ctx) + test_assert_no_error(load_ctx) + + # log progress + if time.monotonic() - last_print > 1: + last_print = time.monotonic() + logger.debug("Envelope -- vel: {:.2f}, I: {:.2f}".format(t_ratio * driver_max_speed, t_ratio * self._load_current_range)) + + time.sleep(1/command_rate) + + request_state(load_ctx, AXIS_STATE_IDLE) + request_state(driver_ctx, AXIS_STATE_IDLE) + test_assert_no_error(driver_ctx) + test_assert_no_error(load_ctx) class TestVelCtrlVsPosCtrl(DualAxisTest): """ diff --git a/tools/run_tests.py b/tools/run_tests.py index 96773d86..c6bc9999 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -49,7 +49,7 @@ else: all_tests.append(TestDiscoverAndGotoIdle()) all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) -if 'test-rig-parallel.yaml' in test_rig_yaml: +if test_rig_yaml['type'] == 'parallel': #all_tests.append(TestHighVelocity()) all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45)) # all_tests.append(TestVelCtrlVsPosCtrl()) @@ -57,8 +57,9 @@ if 'test-rig-parallel.yaml' in test_rig_yaml: # TODO: test sensorless # TODO: test ASCII protocol # TODO: test protocol over UART -elif 'test-rig-loopback.yaml' in test_rig_yaml: - pass +elif test_rig_yaml['type'] == 'loopback': + all_tests.append(TestSelfLoadedPosVelDistribution( + rpm_range=2500, load_current_range=50, driver_current_lim=60)) print(str(args.ignore)) diff --git a/tools/test-rig-loopback.yaml b/tools/test-rig-loopback.yaml index 6cb15c75..a55f6e72 100644 --- a/tools/test-rig-loopback.yaml +++ b/tools/test-rig-loopback.yaml @@ -1,4 +1,6 @@ +type: loopback + odrives: - name: odrive-48V board-version: v3.5-48V diff --git a/tools/test-rig-parallel.yaml b/tools/test-rig-parallel.yaml index c1a16ebf..e7559105 100644 --- a/tools/test-rig-parallel.yaml +++ b/tools/test-rig-parallel.yaml @@ -1,4 +1,6 @@ +type: parallel + # ODrives odrives: - name: top-odrive From 27574234d5991e1c15725ae86bcbc460f6575233 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 16 Apr 2018 20:04:02 -0700 Subject: [PATCH 082/112] improve resilience and safety of entering DFU mode - Disable interrupts before setting the reboot cookie - Delay after NVIC_SystemReset before jumping to bootloader (see comment for an explanation) - Make a variable for the reboot cookie - Do reboot cookie checks before C++ static initializers --- Firmware/Board/v3/Src/main.c | 64 ++++++++++++++++--------- Firmware/Board/v3/startup_stm32f405xx.s | 2 + Firmware/MotorControl/communication.cpp | 3 +- Firmware/MotorControl/odrive_main.hpp | 2 + 4 files changed, 47 insertions(+), 24 deletions(-) diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 64810aac..67c638df 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -81,12 +81,47 @@ void MX_FREERTOS_Init(void); /* USER CODE BEGIN 0 */ -void jump_to_builtin_bootloader(void) { - __set_MSP(0x20001000); - // http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf - void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004)); - builtin_bootloader(); - for (;;); +uint32_t _reboot_cookie __attribute__ ((section (".noinit"))); +extern char _estack; // provided by the linker script + +// Gets called from the startup assembly code +void early_start_checks(void) { + /* We could jump to the bootloader directly on demand without rebooting + but that requires us to reset several peripherals and interrupts for it + to function correctly. Therefore it's easier to just reset the entire chip. */ + if(_reboot_cookie == 0xDEADBEEF) { + _reboot_cookie = 0xCAFEFEED; //Reset bootloader trigger + + /* + * This wait loop solves an obscure timing issue, but we don't exactly understand why. + * When the transition NVIC_SystemReset() => STM bootloader happens very quickly, + * there is a yet unexplained phenomenon where the ODrive would emit an audible click, + * followed by one the following symptoms: + * - Device reboots in normal mode (possibly due to the bootloader exiting immidiately) + * - Device goes into DFU mode and then the power supply turns off + * This manifests in the DFU script detecting the device in DFU mode but then + * losing the device immidiately after. + * There were no motors/encoders/brake resistor connected when testing this. As far as + * we can tell, the only way for the software to cause a short circuit is through the + * brake FETs. + */ + for (size_t i = 0; i < 1000000; ++i) { + __NOP(); + } + + __set_MSP((uintptr_t)&_estack); + // http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf + void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004)); + builtin_bootloader(); + } + + /* The bootloader might fail to properly clean up after itself, + so if we're not sure that the system is in a clean state we + just reset it again */ + if(_reboot_cookie != 42) { + _reboot_cookie = 42; + NVIC_SystemReset(); + } } /* USER CODE END 0 */ @@ -100,22 +135,6 @@ int main(void) { /* USER CODE BEGIN 1 */ - /* We could jump to the bootloader directly on demand without rebooting - but that requires us to reset several peripherals and interrupts for it - to function correctly. Therefore it's easier to just reset the entire chip. */ - if(*((unsigned long *)0x2001C000) == 0xDEADBEEF) { - *((unsigned long *)0x2001C000) = 0xCAFEFEED; //Reset bootloader trigger - jump_to_builtin_bootloader(); - } - - /* The bootloader might fail to properly clean up after itself, - so if we're not sure that the system is in a clean state we - just reset it again */ - if(*((unsigned long *)0x2001C000) != 42) { - *((unsigned long *)0x2001C000) = 42; - NVIC_SystemReset(); - } - // This procedure of building a USB serial number should be identical // to the way the STM's built-in USB bootloader does it. This means // that the device will have the same serial number in normal and DFU mode. @@ -155,7 +174,6 @@ int main(void) MX_DMA_Init(); MX_ADC1_Init(); MX_ADC2_Init(); - MX_CAN1_Init(); MX_TIM1_Init(); MX_TIM8_Init(); MX_TIM3_Init(); diff --git a/Firmware/Board/v3/startup_stm32f405xx.s b/Firmware/Board/v3/startup_stm32f405xx.s index ea0e76a9..34c01d1d 100644 --- a/Firmware/Board/v3/startup_stm32f405xx.s +++ b/Firmware/Board/v3/startup_stm32f405xx.s @@ -107,6 +107,8 @@ LoopFillZerobss: /* Call the clock system intitialization function.*/ bl SystemInit + bl early_start_checks + /* Call static constructors */ bl __libc_init_array /* Call the application's entry point.*/ diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 7bd1a391..87711595 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -140,7 +140,8 @@ StreamToPacketConverter uart4_stream_input(uart4_channel); /* Function implementations --------------------------------------------------*/ void enter_dfu_mode() { - *((unsigned long *)0x2001C000) = 0xDEADBEEF; + __asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts + _reboot_cookie = 0xDEADBEEF; NVIC_SystemReset(); } diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index f1d91007..f1ebb703 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -46,6 +46,8 @@ extern BoardConfig_t board_config; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; +extern uint32_t _reboot_cookie; + // TODO: move // this is technically not thread-safe but practically it might be #define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \ From 9593ee4f4aa35b45aba5a62a76e7b138aa76825e Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 17 Apr 2018 00:41:31 -0700 Subject: [PATCH 083/112] change to 48V for loopback --- tools/odrive/tests.py | 2 +- tools/run_tests.py | 2 +- tools/test-rig-loopback.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 9e3d47f1..182f70aa 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -583,7 +583,7 @@ class TestSelfLoadedPosVelDistribution(DualAxisTest): # Spiral parameters command_rate = 500.0 #Hz (nominal, achived rate is less due to time.sleep approx) - test_duration = 15.0 #s + test_duration = 20.0 #s num_cycles = 3.0 # number of spiral "rotations" t_0 = time.monotonic() diff --git a/tools/run_tests.py b/tools/run_tests.py index c6bc9999..f2f67a19 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -59,7 +59,7 @@ if test_rig_yaml['type'] == 'parallel': # TODO: test protocol over UART elif test_rig_yaml['type'] == 'loopback': all_tests.append(TestSelfLoadedPosVelDistribution( - rpm_range=2500, load_current_range=50, driver_current_lim=60)) + rpm_range=3000, load_current_range=60, driver_current_lim=70)) print(str(args.ignore)) diff --git a/tools/test-rig-loopback.yaml b/tools/test-rig-loopback.yaml index a55f6e72..34989214 100644 --- a/tools/test-rig-loopback.yaml +++ b/tools/test-rig-loopback.yaml @@ -9,7 +9,7 @@ odrives: uart: /dev/serial/by-id/[not-yet-used] usb: auto programmer: '533f7506493f49514454193f' - vbus-voltage: 24 # [V] + vbus-voltage: 48 # [V] max-brake-power: 150 # [W] axes: - name: 'M0' From 8407721744acc642ff19b8625a85b30c2da2afa4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 17 Apr 2018 03:21:03 -0700 Subject: [PATCH 084/112] fix external interrupts, step dir now working --- Firmware/.vscode/c_cpp_properties.json | 4 ++-- Firmware/Board/v3/Src/gpio.c | 6 ++++-- Firmware/Board/v3/Src/stm32f4xx_it.c | 8 ++++++++ Firmware/MotorControl/axis.cpp | 8 ++++---- Firmware/MotorControl/main.cpp | 2 ++ Firmware/tup.config.default | 1 - 6 files changed, 20 insertions(+), 9 deletions(-) diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 5042079e..eab99cd5 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -27,8 +27,8 @@ "STM32F405xx", "USE_HAL_DRIVER", "HW_VERSION_MAJOR=3", - "HW_VERSION_MINOR=4", - "HW_VERSION_VOLTAGE=24", + "HW_VERSION_MINOR=5", + "HW_VERSION_VOLTAGE=48", "__weak=\"__attribute__((weak))\"", "__packed=\"__attribute__((__packed__))\"", "__GNUC__" diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 91b3f3cd..fb163f71 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -138,8 +138,9 @@ void MX_GPIO_Init(void) HAL_GPIO_Init(nFAULT_GPIO_Port, &GPIO_InitStruct); /* EXTI interrupt init*/ - HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI2_IRQn); + // TODO get Cube to not emit this + // HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0); + // HAL_NVIC_EnableIRQ(EXTI2_IRQn); } @@ -151,6 +152,7 @@ void MX_GPIO_Init(void) // no matter which port they belong to. IRQn_Type get_irq_number(uint16_t pin) { uint16_t pin_number = 0; + pin >>= 1; while (pin) { pin >>= 1; pin_number++; diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index cd96644a..a7fe3a12 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -341,6 +341,14 @@ void EXTI4_IRQHandler(void) HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); } +/** +* @brief This function handles EXTI lines 5-9 interrupt. +*/ +void EXTI9_5_IRQHandler(void) +{ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_9); +} + /** * @brief This function handles EXTI lines 10-15 interrupt. */ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index bde389d4..4486199a 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -25,6 +25,10 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config, motor_.axis_ = this; } +static void step_cb_wrapper(void* ctx) { + reinterpret_cast(ctx)->step_cb(); +} + // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. void Axis::setup() { @@ -56,10 +60,6 @@ bool Axis::wait_for_current_meas() { return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal; } -static void step_cb_wrapper(void* ctx) { - reinterpret_cast(ctx)->step_cb(); -} - // step/direction interface void Axis::step_cb() { if (enable_step_dir_) { diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 39a60356..75cd43d2 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -31,6 +31,7 @@ void save_configuration(void) { } void load_configuration(void) { + // Try to load configs if (NVM_init() || ConfigFormat::safe_load_config( &board_config, @@ -38,6 +39,7 @@ void load_configuration(void) { &controller_configs, &motor_configs, &axis_configs)) { + //If loading failed, restore defaults board_config = BoardConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { encoder_configs[i] = EncoderConfig_t(); diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default index 5cd89434..8ebb4402 100644 --- a/Firmware/tup.config.default +++ b/Firmware/tup.config.default @@ -3,4 +3,3 @@ #CONFIG_BOARD_VERSION=v3.5-24V CONFIG_USB_PROTOCOL=native CONFIG_UART_PROTOCOL=ascii -CONFIG_STEP_DIR=n From dee8efef22d3bf83cde641e5acaec5a367fc1f36 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 20 Apr 2018 21:14:14 -0700 Subject: [PATCH 085/112] initialize USB interrupt pump before initializing USB device --- Firmware/Board/v3/Src/freertos.c | 3 ++- Firmware/MotorControl/axis_c_interface.h | 14 -------------- Firmware/MotorControl/communication.cpp | 16 +++++++++------- Firmware/MotorControl/communication.h | 3 ++- 4 files changed, 13 insertions(+), 23 deletions(-) delete mode 100644 Firmware/MotorControl/axis_c_interface.h diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index b247994a..9d266640 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -53,7 +53,7 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include "axis_c_interface.h" +#include "communication.h" int odrive_main(void); /* USER CODE END Includes */ @@ -126,6 +126,7 @@ void MX_FREERTOS_Init(void) { osSemaphoreDef(sem_usb_tx); sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); + init_deferred_interrupts(); /* USER CODE END RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_TIMERS */ diff --git a/Firmware/MotorControl/axis_c_interface.h b/Firmware/MotorControl/axis_c_interface.h deleted file mode 100644 index f891e19b..00000000 --- a/Firmware/MotorControl/axis_c_interface.h +++ /dev/null @@ -1,14 +0,0 @@ -#ifndef __AXIS_C_INTERFACE_H -#define __AXIS_C_INTERFACE_H - -#ifdef __cplusplus -extern "C" { -#endif - -void axis_thread_entry(void const * temp_motor_ptr); - -#ifdef __cplusplus -} -#endif - -#endif /* __AXIS_C_INTERFACE_H */ diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp index 87711595..ce1d2360 100644 --- a/Firmware/MotorControl/communication.cpp +++ b/Firmware/MotorControl/communication.cpp @@ -128,8 +128,8 @@ private: } uart4_stream_output; #if defined(UART_PROTOCOL_NATIVE) -PacketToStreamConverter uart4_packet_sender(uart4_stream_output); -BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); +PacketToStreamConverter uart4_packet_output(uart4_stream_output); +BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); StreamToPacketConverter uart4_stream_input(uart4_channel); #endif @@ -145,16 +145,18 @@ void enter_dfu_mode() { NVIC_SystemReset(); } +void init_deferred_interrupts(void) { + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); +} + void init_communication(void) { printf("hi!\r\n"); // Start command handling thread osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); - - // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityAboveNormal, 0, 512); - thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); } @@ -304,7 +306,7 @@ void set_cmd_buffer(uint8_t *buf, uint32_t len) { usb_len = len; } -void usb_update_thread(void * ctx) { +void usb_deferred_interrupt_thread(void * ctx) { (void) ctx; // unused parameter for (;;) { diff --git a/Firmware/MotorControl/communication.h b/Firmware/MotorControl/communication.h index 88e37921..18522ffe 100644 --- a/Firmware/MotorControl/communication.h +++ b/Firmware/MotorControl/communication.h @@ -13,10 +13,11 @@ extern "C" { #endif +void init_deferred_interrupts(void); void init_communication(void); void communication_task(void * ctx); void set_cmd_buffer(uint8_t *buf, uint32_t len); -void usb_update_thread(void * ctx); +void usb_deferred_interrupt_thread(void * ctx); void USB_receive_packet(const uint8_t *buffer, size_t length); extern uint64_t serial_number; From abdb037d7502c12467fa66e601230dbed9406e6f Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 20 Apr 2018 21:20:33 -0700 Subject: [PATCH 086/112] move communication related files --- Firmware/Board/v3/Src/freertos.c | 2 +- Firmware/Board/v3/Src/main.c | 9 ++++----- Firmware/Board/v3/Src/usbd_cdc_if.c | 3 +-- Firmware/Board/v3/Src/usbd_desc.c | 2 +- Firmware/MotorControl/main.cpp | 2 +- Firmware/MotorControl/nvm_config.hpp | 2 +- Firmware/MotorControl/odrive_main.hpp | 2 +- Firmware/MotorControl/utils.h | 16 ---------------- Firmware/Tupfile.lua | 11 ++++++----- .../ascii_protocol.cpp | 0 .../ascii_protocol.h | 0 .../communication.cpp | 0 .../communication.h | 0 Firmware/{MotorControl => communication}/crc.hpp | 0 .../{MotorControl => communication}/protocol.cpp | 0 .../{MotorControl => communication}/protocol.hpp | 0 16 files changed, 16 insertions(+), 33 deletions(-) rename Firmware/{MotorControl => communication}/ascii_protocol.cpp (100%) rename Firmware/{MotorControl => communication}/ascii_protocol.h (100%) rename Firmware/{MotorControl => communication}/communication.cpp (100%) rename Firmware/{MotorControl => communication}/communication.h (100%) rename Firmware/{MotorControl => communication}/crc.hpp (100%) rename Firmware/{MotorControl => communication}/protocol.cpp (100%) rename Firmware/{MotorControl => communication}/protocol.hpp (100%) diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index 9d266640..0d178a56 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -53,7 +53,7 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include "communication.h" +#include int odrive_main(void); /* USER CODE END Includes */ diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 67c638df..74974bd9 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -59,8 +59,7 @@ #include "gpio.h" /* USER CODE BEGIN Includes */ -#include "utils.h" -#include "communication.h" +#include /* USER CODE END Includes */ /* Private variables ---------------------------------------------------------*/ @@ -138,9 +137,9 @@ int main(void) // This procedure of building a USB serial number should be identical // to the way the STM's built-in USB bootloader does it. This means // that the device will have the same serial number in normal and DFU mode. - uint32_t uuid0 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 0); - uint32_t uuid1 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 4); - uint32_t uuid2 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 8); + uint32_t uuid0 = *(uint32_t *)(UID_BASE + 0); + uint32_t uuid1 = *(uint32_t *)(UID_BASE + 4); + uint32_t uuid2 = *(uint32_t *)(UID_BASE + 8); uint32_t uuid_mixed_part = uuid0 + uuid2; serial_number = ((uint64_t)uuid_mixed_part << 16) | (uint64_t)(uuid1 >> 16); diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index b500c6c4..0b5b1807 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -53,8 +53,7 @@ /* USER CODE BEGIN INCLUDE */ #include "cmsis_os.h" #include "freertos_vars.h" -#include "utils.h" -#include "communication.h" +#include #include /* USER CODE END INCLUDE */ diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index ea8584d8..fc9079d9 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -53,7 +53,7 @@ #include "usbd_conf.h" /* USER CODE BEGIN INCLUDE */ -#include "communication.h" +#include /* USER CODE END INCLUDE */ /* Private typedef -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index cd360578..aef0fd5e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,7 +1,7 @@ #include "odrive_main.hpp" #include "nvm_config.hpp" -#include "communication.h" +#include BoardConfig_t board_config; EncoderConfig_t encoder_configs[AXIS_COUNT]; diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp index 7784322b..bf0f9134 100644 --- a/Firmware/MotorControl/nvm_config.hpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -12,7 +12,7 @@ #include #include "nvm.h" -#include "crc.hpp" +#include /* Private defines -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp index f1ebb703..058fbc7d 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.hpp @@ -61,7 +61,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c // ODrive specific includes -#include +#include #include #include #include diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index a4d7a6e0..550a51cb 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -8,11 +8,6 @@ extern "C" { #include -/** - * @brief Unique ID register address location - */ -#define ID_UNIQUE_ADDRESS (0x1FFF7A10) - /** * @brief Flash size register address */ @@ -59,17 +54,6 @@ extern "C" { */ #define STM_ID_GetFlashSize() (*(uint16_t *)(ID_FLASH_ADDRESS)) -/** - * "Returns" the given 32-bit value of the UUID. - * - * Parameters: - * - uint8_t x: - * Value between 0 and 2, corresponding to 4-bytes you want to read from 96bits (12bytes) - * - * Returned data is 32-bit - */ -#define STM_ID_GetUUID(x) ((x >= 0 && x < 3) ? (*(uint32_t *)(ID_UNIQUE_ADDRESS + 4 * (x))) : 0) - #ifdef M_PI #undef M_PI #endif diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index aa122ca5..87d6243e 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -117,7 +117,7 @@ for src in string.gmatch(vars['C_INCLUDES'] or '', "%S+") do end -- TODO: cleaner separation of the platform code and the rest -stm_includes += 'MotorControl' +stm_includes += '.' stm_includes += 'Drivers/DRV8301' stm_sources += boarddir..'/Src/syscalls.c' build{ @@ -137,21 +137,22 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', - 'MotorControl/ascii_protocol.cpp', 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', - 'MotorControl/communication.cpp', - 'MotorControl/protocol.cpp', 'MotorControl/motor.cpp', 'MotorControl/encoder.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', 'MotorControl/main.cpp', + 'communication/communication.cpp', + 'communication/ascii_protocol.cpp', + 'communication/protocol.cpp', 'FreeRTOS-openocd.c' }, includes={ 'Drivers/DRV8301', - 'MotorControl' + 'MotorControl', + '.' } } diff --git a/Firmware/MotorControl/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp similarity index 100% rename from Firmware/MotorControl/ascii_protocol.cpp rename to Firmware/communication/ascii_protocol.cpp diff --git a/Firmware/MotorControl/ascii_protocol.h b/Firmware/communication/ascii_protocol.h similarity index 100% rename from Firmware/MotorControl/ascii_protocol.h rename to Firmware/communication/ascii_protocol.h diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/communication/communication.cpp similarity index 100% rename from Firmware/MotorControl/communication.cpp rename to Firmware/communication/communication.cpp diff --git a/Firmware/MotorControl/communication.h b/Firmware/communication/communication.h similarity index 100% rename from Firmware/MotorControl/communication.h rename to Firmware/communication/communication.h diff --git a/Firmware/MotorControl/crc.hpp b/Firmware/communication/crc.hpp similarity index 100% rename from Firmware/MotorControl/crc.hpp rename to Firmware/communication/crc.hpp diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/communication/protocol.cpp similarity index 100% rename from Firmware/MotorControl/protocol.cpp rename to Firmware/communication/protocol.cpp diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/communication/protocol.hpp similarity index 100% rename from Firmware/MotorControl/protocol.hpp rename to Firmware/communication/protocol.hpp From 741a51442f0d5af225b17d0bb61d2f20264cefef Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 20 Apr 2018 23:44:55 -0700 Subject: [PATCH 087/112] split communication into multiple files --- Firmware/Board/v3/Inc/freertos_vars.h | 14 +- Firmware/Board/v3/Src/freertos.c | 33 ++- Firmware/Board/v3/Src/main.c | 3 +- Firmware/Board/v3/Src/usbd_cdc_if.c | 7 +- Firmware/Board/v3/Src/usbd_desc.c | 2 +- Firmware/MotorControl/axis.cpp | 2 +- Firmware/MotorControl/axis.hpp | 4 +- Firmware/MotorControl/board_config_v3.h | 27 +- Firmware/MotorControl/controller.cpp | 2 +- Firmware/MotorControl/controller.hpp | 4 +- Firmware/MotorControl/encoder.cpp | 3 +- Firmware/MotorControl/encoder.hpp | 4 +- Firmware/MotorControl/low_level.cpp | 2 +- Firmware/MotorControl/low_level.h | 4 +- Firmware/MotorControl/main.cpp | 4 +- Firmware/MotorControl/motor.cpp | 3 +- Firmware/MotorControl/motor.hpp | 4 +- .../{odrive_main.hpp => odrive_main.h} | 63 +++-- .../MotorControl/sensorless_estimator.cpp | 3 +- Firmware/Tupfile.lua | 2 + Firmware/communication/ascii_protocol.cpp | 2 +- Firmware/communication/ascii_protocol.h | 27 +- Firmware/communication/communication.cpp | 237 +----------------- Firmware/communication/communication.h | 7 - Firmware/communication/interface_uart.cpp | 101 ++++++++ Firmware/communication/interface_uart.h | 14 ++ Firmware/communication/interface_usb.cpp | 103 ++++++++ Firmware/communication/interface_usb.h | 17 ++ 28 files changed, 365 insertions(+), 333 deletions(-) rename Firmware/MotorControl/{odrive_main.hpp => odrive_main.h} (89%) create mode 100644 Firmware/communication/interface_uart.cpp create mode 100644 Firmware/communication/interface_uart.h create mode 100644 Firmware/communication/interface_usb.cpp create mode 100644 Firmware/communication/interface_usb.h diff --git a/Firmware/Board/v3/Inc/freertos_vars.h b/Firmware/Board/v3/Inc/freertos_vars.h index c1e122a0..2eb52d7d 100644 --- a/Firmware/Board/v3/Inc/freertos_vars.h +++ b/Firmware/Board/v3/Inc/freertos_vars.h @@ -3,15 +3,9 @@ #define __FREERTOS_H // List of semaphores -osSemaphoreId sem_usb_irq; -osSemaphoreId sem_uart_dma; -osSemaphoreId sem_usb_rx; -osSemaphoreId sem_usb_tx; - -// List of threads -osThreadId thread_motor_0; -osThreadId thread_motor_1; -osThreadId thread_cmd_parse; -osThreadId thread_usb_pump; +extern osSemaphoreId sem_usb_irq; +extern osSemaphoreId sem_uart_dma; +extern osSemaphoreId sem_usb_rx; +extern osSemaphoreId sem_usb_tx; #endif /* __FREERTOS_H */ \ No newline at end of file diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index 0d178a56..28ead46d 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -53,7 +53,8 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include +#include "usb_device.h" +extern PCD_HandleTypeDef hpcd_USB_OTG_FS; int odrive_main(void); /* USER CODE END Includes */ @@ -63,11 +64,9 @@ osThreadId defaultTaskHandle; /* USER CODE BEGIN Variables */ // List of semaphores osSemaphoreId sem_usb_irq; - -// List of threads -osThreadId thread_motor_0; -osThreadId thread_motor_1; -osThreadId thread_cmd_parse; +osSemaphoreId sem_uart_dma; +osSemaphoreId sem_usb_rx; +osSemaphoreId sem_usb_tx; // Place FreeRTOS heap in core coupled memory for better performance __attribute__((section(".ccmram"))) @@ -94,6 +93,28 @@ __weak void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTask configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is called if a stack overflow is detected. */ } + +void usb_deferred_interrupt_thread(void * ctx) { + (void) ctx; // unused parameter + + for (;;) { + // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) + osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); + if (semaphore_status == osOK) { + // We have a new incoming USB transmission: handle it + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + // Let the irq (OTG_FS_IRQHandler) fire again. + HAL_NVIC_EnableIRQ(OTG_FS_IRQn); + } + } +} + +void init_deferred_interrupts(void) { + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512); + osThreadCreate(osThread(task_usb_pump), NULL); +} + /* USER CODE END 4 */ /* Init FreeRTOS */ diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 74974bd9..bafa4d3f 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -59,7 +59,8 @@ #include "gpio.h" /* USER CODE BEGIN Includes */ -#include +#include +#include "freertos_vars.h" /* USER CODE END Includes */ /* Private variables ---------------------------------------------------------*/ diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index 0b5b1807..1a9c43c4 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -52,8 +52,7 @@ /* USER CODE BEGIN INCLUDE */ #include "cmsis_os.h" -#include "freertos_vars.h" -#include +#include #include /* USER CODE END INCLUDE */ @@ -291,9 +290,7 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length) static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ - - set_cmd_buffer(Buf, *Len); - osSemaphoreRelease(sem_usb_rx); + usb_process_packet(Buf, *Len); return (USBD_OK); /* USER CODE END 6 */ diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index fc9079d9..856d05e2 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -53,7 +53,7 @@ #include "usbd_conf.h" /* USER CODE BEGIN INCLUDE */ -#include +#include /* USER CODE END INCLUDE */ /* Private typedef -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index bde389d4..2877816d 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -4,7 +4,7 @@ #include "gpio.h" #include "utils.h" -#include "odrive_main.hpp" +#include "odrive_main.h" Axis::Axis(const AxisHardwareConfig_t& hw_config, AxisConfig_t& config, diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 601b2367..2a49b92e 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -1,8 +1,8 @@ #ifndef __AXIS_HPP #define __AXIS_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif // Warning: Do not reorder these enum values. diff --git a/Firmware/MotorControl/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h index 3e2d965d..791246de 100644 --- a/Firmware/MotorControl/board_config_v3.h +++ b/Firmware/MotorControl/board_config_v3.h @@ -20,25 +20,25 @@ #endif -struct AxisHardwareConfig_t { +typedef struct { GPIO_TypeDef* step_port; uint16_t step_pin; GPIO_TypeDef* dir_port; uint16_t dir_pin; osPriority thread_priority; -}; +} AxisHardwareConfig_t; -struct EncoderHardwareConfig_t { +typedef struct { TIM_HandleTypeDef* timer; GPIO_TypeDef* index_port; uint16_t index_pin; -}; -struct MotorHardwareConfig_t { +} EncoderHardwareConfig_t; +typedef struct { TIM_HandleTypeDef* timer; uint16_t control_deadline; float shunt_conductance; -}; -struct GateDriverHardwareConfig_t { +} MotorHardwareConfig_t; +typedef struct { SPI_HandleTypeDef* spi; GPIO_TypeDef* enable_port; uint16_t enable_pin; @@ -46,15 +46,18 @@ struct GateDriverHardwareConfig_t { uint16_t nCS_pin; GPIO_TypeDef* nFAULT_port; uint16_t nFAULT_pin; -}; -struct BoardHardwareConfig_t { +} GateDriverHardwareConfig_t; +typedef struct { AxisHardwareConfig_t axis_config; EncoderHardwareConfig_t encoder_config; MotorHardwareConfig_t motor_config; GateDriverHardwareConfig_t gate_driver_config; -}; +} BoardHardwareConfig_t; -const BoardHardwareConfig_t hw_configs[] = { { +extern const BoardHardwareConfig_t hw_configs[2]; + +#ifdef __MAIN_CPP__ +const BoardHardwareConfig_t hw_configs[2] = { { .axis_config = { .step_port = GPIO_1_GPIO_Port, .step_pin = GPIO_1_Pin, @@ -111,5 +114,7 @@ const BoardHardwareConfig_t hw_configs[] = { { .nFAULT_pin = nFAULT_Pin, } } }; +#endif + #endif // __BOARD_CONFIG_H diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 3e1b5661..db3be18d 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -1,5 +1,5 @@ -#include "odrive_main.hpp" +#include "odrive_main.h" Controller::Controller(ControllerConfig_t& config) : diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b5767b6f..5284cada 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -1,8 +1,8 @@ #ifndef __CONTROLLER_HPP #define __CONTROLLER_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif // Note: these should be sorted from lowest level of control to diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index fad27267..d6d44483 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -1,6 +1,5 @@ -//#include "encoder.hpp" -#include "odrive_main.hpp" +#include "odrive_main.h" Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index ae4b3c0e..6c456b28 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -1,8 +1,8 @@ #ifndef __ENCODER_HPP #define __ENCODER_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif struct EncoderConfig_t { diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 98ac5620..41d58990 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -19,7 +19,7 @@ #include #include -#include "odrive_main.hpp" +#include "odrive_main.h" /* Private defines -----------------------------------------------------------*/ diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 2bd9fe04..e3784788 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -2,8 +2,8 @@ #ifndef __LOW_LEVEL_H #define __LOW_LEVEL_H -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif #ifdef __cplusplus diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index aef0fd5e..f404bcc1 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -1,7 +1,7 @@ -#include "odrive_main.hpp" +#define __MAIN_CPP__ +#include "odrive_main.h" #include "nvm_config.hpp" -#include BoardConfig_t board_config; EncoderConfig_t encoder_configs[AXIS_COUNT]; diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 0229e697..d508422e 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -2,8 +2,7 @@ #include #include "drv8301.h" -//#include "motor.hpp" -#include "odrive_main.hpp" +#include "odrive_main.h" Motor::Motor(const MotorHardwareConfig_t& hw_config, diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index b2a79f88..bdb8720d 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -1,8 +1,8 @@ #ifndef __MOTOR_HPP #define __MOTOR_HPP -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." +#ifndef __ODRIVE_MAIN_H +#error "This file should not be included directly. Include odrive_main.h instead." #endif #include "drv8301.h" diff --git a/Firmware/MotorControl/odrive_main.hpp b/Firmware/MotorControl/odrive_main.h similarity index 89% rename from Firmware/MotorControl/odrive_main.hpp rename to Firmware/MotorControl/odrive_main.h index 058fbc7d..10ce1d13 100644 --- a/Firmware/MotorControl/odrive_main.hpp +++ b/Firmware/MotorControl/odrive_main.h @@ -1,38 +1,25 @@ -#ifndef __ODRIVE_MAIN_HPP -#define __ODRIVE_MAIN_HPP +#ifndef __ODRIVE_MAIN_H +#define __ODRIVE_MAIN_H -// stdlib includes -#include - -// System includes -#include +#ifdef __cplusplus +extern "C" { +#endif // STM specific includes #include // Sets up the correct chip specifc defines required by arm_math #define ARM_MATH_CM4 // TODO: might change in future board versions #include +// OS includes +#include + // Hardware configuration #if HW_VERSION_MAJOR == 3 -#include +#include "board_config_v3.h" #else #error "unknown board version" #endif -// @brief general user configurable board configuration -struct BoardConfig_t { - bool enable_uart = true; - float brake_resistance = 0.47f; // [ohm] - float dc_bus_undervoltage_trip_level = 8.0f; //(~static_c #include #include #include +#include -// defined in main.cpp +#endif // __cplusplus + + +// general system functions defined in main.cpp void save_configuration(void); void erase_configuration(void); -#endif /* __ODRIVE_MAIN_HPP */ +#endif /* __ODRIVE_MAIN_H */ diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 4ae081f4..1098b38c 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,6 +1,5 @@ -//#include "sensorless_estimator.hpp" -#include "odrive_main.hpp" +#include "odrive_main.h" SensorlessEstimator::SensorlessEstimator() { diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 87d6243e..2d24ff2a 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -148,6 +148,8 @@ build{ 'communication/communication.cpp', 'communication/ascii_protocol.cpp', 'communication/protocol.cpp', + 'communication/interface_uart.cpp', + 'communication/interface_usb.cpp', 'FreeRTOS-openocd.c' }, includes={ diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index ad24b1b5..98f1cd02 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -7,7 +7,7 @@ /* Includes ------------------------------------------------------------------*/ -#include "odrive_main.hpp" +#include "odrive_main.h" #include "communication.h" #include "ascii_protocol.h" #include diff --git a/Firmware/communication/ascii_protocol.h b/Firmware/communication/ascii_protocol.h index 680dd9f3..82830e10 100644 --- a/Firmware/communication/ascii_protocol.h +++ b/Firmware/communication/ascii_protocol.h @@ -1,34 +1,21 @@ -#ifndef ASCII_PROTOCOL_H -#define ASCII_PROTOCOL_H - -#ifndef __ODRIVE_MAIN_HPP -#error "This file should not be included directly. Include odrive_main.hpp instead." -#endif +#ifndef __ASCII_PROTOCOL_H +#define __ASCII_PROTOCOL_H #ifdef __cplusplus extern "C" { #endif /* Includes ------------------------------------------------------------------*/ + +#include "protocol.hpp" + #include #include #include + /* Exported types ------------------------------------------------------------*/ - -typedef enum { - SERIAL_PRINTF_IS_NONE, - SERIAL_PRINTF_IS_USB, - SERIAL_PRINTF_IS_UART, -} SerialPrintf_t; - /* Exported constants --------------------------------------------------------*/ /* Exported variables --------------------------------------------------------*/ -extern SerialPrintf_t serial_printf_select; -// Exposed comms table during refactor transition -extern float* exposed_floats[]; -extern int* exposed_ints[]; -extern bool* exposed_bools[]; -extern uint16_t* exposed_uint16[]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ @@ -39,4 +26,4 @@ void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& } #endif -#endif /* ASCII_PROTOCOL_H */ +#endif /* __ASCII_PROTOCOL_H */ diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index ce1d2360..618a6215 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -1,30 +1,23 @@ /* Includes ------------------------------------------------------------------*/ -// TODO: remove this option -// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py -// todo: make clean switches for protocol -#define ENABLE_ASCII_PROTOCOL - #include "communication.h" -//#include "low_level.h" -#include "odrive_main.hpp" + +#include "interface_usb.h" +#include "interface_uart.h" + +#include "odrive_main.h" #include "protocol.hpp" #include "freertos_vars.h" #include "utils.h" -#ifdef ENABLE_ASCII_PROTOCOL -#include "ascii_protocol.h" -#endif #include #include -#include -#include -#include -#include - -#define UART_TX_BUFFER_SIZE 64 +//#include +//#include +//#include +//#include /* Private defines -----------------------------------------------------------*/ /* Private macros ------------------------------------------------------------*/ @@ -32,110 +25,11 @@ /* Global constant data ------------------------------------------------------*/ /* Global variables ----------------------------------------------------------*/ -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern USBD_HandleTypeDef hUsbDeviceFS; uint64_t serial_number; char serial_number_str[13]; // 12 digits + null termination /* Private constant data -----------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/ - -static uint8_t* usb_buf; -static uint32_t usb_len; - -// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -static thread_local uint32_t deadline_ms = 0; - - -#if !defined(USB_PROTOCOL_NONE) - -class USBSender : public PacketSink { -public: - int process_packet(const uint8_t* buffer, size_t length) { - // cannot send partial packets - if (length > USB_TX_DATA_SIZE) - return -1; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit packet - uint8_t status = CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length); - return (status == USBD_OK) ? 0 : -1; - } -} usb_packet_output; - -#if !defined(USB_PROTOCOL_NATIVE) -class TreatPacketSinkAsStreamSink : public StreamSink { -public: - TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - if (output_.process_packet(buffer, length) != 0) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - size_t get_free_space() { return SIZE_MAX; } -private: - PacketSink& output_; -} usb_stream_output(usb_packet_output); -#endif - -#if defined(USB_PROTOCOL_NATIVE) -BidirectionalPacketBasedChannel usb_channel(usb_packet_output); -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) -PacketToStreamConverter usb_packetized_output(usb_stream_output); -BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); -#endif - -#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) -StreamToPacketConverter usb_native_stream_input(usb_channel); -#endif - -#endif // !defined(USB_PROTOCOL_NONE) - - -#if !defined(UART_PROTOCOL_NONE) -class UART4Sender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_stream_output; - -#if defined(UART_PROTOCOL_NATIVE) -PacketToStreamConverter uart4_packet_output(uart4_stream_output); -BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); -StreamToPacketConverter uart4_stream_input(uart4_channel); -#endif - -#endif // !defined(UART_PROTOCOL_NONE) - - /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -145,18 +39,12 @@ void enter_dfu_mode() { NVIC_SystemReset(); } -void init_deferred_interrupts(void) { - // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512); - thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); -} - void init_communication(void) { printf("hi!\r\n"); // Start command handling thread osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues - thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + osThreadCreate(osThread(task_cmd_parse), NULL); } @@ -220,107 +108,12 @@ void communication_task(void * ctx) { set_application_endpoints(&endpoint_provider); comm_stack_info = uxTaskGetStackHighWaterMark(nullptr); -#if !defined(UART_PROTOCOL_NONE) - //DMA open loop continous circular buffer - //1ms delay periodic, chase DMA ptr around - - #define UART_RX_BUFFER_SIZE 64 - static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; - - // DMA is set up to recieve in a circular buffer forever. - // We dont use interrupts to fetch the data, instead we periodically read - // data out of the circular buffer into a parse buffer, controlled by a state machine - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; -#endif - - // Re-run state-machine forever - for (;;) { -#if !defined(UART_PROTOCOL_NONE) - // Check for UART errors and restart recieve DMA transfer if required - if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { - HAL_UART_AbortReceive(&huart4); - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - } - // Fetch the circular buffer "write pointer", where it would write next - uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { -#if defined(UART_PROTOCOL_NATIVE) - uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); -#endif -#if defined(UART_PROTOCOL_ASCII) - ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx, uart4_stream_output); -#endif - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { -#if defined(UART_PROTOCOL_NATIVE) - uart4_stream_input.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); -#endif -#if defined(UART_PROTOCOL_ASCII) - ASCII_protocol_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx, uart4_stream_output); -#endif - last_rcv_idx = new_rcv_idx; - } -#endif - -#if !defined(USB_PROTOCOL_NONE) - // When we reach here, we are out of immediate characters to fetch out of UART buffer - // Now we check if there is any USB processing to do: we wait for up to 1 ms, - // before going back to checking UART again. - const uint32_t usb_check_timeout = 1; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); - if (sem_stat == osOK) { - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(USB_PROTOCOL_NATIVE) - usb_channel.process_packet(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_native_stream_input.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_ASCII) - ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); -#endif - USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet - } -#endif - -#if defined(USB_PROTOCOL_NONE) && defined(UART_PROTOCOL_NONE) - osDelay(1); // don't starve other threads -#endif - } - - // If we get here, then this task is done - vTaskDelete(osThreadGetId()); -} - -// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the -// incoming USB data -void set_cmd_buffer(uint8_t *buf, uint32_t len) { - usb_buf = buf; - usb_len = len; -} - -void usb_deferred_interrupt_thread(void * ctx) { - (void) ctx; // unused parameter + serve_on_uart(); + serve_on_usb(); for (;;) { - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); - if (semaphore_status == osOK) { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } + osDelay(1000); // nothing to do } - - vTaskDelete(osThreadGetId()); } extern "C" { @@ -337,7 +130,3 @@ int _write(int file, const char* data, int len) { #endif return len; } - -void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { - osSemaphoreRelease(sem_uart_dma); -} diff --git a/Firmware/communication/communication.h b/Firmware/communication/communication.h index 18522ffe..9d1dff2e 100644 --- a/Firmware/communication/communication.h +++ b/Firmware/communication/communication.h @@ -13,15 +13,8 @@ extern "C" { #endif -void init_deferred_interrupts(void); void init_communication(void); void communication_task(void * ctx); -void set_cmd_buffer(uint8_t *buf, uint32_t len); -void usb_deferred_interrupt_thread(void * ctx); -void USB_receive_packet(const uint8_t *buffer, size_t length); - -extern uint64_t serial_number; -extern char serial_number_str[13]; #ifdef __cplusplus } diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp new file mode 100644 index 00000000..d83442af --- /dev/null +++ b/Firmware/communication/interface_uart.cpp @@ -0,0 +1,101 @@ + +#include "interface_uart.h" +#include "protocol.hpp" + +#include "ascii_protocol.h" + +#include + +#include +#include +#include + +#define UART_TX_BUFFER_SIZE 64 +#define UART_RX_BUFFER_SIZE 64 + +// DMA open loop continous circular buffer +// 1ms delay periodic, chase DMA ptr around +static uint8_t dma_rx_buffer[UART_RX_BUFFER_SIZE]; +static uint32_t dma_last_rcv_idx; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + + +class UART4Sender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart4_stream_output; + +PacketToStreamConverter uart4_packet_output(uart4_stream_output); +BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); +StreamToPacketConverter uart4_stream_input(uart4_channel); + +static void uart_server_thread(void * ctx) { + (void) ctx; + + for (;;) { + // Check for UART errors and restart recieve DMA transfer if required + if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { + HAL_UART_AbortReceive(&huart4); + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); + } + // Fetch the circular buffer "write pointer", where it would write next + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < dma_last_rcv_idx) { + uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, + UART_RX_BUFFER_SIZE - dma_last_rcv_idx); + ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx, + UART_RX_BUFFER_SIZE - dma_last_rcv_idx, uart4_stream_output); + dma_last_rcv_idx = 0; + } + if (new_rcv_idx > dma_last_rcv_idx) { + uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, + new_rcv_idx - dma_last_rcv_idx); + ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx, + new_rcv_idx - dma_last_rcv_idx, uart4_stream_output); + dma_last_rcv_idx = new_rcv_idx; + } + + osDelay(1); + }; +} + +void serve_on_uart() { + // DMA is set up to recieve in a circular buffer forever. + // We dont use interrupts to fetch the data, instead we periodically read + // data out of the circular buffer into a parse buffer, controlled by a state machine + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); + dma_last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + // Start UART communication thread + osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 512); + osThreadCreate(osThread(uart_server_thread_def), NULL); +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); +} diff --git a/Firmware/communication/interface_uart.h b/Firmware/communication/interface_uart.h new file mode 100644 index 00000000..02c47331 --- /dev/null +++ b/Firmware/communication/interface_uart.h @@ -0,0 +1,14 @@ +#ifndef __INTERFACE_UART_HPP +#define __INTERFACE_UART_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +void serve_on_uart(void); + +#ifdef __cplusplus +} +#endif + +#endif // __INTERFACE_UART_HPP diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp new file mode 100644 index 00000000..0bca55c1 --- /dev/null +++ b/Firmware/communication/interface_usb.cpp @@ -0,0 +1,103 @@ + +#include "interface_usb.h" +#include "protocol.hpp" + +#include + +#include +#include +#include +#include +#include + +static uint8_t* usb_buf; +static uint32_t usb_len; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + + + +class USBSender : public PacketSink { +public: + int process_packet(const uint8_t* buffer, size_t length) { + // cannot send partial packets + if (length > USB_TX_DATA_SIZE) + return -1; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit packet + uint8_t status = CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, length); + return (status == USBD_OK) ? 0 : -1; + } +} usb_packet_output; + +#if !defined(USB_PROTOCOL_NATIVE) +class TreatPacketSinkAsStreamSink : public StreamSink { +public: + TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; + if (output_.process_packet(buffer, length) != 0) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + size_t get_free_space() { return SIZE_MAX; } +private: + PacketSink& output_; +} usb_stream_output(usb_packet_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE) +BidirectionalPacketBasedChannel usb_channel(usb_packet_output); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +PacketToStreamConverter usb_packetized_output(usb_stream_output); +BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +StreamToPacketConverter usb_native_stream_input(usb_channel); +#endif + + +static void usb_server_thread(void * ctx) { + (void) ctx; + + for (;;) { + const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); + if (sem_stat == osOK) { + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(USB_PROTOCOL_NATIVE) + usb_channel.process_packet(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + usb_native_stream_input.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); +#endif + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet + } + } +} + +// Called from CDC_Receive_FS callback function, this allows the communication +// thread to handle the incoming data +void usb_process_packet(uint8_t *buf, uint32_t len) { + usb_buf = buf; + usb_len = len; + osSemaphoreRelease(sem_usb_rx); +} + +void serve_on_usb() { + // Start USB communication thread + osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512); + osThreadCreate(osThread(usb_server_thread_def), NULL); +} diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h new file mode 100644 index 00000000..3602843f --- /dev/null +++ b/Firmware/communication/interface_usb.h @@ -0,0 +1,17 @@ +#ifndef __INTERFACE_USB_HPP +#define __INTERFACE_USB_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +void usb_process_packet(uint8_t *buf, uint32_t len); +void serve_on_usb(void); + +#ifdef __cplusplus +} +#endif + +#endif // __INTERFACE_USB_HPP From 0ac2871deccbd9bbc25ccf114cbd648008483bb2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 21 Apr 2018 14:27:37 -0700 Subject: [PATCH 088/112] drv fault only reads fault regs, add blackside loopback tests --- Firmware/MotorControl/motor.cpp | 6 +++--- Firmware/MotorControl/motor.hpp | 10 +++++----- tools/odrive/tests.py | 8 ++++---- tools/test-rig-loopback.yaml | 35 +++++++++++++++++++++++++++++++-- 4 files changed, 45 insertions(+), 14 deletions(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 6f12474d..9558dc35 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -117,9 +117,9 @@ bool Motor::check_DRV_fault() { // Update DRV Fault Code drv_fault_ = DRV8301_getFaultType(&gate_driver_); // Update/Cache all SPI device registers - DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; - local_regs->RcvCmd = true; - DRV8301_readData(&gate_driver_, local_regs); + // DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_; + // local_regs->RcvCmd = true; + // DRV8301_readData(&gate_driver_, local_regs); return false; }; return true; diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index ebc47b9c..7f9e9b08 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -180,11 +180,11 @@ public: make_protocol_property("max_allowed_current", ¤t_control_.max_allowed_current) ), make_protocol_object("gate_driver", - make_protocol_ro_property("drv_fault", &drv_fault_), - make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), - make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), - make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), - make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) + make_protocol_ro_property("drv_fault", &drv_fault_) + // make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), + // make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), + // make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), + // make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) ), make_protocol_object("timing_log", make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]), diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index 182f70aa..43ef182b 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -61,13 +61,13 @@ def test_assert_eq(observed, expected, range=None, accuracy=None): def get_errors(axis_ctx: AxisTestContext): errors = [] if axis_ctx.handle.motor.error != 0: - errors.append("motor failed with error {:04X}".format(axis_ctx.handle.motor.error)) + errors.append("motor failed with error 0x{:04X}".format(axis_ctx.handle.motor.error)) if axis_ctx.handle.encoder.error != 0: - errors.append("encoder failed with error {:04X}".format(axis_ctx.handle.encoder.error)) + errors.append("encoder failed with error 0x{:04X}".format(axis_ctx.handle.encoder.error)) if axis_ctx.handle.sensorless_estimator.error != 0: - errors.append("sensorless_estimator failed with error {:04X}".format(axis_ctx.handle.sensorless_estimator.error)) + errors.append("sensorless_estimator failed with error 0x{:04X}".format(axis_ctx.handle.sensorless_estimator.error)) if axis_ctx.handle.error != 0: - errors.append("axis failed with error {:04X}".format(axis_ctx.handle.error)) + errors.append("axis failed with error 0x{:04X}".format(axis_ctx.handle.error)) elif len(errors) > 0: errors.append("and by the way: axis reports no error even though there is one") return errors diff --git a/tools/test-rig-loopback.yaml b/tools/test-rig-loopback.yaml index 34989214..359c3a61 100644 --- a/tools/test-rig-loopback.yaml +++ b/tools/test-rig-loopback.yaml @@ -2,7 +2,37 @@ type: loopback odrives: - - name: odrive-48V + - name: odrv-blackside + board-version: v3.4-24V + serial-number: "3061395B3235" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '493f6f06493f56540929113f' + vbus-voltage: 24 # [V] + max-brake-power: 150 # [W] + axes: + - name: 'M0' + motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 270 + motor-max-current: 70 + motor-max-voltage: 32 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: 'M1' + motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 270 + motor-max-current: 70 + motor-max-voltage: 32 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: odrv-yellowside board-version: v3.5-48V serial-number: "3660335E3037" brake-resistance: 0.47 @@ -35,4 +65,5 @@ odrives: # Mechanical couplings couplings: - - [ odrive-48V.M0, odrive-48V.M1 ] \ No newline at end of file + - [ odrv-blackside.M0, odrv-blackside.M1 ] + - [ odrv-yellowside.M0, odrv-yellowside.M1 ] \ No newline at end of file From 9d884e59703cb3848425e1fd3014397dcca557c7 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 21 Apr 2018 18:17:50 -0700 Subject: [PATCH 089/112] Update CHANGELOG.md --- Firmware/CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 7b959fca..8131e200 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -25,6 +25,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Most of the code from `lowlevel.c` moved to `axis.cpp`, `encoder.cpp`, `controller.cpp`, `sensorless_estimator.cpp`, `motor.cpp` and the corresponding header files * Refactoring of the developer-facing communication protocol interface. See e.g. `axis.hpp` or `controller.hpp` for examples on how to add your own fields and functions * Change of the user-facing field paths. E.g. `my_odrive.motor0.pos_setpoint` is now at `my_odrive.axis0.controller.pos_setpoint`. Names are mostly unchanged. +* Rewrite of the top-level per-axis state-machine * The build is now configured using the `tup.config` file instead of editing source files. Make sure you set your board version correctly. See [here](README.md#configuring-the-build) for details. * The toplevel directory for tup is now `Firmware`. If you used tup before, go to `Firmware` and run `rm -rd ../.tup; rm -rd build/*; make`. * Update CubeMX generated STM platform code to version 1.19.0 From 9e62f083499f09326425967753a71678493c5174 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 21 Apr 2018 19:29:21 -0700 Subject: [PATCH 090/112] update Changelog --- Firmware/CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 2774a8f1..856c9d15 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -2,6 +2,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added +* Encoder can now go forever in velocity/torque mode due to using circular encoder space. * `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you should run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `board_version_[...]` properties. * bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_version_[...]` properties. * infrastructure to publish the python tools to PyPi. See `tools/setup.py` for details. From 0bf5debdcab368107d2a1d6801a5beaec94930c1 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 21 Apr 2018 19:33:39 -0700 Subject: [PATCH 091/112] GPIO_3 input by default and EXTI2 disabled by default --- Firmware/Board/v3/Inc/main.h | 1 - Firmware/Board/v3/Odrive.ioc | 8 ++------ Firmware/Board/v3/Src/gpio.c | 15 ++------------- 3 files changed, 4 insertions(+), 20 deletions(-) diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index 1ff706df..cef4368a 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -93,7 +93,6 @@ #define GPIO_2_GPIO_Port GPIOA #define GPIO_3_Pin GPIO_PIN_2 #define GPIO_3_GPIO_Port GPIOA -#define GPIO_3_EXTI_IRQn EXTI2_IRQn #define GPIO_4_Pin GPIO_PIN_3 #define GPIO_4_GPIO_Port GPIOA #define M1_TEMP_Pin GPIO_PIN_4 diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index f13657b1..70c238a4 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -199,7 +199,6 @@ NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:true NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true -NVIC.EXTI2_IRQn=true\:0\:0\:false\:false\:false\:false\:true NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:true NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:true @@ -241,11 +240,10 @@ PA15.GPIOParameters=GPIO_Label PA15.GPIO_Label=GPIO_7 PA15.Locked=true PA15.Signal=GPIO_Input -PA2.GPIOParameters=GPIO_PuPd,GPIO_Label +PA2.GPIOParameters=GPIO_Label PA2.GPIO_Label=GPIO_3 -PA2.GPIO_PuPd=GPIO_PULLDOWN PA2.Locked=true -PA2.Signal=GPXTI2 +PA2.Signal=GPIO_Input PA3.GPIOParameters=GPIO_PuPd,GPIO_Label PA3.GPIO_Label=GPIO_4 PA3.GPIO_PuPd=GPIO_NOPULL @@ -497,8 +495,6 @@ SH.ADCx_IN5.ConfNb=2 SH.ADCx_IN6.0=ADC1_IN6,IN6 SH.ADCx_IN6.1=ADC2_IN6,IN6 SH.ADCx_IN6.ConfNb=2 -SH.GPXTI2.0=GPIO_EXTI2 -SH.GPXTI2.ConfNb=1 SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1 CH1N SH.S_TIM1_CH1.ConfNb=1 SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2 CH2N diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index fb163f71..85b9028f 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -106,14 +106,8 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - /*Configure GPIO pin : PtPin */ - GPIO_InitStruct.Pin = GPIO_3_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct); - - /*Configure GPIO pins : PAPin PAPin */ - GPIO_InitStruct.Pin = GPIO_4_Pin|GPIO_7_Pin; + /*Configure GPIO pins : PAPin PAPin PAPin */ + GPIO_InitStruct.Pin = GPIO_3_Pin|GPIO_4_Pin|GPIO_7_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); @@ -137,11 +131,6 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_PULLUP; HAL_GPIO_Init(nFAULT_GPIO_Port, &GPIO_InitStruct); - /* EXTI interrupt init*/ - // TODO get Cube to not emit this - // HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0); - // HAL_NVIC_EnableIRQ(EXTI2_IRQn); - } /* USER CODE BEGIN 2 */ From 89a3d72b4b34939072a398d77caacc415b4d4347 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 21 Apr 2018 19:44:15 -0700 Subject: [PATCH 092/112] update changelog --- Firmware/CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 856c9d15..afe587f0 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -19,6 +19,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * (experimental: start liveplotter from `odrivetool` shell by typing `start_liveplotter(lambda: odrv0.motor0.encoder.encoder_state)`) * Set thread priority of USB pump thread above protocol thread +* GPIO3 not sensitive to edges by default ### Fixed * Enums now transported with correct underlying type on native protocol From e581cc9c90ba4db9be7ac539895d234bea596519 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sat, 21 Apr 2018 14:21:29 -0700 Subject: [PATCH 093/112] move enable_dfu_mode() to main.cpp --- Firmware/MotorControl/main.cpp | 6 ++++++ Firmware/MotorControl/odrive_main.h | 1 + Firmware/communication/communication.cpp | 6 ------ 3 files changed, 7 insertions(+), 6 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 6cf7ae62..b7ec03d5 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -53,6 +53,12 @@ void erase_configuration(void) { NVM_erase(); } +void enter_dfu_mode(void) { + __asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts + _reboot_cookie = 0xDEADBEEF; + NVIC_SystemReset(); +} + extern "C" { int odrive_main(void); void vApplicationStackOverflowHook(void) { for(;;); } diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index ed28b832..a66fcb74 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -85,5 +85,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c // general system functions defined in main.cpp void save_configuration(void); void erase_configuration(void); +void enter_dfu_mode(void); #endif /* __ODRIVE_MAIN_H */ diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 37f4385b..27e08bfb 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -65,12 +65,6 @@ const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official r /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ -void enter_dfu_mode() { - __asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts - _reboot_cookie = 0xDEADBEEF; - NVIC_SystemReset(); -} - void init_communication(void) { printf("hi!\r\n"); From 3d13da4e921d9d34e6bacb4303f24de19c3012d9 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 22 Apr 2018 00:57:14 -0700 Subject: [PATCH 094/112] implement function return values on protocol (this time for real) --- Firmware/communication/communication.cpp | 4 +- Firmware/communication/protocol.hpp | 170 ++++++++++------------- 2 files changed, 76 insertions(+), 98 deletions(-) diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 8e4f826a..ed2c1d0d 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -92,6 +92,7 @@ public: void NVIC_SystemReset_helper() { NVIC_SystemReset(); } void enter_dfu_mode_helper() { enter_dfu_mode(); } float get_oscilloscope_val(uint32_t index) { return oscilloscope[index]; } + int32_t test_function(int32_t delta) { static int cnt = 0; return cnt += delta; } } static_functions; // When adding new functions/variables to the protocol, be careful not to @@ -120,7 +121,8 @@ static inline auto make_obj_tree() { ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), - make_protocol_function_with_ret("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), + make_protocol_function("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), + make_protocol_function("test_function", static_functions, &StaticFunctions::test_function, "delta"), make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper), diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp index 827b226a..0bafbcb5 100644 --- a/Firmware/communication/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -775,89 +775,50 @@ struct PropertyListFactory { }; -template -class ProtocolFunction : public Endpoint { +template +struct return_type; + +template<> +struct return_type<> { typedef void type; }; +template +struct return_type { typedef T type; }; +template +struct return_type { typedef std::tuple type; }; + + + +template +class ProtocolFunction; + +template + //template typename asd, + //template typename ssss> +class ProtocolFunction, std::tuple> : Endpoint { public: - static constexpr size_t endpoint_count = 1 + MemberList...>::endpoint_count; - template - ProtocolFunction(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) : - name_(name), all_arg_names_{names...}, obj_(obj), func_ptr_(func_ptr), - input_properties_(PropertyListFactory::template make_property_list<0>(all_arg_names_, in_args_)) + + // @brief The return type of the function as written by a C++ programmer + using TRet = typename return_type::type; + + static constexpr size_t endpoint_count = 1 + MemberList...>::endpoint_count + MemberList...>::endpoint_count; + + ProtocolFunction(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TInputs...), + std::array input_names, + std::array output_names) : + name_(name), obj_(obj), func_ptr_(func_ptr), + input_names_{input_names}, output_names_{output_names}, + input_properties_(PropertyListFactory::template make_property_list<0>(input_names_, in_args_)), + output_properties_(PropertyListFactory::template make_property_list<0>(output_names_, out_args_)) { LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); } ProtocolFunction(const ProtocolFunction& other) : - name_(other.name_), all_arg_names_(other.all_arg_names_), obj_(other.obj_), func_ptr_(other.func_ptr_), - input_properties_(PropertyListFactory::template make_property_list<0>( - all_arg_names_, in_args_)) - { - LOG_PROTO("COPIED! my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); - } - - void write_json(size_t id, StreamSink* output) { - // write name - write_string("{\"name\":\"", output); - write_string(name_, output); - - // write endpoint ID - write_string("\",\"id\":", output); - char id_buf[10]; - snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf - write_string(id_buf, output); - - // write arguments - write_string(",\"type\":\"function\",\"arguments\":[", output); - input_properties_.write_json(id + 1, output), - write_string("]}", output); - } - - Endpoint* get_by_name(const char * name, size_t length) { - return nullptr; // can't address functions by name - } - - void register_endpoints(Endpoint** list, size_t id, size_t length) { - if (id < length) - list[id] = this; - input_properties_.register_endpoints(list, id + 1, length); - } - - void handle(const uint8_t* input, size_t input_length, StreamSink* output) { - (void) input; - (void) input_length; - (void) output; - LOG_PROTO("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); - LOG_PROTO("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_)); - invoke_function_with_tuple(obj_, func_ptr_, in_args_); - } - - const char * name_; - std::array all_arg_names_; // TODO: remove - TObj& obj_; - TRet(TObj::*func_ptr_)(TArgs...); - std::tuple in_args_; - MemberList...> input_properties_; -}; - -template -class ProtocolFunctionWithRet : Endpoint { -public: - static constexpr size_t endpoint_count = 1 + MemberList>::endpoint_count + MemberList...>::endpoint_count; - template - ProtocolFunctionWithRet(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) : - name_(name), out_arg_names_{"out"}, all_arg_names_{names...}, obj_(obj), func_ptr_(func_ptr), - output_properties_(PropertyListFactory::template make_property_list<0>(out_arg_names_, out_args_)), - input_properties_(PropertyListFactory::template make_property_list<0>(all_arg_names_, in_args_)) - { - LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); - } - - ProtocolFunctionWithRet(const ProtocolFunctionWithRet& other) : - name_(other.name_), all_arg_names_(other.all_arg_names_), obj_(other.obj_), func_ptr_(other.func_ptr_), - output_properties_(PropertyListFactory::template make_property_list<0>( - out_arg_names_, out_args_)), - input_properties_(PropertyListFactory::template make_property_list<0>( - all_arg_names_, in_args_)) + name_(other.name_), obj_(other.obj_), func_ptr_(other.func_ptr_), + input_names_(other.input_names_), output_names_(other.output_names_), + input_properties_(PropertyListFactory::template make_property_list<0>( + input_names_, in_args_)), + output_properties_(PropertyListFactory::template make_property_list<0>( + output_names_, out_args_)) { LOG_PROTO("COPIED! my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); } @@ -881,6 +842,10 @@ public: write_string("]}", output); } + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; // can't address functions by name + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this; @@ -888,40 +853,51 @@ public: output_properties_.register_endpoints(list, id + 1 + decltype(input_properties_)::endpoint_count, length); } + template std::enable_if_t + handle_ex() { + invoke_function_with_tuple(obj_, func_ptr_, in_args_); + } + + template std::enable_if_t + handle_ex() { + std::get<0>(out_args_) = invoke_function_with_tuple(obj_, func_ptr_, in_args_); + } + + template std::enable_if_t= 2> + handle_ex() { + out_args_ = invoke_function_with_tuple(obj_, func_ptr_, in_args_); + } + void handle(const uint8_t* input, size_t input_length, StreamSink* output) { (void) input; (void) input_length; (void) output; LOG_PROTO("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); LOG_PROTO("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_)); - std::get<0>(out_args_) = invoke_function_with_tuple(obj_, func_ptr_, in_args_); + handle_ex(); } const char * name_; - std::array out_arg_names_; // TODO: remove - std::array all_arg_names_; // TODO: remove TObj& obj_; - TRet(TObj::*func_ptr_)(TArgs...); - //TRet ret_val_; - std::tuple out_args_; - std::tuple in_args_; - MemberList> output_properties_; - MemberList...> input_properties_; + TRet(TObj::*func_ptr_)(TInputs...); + std::array input_names_; // TODO: remove + std::array output_names_; // TODO: remove + std::tuple in_args_; + std::tuple out_args_; + MemberList...> input_properties_; + MemberList...> output_properties_; }; -//template> -//ProtocolFunction make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { -// return ProtocolFunction(name, obj, func_ptr, names...); -//} - -template> -ProtocolFunction make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { - return ProtocolFunction(name, obj, func_ptr, names...); +template> +ProtocolFunction, std::tuple<>> make_protocol_function(const char * name, TObj& obj, void(TObj::*func_ptr)(TArgs...), TNames ... names) { + return ProtocolFunction, std::tuple<>>(name, obj, func_ptr, {names...}, {}); } -template> -ProtocolFunctionWithRet make_protocol_function_with_ret(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { - return ProtocolFunctionWithRet(name, obj, func_ptr, names...); +template::value>> +ProtocolFunction, std::tuple> make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { + return ProtocolFunction, std::tuple>(name, obj, func_ptr, {names...}, {"result"}); } From d190ecddf5b0f1c370fd095a40a41b39471211a9 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Sun, 22 Apr 2018 01:04:15 -0700 Subject: [PATCH 095/112] remove explicit copy constructor that doesn't do anything --- Firmware/communication/protocol.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp index 0bafbcb5..ea4f2c65 100644 --- a/Firmware/communication/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -804,7 +804,7 @@ public: ProtocolFunction(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TInputs...), std::array input_names, std::array output_names) : - name_(name), obj_(obj), func_ptr_(func_ptr), + name_(name), obj_(&obj), func_ptr_(func_ptr), input_names_{input_names}, output_names_{output_names}, input_properties_(PropertyListFactory::template make_property_list<0>(input_names_, in_args_)), output_properties_(PropertyListFactory::template make_property_list<0>(output_names_, out_args_)) @@ -855,17 +855,17 @@ public: template std::enable_if_t handle_ex() { - invoke_function_with_tuple(obj_, func_ptr_, in_args_); + invoke_function_with_tuple(*obj_, func_ptr_, in_args_); } template std::enable_if_t handle_ex() { - std::get<0>(out_args_) = invoke_function_with_tuple(obj_, func_ptr_, in_args_); + std::get<0>(out_args_) = invoke_function_with_tuple(*obj_, func_ptr_, in_args_); } template std::enable_if_t= 2> handle_ex() { - out_args_ = invoke_function_with_tuple(obj_, func_ptr_, in_args_); + out_args_ = invoke_function_with_tuple(*obj_, func_ptr_, in_args_); } void handle(const uint8_t* input, size_t input_length, StreamSink* output) { @@ -878,7 +878,7 @@ public: } const char * name_; - TObj& obj_; + TObj* obj_; TRet(TObj::*func_ptr_)(TInputs...); std::array input_names_; // TODO: remove std::array output_names_; // TODO: remove From cdeca6680a2aba2ba91ad30a8b18903a4d448217 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 22 Apr 2018 16:30:59 -0700 Subject: [PATCH 096/112] add retry in wait_while_state in dfuse --- tools/odrive/dfuse/DfuDevice.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/tools/odrive/dfuse/DfuDevice.py b/tools/odrive/dfuse/DfuDevice.py index dc5ac152..b9ca449c 100644 --- a/tools/odrive/dfuse/DfuDevice.py +++ b/tools/odrive/dfuse/DfuDevice.py @@ -83,7 +83,11 @@ class DfuDevice: else: states = state - status = self.get_status() + try: + status = self.get_status() + except: + time.sleep(0.100) + status = self.get_status() while (status[1] in states): claimed_timeout = status[2] From 3a19ce4b73cb0c1c4765b6a84ef22d1b25172083 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 13:25:25 -0700 Subject: [PATCH 097/112] remove explicit copy constructor that doesn't do anything --- Firmware/communication/protocol.hpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp index ea4f2c65..87ef82cc 100644 --- a/Firmware/communication/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -812,17 +812,6 @@ public: LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); } - ProtocolFunction(const ProtocolFunction& other) : - name_(other.name_), obj_(other.obj_), func_ptr_(other.func_ptr_), - input_names_(other.input_names_), output_names_(other.output_names_), - input_properties_(PropertyListFactory::template make_property_list<0>( - input_names_, in_args_)), - output_properties_(PropertyListFactory::template make_property_list<0>( - output_names_, out_args_)) - { - LOG_PROTO("COPIED! my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); - } - void write_json(size_t id, StreamSink* output) { // write name write_string("{\"name\":\"", output); From 1734f86ea5be80ccdd51b387ac87812856c35f0f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 23 Apr 2018 14:31:07 -0700 Subject: [PATCH 098/112] catch brake resistor deadtime violations --- Firmware/MotorControl/low_level.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index d7cfcc86..f84b90fc 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -182,6 +182,8 @@ void safety_critical_disarm_brake_resistor() { // @brief Updates the brake resistor PWM timings unless // the brake resistor is disarmed. void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) { + if (high_on - low_off > TIM_APB1_DEADTIME_CLOCKS) + for(;;); uint8_t sr = cpu_enter_critical(); if (brake_resistor_armed_) { // Safe update of low and high side timings @@ -431,7 +433,7 @@ void update_brake_current() { float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; // Duty limit at 90% to allow bootstrap caps to charge - // If brake_duty is NaN, this expression will also evaluate to true + // If brake_duty is NaN, this expression will also evaluate to false if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) { int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; From 8455c0a5a6ebb5aeb083e43eaf3e91d2b53f40fe Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 23 Apr 2018 16:05:16 -0700 Subject: [PATCH 099/112] remove old unused version of shunt conductance --- Firmware/MotorControl/motor.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index fb85163d..950141bb 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -138,7 +138,6 @@ public: bool is_calibrated_ = config_.pre_calibrated; Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; - const float shunt_conductance_ = 1.0f / SHUNT_RESISTANCE; //[S] float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) Current_control_t current_control_ = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement @@ -165,7 +164,6 @@ public: make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), make_protocol_property("DC_calib_phB", &DC_calib_.phB), make_protocol_property("DC_calib_phC", &DC_calib_.phC), - make_protocol_property("shunt_conductance", &shunt_conductance_), make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_), make_protocol_object("current_control", make_protocol_property("p_gain", ¤t_control_.p_gain), From 535dbcf48132bb5428d33dc62d6e3b2bb1fd2418 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 15:12:25 -0700 Subject: [PATCH 100/112] add system_stats to monitor resource usage --- Firmware/Board/v3/Inc/FreeRTOSConfig.h | 2 +- Firmware/Board/v3/Inc/freertos_vars.h | 3 +++ Firmware/Board/v3/Src/freertos.c | 4 +++- Firmware/MotorControl/main.cpp | 24 +++++++++++++++++++++-- Firmware/MotorControl/odrive_main.h | 15 +++++++++++++- Firmware/communication/communication.cpp | 21 ++++++++++++++------ Firmware/communication/communication.h | 4 ++++ Firmware/communication/interface_uart.cpp | 4 +++- Firmware/communication/interface_uart.h | 4 ++++ Firmware/communication/interface_usb.cpp | 3 ++- Firmware/communication/interface_usb.h | 3 +++ 11 files changed, 74 insertions(+), 13 deletions(-) diff --git a/Firmware/Board/v3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3/Inc/FreeRTOSConfig.h index fd592cbe..53280d99 100644 --- a/Firmware/Board/v3/Inc/FreeRTOSConfig.h +++ b/Firmware/Board/v3/Inc/FreeRTOSConfig.h @@ -96,7 +96,7 @@ #define configUSE_PREEMPTION 1 #define configSUPPORT_STATIC_ALLOCATION 0 #define configSUPPORT_DYNAMIC_ALLOCATION 1 -#define configUSE_IDLE_HOOK 0 +#define configUSE_IDLE_HOOK 1 #define configUSE_TICK_HOOK 0 #define configCPU_CLOCK_HZ ( SystemCoreClock ) #define configTICK_RATE_HZ ((TickType_t)1000) diff --git a/Firmware/Board/v3/Inc/freertos_vars.h b/Firmware/Board/v3/Inc/freertos_vars.h index 2eb52d7d..6982ee28 100644 --- a/Firmware/Board/v3/Inc/freertos_vars.h +++ b/Firmware/Board/v3/Inc/freertos_vars.h @@ -8,4 +8,7 @@ extern osSemaphoreId sem_uart_dma; extern osSemaphoreId sem_usb_rx; extern osSemaphoreId sem_usb_tx; +extern osThreadId defaultTaskHandle; +extern osThreadId usb_irq_thread; + #endif /* __FREERTOS_H */ \ No newline at end of file diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index 28ead46d..f1d46642 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -68,6 +68,8 @@ osSemaphoreId sem_uart_dma; osSemaphoreId sem_usb_rx; osSemaphoreId sem_usb_tx; +osThreadId usb_irq_thread; + // Place FreeRTOS heap in core coupled memory for better performance __attribute__((section(".ccmram"))) uint8_t ucHeap[configTOTAL_HEAP_SIZE]; @@ -112,7 +114,7 @@ void usb_deferred_interrupt_thread(void * ctx) { void init_deferred_interrupts(void) { // Start USB interrupt handler thread osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512); - osThreadCreate(osThread(task_usb_pump), NULL); + usb_irq_thread = osThreadCreate(osThread(task_usb_pump), NULL); } /* USER CODE END 4 */ diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 56b57bb2..b250afe6 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -3,6 +3,10 @@ #include "odrive_main.h" #include "nvm_config.hpp" +#include "freertos_vars.h" +#include +#include + BoardConfig_t board_config; EncoderConfig_t encoder_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; @@ -10,7 +14,7 @@ MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; bool user_config_loaded_; -bool user_config_loaded = false; +SystemStats_t system_stats_ = { 0 }; Axis *axes[AXIS_COUNT]; @@ -66,7 +70,22 @@ void enter_dfu_mode(void) { extern "C" { int odrive_main(void); -void vApplicationStackOverflowHook(void) { for(;;); } +void vApplicationStackOverflowHook(void) { + for (;;); // TODO: safe action +} +void vApplicationIdleHook(void) { + if (system_stats_.fully_booted) { + system_stats_.uptime = xTaskGetTickCount(); + system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize(); + system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread); + system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_); + system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_); + system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread); + system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread); + system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread); + system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle); + } +} } int odrive_main(void) { @@ -121,5 +140,6 @@ int odrive_main(void) { axes[i]->start_thread(); } + system_stats_.fully_booted = true; return 0; } diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 0b75af48..bddf7e88 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -29,11 +29,24 @@ extern float vbus_voltage; extern bool brake_resistor_armed_; extern const float elec_rad_per_enc; extern uint32_t _reboot_cookie; -extern bool user_config_loaded; +extern bool user_config_loaded_; extern uint64_t serial_number; extern char serial_number_str[13]; +typedef struct { + bool fully_booted; + uint32_t uptime; // [ms] + uint32_t min_heap_space; // FreeRTOS heap [Bytes] + uint32_t min_stack_space_axis0; // minimum remaining space since startup [Bytes] + uint32_t min_stack_space_axis1; + uint32_t min_stack_space_comms; + uint32_t min_stack_space_usb; + uint32_t min_stack_space_uart; + uint32_t min_stack_space_usb_irq; + uint32_t min_stack_space_startup; +} SystemStats_t; +extern SystemStats_t system_stats_; #ifdef __cplusplus } diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index ed2c1d0d..93f25508 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -62,6 +62,8 @@ const uint8_t fw_version_minor = FW_VERSION_MINOR; const uint8_t fw_version_revision = FW_VERSION_REVISION; const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise +osThreadId comm_thread; + /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -70,7 +72,7 @@ void init_communication(void) { // Start command handling thread osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues - osThreadCreate(osThread(task_cmd_parse), NULL); + comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL); } @@ -80,8 +82,6 @@ float oscilloscope[OSCILLOSCOPE_SIZE] = { size_t oscilloscope_pos = 0; -uint32_t comm_stack_info = 0; // for debugging only - // Helper class because the protocol library doesn't yet // support non-member functions // TODO: make this go away @@ -101,7 +101,6 @@ public: static inline auto make_obj_tree() { return make_protocol_member_list( make_protocol_ro_property("vbus_voltage", &vbus_voltage), - make_protocol_ro_property("comm_stack_info", &comm_stack_info), make_protocol_ro_property("serial_number", &serial_number), make_protocol_ro_property("hw_version_major", &hw_version_major), make_protocol_ro_property("hw_version_minor", &hw_version_minor), @@ -110,8 +109,19 @@ static inline auto make_obj_tree() { make_protocol_ro_property("fw_version_minor", &fw_version_minor), make_protocol_ro_property("fw_version_revision", &fw_version_revision), make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased), - make_protocol_ro_property("user_config_loaded", const_cast(&user_config_loaded)), + make_protocol_ro_property("user_config_loaded", const_cast(&user_config_loaded_)), make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_), + make_protocol_object("system_stats", + make_protocol_ro_property("uptime", &system_stats_.uptime), + make_protocol_ro_property("min_heap_space", &system_stats_.min_heap_space), + make_protocol_ro_property("min_stack_space_axis0", &system_stats_.min_stack_space_axis0), + make_protocol_ro_property("min_stack_space_axis1", &system_stats_.min_stack_space_axis1), + make_protocol_ro_property("min_stack_space_comms", &system_stats_.min_stack_space_comms), + make_protocol_ro_property("min_stack_space_usb", &system_stats_.min_stack_space_usb), + make_protocol_ro_property("min_stack_space_uart", &system_stats_.min_stack_space_uart), + make_protocol_ro_property("min_stack_space_usb_irq", &system_stats_.min_stack_space_usb_irq), + make_protocol_ro_property("min_stack_space_startup", &system_stats_.min_stack_space_startup) + ), make_protocol_object("config", make_protocol_property("brake_resistance", &board_config.brake_resistance), // TODO: changing this currently requires a reboot - fix this @@ -150,7 +160,6 @@ void communication_task(void * ctx) { auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); set_application_endpoints(&endpoint_provider); - comm_stack_info = uxTaskGetStackHighWaterMark(nullptr); serve_on_uart(); serve_on_usb(); diff --git a/Firmware/communication/communication.h b/Firmware/communication/communication.h index 9d1dff2e..8e68e508 100644 --- a/Firmware/communication/communication.h +++ b/Firmware/communication/communication.h @@ -13,6 +13,10 @@ extern "C" { #endif +#include + +extern osThreadId comm_thread; + void init_communication(void); void communication_task(void * ctx); diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp index d83442af..90d976aa 100644 --- a/Firmware/communication/interface_uart.cpp +++ b/Firmware/communication/interface_uart.cpp @@ -21,6 +21,8 @@ static uint32_t dma_last_rcv_idx; // FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable static thread_local uint32_t deadline_ms = 0; +osThreadId uart_thread; + class UART4Sender : public StreamSink { public: @@ -93,7 +95,7 @@ void serve_on_uart() { // Start UART communication thread osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 512); - osThreadCreate(osThread(uart_server_thread_def), NULL); + uart_thread = osThreadCreate(osThread(uart_server_thread_def), NULL); } void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { diff --git a/Firmware/communication/interface_uart.h b/Firmware/communication/interface_uart.h index 02c47331..b5f1ed72 100644 --- a/Firmware/communication/interface_uart.h +++ b/Firmware/communication/interface_uart.h @@ -5,6 +5,10 @@ extern "C" { #endif +#include + +extern osThreadId uart_thread; + void serve_on_uart(void); #ifdef __cplusplus diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index 0bca55c1..541128b2 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -16,6 +16,7 @@ static uint32_t usb_len; // FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable static thread_local uint32_t deadline_ms = 0; +osThreadId usb_thread; class USBSender : public PacketSink { @@ -99,5 +100,5 @@ void usb_process_packet(uint8_t *buf, uint32_t len) { void serve_on_usb() { // Start USB communication thread osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512); - osThreadCreate(osThread(usb_server_thread_def), NULL); + usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL); } diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h index 3602843f..27d40a10 100644 --- a/Firmware/communication/interface_usb.h +++ b/Firmware/communication/interface_usb.h @@ -5,8 +5,11 @@ extern "C" { #endif +#include #include +extern osThreadId usb_thread; + void usb_process_packet(uint8_t *buf, uint32_t len); void serve_on_usb(void); From dc303606b14c0180ebfa6c270db6b0ec4b47de6e Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 19:03:42 -0700 Subject: [PATCH 101/112] fix USB TX lockup issue When the host reset the connection without reading the response packet first, the TX-empty semaphore would never get released, thereby blocking any further TX communication. This commit just overrides the TX buffer if the semaphore wait times out. --- Firmware/MotorControl/main.cpp | 14 +++++++------- Firmware/communication/communication.cpp | 7 ++++++- Firmware/communication/interface_usb.cpp | 18 +++++++++++++++--- Firmware/communication/interface_usb.h | 8 ++++++++ tools/odrive/protocol.py | 1 + 5 files changed, 37 insertions(+), 11 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index b250afe6..9970e84b 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -77,13 +77,13 @@ void vApplicationIdleHook(void) { if (system_stats_.fully_booted) { system_stats_.uptime = xTaskGetTickCount(); system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize(); - system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread); - system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_); - system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_); - system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread); - system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread); - system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread); - system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle); + system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t); + system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t); + system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t); + system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); + system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); + system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t); + system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t); } } } diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 93f25508..86891a8f 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -120,7 +120,12 @@ static inline auto make_obj_tree() { make_protocol_ro_property("min_stack_space_usb", &system_stats_.min_stack_space_usb), make_protocol_ro_property("min_stack_space_uart", &system_stats_.min_stack_space_uart), make_protocol_ro_property("min_stack_space_usb_irq", &system_stats_.min_stack_space_usb_irq), - make_protocol_ro_property("min_stack_space_startup", &system_stats_.min_stack_space_startup) + make_protocol_ro_property("min_stack_space_startup", &system_stats_.min_stack_space_startup), + make_protocol_object("usb", + make_protocol_ro_property("rx_cnt", &usb_stats_.rx_cnt), + make_protocol_ro_property("tx_cnt", &usb_stats_.tx_cnt), + make_protocol_ro_property("tx_overrun_cnt", &usb_stats_.tx_overrun_cnt) + ) ), make_protocol_object("config", make_protocol_property("brake_resistance", &board_config.brake_resistance), diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index 541128b2..cee94a99 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/Firmware/communication/interface_usb.cpp @@ -18,6 +18,7 @@ static thread_local uint32_t deadline_ms = 0; osThreadId usb_thread; +USBStats_t usb_stats_ = {0}; class USBSender : public PacketSink { public: @@ -26,13 +27,23 @@ public: if (length > USB_TX_DATA_SIZE) return -1; // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) { + // If the host resets the device it might be that the TX-complete handler is never called + // and the sem_usb_tx semaphore is never released. To handle this we just override the + // TX buffer if this wait times out. The implication is that the channel is no longer lossless. + // TODO: handle endpoint reset properly + usb_stats_.tx_overrun_cnt++; + } // transmit packet uint8_t status = CDC_Transmit_FS( const_cast(buffer) /* casting this const away is safe because... well... it's not actually. Stupid STM. */, length); - return (status == USBD_OK) ? 0 : -1; + if (status != USBD_OK) { + osSemaphoreRelease(sem_usb_tx); + return -1; + } + usb_stats_.tx_cnt = 0; + return 0; } } usb_packet_output; @@ -76,6 +87,7 @@ static void usb_server_thread(void * ctx) { const uint32_t usb_check_timeout = 1; // ms osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); if (sem_stat == osOK) { + usb_stats_.rx_cnt++; deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); #if defined(USB_PROTOCOL_NATIVE) usb_channel.process_packet(usb_buf, usb_len); diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h index 27d40a10..a56bca36 100644 --- a/Firmware/communication/interface_usb.h +++ b/Firmware/communication/interface_usb.h @@ -10,6 +10,14 @@ extern "C" { extern osThreadId usb_thread; +typedef struct { + uint32_t rx_cnt; + uint32_t tx_cnt; + uint32_t tx_overrun_cnt; +} USBStats_t; + +extern USBStats_t usb_stats_; + void usb_process_packet(uint8_t *buf, uint32_t len); void serve_on_usb(void); diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 4c11971c..09d26707 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -345,6 +345,7 @@ class Channel(PacketSink): if (ack_signal): self._responses[seq_no] = packet[2:] ack_signal.set() + #print("received ack for packet " + str(seq_no)) else: print("received unexpected ACK: " + str(seq_no)) From 9d04108b1073a5c548e1c72952808fb66b94daf4 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 19:07:21 -0700 Subject: [PATCH 102/112] make GPIO interrupts work for all pin numbers --- Firmware/Board/v3/Src/stm32f4xx_it.c | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index a7fe3a12..1a650abc 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -346,6 +346,11 @@ void EXTI4_IRQHandler(void) */ void EXTI9_5_IRQHandler(void) { + // The true source of the interrupt is checked inside HAL_GPIO_EXTI_IRQHandler() + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_7); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_8); HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_9); } @@ -354,6 +359,12 @@ void EXTI9_5_IRQHandler(void) */ void EXTI15_10_IRQHandler(void) { + // The true source of the interrupt is checked inside HAL_GPIO_EXTI_IRQHandler() + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_10); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_11); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13); + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_14); HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15); } From cc1eddc65edc69e45bb8b4b48c97e3e0f14335bd Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 19:52:39 -0700 Subject: [PATCH 103/112] store the programmer serial number with escape characters. An STLink/v2 programmer is now identified by a string of the format "\x12\x34..." instead of "1234..." This navigates around differences of how makefiles are parsed in windows and linux --- Firmware/Makefile | 3 +-- Firmware/find_programmer.sh | 5 ++++- tools/test-rig-loopback.yaml | 4 ++-- tools/test-rig-parallel.yaml | 4 ++-- 4 files changed, 9 insertions(+), 7 deletions(-) diff --git a/Firmware/Makefile b/Firmware/Makefile index eee7c551..4b823b60 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -5,9 +5,8 @@ BUILD_DIR = build FIRMWARE = $(BUILD_DIR)/ODriveFirmware.elf FIRMWARE_HEX = $(BUILD_DIR)/ODriveFirmware.hex -PROGRAMMER_HEX := $(shell echo $(PROGRAMMER) | sed -e 's/.\\{2\\}/\\\\x&/g') OPENOCD := openocd -f interface/stlink-v2.cfg \ - $(if $(value PROGRAMMER),-c 'hla_serial $(PROGRAMMER_HEX)',) \ + $(if $(value PROGRAMMER),-c 'hla_serial $(PROGRAMMER)',) \ -f target/stm32f4x.cfg diff --git a/Firmware/find_programmer.sh b/Firmware/find_programmer.sh index 97a94928..4b184b6a 100755 --- a/Firmware/find_programmer.sh +++ b/Firmware/find_programmer.sh @@ -1,2 +1,5 @@ #!/bin/bash -openocd -d3 -f board/stm32f4discovery.cfg -c "hla_serial wrong_serial" 2>&1 | xxd -p | tr -d '\n' | sed -n 's/^.*6e756d6265722027\([0-9a-f]*\)2720646f65736e27.*$/\1/p'; echo +openocd -d3 -f board/stm32f4discovery.cfg -c "hla_serial wrong_serial" 2>&1 | \ + xxd -p | \ + tr -d '\n' | \ + sed -n 's/^.*6e756d6265722027\([0-9a-f]*\)2720646f65736e27.*$/\1/p' | sed -e 's/.\{2\}/\\x&/g'; echo diff --git a/tools/test-rig-loopback.yaml b/tools/test-rig-loopback.yaml index 359c3a61..12b87235 100644 --- a/tools/test-rig-loopback.yaml +++ b/tools/test-rig-loopback.yaml @@ -8,7 +8,7 @@ odrives: brake-resistance: 0.47 uart: /dev/serial/by-id/[not-yet-used] usb: auto - programmer: '493f6f06493f56540929113f' + programmer: '\x49\x3f\x6f\x06\x49\x3f\x56\x54\x09\x29\x11\x3f' vbus-voltage: 24 # [V] max-brake-power: 150 # [W] axes: @@ -38,7 +38,7 @@ odrives: brake-resistance: 0.47 uart: /dev/serial/by-id/[not-yet-used] usb: auto - programmer: '533f7506493f49514454193f' + programmer: '\x53\x3f\x75\x06\x49\x3f\x49\x51\x44\x54\x19\x3f' vbus-voltage: 48 # [V] max-brake-power: 150 # [W] axes: diff --git a/tools/test-rig-parallel.yaml b/tools/test-rig-parallel.yaml index e7559105..47173166 100644 --- a/tools/test-rig-parallel.yaml +++ b/tools/test-rig-parallel.yaml @@ -9,7 +9,7 @@ odrives: brake-resistance: 0.47 uart: /dev/serial/by-id/[not-yet-used] usb: auto - programmer: '533f7506493f49514454193f' + programmer: '\x53\x3f\x75\x06\x49\x3f\x49\x51\x44\x54\x19\x3f' vbus-voltage: 24 # [V] max-brake-power: 150 # [W] axes: @@ -39,7 +39,7 @@ odrives: brake-resistance: 0.47 uart: /dev/serial/by-id/[not-yet-used] usb: auto - programmer: '493f6f06493f56540929113f' + programmer: '\x49\x3f\x6f\x06\x49\x3f\x56\x54\x09\x29\x11\x3f' vbus-voltage: 24 # [V] max-brake-power: 150 # [W] axes: From 8b696d29e0de27656626f09e7ab2ee13c54f566d Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 20:10:02 -0700 Subject: [PATCH 104/112] change pos_cpr to pos_cpr_ --- Firmware/MotorControl/encoder.cpp | 10 +++++----- Firmware/MotorControl/encoder.hpp | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 0e736ae1..6267e152 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -71,7 +71,7 @@ void Encoder::set_circular_count(int32_t count) { offset_ = mod(offset_, config_.cpr); // Update states count_in_cpr_ = mod(count, config_.cpr); - pos_cpr = (float)count_in_cpr_; + pos_cpr_ = (float)count_in_cpr_; __set_PRIMASK(prim); } @@ -233,15 +233,15 @@ bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_outp // run pll (for now pll is in units of encoder counts) // Predict current pos pos_estimate_ += current_meas_period * pll_vel_; - pos_cpr += current_meas_period * pll_vel_; + pos_cpr_ += current_meas_period * pll_vel_; // discrete phase detector float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_)); - float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr)); + float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_)); delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; - pos_cpr += current_meas_period * pll_kp_ * delta_pos_cpr; - pos_cpr = fmodf_pos(pos_cpr, (float)(config_.cpr)); + pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; + pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr; if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_) pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index a97b94a1..9a59654e 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -56,7 +56,7 @@ public: int32_t offset_ = 0; float phase_ = 0.0f; // [rad] float pos_estimate_ = 0.0f; // [rad] - float pos_cpr = 0.0f; // [rad] + float pos_cpr_ = 0.0f; // [rad] float pll_vel_ = 0.0f; // [rad/s] float pll_kp_ = 0.0f; // [rad/s / rad] float pll_ki_ = 0.0f; // [(rad/s^2) / rad] @@ -72,7 +72,7 @@ public: make_protocol_property("offset", &offset_), make_protocol_property("phase", &phase_), make_protocol_property("pos_estimate", &pos_estimate_), - make_protocol_property("pos_cpr", &pos_cpr), + make_protocol_property("pos_cpr", &pos_cpr_), make_protocol_property("pll_vel", &pll_vel_), make_protocol_property("pll_kp", &pll_kp_), make_protocol_property("pll_ki", &pll_ki_), From f3a484d6dbed32c5b2cb4317eeb5d5dee36d93b6 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 23 Apr 2018 20:21:18 -0700 Subject: [PATCH 105/112] fix deadtime violation check polarity --- Firmware/MotorControl/low_level.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index f84b90fc..a984b789 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -182,7 +182,7 @@ void safety_critical_disarm_brake_resistor() { // @brief Updates the brake resistor PWM timings unless // the brake resistor is disarmed. void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) { - if (high_on - low_off > TIM_APB1_DEADTIME_CLOCKS) + if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS) for(;;); uint8_t sr = cpu_enter_critical(); if (brake_resistor_armed_) { From 3125044b96dc42b551d84cc11d5a0b52cdadb61c Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Mon, 23 Apr 2018 20:26:41 -0700 Subject: [PATCH 106/112] amend changelog --- Firmware/CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index eb7c0769..66a71605 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -6,6 +6,9 @@ Please add a note of your changes below this heading if you make a Pull Request. * `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you should run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `hw_version_[...]` properties. * bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_version_[...]` properties. * infrastructure to publish the python tools to PyPi. See `tools/setup.py` for details. + * Automated test script `run_tests.py` + * Protocol supports function return values + * System stats (e.g. stack usage) are exposed under `.system_stats` ### Changed * The DFU script now verifies the flash after writing @@ -24,6 +27,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Fixed * Enums now transported with correct underlying type on native protocol +* USB issue where the device would stop responding when the host script would quit abruptly or reset the device during operation # Releases From 5735d5cd96afa570051737f140991dad08937f02 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 24 Apr 2018 12:20:08 -0700 Subject: [PATCH 107/112] [firmware] disable DFU feature for board version <= 3.4 because it can break the board --- Firmware/Board/v3/Src/main.c | 33 ++++++++++++++++++------------ Firmware/MotorControl/commands.cpp | 21 ++++++++++++++----- 2 files changed, 36 insertions(+), 18 deletions(-) diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 20f4ba8a..1691e4a6 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -100,19 +100,26 @@ int main(void) { /* USER CODE BEGIN 1 */ - /* - * This wait loop works around an obscure timing issue. - * When the transition NVIC_SystemReset() => STM bootloader happens quickly, - * there is a yet unexplained phenomenon where both the high side and low side - * brake resistor FETs would turn on simultaneously for about 2.5ms. - * This manifests in an audible click and may lead to failure of the FETs. - * When adding a delay before entering DFU mode the issue does not occur. - * - * This loop takes about 5 cycles per iteration, so the delay - * is about 1/168000kHz*5*1000000 = 30ms - */ - for (size_t i = 0; i < 1000000; ++i) { - __NOP(); + if(*((unsigned long *)0x2001C000) == 0xDEADFE75) { + /* The STM DFU bootloader enables internal pull-up resistors on PB10 (AUX_H) + * and PB11 (AUX_L), thereby causing shoot-through on the brake resistor + * FETs and obliterating them unless external 3.3k pull-down resistors are + * present. Pull-downs are only present on ODrive 3.5 or newer. + * On older boards we disable DFU by default but if the user insists + * there's only one thing left that might save it: time. + * The brake resistor gate driver needs a certain 10V supply (GVDD) to + * make it work. This voltage is supplied by the motor gate drivers which get + * disabled at system reset. So over time GVDD voltage _should_ below + * dangerous levels. This is completely handwavy and should not be relied on + * so you are on your own on if you ignore this warning. + * + * This loop takes 5 cycles per iteration and at this point the system runs + * on the internal 16MHz RC oscillator so the delay is about 2 seconds. + */ + for (size_t i = 0; i < (16000000UL / 5UL * 2UL); ++i) { + __NOP(); + } + *((unsigned long *)0x2001C000) == 0xDEADBEEF; } /* We could jump to the bootloader directly on demand without rebooting diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index 4f8453d8..2ca2a066 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -109,11 +109,6 @@ void motors_run_anticogging_calibration_func() { } } -void enter_dfu_mode() { - *((unsigned long *)0x2001C000) = 0xDEADBEEF; - NVIC_SystemReset(); -} - #if HW_VERSION_MAJOR == 3 // Determine start address of the OTP struct: // The OTP is organized into 16-byte blocks. @@ -142,6 +137,22 @@ const uint8_t fw_version_minor = FW_VERSION_MINOR; const uint8_t fw_version_revision = FW_VERSION_REVISION; const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise +void enter_dfu_mode() { + if ((board_version_major == 3) && (board_version_minor >= 5)) { + *((unsigned long *)0x2001C000) = 0xDEADBEEF; + NVIC_SystemReset(); + } else { + /* + * DFU mode is only allowed on board version >= 3.5 because it can burn + * the brake resistor FETs on older boards. + * If you really want to use it on an older board, add 3.3k pull-down resistors + * to the AUX_L and AUX_H signals and _only then_ uncomment these lines. + */ + //*((unsigned long *)0x2001C000) = 0xDEADFE75; + //NVIC_SystemReset(); + } +} + // This table specifies which fields and functions are exposed on the USB and UART ports. // TODO: Autogenerate this table. It will come up again very soon in the Arduino library. // clang-format off From 86a21838eac2e7742a521a2d782f60c539f08936 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 24 Apr 2018 12:32:33 -0700 Subject: [PATCH 108/112] [odrivetool] disable DFU feature for board version <= 3.4 because it can break the board --- tools/odrive/dfu.py | 32 +++++++++++++++++++++----------- 1 file changed, 21 insertions(+), 11 deletions(-) diff --git a/tools/odrive/dfu.py b/tools/odrive/dfu.py index d1b59f82..1efd4592 100755 --- a/tools/odrive/dfu.py +++ b/tools/odrive/dfu.py @@ -206,7 +206,7 @@ def show_deferred_message(message, cancellation_token): t.daemon = True t.start() -def put_odrive_into_dfu_mode(my_drive): +def put_odrive_into_dfu_mode(my_drive, cancellation_token): """ Puts the specified device into DFU mode """ @@ -216,15 +216,23 @@ def put_odrive_into_dfu_mode(my_drive): "DFU with this script should work fine." .format(my_drive.__channel__.usb_device.serial_number)) return - print("Putting device {} into DFU mode...".format(my_drive.__channel__.usb_device.serial_number)) - try: - my_drive.enter_dfu_mode() - except odrive.protocol.ChannelBrokenException as ex: - pass # this is expected because the device reboots - if platform.system() == "Windows": - show_deferred_message("Still waiting for the device to reappear.\n" - "Use the Zadig utility to set the driver of 'STM32 BOOTLOADER' to libusb-win32.", - find_odrive_cancellation_token) + hw_version_major = my_drive.hw_version_major if hasattr(my_drive, 'hw_version_major') else 3 + hw_version_minor = my_drive.hw_version_minor if hasattr(my_drive, 'hw_version_minor') else 4 + if hw_version_major == 3 and hw_version_minor >= 5: + print("Putting device {} into DFU mode...".format(my_drive.__channel__.usb_device.serial_number)) + try: + my_drive.enter_dfu_mode() + except odrive.protocol.ChannelBrokenException: + pass # this is expected because the device reboots + if platform.system() == "Windows": + show_deferred_message("Still waiting for the device to reappear.\n" + "Use the Zadig utility to set the driver of 'STM32 BOOTLOADER' to libusb-win32.", + cancellation_token) + else: + print("Found device {}".format(my_drive.__channel__.usb_device.serial_number)) + print(" DFU mode is not supported on board version 3.4 or earlier.") + print(" This is because entering DFU mode on such a device would") + print(" break the brake resistor FETs under some circumstances.") def launch_dfu(args, app_shutdown_token): """ @@ -251,7 +259,9 @@ def launch_dfu(args, app_shutdown_token): # Scan for ODrives not in DFU mode and put them into DFU mode once they appear # We only scan on USB because DFU is only possible over USB - odrive.discovery.find_all(args.path, serial_number, put_odrive_into_dfu_mode, find_odrive_cancellation_token, app_shutdown_token) + odrive.discovery.find_all(args.path, serial_number, + lambda dev: put_odrive_into_dfu_mode(dev, find_odrive_cancellation_token), + find_odrive_cancellation_token, app_shutdown_token) # Poll libUSB until a device in DFU mode is found while not app_shutdown_token.is_set(): From 9ba52528257f97dd795eab42add08a9eff553e23 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 24 Apr 2018 13:11:37 -0700 Subject: [PATCH 109/112] invoke scripts with python command required on windows --- Firmware/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/Makefile b/Firmware/Makefile index 4b823b60..794b7695 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -24,7 +24,7 @@ gdb: all arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit dfu: all - ../tools/odrivetool $(if $(value SERIAL_NUMBER),--serial-number $(SERIAL_NUMBER),) dfu $(FIRMWARE_HEX) + python ../tools/odrivetool $(if $(value SERIAL_NUMBER),--serial-number $(SERIAL_NUMBER),) dfu $(FIRMWARE_HEX) bmp: all arm-none-eabi-gdb --ex 'target extended-remote /dev/stlink' \ From e2c14c00829f87bfc586ec123e9b09d4a3eb43ea Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 24 Apr 2018 13:49:39 -0700 Subject: [PATCH 110/112] fix excessive firmware size due to oscilloscope array --- Firmware/MotorControl/odrive_main.h | 3 ++- Firmware/communication/communication.cpp | 4 +--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index bddf7e88..2b29b6ac 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -70,7 +70,8 @@ class Motor; constexpr size_t AXIS_COUNT = 2; extern Axis *axes[AXIS_COUNT]; -#define OSCILLOSCOPE_SIZE 18000 +// if you use the oscilloscope feature you can bump up this value +#define OSCILLOSCOPE_SIZE 128 extern float oscilloscope[OSCILLOSCOPE_SIZE]; extern size_t oscilloscope_pos; diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 86891a8f..ae611f24 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -76,9 +76,7 @@ void init_communication(void) { } -float oscilloscope[OSCILLOSCOPE_SIZE] = { - 0.123f, 0.345f, 0.4576f, 1.543f, -50.0f -}; +float oscilloscope[OSCILLOSCOPE_SIZE] = {0}; size_t oscilloscope_pos = 0; From 2fde5b4d857e9f783d0e829ccd4898559ddf8a80 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 24 Apr 2018 15:35:10 -0700 Subject: [PATCH 111/112] improve cross platform compatibility - python odrive module can be used from python2 again - the automatic firmware version.h generation is now based on python instead of bash (which didn't work well on Windows) --- Firmware/Tupfile.lua | 2 +- Firmware/dump_version.sh | 47 --------------------------- tools/odrive/__init__.py | 6 ++-- tools/odrive/utils.py | 41 ++---------------------- tools/odrive/version.py | 69 +++++++++++++++++++++++++--------------- tools/odrivetool | 3 +- tools/run_tests.py | 40 ++++++++++++++++++++++- tools/setup.py | 2 +- 8 files changed, 92 insertions(+), 118 deletions(-) delete mode 100755 Firmware/dump_version.sh diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index feae2670..ff95f0d1 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -138,7 +138,7 @@ build{ } tup.frule{ - command='bash dump_version.sh %o', + command='python ../tools/odrive/version.py --output %o', outputs={'build/version.h'} } diff --git a/Firmware/dump_version.sh b/Firmware/dump_version.sh deleted file mode 100755 index cdaaa372..00000000 --- a/Firmware/dump_version.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -set -euo pipefail - -if [ $# -eq 1 ]; then - OUTPUT="$1" -else - OUTPUT="/dev/stdout" -fi - -# The git root lies outside of the tup root -export GIT_DISCOVERY_ACROSS_FILESYSTEM=1 - -# Get a description of the current Git state -# Examples of what this string may become: -# fw-v0.3.6 The current commit is exactly at tag "fw-v0.3.6" -# There may or may not be untracked files in the -# working directory. -# fw-v0.3.6* The current commit is at tag "fw-v0.3.6" and there -# are uncommitted changes in the working directory. -# fw-v0.3.6-4-g3703ae5 The working directory at a commit with hash 3703ae5, -# 4 commits ahead of tag fw-v0.3.6 and clean. -FW_VERSION="$(git describe --always --tags --dirty=* || echo "[unknown commit]")" - -# Extract version numbers -FW_VERSION_MAJOR="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\1/p' <<< "$FW_VERSION")" -FW_VERSION_MINOR="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\2/p' <<< "$FW_VERSION")" -FW_VERSION_REVISION="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\3/p' <<< "$FW_VERSION")" -FW_VERSION_SUFFIX="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\4/p' <<< "$FW_VERSION")" - -# Fall back to 0 if the verions does not match the expected pattern -[ "$FW_VERSION_MAJOR" == "" ] && FW_VERSION_MAJOR=0 -[ "$FW_VERSION_MINOR" == "" ] && FW_VERSION_MINOR=0 -[ "$FW_VERSION_REVISION" == "" ] && FW_VERSION_REVISION=0 - -if [ "$FW_VERSION_SUFFIX" == "" ]; then - FW_VERSION_UNRELEASED=0 -else - FW_VERSION_UNRELEASED=1 -fi - -cat > "$OUTPUT" < 1: - msg = "task {} and {} failed.".format( - tracebacks[0][0], - "one other" if len(tracebacks) == 2 else str(len(tracebacks)-1) + " others" - ) - raise Exception(msg) from tracebacks[0][1] - - class Logger(): """ Logs messages to stdout diff --git a/tools/odrive/version.py b/tools/odrive/version.py index 327c915c..68bad7d0 100644 --- a/tools/odrive/version.py +++ b/tools/odrive/version.py @@ -4,7 +4,29 @@ import subprocess import os import sys -def get_version(git_only=False): +def get_version_from_git(): + script_dir = os.path.dirname(os.path.realpath(__file__)) + try: + # Determine the current git commit version + git_tag = subprocess.check_output(["git", "describe", "--always", "--tags", "--dirty=*"], + cwd=script_dir) + git_tag = git_tag.decode(sys.stdout.encoding).rstrip('\n') + + regex=r'.*v([0-9a-zA-Z]).([0-9a-zA-Z]).([0-9a-zA-Z])(.*)' + package_version_major = int(re.sub(regex, r"\1", git_tag)) + package_version_minor = int(re.sub(regex, r"\2", git_tag)) + package_version_revision = int(re.sub(regex, r"\3", git_tag)) + package_version_unreleased = (re.sub(regex, r"\4", git_tag) != "") + + if package_version_unreleased: + package_version_revision += 1 + + except Exception as ex: + print(ex) + return "[unknown version]", 0, 0, 0, 1 + return git_tag, package_version_major, package_version_minor, package_version_revision, package_version_unreleased + +def get_version_str(git_only=False): """ Returns the versions of the tools If git_only is true, the version.txt file is ignored even @@ -18,29 +40,24 @@ def get_version(git_only=False): if os.path.exists(version_file_path) and git_only == False: with open(version_file_path) as version_file: return version_file.readline().rstrip('\n') - - try: - # Determine the current git commit version - git_result = subprocess.run(["git", "describe", "--always", "--tags", "--dirty=*"], - cwd=script_dir, - stdout=subprocess.PIPE, timeout=10) - git_tag = git_result.stdout.decode(sys.stdout.encoding) - - regex=r'.*v([0-9a-zA-Z]).([0-9a-zA-Z]).([0-9a-zA-Z])(.*)' - package_version_major = int(re.sub(regex, r"\1", git_tag)) - package_version_minor = int(re.sub(regex, r"\2", git_tag)) - package_version_revision = int(re.sub(regex, r"\3", git_tag)) - package_version_unreleased = (re.sub(regex, r"\4", git_tag) != "") - - if package_version_unreleased: - package_version_revision += 1 - - # TODO: fetch from Git describe - version = '{}.{}.{}'.format(package_version_major, package_version_minor, package_version_revision) - - if package_version_unreleased: - version += ".dev" - except Exception as ex: - print(ex) - version = "whatever version in " + script_dir + + _, major, minor, revision, unreleased = get_version_from_git() + version = '{}.{}.{}'.format(major, minor, revision) + if unreleased: + version += ".dev" return version + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser(description='Version Dump\n') + parser.add_argument("--output", type=argparse.FileType('w'), default='-', + help="C header output file") + + args = parser.parse_args() + + git_name, major, minor, revision, unreleased = get_version_from_git() + args.output.write('#define FW_VERSION "{}"\n'.format(git_name)) + args.output.write('#define FW_VERSION_MAJOR {}\n'.format(major)) + 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)) diff --git a/tools/odrivetool b/tools/odrivetool index a6e80acf..901ebde3 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -86,7 +86,8 @@ else: logger = Logger(verbose=args.verbose) def print_version(): - print("ODrive control utility v" + odrive.__version__) + sys.stderr.write("ODrive control utility v" + odrive.__version__ + "\n") + sys.stderr.flush() app_shutdown_token = Event() diff --git a/tools/run_tests.py b/tools/run_tests.py index f2f67a19..1cb1bcd8 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -14,7 +14,45 @@ import threading import traceback import argparse from odrive.tests import * -from odrive.utils import Logger, for_all_parallel, Event +from odrive.utils import Logger, Event + + +def for_all_parallel(objects, get_name, callback): + """ + Executes the specified callback for every object in the objects + list concurrently. This function waits for all callbacks to + finish and throws an exception if any of the callbacks throw + an exception. + """ + tracebacks = [] + + def run_callback(element): + try: + callback(element) + except Exception as ex: + tracebacks.append((get_name(element), ex)) + + # Start a thread for each element in the list + all_threads = [] + for element in objects: + thread = threading.Thread(target=run_callback, args=(element,)) + thread.start() + all_threads.append(thread) + + # Wait for all threads to complete + for thread in all_threads: + thread.join() + + if len(tracebacks) == 1: + msg = "task {} failed.".format(tracebacks[0][0]) + raise Exception(msg) from tracebacks[0][1] + elif len(tracebacks) > 1: + msg = "task {} and {} failed.".format( + tracebacks[0][0], + "one other" if len(tracebacks) == 2 else str(len(tracebacks)-1) + " others" + ) + raise Exception(msg) from tracebacks[0][1] + script_path=os.path.dirname(os.path.realpath(__file__)) diff --git a/tools/setup.py b/tools/setup.py index 946e6900..3cdb67d6 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -40,7 +40,7 @@ creating_package = "sdist" in sys.argv # Load version from Git tag import odrive.version -version = odrive.version.get_version(git_only=creating_package) +version = odrive.version.get_version_str(git_only=creating_package) # Change this if you already uploaded the current # version but need to release a hotfix From e19c477875335a50b3c5a0b37e14a525726bd5b3 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 24 Apr 2018 19:36:07 -0700 Subject: [PATCH 112/112] add test property --- Firmware/communication/communication.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index ae611f24..1c4accfd 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -64,6 +64,8 @@ const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official r osThreadId comm_thread; +static uint32_t test_property = 0; + /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ @@ -134,8 +136,9 @@ static inline auto make_obj_tree() { ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), - make_protocol_function("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), + make_protocol_property("test_property", &test_property), make_protocol_function("test_function", static_functions, &StaticFunctions::test_function, "delta"), + make_protocol_function("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper),