From b32c87df5a92fb2bdff2f96124992b0080b13484 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Fri, 2 Mar 2018 13:00:59 -0800 Subject: [PATCH 01/15] [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 02/15] 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 03/15] 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 04/15] 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 05/15] 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 06/15] 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 07/15] 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 08/15] 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 09/15] 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 10/15] 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 11/15] 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 12/15] 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 13/15] 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 14/15] 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 15/15] 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)