diff --git a/Firmware/.gitignore b/Firmware/.gitignore index d50af3bb..496462db 100644 --- a/Firmware/.gitignore +++ b/Firmware/.gitignore @@ -17,6 +17,9 @@ Odrive.xml .settings/ .project +# VSCode stuff +/.vscode/.cortex-debug.*.state.json + # STM32CubeMX (in case you put it in this folder, or a symlink) STM32CubeMX diff --git a/Firmware/Board/v3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3/Inc/FreeRTOSConfig.h index 93f9b39d..6da2e557 100644 --- a/Firmware/Board/v3/Inc/FreeRTOSConfig.h +++ b/Firmware/Board/v3/Inc/FreeRTOSConfig.h @@ -109,6 +109,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 @@ -124,6 +125,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/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index 756ea9f3..f8ffe61b 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -59,7 +59,7 @@ #include "main.h" /* USER CODE BEGIN Includes */ - +#include /* USER CODE END Includes */ /* USER CODE BEGIN Private defines */ @@ -71,8 +71,12 @@ void MX_GPIO_Init(void); /* USER CODE BEGIN Prototypes */ 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/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index 09cdf273..2e1c9e0b 100644 --- a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -270,11 +270,11 @@ typedef enum { /// Entry point of a thread. /// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS. -typedef void (*os_pthread) (void const *argument); +typedef void (*os_pthread) (void *argument); /// Entry point of a timer call back function. /// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS. -typedef void (*os_ptimer) (void const *argument); +typedef void (*os_ptimer) (void *argument); // >>> the following data type definitions may shall adapted towards a specific RTOS @@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t; /// Thread Definition structure contains startup information of a thread. /// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS. typedef struct os_thread_def { - char *name; ///< Thread name + const char *name; ///< Thread name os_pthread pthread; ///< start address of thread function osPriority tpriority; ///< initial thread priority uint32_t instances; ///< maximum number of instances of that thread function diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index 9439fd97..002ac979 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -53,10 +53,8 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include "low_level.h" #include "axis_c_interface.h" -#include "commands.h" -#include "config.h" +int odrive_main(void); /* USER CODE END Includes */ /* Variables -----------------------------------------------------------------*/ @@ -77,7 +75,7 @@ uint8_t ucHeap[configTOTAL_HEAP_SIZE]; /* USER CODE END Variables */ /* Function prototypes -------------------------------------------------------*/ -void StartDefaultTask(void const * argument); +void StartDefaultTask(void * argument); extern void MX_USB_DEVICE_Init(void); void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ @@ -114,7 +112,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); @@ -126,7 +124,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 */ @@ -139,35 +137,14 @@ 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(); /* 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/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 02b7d691..9585749f 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/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 @@ -140,10 +140,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; @@ -160,59 +194,78 @@ 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) { + bool is_pin_in_use = false; + 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; + } else if (subscriptions[i].GPIO_pin == GPIO_pin) { + is_pin_in_use = true; + } + } + if (!is_pin_in_use) + HAL_NVIC_DisableIRQ(get_irq_number(GPIO_pin)); } - //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/Src/main.c b/Firmware/Board/v3/Src/main.c index 644799de..777a5f20 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -60,7 +60,7 @@ /* USER CODE BEGIN Includes */ #include "utils.h" -#include "commands.h" +#include "communication.h" /* USER CODE END Includes */ /* Private variables ---------------------------------------------------------*/ diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 07d3bb9d..0a940b15 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -217,7 +217,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/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index a7c0bd8c..b500c6c4 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -54,7 +54,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/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index 37b43026..71b353a8 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -51,7 +51,7 @@ #include "usbd_core.h" #include "usbd_desc.h" #include "usbd_conf.h" -#include "commands.h" +#include "communication.h" /* USER CODE BEGIN INCLUDE */ diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 1d7dbd71..36d89bc4 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -7,6 +7,9 @@ Please add a note of your changes below this heading if you make a Pull Request. * `make erase_config` to erase the configuration with an STLink (the configuration can also be erased from within explore_odrive.py, using `my_odrive.erase_configuration()`) ### Changed +* 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. * The build is now configured using the `tup.config` file instead of editing source files. Make sure you set your board version correctly. See [here](README.md#configuring-the-build) for details. * The toplevel directory for tup is now `Firmware`. If you used tup before, go to `Firmware` and run `rm -rd ../.tup; rm -rd build/*; make`. * Update CubeMX generated STM platform code to version 1.19.0 diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 553c7190..e186e864 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -1,97 +1,286 @@ -#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 "odrive_main.hpp" -//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_; -} - -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), +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), - legacy_motor_ref_(legacy_motor_ref) { - SetupLegacyMappings(); + encoder_(encoder), + sensorless_estimator_(sensorless_estimator), + controller_(controller), + motor_(motor) +{ + encoder_.axis_ = this; + sensorless_estimator_.axis_ = this; + controller_.axis_ = this; + motor_.axis_ = this; } -void Axis::StateMachineLoop() { +// @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_wrapper, hw_config_.thread_priority, 0, 4*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_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) { + reinterpret_cast(ctx)->step_cb(); +} + +// 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; + } +}; + +// @brief Enables or disables step/dir input +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); + } +} + +// @brief Returns true if the power supply is within range +bool Axis::check_PSU_brownout() { + return vbus_voltage >= config_.dc_bus_brownout_trip_level; +} + +// @brief Returns true if everything is ok. +// Sets error and returns false otherwise. +bool Axis::do_checks() { + if (!motor_.do_checks()) + return error_ = ERROR_MOTOR_FAILED, false; + if (!check_PSU_brownout()) + return error_ = ERROR_DC_BUS_UNDER_VOLTAGE, 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 error_ = ERROR_MOTOR_FAILED, false; + return x < 1.0f; + }); + 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); + 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 error_ = ERROR_MOTOR_FAILED, false; + 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. +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; + + // 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; + return true; + }); + 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; + return true; + }); + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; +} + +bool Axis::run_idle_loop() { + // 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 +void Axis::run_state_machine_loop() { - //TODO: Move this somewhere else // 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) { + // 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) { 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; - bool calibration_ok = false; + // arm! + motor_.arm(); + 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; - - __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 - } - - 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; - - if (enable_control_) { // if control is still enabled, we exited because of error - calibration_ok = false; - enable_control_ = false; + // 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: + status = encoder_.run_calibration(); + break; + + case AXIS_STATE_SENSORLESS_CONTROL: + status = run_sensorless_spin_up(); // TODO: restart if desired + if (status) + status = run_sensorless_control_loop(); + break; + + case AXIS_STATE_CLOSED_LOOP_CONTROL: + status = run_closed_loop_control_loop(); + break; + + case AXIS_STATE_IDLE: + run_idle_loop(); + status = motor_.arm(); // done with idling - try to arm the motor + break; + + default: + error_ = ERROR_INVALID_STATE; + status = false; // this will set the state to idle + break; + } + + // If the state failed, go to idle, else advance task chain + if (!status) + current_state_ = AXIS_STATE_IDLE; + else + memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0])); } - 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..d4feadbd --- /dev/null +++ b/Firmware/MotorControl/axis.hpp @@ -0,0 +1,188 @@ +#ifndef __AXIS_HPP +#define __AXIS_HPP + +#ifndef __ODRIVE_MAIN_HPP +#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_UNDEFINED, // + void run_control_loop(const T& update_handler) { + while (requested_state_ == AXIS_STATE_UNDEFINED) { + if (motor_.error_ != Motor::ERROR_NO_ERROR) { + error_ = ERROR_MOTOR_FAILED; + break; + } + if ((current_state_ != AXIS_STATE_IDLE) && missed_control_deadline_) { + error_ = ERROR_CONTROL_LOOP_TIMEOUT; + break; + } + + if (!do_checks()) // error set during function call + break; + + if (!update_handler()) // error set during function call + break; + + // Check we meet deadlines after queueing + ++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; + } + } + } + + bool run_sensorless_spin_up(); + bool run_sensorless_control_loop(); + bool run_closed_loop_control_loop(); + bool run_idle_loop(); + + void run_state_machine_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; + + // variables exposed on protocol + Error_t error_ = ERROR_NO_ERROR; + bool missed_control_deadline_ = true; // this flag is raised by the interrupt handler + // whenever there's no active control loop that + // sets the timings. The flag must be explicitly + // cleared by a call to motors.arm(). + bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir + AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE; + AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED }; + 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_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/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h new file mode 100644 index 00000000..3e2d965d --- /dev/null +++ b/Firmware/MotorControl/board_config_v3.h @@ -0,0 +1,115 @@ +/* +* @brief Contains board specific configuration for ODrive v3.x +*/ + +#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 deleted file mode 100644 index d725afa0..00000000 --- a/Firmware/MotorControl/commands.cpp +++ /dev/null @@ -1,551 +0,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 "axis.h" -#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; -uint64_t serial_number; - -/* Private constant data -----------------------------------------------------*/ -// TODO: make command to switch gpio_mode during run-time - -typedef enum { - GPIO_MODE_NONE, - GPIO_MODE_UART, - GPIO_MODE_STEP_DIR, -} GpioMode_t; - -#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; - } - } -} - -void enter_dfu_mode() { - *((unsigned long *)0x2001C000) = 0xDEADBEEF; - NVIC_SystemReset(); -} - -// This table specifies which fields and functions are exposed on the USB and UART ports. -// TODO: Autogenerate this table. It will come up again very soon in the Arduino library. -// clang-format off -const Endpoint endpoints[] = { - Endpoint::make_property("vbus_voltage", const_cast(&vbus_voltage)), - Endpoint::make_property("serial_number", const_cast(&serial_number)), - 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_at_start", &axis_configs[0].enable_control_at_start), - Endpoint::make_property("do_calibration_at_start", &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_ready", &motors[0].thread_ready), - 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("manually_calibrated", &motors[0].encoder.manually_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::make_object("timing_log"), - Endpoint::make_property("TIMING_LOG_GENERAL", &motors[0].timing_log[TIMING_LOG_GENERAL]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_I", &motors[0].timing_log[TIMING_LOG_ADC_CB_M0_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_DC", &motors[0].timing_log[TIMING_LOG_ADC_CB_M0_DC]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_I", &motors[0].timing_log[TIMING_LOG_ADC_CB_M1_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_DC", &motors[0].timing_log[TIMING_LOG_ADC_CB_M1_DC]), - Endpoint::make_property("TIMING_LOG_MEAS_R", &motors[0].timing_log[TIMING_LOG_MEAS_R]), - Endpoint::make_property("TIMING_LOG_MEAS_L", &motors[0].timing_log[TIMING_LOG_MEAS_L]), - Endpoint::make_property("TIMING_LOG_ENC_CALIB", &motors[0].timing_log[TIMING_LOG_ENC_CALIB]), - Endpoint::make_property("TIMING_LOG_IDX_SEARCH", &motors[0].timing_log[TIMING_LOG_IDX_SEARCH]), - Endpoint::make_property("TIMING_LOG_FOC_VOLTAGE", &motors[0].timing_log[TIMING_LOG_FOC_VOLTAGE]), - Endpoint::make_property("TIMING_LOG_FOC_CURRENT", &motors[0].timing_log[TIMING_LOG_FOC_CURRENT]), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor0 - Endpoint::make_object("axis1"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control_at_start", &axis_configs[1].enable_control_at_start), - Endpoint::make_property("do_calibration_at_start", &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_ready", &motors[1].thread_ready), - 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("manually_calibrated", &motors[1].encoder.manually_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::make_object("timing_log"), - Endpoint::make_property("TIMING_LOG_GENERAL", &motors[1].timing_log[TIMING_LOG_GENERAL]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_I", &motors[1].timing_log[TIMING_LOG_ADC_CB_M0_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_DC", &motors[1].timing_log[TIMING_LOG_ADC_CB_M0_DC]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_I", &motors[1].timing_log[TIMING_LOG_ADC_CB_M1_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_DC", &motors[1].timing_log[TIMING_LOG_ADC_CB_M1_DC]), - Endpoint::make_property("TIMING_LOG_MEAS_R", &motors[1].timing_log[TIMING_LOG_MEAS_R]), - Endpoint::make_property("TIMING_LOG_MEAS_L", &motors[1].timing_log[TIMING_LOG_MEAS_L]), - Endpoint::make_property("TIMING_LOG_ENC_CALIB", &motors[1].timing_log[TIMING_LOG_ENC_CALIB]), - Endpoint::make_property("TIMING_LOG_IDX_SEARCH", &motors[1].timing_log[TIMING_LOG_IDX_SEARCH]), - Endpoint::make_property("TIMING_LOG_FOC_VOLTAGE", &motors[1].timing_log[TIMING_LOG_FOC_VOLTAGE]), - Endpoint::make_property("TIMING_LOG_FOC_CURRENT", &motors[1].timing_log[TIMING_LOG_FOC_CURRENT]), - 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(), - Endpoint::make_function("enter_dfu_mode", &enter_dfu_mode), - // 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()); -} diff --git a/Firmware/MotorControl/communication.cpp b/Firmware/MotorControl/communication.cpp new file mode 100644 index 00000000..8f951fbb --- /dev/null +++ b/Firmware/MotorControl/communication.cpp @@ -0,0 +1,314 @@ + +/* 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; +uint64_t serial_number; + +/* 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 + + +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void enter_dfu_mode() { + *((unsigned long *)0x2001C000) = 0xDEADBEEF; + NVIC_SystemReset(); +} + +void init_communication(void) { + printf("hi!\r\n"); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues + thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + 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 +class StaticFunctions { +public: + void save_configuration_helper() { save_configuration(); } + void erase_configuration_helper() { erase_configuration(); } + void NVIC_SystemReset_helper() { NVIC_SystemReset(); } + void enter_dfu_mode_helper() { enter_dfu_mode(); } +} static_functions; + +// 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("serial_number", &serial_number), + 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_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) + ); +} + +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 + + // 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 + //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 84% rename from Firmware/MotorControl/commands.h rename to Firmware/MotorControl/communication.h index f7a7fe19..2b0f8543 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/MotorControl/communication.h @@ -14,9 +14,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); extern uint64_t serial_number; 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..c20cf504 --- /dev/null +++ b/Firmware/MotorControl/controller.cpp @@ -0,0 +1,137 @@ + +#include "odrive_main.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", 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; +#ifdef DEBUG_PRINT + 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; +#ifdef DEBUG_PRINT + 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, + * then samples the current required to maintain that position. + * + * This holding current is added as a feedforward term in the control loop. + */ +bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) { + 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 + 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; + } + + // 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..b5767b6f --- /dev/null +++ b/Firmware/MotorControl/controller.hpp @@ -0,0 +1,101 @@ +#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. +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_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] +}; + +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 + 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 + + // 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; + 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, + }; + + // 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] + + // 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 new file mode 100644 index 00000000..ce6bf0b6 --- /dev/null +++ b/Firmware/MotorControl/encoder.cpp @@ -0,0 +1,210 @@ + +//#include "encoder.hpp" +#include "odrive_main.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: disable 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(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; + __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_omega = 4.0f * M_PI; + static const float scan_distance = 16.0f * M_PI; + static const int num_steps = scan_distance / scan_omega * current_meas_hz; + + // go to motor zero phase for start_lock_duration to get ready to scan + int i = 0; + axis_->run_control_loop([&](){ + axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f); + axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); + 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; + + // scan forward + i = 0; + axis_->run_control_loop([&](){ + float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f); + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); + + encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; + + return ++i < num_steps; + }); + 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 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) + { + error_ = ERROR_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 + error_ = ERROR_RESPONSE; + return false; + } + + // scan backwards + i = 0; + axis_->run_control_loop([&](){ + float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f); + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB); + + encvaluesum += (int16_t)hw_config_.timer->Instance->CNT; + + return ++i < num_steps; + }); + if (axis_->error_ != Axis::ERROR_NO_ERROR) + return false; + + int offset = encvaluesum / (num_steps * 2); + config_.offset = offset; + is_calibrated_ = true; + return true; +} + +bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { + index_found_ = false; + float phase = 0.0f; + axis_->run_control_loop([&](){ + phase = wrap_pm_pi(phase + omega * current_meas_period); + + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta); + axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH); + + // continue until the index is found + return !index_found_; + }); + return axis_->error_ == Axis::ERROR_NO_ERROR; +} + +bool Encoder::run_calibration() { + float enc_calibration_voltage; + if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + enc_calibration_voltage = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance; + else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL) + enc_calibration_voltage = axis_->motor_.config_.calibration_current; + else + return false; + + if (config_.use_index && !index_found_) + if (!scan_for_enc_idx( + (float)(axis_->motor_.config_.direction) * config_.idx_search_speed, + enc_calibration_voltage)) + return false; + if (!config_.hand_calibrated) // TODO: discuss what logic we want here + if (!calib_enc_offset(enc_calibration_voltage)) + return false; + 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)) { + 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; + + // 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..cc1701a0 --- /dev/null +++ b/Firmware/MotorControl/encoder.hpp @@ -0,0 +1,78 @@ +#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 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; + float calib_range = 0.02; +}; + +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); + + void setup(); + + void enc_index_cb(); + + 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 + + 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_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), + 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) + ) + ); + } +}; + +#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..29a6c33a 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -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 @@ -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 c1e99742..00000000 --- a/Firmware/MotorControl/low_level.c +++ /dev/null @@ -1,1455 +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, - .manually_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 = {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, - .manually_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 = {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, TimingLog_t log_idx) { - 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 (log_idx < TIMING_LOG_SIZE) { - motor->timing_log[log_idx] = 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, TIMING_LOG_ADC_CB_M1_DC); - - } 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, TIMING_LOG_ADC_CB_M0_I); - - } 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, TIMING_LOG_ADC_CB_M1_I); - - } 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, TIMING_LOG_ADC_CB_M0_DC); - - } 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, TIMING_LOG_MEAS_R); - 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, TIMING_LOG_MEAS_L); - 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; - 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.manually_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, TIMING_LOG_IDX_SEARCH); - 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, TIMING_LOG_FOC_VOLTAGE) < 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, TIMING_LOG_FOC_CURRENT); - 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..b6f36d39 --- /dev/null +++ b/Firmware/MotorControl/low_level.cpp @@ -0,0 +1,271 @@ +/* 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 "odrive_main.hpp" + +/* 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; + +/* 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 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; +} + +// @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 +//-------------------------------- + + +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)) { + disable_all_pwms(Motor::ERROR_ADC_FAILED); + return; + }; + + // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion + // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + Axis& axis = injected ? *axes[0] : *axes[1]; + 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; + 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 + if (current_meas_not_DC_CAL) + axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_I); + else + axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_DC); + + 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_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; + } + } +} + +// @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; + } + 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 * board_config.brake_resistance / vbus_voltage; + + // Duty limit at 90% to allow bootstrap caps to charge + if (brake_duty > 0.9f) brake_duty = 0.9f; + int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; +} diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e92774e4..3105bae0 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -8,279 +8,27 @@ 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 manually_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; - 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 enum { - TIMING_LOG_GENERAL, - TIMING_LOG_ADC_CB_M0_I, - TIMING_LOG_ADC_CB_M0_DC, - TIMING_LOG_ADC_CB_M1_I, - TIMING_LOG_ADC_CB_M1_DC, - TIMING_LOG_MEAS_R, - TIMING_LOG_MEAS_L, - TIMING_LOG_ENC_CALIB, - TIMING_LOG_IDX_SEARCH, - TIMING_LOG_FOC_VOLTAGE, - TIMING_LOG_FOC_CURRENT, -} TimingLog_t; - -typedef struct{ - int type; - int index; -} 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, TimingLog_t log_idx); -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..076d8dac --- /dev/null +++ b/Firmware/MotorControl/main.cpp @@ -0,0 +1,101 @@ + +#include "odrive_main.hpp" +#include "nvm_config.hpp" +#include "communication.h" + +BoardConfig_t board_config; +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]; + +typedef Config ConfigFormat; + +void save_configuration(void) { + if (ConfigFormat::safe_store_config( + &board_config, + &axis_configs, + &motor_configs)) { + //printf("saving configuration failed\r\n"); osDelay(5); + } +} + +void load_configuration(void) { + if (NVM_init() || + ConfigFormat::safe_load_config( + &board_config, + &axis_configs, + &motor_configs)) { + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axis_configs[i] = AxisConfig_t(); + motor_configs[i] = MotorConfig_t(); + } + board_config = BoardConfig_t(); + } +} + +void erase_configuration(void) { + NVM_erase(); +} + +extern "C" { +int odrive_main(void); +void vApplicationStackOverflowHook(void) { for(;;); } +} + +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 HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + if (board_config.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(); + + // 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..75e81202 --- /dev/null +++ b/Firmware/MotorControl/motor.cpp @@ -0,0 +1,358 @@ + +#include + +#include "drv8301.h" +//#include "motor.hpp" +#include "odrive_main.hpp" + + +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, + }) +{ +} + +// @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() { + // disable pwm + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config_.timer); + // set this motor's contribution to 0 + current_control_.Ibus = 0.0f; + update_brake_current(); + // ensure the PWM is not re-enabled without the state machine explicitly + // calling motor.arm() + axis_->missed_control_deadline_ = true; +} + +// @brief 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); +} + +// @brief Checks if the gate driver is in operational state. +// @returns: true if the gate driver is OK (no fault), false otherwise +bool Motor::check_DRV_fault() { + //TODO: make this pin configurable per motor ch + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin); + if (nFAULT_state == GPIO_PIN_RESET) { + // Update DRV Fault Code + 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; +} + +bool Motor::do_checks() { + if (!check_DRV_fault()) { + error_ = ERROR_DRV_FAULT; + return false; + } + return true; +} + +void Motor::log_timing(TimingLog_t log_idx) { + 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 (log_idx < TIMING_LOG_NUM_SLOTS) { + timing_log_[log_idx] = timing; + } +} + +float Motor::phase_current_from_adcval(uint32_t ADCValue) { + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; + float shunt_volt = amp_out_volt * phase_current_rev_gain_; + float current = shunt_volt * hw_config_.shunt_conductance; + return current; +} + +//-------------------------------- +// Measurement and calibration +//-------------------------------- + +// TODO check Ibeta balance to verify good motor connection +bool Motor::measure_phase_resistance(float test_current, float max_voltage) { + static const float kI = 10.0f; // [(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + float test_voltage = 0.0f; + + size_t i = 0; + axis_->run_control_loop([&](){ + float Ialpha = -(current_meas_.phB + current_meas_.phC); + test_voltage += (kI * current_meas_period) * (test_current - Ialpha); + if (test_voltage > max_voltage || test_voltage < -max_voltage) + return error_ = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false; + + // Test voltage along phase A + enqueue_voltage_timings(test_voltage, 0.0f); + log_timing(TIMING_LOG_MEAS_R); + + 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 true; // if we ran to completion that means success +} + +bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { + float test_voltages[2] = {voltage_low, voltage_high}; + float Ialphas[2] = {0.0f}; + static const int num_cycles = 5000; + + size_t t = 0; + axis_->run_control_loop([&](){ + int i = t & 1; + Ialphas[i] += -current_meas_.phB - current_meas_.phC; + + // Test voltage along phase A + enqueue_voltage_timings(test_voltages[i], 0.0f); + log_timing(TIMING_LOG_MEAS_L); + + return ++t < (num_cycles << 1); + }); + if (axis_->error_ != Axis::ERROR_NO_ERROR) + return false; + + //// 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) + return error_ = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false; + return true; +} + + +bool Motor::run_calibration() { + error_ = ERROR_NO_ERROR; + + float R_calib_max_voltage = config_.resistance_calib_max_voltage; + if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage)) + 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; + + 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_valid_ = true; +} + +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); + log_timing(TIMING_LOG_FOC_VOLTAGE); +} + +// 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); + log_timing(TIMING_LOG_FOC_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..caa133e7 --- /dev/null +++ b/Firmware/MotorControl/motor.hpp @@ -0,0 +1,196 @@ +#ifndef __MOTOR_HPP +#define __MOTOR_HPP + +#ifndef __ODRIVE_MAIN_HPP +#error "This file should not be included directly. Include odrive_main.hpp instead." +#endif + +#include "drv8301.h" + +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 { + 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. + 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; + +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, + }; + + enum TimingLog_t { + TIMING_LOG_GENERAL, + TIMING_LOG_ADC_CB_I, + TIMING_LOG_ADC_CB_DC, + TIMING_LOG_MEAS_R, + TIMING_LOG_MEAS_L, + TIMING_LOG_ENC_CALIB, + TIMING_LOG_IDX_SEARCH, + TIMING_LOG_FOC_VOLTAGE, + TIMING_LOG_FOC_CURRENT, + TIMING_LOG_NUM_SLOTS + }; + + Motor(const MotorHardwareConfig_t& hw_config, + const GateDriverHardwareConfig_t& gate_driver_config, + MotorConfig_t& config); + + bool arm(); + void disarm(); + void setup() { + DRV8301_setup(); + } + void DRV8301_setup(); + bool check_DRV_fault(); + bool do_checks(); + void log_timing(TimingLog_t log_idx); + float phase_current_from_adcval(uint32_t ADCValue); + bool measure_phase_resistance(float test_current, float max_voltage); + bool measure_phase_inductance(float voltage_low, float voltage_high); + bool run_calibration(); + 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 + uint16_t next_timings_[3] = { + TIM_1_8_PERIOD_CLOCKS / 2, + TIM_1_8_PERIOD_CLOCKS / 2, + TIM_1_8_PERIOD_CLOCKS / 2 + }; + bool next_timings_valid_ = false; + uint16_t last_cpu_time_ = 0; + int timing_log_index_ = 0; + uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 }; + + // variables exposed on protocol + 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, + .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, + }; + 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", &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), + make_protocol_property("DC_calib_phC", &DC_calib_.phC), + make_protocol_property("shunt_conductance", &shunt_conductance_), + make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_), + make_protocol_object("current_control", + make_protocol_property("p_gain", ¤t_control_.p_gain), + 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", &drv_fault_), + make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), + make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), + make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), + make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) + ), + make_protocol_object("timing_log", + make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]), + make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]), + make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]), + make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]), + make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]), + make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]), + make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]), + make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]), + make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]) + ), + make_protocol_object("config", + 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) + ) + ); + } +}; + +#endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/config.cpp b/Firmware/MotorControl/nvm_config.hpp similarity index 50% rename from Firmware/MotorControl/config.cpp rename to Firmware/MotorControl/nvm_config.hpp index f860381e..7784322b 100644 --- a/Firmware/MotorControl/config.cpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -1,57 +1,38 @@ +/* +* 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 "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 +#define CONFIG_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_manually_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 -----------------------------------------------------*/ + +// 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. @@ -121,11 +102,11 @@ struct Config { // @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) { + 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 = CRC16_INIT ^ config_version; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; if (Config::load_config(0, &crc16, val0, vals..., &crc16)) return -1; if (crc16) @@ -140,14 +121,14 @@ struct Config { // 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) { + 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 = CRC16_INIT ^ config_version; + 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)) @@ -157,96 +138,3 @@ struct Config { 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.manually_calibrated = config->encoder_manually_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_manually_calibrated = motor->encoder.manually_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(); - - // TODO: temporary hack, this is gonna change after refactoring - axis_configs[0] = AxisConfig(); - axis_configs[1] = AxisConfig(); - brake_resistance = 0.47f; - - // 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/odrive_main.hpp b/Firmware/MotorControl/odrive_main.hpp new file mode 100644 index 00000000..730f040d --- /dev/null +++ b/Firmware/MotorControl/odrive_main.hpp @@ -0,0 +1,57 @@ +#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 + +// @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 +#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 const float elec_rad_per_enc; +extern BoardConfig_t board_config; + +constexpr size_t AXIS_COUNT = 2; +extern Axis *axes[AXIS_COUNT]; + + +// ODrive specific includes +#include +#include +#include +#include +#include +#include +#include +#include + +// 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..c0cd07f0 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,75 @@ 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(); +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 +193,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 +211,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 46a59f06..bd853608 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; @@ -87,7 +95,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<> @@ -294,15 +303,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 @@ -319,8 +319,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 @@ -332,11 +331,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 @@ -344,132 +341,86 @@ 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\":\"uint64\",\"access\":\"r\""; } template<> -inline const char* get_default_json_modifier() { +inline constexpr const char* get_default_json_modifier() { return "\"type\":\"uint64\",\"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\""; } 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. * @@ -480,55 +431,349 @@ 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; + 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; + 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 : public 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) + {} + +/* 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) { + //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_; +}; + +// Non-const non-enum types +template::value>> +ProtocolProperty make_protocol_property(const char * name, TProperty* property) { + return ProtocolProperty(name, property); +}; + +// 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 +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 T::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); + + +// 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 new file mode 100644 index 00000000..c1358150 --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -0,0 +1,101 @@ + +//#include "sensorless_estimator.hpp" +#include "odrive_main.hpp" + +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)) { + 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)}; + + // 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_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); + + // 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..569c9a09 --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -0,0 +1,31 @@ +#ifndef __SENSORLESS_ESTIMATOR_HPP +#define __SENSORLESS_ESTIMATOR_HPP + +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 + + // 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; +}; + +#endif /* __SENSORLESS_ESTIMATOR_HPP */ diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index a6a02a5a..269d1e7e 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/README.md b/Firmware/README.md index 540265d6..2888193f 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -43,8 +43,6 @@ __CONFIG_UART_PROTOCOL__: Defines which protocol the ODrive should use on the UA * `ascii`: The ASCII protocol. Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol. * `none`: Disable UART. -__CONFIG_STEP_DIR__: Set to `y` to use the GPIO1 and GPIO2 for step/direction input. Set to `n` otherwise. To use this, `CONFIG_UART_PROTOCOL` must be `none` because UART uses the same pins. -

## Downloading and Installing Tools diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index c6440db0..d71b2238 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -128,12 +128,16 @@ build{ 'Drivers/DRV8301/drv8301.c', '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/communication.cpp', 'MotorControl/protocol.cpp', - 'MotorControl/config.cpp' + 'MotorControl/motor.cpp', + 'MotorControl/encoder.cpp', + 'MotorControl/controller.cpp', + 'MotorControl/sensorless_estimator.cpp', + 'MotorControl/main.cpp' }, includes={ 'Drivers/DRV8301',