diff --git a/Firmware/.gitignore b/Firmware/.gitignore index 4770c39a..c17ea7e8 100644 --- a/Firmware/.gitignore +++ b/Firmware/.gitignore @@ -15,8 +15,11 @@ Odrive.xml .settings/ .project +# VSCode stuff +/.vscode/.cortex-debug.*.state.json + # STM32CubeMX (in case you put it in this folder, or a symlink) STM32CubeMX #gdb log -openocd.log \ No newline at end of file +openocd.log diff --git a/Firmware/Board/v3.3/Inc/gpio.h b/Firmware/Board/v3.3/Inc/gpio.h index 74ce09da..b0e21c22 100644 --- a/Firmware/Board/v3.3/Inc/gpio.h +++ b/Firmware/Board/v3.3/Inc/gpio.h @@ -59,7 +59,7 @@ #include "main.h" /* USER CODE BEGIN Includes */ - +#include /* USER CODE END Includes */ /* USER CODE BEGIN Private defines */ @@ -73,6 +73,10 @@ void MX_GPIO_Init(void); void SetGPIO12toUART(); void SetGPIO12toStepDir(); void SetupENCIndexGPIO(); +bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, + uint32_t pull_up_down, + void (*callback)(void*), void* ctx); +void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); /* USER CODE END Prototypes */ diff --git a/Firmware/Board/v3.3/Inc/main.h b/Firmware/Board/v3.3/Inc/main.h index a9650093..8ba31542 100644 --- a/Firmware/Board/v3.3/Inc/main.h +++ b/Firmware/Board/v3.3/Inc/main.h @@ -54,7 +54,7 @@ #define HW_VERSION_MAJOR 3 #define HW_VERSION_MINOR 4 -// #define HW_VERSION_HIGH_VOLTAGE true +#define HW_VERSION_HIGH_VOLTAGE true #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 diff --git a/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index 467cb745..3e00082a 100644 --- a/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h +++ b/Firmware/Board/v3.3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h @@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t; /// Thread Definition structure contains startup information of a thread. /// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS. typedef struct os_thread_def { - char *name; ///< Thread name + const char *name; ///< Thread name os_pthread pthread; ///< start address of thread function osPriority tpriority; ///< initial thread priority uint32_t instances; ///< maximum number of instances of that thread function diff --git a/Firmware/Board/v3.3/Src/freertos.c b/Firmware/Board/v3.3/Src/freertos.c index 7794558c..601ff3e9 100644 --- a/Firmware/Board/v3.3/Src/freertos.c +++ b/Firmware/Board/v3.3/Src/freertos.c @@ -53,10 +53,11 @@ /* USER CODE BEGIN Includes */ #include "freertos_vars.h" -#include "low_level.h" +//#include "low_level.h" #include "axis_c_interface.h" -#include "commands.h" -#include "config.h" +//#include "commands.h" +//#include "config.h" +int odrive_main(void); /* USER CODE END Includes */ /* Variables -----------------------------------------------------------------*/ @@ -67,8 +68,6 @@ osThreadId defaultTaskHandle; osSemaphoreId sem_usb_irq; // List of threads -osThreadId thread_motor_0; -osThreadId thread_motor_1; osThreadId thread_cmd_parse; /* USER CODE END Variables */ @@ -110,7 +109,7 @@ void MX_FREERTOS_Init(void) { sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1); osSemaphoreWait(sem_usb_rx, 0); // Remove a token. - // Create a semaphore for USB RX + // Create a semaphore for USB TX osSemaphoreDef(sem_usb_tx); sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1); @@ -142,28 +141,7 @@ void StartDefaultTask(void const * argument) /* USER CODE BEGIN StartDefaultTask */ - // Init and load persistent configuration - init_configuration(); - - // Init communications - init_communication(); - - // Init motor control - init_motor_control(); - - // Start motor threads - osThreadDef(task_motor_0, axis_thread_entry, osPriorityHigh+1, 0, 512); - osThreadDef(task_motor_1, axis_thread_entry, osPriorityHigh, 0, 512); - thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]); - thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]); - - // Start command handling thread - osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512); - thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); - - // Start USB interrupt handler thread - osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); - thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); + odrive_main(); //If we get to here, then the default task is done. vTaskDelete(defaultTaskHandle); diff --git a/Firmware/Board/v3.3/Src/gpio.c b/Firmware/Board/v3.3/Src/gpio.c index 693ab7b1..44948a1c 100644 --- a/Firmware/Board/v3.3/Src/gpio.c +++ b/Firmware/Board/v3.3/Src/gpio.c @@ -50,7 +50,7 @@ /* Includes ------------------------------------------------------------------*/ #include "gpio.h" /* USER CODE BEGIN 0 */ -#include "low_level.h" +#include #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 @@ -158,10 +158,44 @@ void MX_GPIO_Init(void) /* USER CODE BEGIN 2 */ #endif // End GPIO Include +// @brief Returns the IRQ number associated with a certain pin. +// Note that all GPIOs with the same pin number map to the same IRQn, +// no matter which port they belong to. +IRQn_Type get_irq_number(uint16_t pin) { + uint16_t pin_number = 0; + while (pin) { + pin >>= 1; + pin_number++; + } + switch (pin_number) { + case 0: return EXTI0_IRQn; + case 1: return EXTI1_IRQn; + case 2: return EXTI2_IRQn; + case 3: return EXTI3_IRQn; + case 4: return EXTI4_IRQn; + case 5: + case 6: + case 7: + case 8: + case 9: return EXTI9_5_IRQn; + case 10: + case 11: + case 12: + case 13: + case 14: + case 15: return EXTI15_10_IRQn; + default: return 0; // impossible + } +} + +// @brief Puts the GPIO's 1 and 2 into UART mode. +// This will disable any interrupt subscribers of these GPIOs. void SetGPIO12toUART() { GPIO_InitTypeDef GPIO_InitStruct; - HAL_NVIC_DisableIRQ(EXTI0_IRQn); + // make sure nothing is hogging the GPIO's + GPIO_unsubscribe(GPIO_1_GPIO_Port, GPIO_1_Pin); + GPIO_unsubscribe(GPIO_2_GPIO_Port, GPIO_2_Pin); GPIO_InitStruct.Pin = GPIO_1_Pin; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; @@ -178,59 +212,73 @@ void SetGPIO12toUART() { HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); } -void SetGPIO12toStepDir() { +// Expected subscriptions: 2x step signal + 2x encoder index signal +#define MAX_SUBSCRIPTIONS 10 +struct subscription_t { + GPIO_TypeDef* GPIO_port; + uint16_t GPIO_pin; + void (*callback)(void*); + void* ctx; +} subscriptions[MAX_SUBSCRIPTIONS] = { 0 }; +size_t n_subscriptions = 0; + +// Sets up the specified GPIO to trigger the specified callback +// on a rising edge of the GPIO. +// @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN +bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, + uint32_t pull_up_down, + void (*callback)(void*), void* ctx) { + + // Register handler (or reuse existing registration) + // TODO: make thread safe + struct subscription_t* subscription = NULL; + for (size_t i = 0; i < n_subscriptions; ++i) { + if (subscriptions[i].GPIO_port == GPIO_port && + subscriptions[i].GPIO_pin == GPIO_pin) + subscription = &subscriptions[i]; + } + if (!subscription) { + if (n_subscriptions >= MAX_SUBSCRIPTIONS) + return false; + subscription = &subscriptions[n_subscriptions++]; + } + + *subscription = (struct subscription_t){ + .GPIO_port = GPIO_port, + .GPIO_pin = GPIO_pin, + .callback = callback, + .ctx = ctx + }; + + // Set up GPIO GPIO_InitTypeDef GPIO_InitStruct; - - GPIO_InitStruct.Pin = GPIO_1_Pin; + GPIO_InitStruct.Pin = GPIO_pin; GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_PULLDOWN; - HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct); + GPIO_InitStruct.Pull = pull_up_down; + HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct); - GPIO_InitStruct.Pin = GPIO_2_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_INPUT; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct); - - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI0_IRQn); + // Enable interrupt + HAL_NVIC_SetPriority(get_irq_number(GPIO_pin), 0, 0); + HAL_NVIC_EnableIRQ(get_irq_number(GPIO_pin)); + return true; } -//TODO: Enable index on only one channel -void SetupENCIndexGPIO(){ - GPIO_InitTypeDef GPIO_InitStruct; - - /*Configure GPIO pins : PAPin PAPin */ - GPIO_InitStruct.Pin = M0_ENC_Z_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(M0_ENC_Z_GPIO_Port, &GPIO_InitStruct); - - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI15_10_IRQn); - - /*Configure GPIO pins : PBPin PBPin */ - GPIO_InitStruct.Pin = M1_ENC_Z_Pin; - GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING; - GPIO_InitStruct.Pull = GPIO_NOPULL; - HAL_GPIO_Init(M1_ENC_Z_GPIO_Port, &GPIO_InitStruct); - - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0); - HAL_NVIC_EnableIRQ(EXTI3_IRQn); +void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { + for (size_t i = 0; i < n_subscriptions; ++i) { + if (subscriptions[i].GPIO_port == GPIO_port && + subscriptions[i].GPIO_pin == GPIO_pin) { + subscriptions[i].callback = NULL; + subscriptions[i].ctx = NULL; + } + } } - //Dispatch processing of external interrupts based on source -void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { - //Step signals for M0 and M1 - if (GPIO_Pin & GPIO_1_Pin || GPIO_Pin & GPIO_3_Pin) { - step_cb(GPIO_Pin); - } else if(GPIO_Pin & M0_ENC_Z_Pin){ - enc_index_cb(GPIO_Pin, 0); - } else if(GPIO_Pin & M1_ENC_Z_Pin){ - enc_index_cb(GPIO_Pin, 1); +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) { + for (size_t i = 0; i < n_subscriptions; ++i) { + if (subscriptions[i].GPIO_pin == GPIO_pin) // TODO: check for port + if (subscriptions[i].callback) + subscriptions[i].callback(subscriptions[i].ctx); } } diff --git a/Firmware/Board/v3.3/Src/stm32f4xx_it.c b/Firmware/Board/v3.3/Src/stm32f4xx_it.c index 064e1036..48450d1b 100644 --- a/Firmware/Board/v3.3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3.3/Src/stm32f4xx_it.c @@ -209,7 +209,7 @@ void ADC_IRQHandler(void) // The HAL's ADC handling mechanism adds many clock cycles of overhead // So we bypass it and handle the logic ourselves. - //@TODO add vbus meaasurement on adc1 here + //@TODO add vbus measurement on adc1 here ADC_IRQ_Dispatch(&hadc1, &vbus_sense_adc_cb); ADC_IRQ_Dispatch(&hadc2, &pwm_trig_adc_cb); ADC_IRQ_Dispatch(&hadc3, &pwm_trig_adc_cb); diff --git a/Firmware/Board/v3.3/Src/syscalls.c b/Firmware/Board/v3.3/Src/syscalls.c index 3ddb5701..d546266c 100644 --- a/Firmware/Board/v3.3/Src/syscalls.c +++ b/Firmware/Board/v3.3/Src/syscalls.c @@ -23,6 +23,7 @@ static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE]; int _write(int file, char* data, int len) { +#if 0 // TODO: revert! //number of bytes written int written = 0; switch (serial_printf_select) { @@ -57,6 +58,8 @@ int _write(int file, char* data, int len) { } return written; +#endif + return len; } void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { diff --git a/Firmware/Board/v3.3/Src/usbd_cdc_if.c b/Firmware/Board/v3.3/Src/usbd_cdc_if.c index b38385f7..ff11152b 100644 --- a/Firmware/Board/v3.3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3.3/Src/usbd_cdc_if.c @@ -274,7 +274,7 @@ static int8_t CDC_Receive_FS (uint8_t* Buf, uint32_t *Len) { /* USER CODE BEGIN 6 */ - set_cmd_buffer(Buf, *Len); + //set_cmd_buffer(Buf, *Len); TODO: revert! osSemaphoreRelease(sem_usb_rx); return (USBD_OK); diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 553c7190..1ee31a0d 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -1,65 +1,239 @@ -#include "axis.h" #include -#include "legacy_commands.h" +#include +#include "gpio.h" -//TODO: goal of refactor is to kick this out completely -extern "C" { -#include "low_level.h" +#include "utils.h" +#include "axis.hpp" + +Axis::Axis(const AxisHardwareConfig_t& hw_config, + AxisConfig_t& config, + Encoder& encoder, + SensorlessEstimator& sensorless_estimator, + Controller& controller, + Motor& motor) + : hw_config(hw_config), + config(config), + encoder(encoder), + sensorless_estimator(sensorless_estimator), + controller(controller), + motor(motor) +{ + encoder.axis = this; + sensorless_estimator.axis = this; + controller.axis = this; + motor.axis = this; } -//TODO: Make it really clear where this is loaded. -AxisConfig axis_configs[2]; //TODO: get a constexpr for num motors - -// C interface -extern "C" { -void axis_thread_entry(void const* temp_motor_ptr) { - Motor_t* motor = (Motor_t*)temp_motor_ptr; - - //TODO: explicit axis number assignment - //for now we search for it - uint8_t ax_number = 0; - while (&motors[ax_number] != motor) - ++ax_number; - - Axis axis(axis_configs[ax_number], ax_number, motor); - axis.StateMachineLoop(); -} -} // extern "C" - -void Axis::SetupLegacyMappings() { - // Legacy reachability from C - legacy_motor_ref_->axis_legacy.enable_control = &enable_control_; - - // override for compatibility with legacy comms paradigm - // TODO next gen comms - exposed_bools[4 * axis_number_ + 1] = &enable_control_; - exposed_bools[4 * axis_number_ + 2] = &do_calibration_; +static void step_cb_wrapper(void* ctx) { + reinterpret_cast(ctx)->step_cb(); } -Axis::Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref) - : axis_number_(axis_number), - enable_control_(config.enable_control_at_start), - do_calibration_(config.do_calibration_at_start), - config_(config), - legacy_motor_ref_(legacy_motor_ref) { - SetupLegacyMappings(); +void Axis::setup() { + encoder.setup(); + motor.setup(); } -void Axis::StateMachineLoop() { +void Axis::start_thread() { + osThreadDef(thread_def, run_state_machine_loop, hw_config.thread_priority, 0, 512); + thread_id = osThreadCreate(osThread(thread_def), this); + thread_id_valid = true; +} + +void Axis::signal_thread(thread_signals sig) { + if (thread_id_valid) + osSignalSet(thread_id, sig); +} + +// step/direction interface +void Axis::step_cb() { + if (enable_step_dir) { + GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config.dir_port, hw_config.dir_pin); + float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; + controller.pos_setpoint += dir * config.counts_per_step; + } +}; + +void Axis::set_step_dir_enabled(bool enable) { + if (enable) { + // Set up the direction GPIO as input + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Pin = hw_config.dir_pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(hw_config.dir_port, &GPIO_InitStruct); + + // Subscribe to rising edges of the step GPIO + GPIO_subscribe(hw_config.step_port, hw_config.step_pin, GPIO_PULLDOWN, + step_cb_wrapper, this); + + enable_step_dir = true; + } else { + enable_step_dir = false; + + // Unsubscribe from step GPIO + GPIO_unsubscribe(hw_config.step_port, hw_config.step_pin); + } +} + +//Returns true if everything is OK (no fault) +bool Axis::check_PSU_brownout() { + if(vbus_voltage < config.dc_bus_brownout_trip_level) + return false; + return true; +} + +// Returns true if everything is ok. Sets motor->error and returns false otherwise. +bool Axis::do_checks() { + if (!motor.check_DRV_fault()) { + motor.error = ERROR_DRV_FAULT; + return false; + } + if (!check_PSU_brownout()) { + motor.error = ERROR_DC_BUS_BROWNOUT; + return false; + } + return true; +} + +bool Axis::run_sensorless_spin_up() { + // Early Spin-up: spiral up current + float x = 0.0f; + run_control_loop([&](){ + float phase = wrap_pm_pi(config.ramp_up_distance * x); + float I_mag = config.spin_up_current * x; + x += current_meas_period / config.ramp_up_time; + if (!motor.update(I_mag, phase)) + return false; + return x < 1.0f; + }); + if (x < 1.0f) + return false; + + // Late Spin-up: accelerate + float vel = config.ramp_up_distance / config.ramp_up_time; + float phase = wrap_pm_pi(config.ramp_up_distance); + run_control_loop([&](){ + vel += config.spin_up_acceleration * current_meas_period; + phase = wrap_pm_pi(phase + vel * current_meas_period); + float I_mag = config.spin_up_current; + if (!motor.update(I_mag, phase)) + return false; + return vel < config.spin_up_target_vel; + }); + return vel >= config.spin_up_target_vel; +} + +// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from. +bool Axis::run_sensorless_control_loop() { + run_control_loop([this](){ + float pos_estimate, vel_estimate, phase, current_setpoint; + + // We update the encoder just in case someone needs the output for testing + encoder.update(nullptr, nullptr, nullptr); + if (!sensorless_estimator.update(&pos_estimate, &vel_estimate, &phase)) + return false; + if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return false; + return motor.update(current_setpoint, phase); + }); + return false; +} + +bool Axis::run_closed_loop_control_loop() { + run_control_loop([this](){ + float pos_estimate, vel_estimate, phase, current_setpoint; + + // We update the sensorless estimator just in case someone needs the output for testing + sensorless_estimator.update(nullptr, nullptr, nullptr); + if (!encoder.update(&pos_estimate, &vel_estimate, &phase)) + return false; + if (!controller.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return false; + return motor.update(current_setpoint, phase); + }); + return false; +} + +bool Axis::run_idle_loop() { + // TODO: allow preemption + for (;;) { + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT; + break; + } + } + return false; +} + +void Axis::run_state_machine_loop() { //TODO: Move this somewhere else + // TODO: respect changes of CPR // Allocate the map for anti-cogging algorithm and initialize all values to 0.0f - int encoder_cpr = legacy_motor_ref_->encoder.encoder_cpr; - legacy_motor_ref_->anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); - if (legacy_motor_ref_->anticogging.cogging_map != NULL) { + int encoder_cpr = encoder.config.cpr; + controller.anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float)); + if (controller.anticogging.cogging_map != NULL) { for (int i = 0; i < encoder_cpr; i++) { - legacy_motor_ref_->anticogging.cogging_map[i] = 0.0f; + controller.anticogging.cogging_map[i] = 0.0f; } } - legacy_motor_ref_->motor_thread = osThreadGetId(); - legacy_motor_ref_->thread_ready = true; + enum AxisState_t { + AXIS_STATE_MOTOR_CALIBRATION, + AXIS_STATE_ENCODER_CALIBRATION, + AXIS_STATE_SENSORLESS_SPINUP, + AXIS_STATE_SENSORLESS_CONTROL, + AXIS_STATE_CLOSED_LOOP_CONTROL, + AXIS_STATE_IDLE + }; + AxisState_t axis_state = AXIS_STATE_MOTOR_CALIBRATION; + + for (;;) { + switch (axis_state) { + case AXIS_STATE_MOTOR_CALIBRATION: + if (!config.enable_motor_calibration || motor.run_calibration()) { + axis_state = AXIS_STATE_ENCODER_CALIBRATION; + } else { + axis_state = AXIS_STATE_IDLE; + } + break; + case AXIS_STATE_ENCODER_CALIBRATION: + if (!config.enable_encoder_calibration || encoder.run_calibration()) { + axis_state = config.enable_control ? + config.sensorless ? + AXIS_STATE_SENSORLESS_SPINUP : + AXIS_STATE_CLOSED_LOOP_CONTROL : + AXIS_STATE_IDLE; + if (axis_state != AXIS_STATE_IDLE) + set_step_dir_enabled(config.enable_step_dir_after_calibration); + } else { + axis_state = AXIS_STATE_IDLE; + } + break; + case AXIS_STATE_SENSORLESS_SPINUP: + if (run_sensorless_spin_up()) { + axis_state = AXIS_STATE_SENSORLESS_CONTROL; + } else { + axis_state = AXIS_STATE_IDLE; + } + break; + case AXIS_STATE_SENSORLESS_CONTROL: + run_sensorless_control_loop(); + axis_state = AXIS_STATE_IDLE; // TODO: restart if desired + break; + case AXIS_STATE_CLOSED_LOOP_CONTROL: + run_closed_loop_control_loop(); + axis_state = AXIS_STATE_IDLE; + break; + case AXIS_STATE_IDLE: + default: + run_idle_loop(); + break; + } + } + +/* bool calibration_ok = false; for (;;) { // Keep rotor estimation up to date while idling @@ -68,30 +242,26 @@ void Axis::StateMachineLoop() { if (do_calibration_) { do_calibration_ = false; - - __HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); // enable pwm outputs - calibration_ok = motor_calibration(legacy_motor_ref_); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); // disables pwm outputs + calibration_ok = motor.do_calibration(); + if (calibration_ok) + calibration_ok = encoder.do_calibration(); } if (calibration_ok && enable_control_) { - legacy_motor_ref_->enable_step_dir = true; - __HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); - - bool spin_up_ok = true; - if (legacy_motor_ref_->rotor_mode == ROTOR_MODE_SENSORLESS) - spin_up_ok = spin_up_sensorless(legacy_motor_ref_); - if (spin_up_ok) - control_motor_loop(legacy_motor_ref_); - - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); - legacy_motor_ref_->enable_step_dir = false; + enable_step_dir = true; + if (rotor_mode == ROTOR_MODE_SENSORLESS) { + bool spin_up_ok = do_sensorless_spin_up(); + if (spin_up_ok) + do_sensorless_control(); + } else { + do_closed_loop_control(); + } if (enable_control_) { // if control is still enabled, we exited because of error calibration_ok = false; enable_control_ = false; } } - } - legacy_motor_ref_->thread_ready = false; -} \ No newline at end of file + }*/ + thread_id_valid = false; +} diff --git a/Firmware/MotorControl/axis.h b/Firmware/MotorControl/axis.h deleted file mode 100644 index 455515b1..00000000 --- a/Firmware/MotorControl/axis.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef __AXIS_HPP -#define __AXIS_HPP - -//TODO: goal of refactor is to kick this out completely -extern "C" { -#include "low_level.h" -} - -//Outside axis: - //command handler - //callback dispatch - -// TODO: decide if we want to consolidate all default configs in one file for ease of use? -struct AxisConfig { - bool enable_control_at_start = true; - bool do_calibration_at_start = true; -}; -extern AxisConfig axis_configs[]; - -class Axis { -public: - //thread/os/system management - //timing log - //thread id - //etc. - //state machine - //control mode - //control_en/calib_ok - //error state - //motor - //current controller - //contains rotor phase logic - //motor level calibration routines - //low_level (implementation specifics) - //DRV driver - //adc callback handling - //pwm queueing - //rotor estimator - //kick out rotor phase logic - //pos/vel controller - //step/dir handler - - // Object operation requires ptr to legacy object for now, TODO: get rid of this dep - Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref); - - // Infinite loop that does calibration and enters main control loop as appropriate - void StateMachineLoop(); - - uint8_t axis_number_; - - bool enable_control_; - bool do_calibration_; - - AxisConfig& config_; - - Motor_t* legacy_motor_ref_; - -private: - void SetupLegacyMappings(); - -}; - -#endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp new file mode 100644 index 00000000..f8bbf79e --- /dev/null +++ b/Firmware/MotorControl/axis.hpp @@ -0,0 +1,209 @@ +#ifndef __AXIS_HPP +#define __AXIS_HPP + +#include +#include +#include + +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include + + +#include + +/*class Estimator { +public: + virtual float get_position_estimation(void); + virtual float get_velocity_estimation(void); +};*/ +/* +class Motor { +public: + // @brief Updates the current control loop + virtual void update(float current_ref); +};*/ + +// The Axis declaration is needed in the other header files +class Axis; + +#include +#include +#include +#include +#include + +//default timeout waiting for phase measurement signals +#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] + +static const float current_meas_period = CURRENT_MEAS_PERIOD; +static const int current_meas_hz = CURRENT_MEAS_HZ; +extern float vbus_voltage; +extern float brake_resistance; // [ohm] + +constexpr size_t AXIS_COUNT = 2; +extern Axis *axes[AXIS_COUNT]; + +/* +class Controller { +public: + // @brief Updates the controller loop(s) + virtual void update(void); +};*/ + + +//Outside axis: + //command handler + //callback dispatch + +typedef enum { + ROTOR_MODE_ENCODER, + ROTOR_MODE_SENSORLESS, + ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing +} Rotor_mode_t; + +// TODO: decide if we want to consolidate all default configs in one file for ease of use? +struct AxisConfig_t { + bool enable_motor_calibration = true; + bool enable_encoder_calibration = true; + bool enable_control = true; + bool sensorless = false; + bool enable_step_dir_after_calibration = true; // For M0 this has no effect if enable_uart is true + float counts_per_step = 2.0f; + float dc_bus_brownout_trip_level = 8.0f; // [V] + Rotor_mode_t rotor_mode = ROTOR_MODE_ENCODER; + + // Spinup settings + float ramp_up_time = 0.4f; // [s] + float ramp_up_distance = 4 * M_PI; // [rad] + float spin_up_current = 10.0f; // [A] + float spin_up_acceleration = 400.0f; // [rad/s^2] + float spin_up_target_vel = 400.0f; // [rad/s] +}; + +class Axis { +public: + //thread/os/system management + //timing log + //thread id + //etc. + //state machine + //control mode + //control_en/calib_ok + //error state + //motor + //current controller + //contains rotor phase logic + //motor level calibration routines + //low_level (implementation specifics) + //DRV driver + //adc callback handling + //pwm queueing + //rotor estimator + //kick out rotor phase logic + //pos/vel controller + //step/dir handler + + enum thread_signals { + M_SIGNAL_PH_CURRENT_MEAS = 1u << 0 + }; + + Axis(const AxisHardwareConfig_t& hw_config, + AxisConfig_t& config, + Encoder& encoder, + SensorlessEstimator& sensorless_estimator, + Controller& controller, + Motor& motor); + + void setup(); + void start_thread(); + void signal_thread(thread_signals sig); + + // Infinite loop that does calibration and enters main control loop as appropriate + void run_state_machine_loop(); + static void run_state_machine_loop(const void* ctx) { + const_cast(reinterpret_cast(ctx))->run_state_machine_loop(); + }; + + void step_cb(); + void set_step_dir_enabled(bool enable); + + bool check_DRV_fault(); + bool check_PSU_brownout(); + bool do_checks(); + + // TODO: check if this uses dynamic memory + + + // @brief Runs the update handler at the frequency of the current measurements. + // + // The loop runs until one of the following conditions: + // - the update handler returns false + // - the current measurement times out + // - do_checks() becomes false + // - update_handler doesn't finish in time + // + // The function arms the motor at the beginning of the control loop and disarms it at + // the end of the control loop. + // Note that if this function returns, this should generally be considered an error condition, + // unless the termination was deliberately caused by the update_handler because it was of the + // opinion that the loop's task was completed. + // @tparam T Must be a callable type that takes no arguments and returns a bool + template + void run_control_loop(const T& update_handler) { + motor.arm(); + while (true /*enable_control*/) { // TODO: check for state change + if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { + motor.error = ERROR_FOC_MEASUREMENT_TIMEOUT; + break; + } + + // Proactively set phase voltages to 0. If the control deadline is missed, + // the voltages will go to zero. + motor.enqueue_voltage_timings(0.0f, 0.0f); + + if (!do_checks()) + break; + + if (!update_handler()) + break; + + update_brake_current(); + + // Check we meet deadlines after queueing + motor.last_cpu_time = motor.check_timing(); + if (!(motor.last_cpu_time < motor.hw_config.control_deadline)) { + motor.error = ERROR_PHASE_RESISTANCE_TIMING; + break; + } + ++loop_counter; + + // TODO: maybe we should just abort automatically as soon as error is set + } + + // We are exiting control: disarm motor, reset Ibus, and update brake current + motor.disarm(); + motor.current_control.Ibus = 0.0f; + update_brake_current(); + } + + bool run_sensorless_spin_up(); + bool run_sensorless_control_loop(); + bool run_closed_loop_control_loop(); + bool run_idle_loop(); + + const AxisHardwareConfig_t& hw_config; + AxisConfig_t& config; + + Encoder& encoder; + SensorlessEstimator& sensorless_estimator; + Controller& controller; + Motor& motor; + + osThreadId thread_id; + volatile bool thread_id_valid = false; + bool enable_step_dir = false; //auto enabled after calibration + uint32_t loop_counter = 0; +}; + +#endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/board_config_v3.3.h b/Firmware/MotorControl/board_config_v3.3.h new file mode 100644 index 00000000..9ef2d2d6 --- /dev/null +++ b/Firmware/MotorControl/board_config_v3.3.h @@ -0,0 +1,112 @@ + +#ifndef __BOARD_CONFIG_H +#define __BOARD_CONFIG_H + +// STM specific includes +#include +#include +#include +#include + +#if HW_VERSION_MAJOR == 3 +#if HW_VERSION_MINOR <= 3 +#define SHUNT_RESISTANCE (675e-6f) +#else +#define SHUNT_RESISTANCE (500e-6f) +#endif +#endif + + +struct AxisHardwareConfig_t { + GPIO_TypeDef* step_port; + uint16_t step_pin; + GPIO_TypeDef* dir_port; + uint16_t dir_pin; + osPriority thread_priority; +}; + +struct EncoderHardwareConfig_t { + TIM_HandleTypeDef* timer; + GPIO_TypeDef* index_port; + uint16_t index_pin; +}; +struct MotorHardwareConfig_t { + TIM_HandleTypeDef* timer; + uint16_t control_deadline; + float shunt_conductance; +}; +struct GateDriverHardwareConfig_t { + SPI_HandleTypeDef* spi; + GPIO_TypeDef* enable_port; + uint16_t enable_pin; + GPIO_TypeDef* nCS_port; + uint16_t nCS_pin; + GPIO_TypeDef* nFAULT_port; + uint16_t nFAULT_pin; +}; +struct BoardHardwareConfig_t { + AxisHardwareConfig_t axis_config; + EncoderHardwareConfig_t encoder_config; + MotorHardwareConfig_t motor_config; + GateDriverHardwareConfig_t gate_driver_config; +}; + +const BoardHardwareConfig_t hw_configs[] = { { + .axis_config = { + .step_port = GPIO_1_GPIO_Port, + .step_pin = GPIO_1_Pin, + .dir_port = GPIO_2_GPIO_Port, + .dir_pin = GPIO_2_Pin, + .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1), + }, + .encoder_config = { + .timer = &htim3, + .index_port = M0_ENC_Z_GPIO_Port, + .index_pin = M0_ENC_Z_Pin, + }, + .motor_config = { + .timer = &htim1, + .control_deadline = TIM_1_8_PERIOD_CLOCKS, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + }, + .gate_driver_config = { + .spi = &hspi3, + // Note: this board has the EN_Gate pin shared! + .enable_port = EN_GATE_GPIO_Port, + .enable_pin = EN_GATE_Pin, + .nCS_port = M0_nCS_GPIO_Port, + .nCS_pin = M0_nCS_Pin, + .nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors + .nFAULT_pin = nFAULT_Pin, + } +},{ + .axis_config = { + .step_port = GPIO_3_GPIO_Port, + .step_pin = GPIO_3_Pin, + .dir_port = GPIO_4_GPIO_Port, + .dir_pin = GPIO_4_Pin, + .thread_priority = osPriorityHigh, + }, + .encoder_config = { + .timer = &htim4, + .index_port = M1_ENC_Z_GPIO_Port, + .index_pin = M1_ENC_Z_Pin, + }, + .motor_config = { + .timer = &htim8, + .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, + .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] + }, + .gate_driver_config = { + .spi = &hspi3, + // Note: this board has the EN_Gate pin shared! + .enable_port = EN_GATE_GPIO_Port, + .enable_pin = EN_GATE_Pin, + .nCS_port = M1_nCS_GPIO_Port, + .nCS_pin = M1_nCS_Pin, + .nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors + .nFAULT_pin = nFAULT_Pin, + } +} }; + +#endif // __BOARD_CONFIG_H diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp index df27c46d..d95646b4 100644 --- a/Firmware/MotorControl/commands.cpp +++ b/Firmware/MotorControl/commands.cpp @@ -1,4 +1,4 @@ - +#if 0 /* Includes ------------------------------------------------------------------*/ // TODO: remove this option @@ -8,7 +8,7 @@ #include "commands.h" #include "low_level.h" -#include "axis.h" +#include "axis.hpp" #include "protocol.hpp" #include "freertos_vars.h" #include "utils.h" @@ -145,7 +145,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain), - Endpoint::make_property("thread_ready", &motors[0].thread_ready), + Endpoint::make_property("thread_id_valid", &motors[0].thread_id_valid), Endpoint::make_property("control_deadline", &motors[0].control_deadline), Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time), Endpoint::make_property("loop_counter", &motors[0].loop_counter), @@ -232,7 +232,7 @@ const Endpoint endpoints[] = { Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain), - Endpoint::make_property("thread_ready", &motors[1].thread_ready), + Endpoint::make_property("thread_id_valid", &motors[1].thread_id_valid), Endpoint::make_property("control_deadline", &motors[1].control_deadline), Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time), Endpoint::make_property("loop_counter", &motors[1].loop_counter), @@ -509,3 +509,4 @@ void usb_update_thread() { vTaskDelete(osThreadGetId()); } +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/config.h b/Firmware/MotorControl/config.h deleted file mode 100644 index 57fafcca..00000000 --- a/Firmware/MotorControl/config.h +++ /dev/null @@ -1,16 +0,0 @@ -#ifndef __CONFIG_H -#define __CONFIG_H - -#ifdef __cplusplus -extern "C" { -#endif - -void init_configuration(void); -void save_configuration(void); -void erase_configuration(void); - -#ifdef __cplusplus -} -#endif - -#endif /* __CONFIG_H */ diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp new file mode 100644 index 00000000..9f385d2d --- /dev/null +++ b/Firmware/MotorControl/controller.cpp @@ -0,0 +1,130 @@ + +#include "axis.hpp" + + +Controller::Controller(ControllerConfig_t& config) : + config(config) +{} + +//-------------------------------- +// Command Handling +//-------------------------------- + +void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) { + pos_setpoint = pos_setpoint; + vel_setpoint = vel_feed_forward; + current_setpoint = current_feed_forward; + config.control_mode = CTRL_MODE_POSITION_CONTROL; +#ifdef DEBUG_PRINT + printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); +#endif +} + +void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) { + vel_setpoint = vel_setpoint; + current_setpoint = current_feed_forward; + config.control_mode = CTRL_MODE_VELOCITY_CONTROL; +#ifdef DEBUG_PRINT + printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); +#endif +} + +void Controller::set_current_setpoint(float current_setpoint) { + current_setpoint = current_setpoint; + config.control_mode = CTRL_MODE_CURRENT_CONTROL; +#ifdef DEBUG_PRINT + printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); +#endif +} + +/* + * This anti-cogging implementation iterates through each encoder position, + * waits for zero velocity & position error, + * then samples the current required to maintain that position. + * + * This holding current is added as a feedforward term in the control loop. + */ +bool Controller::anti_cogging_calibration(float pos_estimate, float vel_estimate) { + if (anticogging.calib_anticogging && anticogging.cogging_map != NULL) { + float pos_err = anticogging.index - pos_estimate; + if (fabsf(pos_err) <= anticogging.calib_pos_threshold && + fabsf(vel_estimate) < anticogging.calib_vel_threshold) { + anticogging.cogging_map[anticogging.index++] = vel_integrator_current; + } + if (anticogging.index < axis->encoder.config.cpr) { // TODO: remove the dependency on encoder CPR + set_pos_setpoint(anticogging.index, 0.0f, 0.0f); + return false; + } else { + anticogging.index = 0; + set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home + anticogging.use_anticogging = true; // We're good to go, enable anti-cogging + anticogging.calib_anticogging = false; + return true; + } + } + return false; +} + +bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { + // Only runs if anticogging.calib_anticogging is true; non-blocking + anti_cogging_calibration(pos_estimate, vel_estimate); + + // Position control + // TODO Decide if we want to use encoder or pll position here + float vel_des = vel_setpoint; + if (config.control_mode >= CTRL_MODE_POSITION_CONTROL) { + float pos_err = pos_setpoint - pos_estimate; + vel_des += config.pos_gain * pos_err; + } + + // Velocity limiting + float vel_lim = config.vel_limit; + if (vel_des > vel_lim) vel_des = vel_lim; + if (vel_des < -vel_lim) vel_des = -vel_lim; + + // Velocity control + float Iq = current_setpoint; + + // Anti-cogging is enabled after calibration + // We get the current position and apply a current feed-forward + // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) + if (anticogging.use_anticogging) { + Iq += anticogging.cogging_map[mod(pos_estimate, axis->encoder.config.cpr)]; + } + + float v_err = vel_des - vel_estimate; + if (config.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + Iq += config.vel_gain * v_err; + } + + // Velocity integral action before limiting + Iq += vel_integrator_current; + + // Current limiting + float Ilim = std::min(axis->motor.config.current_lim, axis->motor.current_control.max_allowed_current); + bool limited = false; + if (Iq > Ilim) { + limited = true; + Iq = Ilim; + } + if (Iq < -Ilim) { + limited = true; + Iq = -Ilim; + } + + // Velocity integrator (behaviour dependent on limiting) + if (config.control_mode < CTRL_MODE_VELOCITY_CONTROL) { + // reset integral if not in use + vel_integrator_current = 0.0f; + } else { + if (limited) { + // TODO make decayfactor configurable + vel_integrator_current *= 0.99f; + } else { + vel_integrator_current += (config.vel_integrator_gain * current_meas_period) * v_err; + } + } + + if (current_setpoint_output) *current_setpoint_output = Iq; + return true; +} diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp new file mode 100644 index 00000000..79dda566 --- /dev/null +++ b/Firmware/MotorControl/controller.hpp @@ -0,0 +1,72 @@ + +// Note: these should be sorted from lowest level of control to +// highest level of control, to allow "<" style comparisons. +typedef enum { + CTRL_MODE_VOLTAGE_CONTROL = 0, + CTRL_MODE_CURRENT_CONTROL = 1, + CTRL_MODE_VELOCITY_CONTROL = 2, + CTRL_MODE_POSITION_CONTROL = 3 +} Motor_control_mode_t; + +struct ControllerConfig_t { + Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t + float pos_gain = 20.0f; // [(counts/s) / counts] + float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_limit = 20000.0f; // [counts/s] +}; + +class Controller { +public: + Controller(ControllerConfig_t& config); + + void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); + void set_vel_setpoint(float vel_setpoint, float current_feed_forward); + void set_current_setpoint(float current_setpoint); + + // TODO: make this more similar to other calibration loops + bool anti_cogging_calibration(float pos_estimate, float vel_estimate); + + bool update(float pos_estimate, float vel_estimate, float* current_setpoint); + + ControllerConfig_t& config; + Axis* axis = nullptr; // set by Axis constructor + + float pos_setpoint = 0.0f; + float vel_setpoint = 0.0f; + // float vel_setpoint = 800.0f; + // float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] + float vel_integrator_current = 0.0f; // [A] + float current_setpoint = 0.0f; // [A] + + typedef struct { + int index; + float *cogging_map; + bool use_anticogging; + bool calib_anticogging; + float calib_pos_threshold; + float calib_vel_threshold; + } Anticogging_t; + Anticogging_t anticogging = { + .index = 0, + .cogging_map = nullptr, + .use_anticogging = false, + .calib_anticogging = false, + .calib_pos_threshold = 1.0f, + .calib_vel_threshold = 1.0f, + }; + + // Cache for remote procedure calls arguments TODO: remove + struct { + float pos_setpoint; + float vel_feed_forward; + float current_feed_forward; + } set_pos_setpoint_args; + struct { + float vel_setpoint; + float current_feed_forward; + } set_vel_setpoint_args; + struct { + float current_setpoint; + } set_current_setpoint_args; +}; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp new file mode 100644 index 00000000..8432cf83 --- /dev/null +++ b/Firmware/MotorControl/encoder.cpp @@ -0,0 +1,204 @@ + +//#include "encoder.hpp" +#include "axis.hpp" + + +Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, + EncoderConfig_t& config) : + hw_config(hw_config), + config(config) +{ + // Calculate encoder pll gains + // This calculation is currently identical to the PLL in SensorlessEstimator + float pll_bandwidth = 1000.0f; // [rad/s] + pll_kp = 2.0f * pll_bandwidth; + + // Critically damped + pll_ki = 0.25f * (pll_kp * pll_kp); +} + +static void enc_index_cb_wrapper(void* ctx) { + reinterpret_cast(ctx)->enc_index_cb(); +} + +void Encoder::setup() { + HAL_TIM_Encoder_Start(hw_config.timer, TIM_CHANNEL_ALL); + GPIO_subscribe(hw_config.index_port, hw_config.index_pin, GPIO_NOPULL, + enc_index_cb_wrapper, this); +} + +//-------------------- +// Hardware Dependent +//-------------------- + +// Triggered when an encoder passes over the "Index" pin +// TODO: only arm index edge interrupt when we know encoder has powered up +// TODO: disarm interrupt once we found the index +void Encoder::enc_index_cb() { + if (!index_found) { + set_count(0); + index_found = true; + } +} + +// Function that sets the current encoder count to a desired 32-bit value. +void Encoder::set_count(uint32_t count) { + // Disable interrupts to make a critical section to avoid race condition + uint32_t prim = __get_PRIMASK(); + __disable_irq(); + state = count; + hw_config.timer->Instance->CNT = count; + pll_pos = (float)count; + __set_PRIMASK(prim); +} + + +// TODO: Do the scan with current, not voltage! +// TODO: add check_timing +bool Encoder::calib_enc_offset(float voltage_magnitude) { + static const float start_lock_duration = 1.0f; + static const float scan_duration = 1.0f; + static const float scan_range = 16.0f * M_PI; + static const size_t num_steps = scan_duration * current_meas_hz; + + // go to motor zero phase for start_lock_duration to get ready to scan + size_t i = 0; + axis->run_control_loop([&](){ + axis->motor.enqueue_voltage_timings(voltage_magnitude, 0.0f); + return ++i < start_lock_duration * current_meas_hz; + }); + + int32_t init_enc_val = (int16_t)hw_config.timer->Instance->CNT; + int64_t encvaluesum = 0; + + // scan forward + i = 0; + axis->run_control_loop([&](){ + float phase = wrap_pm_pi(scan_range * (float)i / (float)num_steps - scan_range / 2.0f); + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + + encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + + return ++i < num_steps; + }); + if (i < num_steps) + return false; + + //TODO avoid recomputing elec_rad_per_enc every time + float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); + float expected_encoder_delta = scan_range / elec_rad_per_enc; + float actual_encoder_delta_abs = fabsf((int16_t)hw_config.timer->Instance->CNT-init_enc_val); + if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config.calib_range) + { + axis->motor.error = ERROR_ENCODER_CPR_OUT_OF_RANGE; + return false; + } + // check direction + if ((int16_t)hw_config.timer->Instance->CNT > init_enc_val + 8) { + // motor same dir as encoder + axis->motor.config.direction = 1; + } else if ((int16_t)hw_config.timer->Instance->CNT < init_enc_val - 8) { + // motor opposite dir as encoder + axis->motor.config.direction = -1; + } else { + // Encoder response error + axis->motor.error = ERROR_ENCODER_RESPONSE; + return false; + } + + // scan backwards + i = 0; + axis->run_control_loop([&](){ + float phase = wrap_pm_pi(-scan_range * (float)i / (float)num_steps + scan_range / 2.0f); + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + + encvaluesum += (int64_t)hw_config.timer->Instance->CNT; + + return ++i < num_steps; + }); + if (i < num_steps) + return false; + + int offset = encvaluesum / (num_steps * 2); + config.offset = offset; + config.calibrated = true; + return true; +} + +bool Encoder::scan_for_enc_idx(float omega, float voltage_magnitude) { + index_found = false; + float phase = 0.0f; + axis->run_control_loop([&](){ + phase = wrap_pm_pi(phase + omega * current_meas_period); + + float v_alpha = voltage_magnitude * arm_cos_f32(phase); + float v_beta = voltage_magnitude * arm_sin_f32(phase); + axis->motor.enqueue_voltage_timings(v_alpha, v_beta); + + // continue until the index is found + return !index_found; + }); + return index_found; +} + +bool Encoder::run_calibration() { + float enc_calibration_voltage; + if (axis->motor.config.motor_type == MOTOR_TYPE_HIGH_CURRENT) + enc_calibration_voltage = axis->motor.config.calibration_current * axis->motor.config.phase_resistance; + else if (axis->motor.config.motor_type == MOTOR_TYPE_GIMBAL) + enc_calibration_voltage = axis->motor.config.calibration_current; + else + return false; + + if (config.use_index && !index_found) + if (!scan_for_enc_idx( + /*(float)(axis->motor.config.direction) * */ config.idx_search_speed, + enc_calibration_voltage)) + return false; + if (!config.calibrated) + if (!calib_enc_offset(enc_calibration_voltage)) + return false; + return true; +} + +bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) { + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp < 1.0f)) { + axis->motor.error = ERROR_CALIBRATION_TIMING; + return false; + } + + // update internal encoder state + int16_t delta_enc = (int16_t)hw_config.timer->Instance->CNT - (int16_t)state; + state += (int32_t)delta_enc; + + // compute electrical phase + int corrected_enc = state % config.cpr; + corrected_enc -= config.offset; + //corrected_enc *= axis->motor.config.direction; TODO: verify if this still works + //TODO avoid recomputing elec_rad_per_enc every time + float elec_rad_per_enc = axis->motor.config.pole_pairs * 2 * M_PI * (1.0f / (float)(config.cpr)); + float ph = elec_rad_per_enc * (float)corrected_enc; + // ph = fmodf(ph, 2*M_PI); + phase = wrap_pm_pi(ph); + + // run pll (for now pll is in units of encoder counts) + // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? + // Predict current pos + pll_pos += current_meas_period * pll_vel; + // discrete phase detector + float delta_pos = (float)(state - (int32_t)floorf(pll_pos)); + // pll feedback + pll_pos += current_meas_period * pll_kp * delta_pos; + pll_vel += current_meas_period * pll_ki * delta_pos; + + // Assign output arguments + if (*pos_estimate) *pos_estimate = pll_pos; + if (*vel_estimate) *vel_estimate = pll_vel; + if (*phase_output) *phase_output = phase; + return true; +} diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp new file mode 100644 index 00000000..856f00e5 --- /dev/null +++ b/Firmware/MotorControl/encoder.hpp @@ -0,0 +1,42 @@ +#ifndef __ENCODER_HPP +#define __ENCODER_HPP + +struct EncoderConfig_t { + bool use_index = false; + bool calibrated = false; + float idx_search_speed = 10.0f; // [rad/s electrical] + int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder, + int32_t offset = 0; + float calib_range = 0.02; +}; + +class Encoder { +public: + Encoder(const EncoderHardwareConfig_t& hw_config, + EncoderConfig_t& config); + + void setup(); + + void enc_index_cb(); + + void set_count(uint32_t count); + bool calib_enc_offset(float voltage_magnitude); + bool scan_for_enc_idx(float omega, float voltage_magnitude); + + bool update(float* pos_estimate, float* vel_estimate, float* phase); + bool run_calibration(); + + const EncoderHardwareConfig_t& hw_config; + EncoderConfig_t& config; + Axis* axis = nullptr; // set by Axis constructor + + volatile bool index_found = false; + int32_t state = 0; + float phase = 0.0f; // [rad] + float pll_pos = 0.0f; // [rad] + float pll_vel = 0.0f; // [rad/s] + float pll_kp = 0.0f; // [rad/s / rad] + float pll_ki = 0.0f; // [(rad/s^2) / rad] +}; + +#endif // __ENCODER_HPP diff --git a/Firmware/MotorControl/example.json b/Firmware/MotorControl/example.json new file mode 100644 index 00000000..7d3c4dc6 --- /dev/null +++ b/Firmware/MotorControl/example.json @@ -0,0 +1,37 @@ +[ + { + "name": "", + "id": 0, + "type": "json" + }, + { + "name": "subscriptions", + "id": 1, + "type": "int32[]" + }, + { + "name": "motor0", + "id": 2, + "type": "tree", + "content": [ + { + "name": "pos_setpoint", + "id": 3, + "type": "float", + "access": "rw" + }, + { + "name": "pos_gain", + "id": 4, + "type": "float", + "access": "rw" + }, + { + "name": "vel_setpoint", + "id": 5, + "type": "float", + "access": "rw" + } + ] + } +] \ No newline at end of file diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c index 0de2acdf..87df7140 100644 --- a/Firmware/MotorControl/legacy_commands.c +++ b/Firmware/MotorControl/legacy_commands.c @@ -1,7 +1,7 @@ /* Includes ------------------------------------------------------------------*/ #include "legacy_commands.h" #include - +#if 0 /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ @@ -88,14 +88,14 @@ int* exposed_ints[] = { }; bool* exposed_bools[] = { - &motors[0].thread_ready, // ro + &motors[0].thread_id_valid, // ro //For now these are written by Axis::SetupLegacyMappings - NULL, // &motors[0].enable_control, // rw - NULL, // &motors[0].do_calibration, // rw + &axis[0].enable_control, // rw + &axis[0].do_calibration, // rw NULL, // &motors[0].calibration_ok, // ro - &motors[1].thread_ready, // ro - NULL, // &motors[1].enable_control, // rw - NULL, // &motors[1].do_calibration, // rw + &motors[1].thread_id_valid, // ro + &axis[1].enable_control, // rw + &axis[1].do_calibration, // rw NULL, // &motors[1].calibration_ok, // ro }; @@ -289,3 +289,4 @@ static void print_monitoring(int limit) { } printf("\n"); } +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.c b/Firmware/MotorControl/low_level.c deleted file mode 100644 index ad06623f..00000000 --- a/Firmware/MotorControl/low_level.c +++ /dev/null @@ -1,1459 +0,0 @@ -/* Includes ------------------------------------------------------------------*/ - -// Because of broken cmsis_os.h, we need to include arm_math first, -// otherwise chip specific defines are ommited -#include -#include // Sets up the correct chip specifc defines required by arm_math -#define ARM_MATH_CM4 -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -/* Private defines -----------------------------------------------------------*/ - -// #define DEBUG_PRINT - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -// This value is updated by the DC-bus reading ADC. -// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late -float vbus_voltage = 12.0f; - -#if HW_VERSION_MAJOR == 3 -#if HW_VERSION_MINOR <= 3 -#define SHUNT_RESISTANCE (675e-6f) -#else -#define SHUNT_RESISTANCE (500e-6f) -#endif -#endif - -// TODO: Migrate to C++, clearly we are actually doing object oriented code here... -// TODO: For nice encapsulation, consider not having the motor objects public - -// NOTE: for gimbal motors, all units of A are instead V. -// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] -// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. -Motor_t motors[] = { - { - // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pole_pairs = 7, // This value is correct for N5065 motors and Turnigy SK3 series. - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - // .vel_setpoint = 800.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] - // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - .dc_bus_brownout_trip_level = 8.0f, // [V] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = TIM_1_8_PERIOD_CLOCKS, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M0_nCS_GPIO_Port, - .nCSgpioNumber = M0_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .motor_type = MOTOR_TYPE_HIGH_CURRENT, - // .motor_type = MOTOR_TYPE_GIMBAL, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .max_allowed_current = 0.0f, - }, - // .rotor_mode = ROTOR_MODE_SENSORLESS, - // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim3, - .use_index = false, - .index_found = false, - .calibrated = false, - .idx_search_speed = 10.0f, // [rad/s electrical] - .encoder_cpr = (2048 * 4), // Default resolution of CUI-AMT102 encoder, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 1, // 1 or -1 - .encoder_calib_range = 0.02, - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .loop_counter = 0, - .timing_log_index = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - .drv_fault = DRV8301_FaultType_NoFault, - }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pole_pairs = 7, // This value is correct for N5065 motors and Turnigy SK3 series. - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - .dc_bus_brownout_trip_level = 8.0f, // [V] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M1_nCS_GPIO_Port, - .nCSgpioNumber = M1_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .motor_type = MOTOR_TYPE_HIGH_CURRENT, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .max_allowed_current = 0.0f, - }, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim4, - .use_index = false, - .index_found = false, - .calibrated = false, - .idx_search_speed = 10.0f, // [rad/s electrical] - .encoder_cpr = (2048 * 4), // Default resolution of CUI-AMT102 encoder, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 1, // 1 or -1 - .encoder_calib_range = 0.02, - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .loop_counter = 0, - .timing_log_index = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - .drv_fault = DRV8301_FaultType_NoFault, - } -}; -const size_t num_motors = sizeof(motors) / sizeof(motors[0]); - -float brake_resistance = 0.47f; // [ohm] - -/* Private constant data -----------------------------------------------------*/ -static const float one_by_sqrt3 = 0.57735026919f; -static const float sqrt3_by_2 = 0.86602540378f; -static const float current_meas_period = CURRENT_MEAS_PERIOD; -static const int current_meas_hz = CURRENT_MEAS_HZ; - -/* Private variables ---------------------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -//-------------------------------- -// Command Handling -//-------------------------------- - -void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - motor->pos_setpoint = pos_setpoint; - motor->vel_setpoint = vel_feed_forward; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_POSITION_CONTROL; -#ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) { - motor->vel_setpoint = vel_setpoint; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_VELOCITY_CONTROL; -#ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_current_setpoint(Motor_t* motor, float current_setpoint) { - motor->current_setpoint = current_setpoint; - motor->control_mode = CTRL_MODE_CURRENT_CONTROL; -#ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); -#endif -} - -//-------------------------------- -// Utility -//-------------------------------- - -uint16_t check_timing(Motor_t* motor) { - TIM_HandleTypeDef* htim = motor->motor_timer; - uint16_t timing = htim->Instance->CNT; - bool down = htim->Instance->CR1 & TIM_CR1_DIR; - if (down) { - uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; - timing = TIM_1_8_PERIOD_CLOCKS + delta; - } - - if (++(motor->timing_log_index) == TIMING_LOG_SIZE) { - motor->timing_log_index = 0; - } - motor->timing_log[motor->timing_log_index] = timing; - - return timing; -} - -void global_fault(int error) { - // Disable motors NOW! - for (int i = 0; i < num_motors; ++i) { - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); - } - // Set fault codes, etc. - for (int i = 0; i < num_motors; ++i) { - motors[i].error = error; - *(motors[i].axis_legacy.enable_control) = false; - } - // disable brake resistor - set_brake_current(0.0f); -} - -float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1 << 11); - float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; - float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; - float current = shunt_volt * motor->shunt_conductance; - return current; -} - -//-------------------------------- -// Initalisation -//-------------------------------- - -// Initalises the low level motor control and then starts the motor control threads -void init_motor_control() { - // Init gate drivers - DRV8301_setup(&motors[0]); - DRV8301_setup(&motors[1]); - - // Start PWM and enable adc interrupts/callbacks - start_adc_pwm(); - - // Start Encoders - HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); - HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); - //TODO: Enable index on only one channel - if (motors[0].encoder.use_index || motors[1].encoder.use_index) { - SetupENCIndexGPIO(); - } - - // Wait for current sense calibration to converge - // TODO make timing a function of calibration filter tau - osDelay(1500); -} - -// Set up the gate drivers -void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); - - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - // 20V/V on 666uOhm gives a range of +/- 110A - // 40V/V on 666uOhm gives a range of +/- 55A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f / 10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f / 20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f / 40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f / 80.0f; - break; - } - - float margin = 0.90f; - float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); - - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); -} - -void start_adc_pwm() { - // Enable ADC and interrupts - __HAL_ADC_ENABLE(&hadc1); - __HAL_ADC_ENABLE(&hadc2); - __HAL_ADC_ENABLE(&hadc3); - // Warp field stabilize. - osDelay(2); - __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); - - // Ensure that debug halting of the core doesn't leave the motor PWM running - __HAL_DBGMCU_FREEZE_TIM1(); - __HAL_DBGMCU_FREEZE_TIM8(); - - start_pwm(&htim1); - start_pwm(&htim8); - // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); - - // Motor output starts in the disabled state - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); - - // Start brake resistor PWM in floating output configuration - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); -} - -void start_pwm(TIM_HandleTypeDef* htim) { - // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS / 2; - htim->Instance->CCR1 = half_load; - htim->Instance->CCR2 = half_load; - htim->Instance->CCR3 = half_load; - - // This hardware obfustication layer really is getting on my nerves - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); - - htim->Instance->CCR4 = 1; - HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); -} - -void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - // Store intial timer configs - uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t CR2_store = htim_a->Instance->CR2; - uint16_t SMCR_store = htim_b->Instance->SMCR; - // Turn off output - htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); - htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); - // Disable both timer counters - htim_a->Instance->CR1 &= ~TIM_CR1_CEN; - htim_b->Instance->CR1 &= ~TIM_CR1_CEN; - // Set first timer to send TRGO on counter enable - htim_a->Instance->CR2 &= ~TIM_CR2_MMS; - htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; - // Set Trigger Source of second timer to the TRGO of the first timer - htim_b->Instance->SMCR &= ~TIM_SMCR_TS; - htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; - // Set 2nd timer to start on trigger - htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; - htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; - // Dir bit is read only in center aligned mode, so we clear the mode for now - uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; - uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; - htim_a->Instance->CR1 &= ~TIM_CR1_CMS; - htim_b->Instance->CR1 &= ~TIM_CR1_CMS; - // Set both timers to up-counting state - htim_a->Instance->CR1 &= ~TIM_CR1_DIR; - htim_b->Instance->CR1 &= ~TIM_CR1_DIR; - // Restore center aligned mode - htim_a->Instance->CR1 |= CMS_store_a; - htim_b->Instance->CR1 |= CMS_store_b; - // set counter offset - htim_a->Instance->CNT = count_offset; - htim_b->Instance->CNT = 0; - // Start Timer a - htim_a->Instance->CR1 |= (TIM_CR1_CEN); - // Restore timer configs - htim_a->Instance->CR2 = CR2_store; - htim_b->Instance->SMCR = SMCR_store; - // restore output - htim_a->Instance->BDTR |= MOE_store_a; - htim_b->Instance->BDTR |= MOE_store_b; -} - -//-------------------------------- -// IRQ Callbacks -//-------------------------------- - -// step/direction interface -void step_cb(uint16_t GPIO_Pin) { - GPIO_PinState dir_pin; - float dir; - switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; - } -} - -// Triggered when an encoder passes over the "Index" pin -// TODO: only arm index edge interrupt when we know encoder has powered up -void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { - Motor_t* motor = &motors[motor_index]; - if (!motor->encoder.index_found) { - setEncoderCount(motor, 0); - motor->encoder.index_found = true; - } - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - if(GPIO_Pin == M0_ENC_Z_Pin){ - HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); - } else { - HAL_NVIC_DisableIRQ(EXTI3_IRQn); - } -} - -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); - // Only one conversion in sequence, so only rank1 - uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - vbus_voltage = ADCValue * voltage_scale; -} - -// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. -// TODO: Document how the phasing is done, link to timing diagram -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { -#define calib_tau 0.2f //@TOTO make more easily configurable - static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; - - // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)) { - global_fault(ERROR_ADC_FAILED); - return; - }; - - // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion - // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion - // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current - // If we are counting down, we just sampled in SVM vector 7, with zero current - Motor_t* motor = injected ? &motors[0] : &motors[1]; - bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - - bool current_meas_not_DC_CAL; - if (motor == &motors[1] && counting_down) { - // We are measuring M1 DC_CAL here - current_meas_not_DC_CAL = false; - // Load next timings for M0 (only once is sufficient) - if (hadc == &hadc2) { - motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; - motors[0].motor_timer->Instance->CCR2 = motors[0].next_timings[1]; - motors[0].motor_timer->Instance->CCR3 = motors[0].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[0] && !counting_down) { - // We are measuring M0 current here - current_meas_not_DC_CAL = true; - // Load next timings for M1 (only once is sufficient) - if (hadc == &hadc2) { - motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; - motors[1].motor_timer->Instance->CCR2 = motors[1].next_timings[1]; - motors[1].motor_timer->Instance->CCR3 = motors[1].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[1] && !counting_down) { - // We are measuring M1 current here - current_meas_not_DC_CAL = true; - // Check the timing of the sequencing - check_timing(motor); - - } else if (motor == &motors[0] && counting_down) { - // We are measuring M0 DC_CAL here - current_meas_not_DC_CAL = false; - // Check the timing of the sequencing - check_timing(motor); - - } else { - global_fault(ERROR_PWM_SRC_FAIL); - return; - } - - uint32_t ADCValue; - if (injected) { - ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - } else { - ADCValue = HAL_ADC_GetValue(hadc); - } - float current = phase_current_from_adcval(motor, ADCValue); - - if (current_meas_not_DC_CAL) { - // ADC2 and ADC3 record the phB and phC currents concurrently, - // and their interrupts should arrive on the same clock cycle. - // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. - // Therefore we store the value from ADC2 and signal the thread that the - // measurement is ready when we receive the ADC3 measurement - - // return or continue - if (hadc == &hadc2) { - motor->current_meas.phB = current - motor->DC_calib.phB; - return; - } else { - motor->current_meas.phC = current - motor->DC_calib.phC; - } - // Trigger motor thread - if (motor->thread_ready) - osSignalSet(motor->motor_thread, M_SIGNAL_PH_CURRENT_MEAS); - } else { - // DC_CAL measurement - if (hadc == &hadc2) { - motor->DC_calib.phB += (current - motor->DC_calib.phB) * calib_filter_k; - } else { - motor->DC_calib.phC += (current - motor->DC_calib.phC) * calib_filter_k; - } - } -} - -//-------------------------------- -// Measurement and calibration -//-------------------------------- - -// TODO check Ibeta balance to verify good motor connection -bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s - float test_voltage = 0.0f; - for (int i = 0; i < num_test_cycles; ++i) { - osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal) { - motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - - float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); - test_voltage += (kI * current_meas_period) * (test_current - Ialpha); - if (test_voltage > max_voltage) test_voltage = max_voltage; - if (test_voltage < -max_voltage) test_voltage = -max_voltage; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltage, 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_RESISTANCE_TIMING; - return false; - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float R = test_voltage / test_current; - motor->phase_resistance = R; - if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { - motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) { - float test_voltages[2] = {voltage_low, voltage_high}; - float Ialphas[2] = {0.0f}; - static const int num_cycles = 5000; - - for (int t = 0; t < num_cycles; ++t) { - for (int i = 0; i < 2; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - - Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltages[i], 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_INDUCTANCE_TIMING; - return false; - } - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float v_L = 0.5f * (voltage_high - voltage_low); - // Note: A more correct formula would also take into account that there is a finite timestep. - // However, the discretisation in the current control loop inverts the same discrepancy - float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); - float L = v_L / dI_by_dt; - - motor->phase_inductance = L; - // TODO arbitrary values set for now - if (L < 1e-6f || L > 500e-6f) { - motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -// TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { - static const float start_lock_duration = 1.0f; - static const int num_steps = 1024*2; - static const float dt_step = 1.0f / 500.0f; - static const float scan_range = 16.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) - - // go to motor zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - queue_voltage_timings(motor, voltage_magnitude, 0.0f); - } - - int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; - int32_t encvaluesum = 0; - - // scan forwards - for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = motor->pole_pairs * 2 * M_PI * (1.0f / (float)(motor->encoder.encoder_cpr)); - float expected_encoder_delta = scan_range / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > motor->encoder.encoder_calib_range) - { - motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; - return false; - } - // check direction - if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { - // motor same dir as encoder - motor->encoder.motor_dir = 1; - } else if ((int16_t)motor->encoder.encoder_timer->Instance->CNT < init_enc_val - 8) { - // motor opposite dir as encoder - motor->encoder.motor_dir = -1; - } else { - // Encoder response error - motor->error = ERROR_ENCODER_RESPONSE; - return false; - } - // scan backwards - for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - int offset = encvaluesum / (num_steps * 2); - motor->encoder.encoder_offset = offset; - motor->encoder.calibrated = true; - return true; -} - -bool motor_calibration(Motor_t* motor) { - motor->error = ERROR_NO_ERROR; - - float R_calib_max_voltage = motor->resistance_calib_max_voltage; - float enc_calibration_voltage = 0.0f; - if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if (!measure_phase_resistance(motor, motor->calibration_current, R_calib_max_voltage)) - return false; - enc_calibration_voltage = motor->calibration_current * motor->phase_resistance; - - if (!measure_phase_inductance(motor, -R_calib_max_voltage, R_calib_max_voltage)) - return false; - } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - enc_calibration_voltage = motor->calibration_current; - } else { - return false; - } - - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (motor->encoder.use_index && !motor->encoder.index_found) - if (!scan_for_enc_idx(motor, - (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed, - enc_calibration_voltage)) - return false; - if (!motor->encoder.calibrated) - if (!calib_enc_offset(motor, enc_calibration_voltage)) - return false; - } - - // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] - motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; - float plant_pole = motor->phase_resistance / motor->phase_inductance; - motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; - - // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] - motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { - motor->error = ERROR_CALIBRATION_TIMING; - return false; - } - // Critically damped - motor->encoder.pll_ki = 0.25f * (motor->encoder.pll_kp * motor->encoder.pll_kp); - - // sensorless pll same as encoder (for now) - motor->sensorless.pll_kp = motor->encoder.pll_kp; - motor->sensorless.pll_ki = motor->encoder.pll_ki; - - return true; -} - -/* - * This anti-cogging implementation iterates through each encoder position, - * waits for zero velocity & position error, - * then samples the current required to maintain that position. - * - * This holding current is added as a feedforward term in the control loop. - */ -bool anti_cogging_calibration(Motor_t* motor) { - if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { - float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { - motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; - } - if (motor->anticogging.index < motor->encoder.encoder_cpr) { - set_pos_setpoint(motor, motor->anticogging.index, 0.0f, 0.0f); - return false; - } else { - motor->anticogging.index = 0; - set_pos_setpoint(motor, 0.0f, 0.0f, 0.0f); // Send the motor home - motor->anticogging.use_anticogging = true; // We're good to go, enable anti-cogging - motor->anticogging.calib_anticogging = false; - return true; - } - } - return false; -} - -//-------------------------------- -// Test functions -//-------------------------------- - -bool scan_for_enc_idx(Motor_t* motor, float omega, float voltage_magnitude) { - for (;;) { - for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { - osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - if (!do_checks(motor)) - return false; - - if (motor->encoder.index_found) - return true; - - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_SCAN_MOTOR_TIMING; - return false; - } - } - } -} - -//-------------------------------- -// Main motor control -//-------------------------------- - -void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { - case ROTOR_MODE_ENCODER: - case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { - //for convenience - Encoder_t* encoder = &motor->encoder; - - // update internal encoder state - int16_t delta_enc = (int16_t)encoder->encoder_timer->Instance->CNT - (int16_t)encoder->encoder_state; - encoder->encoder_state += (int32_t)delta_enc; - - // compute electrical phase - int corrected_enc = encoder->encoder_state % motor->encoder.encoder_cpr; - corrected_enc -= encoder->encoder_offset; - corrected_enc *= encoder->motor_dir; - //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = motor->pole_pairs * 2 * M_PI * (1.0f / (float)(motor->encoder.encoder_cpr)); - float ph = elec_rad_per_enc * (float)corrected_enc; - // ph = fmodf(ph, 2*M_PI); - encoder->phase = wrap_pm_pi(ph); - - // run pll (for now pll is in units of encoder counts) - // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? - // Predict current pos - encoder->pll_pos += current_meas_period * encoder->pll_vel; - // discrete phase detector - float delta_pos = (float)(encoder->encoder_state - (int32_t)floorf(encoder->pll_pos)); - // pll feedback - encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; - encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; - } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; - case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer - // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf - // In particular, equation 8 (and by extension eqn 4 and 6). - - // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle - // is the one computed two cycles ago. To get the correct measurement, it was stored twice: - // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. - - //for convenience - Sensorless_t* sensorless = &motor->sensorless; - - // Clarke transform - float I_alpha_beta[2] = { - -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; - - // alpha-beta vector operations - float eta[2]; - for (int i = 0; i <= 1; ++i) { - // y is the total flux-driving voltage (see paper eqn 4) - float y = -motor->phase_resistance * I_alpha_beta[i] + sensorless->V_alpha_beta_memory[i]; - // flux dynamics (prediction) - float x_dot = y; - // integrate prediction to current timestep - sensorless->flux_state[i] += x_dot * current_meas_period; - - // eta is the estimated permanent magnet flux (see paper eqn 6) - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = sensorless->pm_flux_linkage * sensorless->pm_flux_linkage; - float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; - float bandwidth_factor = 1.0f / (sensorless->pm_flux_linkage * sensorless->pm_flux_linkage); - float eta_factor = 0.5f * (sensorless->observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); - - static float eta_factor_avg_test = 0.0f; - eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); - - // alpha-beta vector operations - for (int i = 0; i <= 1; ++i) { - // add observer action to flux estimate dynamics - float x_dot = eta_factor * eta[i]; - // convert action to discrete-time - sensorless->flux_state[i] += x_dot * current_meas_period; - // update new eta - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Flux state estimation done, store V_alpha_beta for next timestep - sensorless->V_alpha_beta_memory[0] = motor->current_control.final_v_alpha; - sensorless->V_alpha_beta_memory[1] = motor->current_control.final_v_beta; - - // PLL - // predict PLL phase with velocity - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_vel); - // update PLL phase with observer permanent magnet phase - sensorless->phase = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(sensorless->phase - sensorless->pll_pos); - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_kp * delta_phase); - // update PLL velocity - sensorless->pll_vel += current_meas_period * sensorless->pll_ki * delta_phase; - - //TODO TEMP TEST HACK - // static int trigger_ctr = 0; - // if (++trigger_ctr >= 3*current_meas_hz) { - // trigger_ctr = 0; - - // //Change to sensorless units - // motor->vel_gain = 15.0f / 200.0f; - // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; - - // //Change mode - // motor->rotor_mode = ROTOR_MODE_SENSORLESS; - // } - - } break; - default: - //TODO error handling - break; - } -} - -bool using_encoder(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - return true; - else - return false; -} - -bool using_sensorless(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) - return true; - else - return false; -} - -float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.phase; - else if (using_sensorless(motor)) - return motor->sensorless.phase; - else - //TODO error handling - return 0.0f; -} - -float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.pll_vel; - else if (using_sensorless(motor)) - return motor->sensorless.pll_vel; - else - //TODO error handling - return 0.0f; -} - -// Function that sets the current encoder count to a desired 32-bit value. -void setEncoderCount(Motor_t* motor, uint32_t count) { - // Disable interrupts to make a critical section to avoid race condition - uint32_t prim = __get_PRIMASK(); - __disable_irq(); - motor->encoder.encoder_state = count; - motor->encoder.encoder_timer->Instance->CNT = count; - motor->encoder.pll_pos = (float)count; - __set_PRIMASK(prim); -} - -bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { - // wait for new timestep - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_SPIN_UP_TIMEOUT; - return false; - } - - if (!do_checks(motor)) - return false; - // run estimator - if (!loop_updates(motor)) - return false; - - // override the phase during spinup - motor->sensorless.phase = phase; - // run current control (with the phase override) - FOC_current(motor, I_mag, 0.0f); - - return true; -} - -bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; - static const float ramp_up_distance = 4 * M_PI; - float ramp_step = current_meas_period / ramp_up_time; - - float phase = 0.0f; - float vel = ramp_up_distance / ramp_up_time; - float I_mag = 0.0f; - - // spiral up current - for (float x = 0.0f; x < 1.0f; x += ramp_step) { - phase = wrap_pm_pi(ramp_up_distance * x); - I_mag = motor->sensorless.spin_up_current * x; - if (!spin_up_timestep(motor, phase, I_mag)) - return false; - } - - // accelerate - while (vel < motor->sensorless.spin_up_target_vel) { - vel += motor->sensorless.spin_up_acceleration * current_meas_period; - phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - return false; - } - - // // test keep spinning - // while (true) { - // phase = wrap_pm_pi(phase + vel * current_meas_period); - // if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - // return false; - // } - - return true; - - // TODO: check pll vel (abs ratio, 0.8) -} - -void update_brake_current() { - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: set_brake_current will clip negative values to 0.0f - set_brake_current(-Ibus_sum); -} - -void set_brake_current(float brake_current) { - if (brake_current < 0.0f) brake_current = 0.0f; - float brake_duty = brake_current * brake_resistance / vbus_voltage; - - // Duty limit at 90% to allow bootstrap caps to charge - if (brake_duty > 0.9f) brake_duty = 0.9f; - int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - - // Safe update of low and high side timings - // To avoid race condition, first reset timings to safe state - // ch3 is low side, ch4 is high side - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - htim2.Instance->CCR3 = low_off; - htim2.Instance->CCR4 = high_on; -} - -void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) { - float tA, tB, tC; - SVM(mod_alpha, mod_beta, &tA, &tB, &tC); - motor->next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); -} - -void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { - float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); - float mod_alpha = vfactor * v_alpha; - float mod_beta = vfactor * v_beta; - queue_modulation_timings(motor, mod_alpha, mod_beta); -} - -// TODO: This doesn't update brake current -// We should probably make FOC Current call FOC Voltage to avoid duplication. -bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - if (!(check_timing(motor) < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return false; - } - return true; -} - -bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { - Current_control_t* ictrl = &motor->current_control; - - // For Reporting - ictrl->Iq_setpoint = Iq_des; - - // Clarke transform - float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; - float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); - - // Park transform - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float Id = c * Ialpha + s * Ibeta; - float Iq = c * Ibeta - s * Ialpha; - ictrl->Iq_measured = Iq; - - // Current error - float Ierr_d = Id_des - Id; - float Ierr_q = Iq_des - Iq; - - // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) - // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; - - float mod_to_V = (2.0f / 3.0f) * vbus_voltage; - float V_to_mod = 1.0f / mod_to_V; - float mod_d = V_to_mod * Vd; - float mod_q = V_to_mod * Vq; - - // Vector modulation saturation, lock integrator if saturated - // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); - if (mod_scalefactor < 1.0f) { - mod_d *= mod_scalefactor; - mod_q *= mod_scalefactor; - // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; - } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); - } - - // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; - - // Inverse park transform - float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; - - // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; - - // Apply SVM - queue_modulation_timings(motor, mod_alpha, mod_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_TIMING; - return false; - } - - update_brake_current(); - return true; -} - -//Returns true if everything is OK (no fault) -bool check_DRV_fault(Motor_t* motor) { - //TODO: make this pin configurable per motor ch - GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin); - return (nFAULT_state == GPIO_PIN_RESET) ? false : true; -} - -//Returns true if everything is OK (no fault) -bool check_PSU_brownout(Motor_t* motor) { - if(vbus_voltage < motor->dc_bus_brownout_trip_level) - return false; - return true; -} - -// Returns true if everything is ok. Sets motor->error and returns false otherwise. -bool do_checks(Motor_t* motor) { - if (!check_DRV_fault(motor)) { - motor->error = ERROR_DRV_FAULT; - // Update DRV Fault Code - motor->drv_fault = DRV8301_getFaultType(&motor->gate_driver); - // Update/Cache all SPI device registers - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - local_regs->RcvCmd = true; - DRV8301_readData(&motor->gate_driver, local_regs); - return false; - } - if (!check_PSU_brownout(motor)) { - motor->error = ERROR_DC_BUS_BROWNOUT; - return false; - } - return true; -} - -bool loop_updates(Motor_t* motor) { - update_rotor(motor); - return true; -} - -void control_motor_loop(Motor_t* motor) { - while (*(motor->axis_legacy.enable_control)) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; - break; - } - - if (!do_checks(motor)) - break; - if (!loop_updates(motor)) - break; - - // Only runs if anticogging.calib_anticogging is true; non-blocking - anti_cogging_calibration(motor); - - // Position control - // TODO Decide if we want to use encoder or pll position here - float vel_des = motor->vel_setpoint; - if (motor->control_mode >= CTRL_MODE_POSITION_CONTROL) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) { - motor->error = ERROR_POS_CTRL_DURING_SENSORLESS; - break; - } - float pos_err = motor->pos_setpoint - motor->encoder.pll_pos; - vel_des += motor->pos_gain * pos_err; - } - - // Velocity limiting - float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; - if (vel_des < -vel_lim) vel_des = -vel_lim; - - // Velocity control - float Iq = motor->current_setpoint; - - // Anti-cogging is enabled after calibration - // We get the current position and apply a current feed-forward - // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (motor->anticogging.use_anticogging) { - Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, motor->encoder.encoder_cpr)]; - } - - float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += motor->vel_gain * v_err; - } - - // Velocity integral action before limiting - Iq += motor->vel_integrator_current; - - // Apply motor direction correction - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - Iq *= motor->encoder.motor_dir; - } - - // Current limiting - float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); - bool limited = false; - if (Iq > Ilim) { - limited = true; - Iq = Ilim; - } - if (Iq < -Ilim) { - limited = true; - Iq = -Ilim; - } - - // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { - // reset integral if not in use - motor->vel_integrator_current = 0.0f; - } else { - if (limited) { - // TODO make decayfactor configurable - motor->vel_integrator_current *= 0.99f; - } else { - motor->vel_integrator_current += (motor->vel_integrator_gain * current_meas_period) * v_err; - } - } - - // Execute current command - if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current - } - } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_voltage - } - } else { - motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; - break; - } - - ++(motor->loop_counter); - } - - //We are exiting control, reset Ibus, and update brake current - motor->current_control.Ibus = 0.0f; - update_brake_current(); -} diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp new file mode 100644 index 00000000..8c907a5a --- /dev/null +++ b/Firmware/MotorControl/low_level.cpp @@ -0,0 +1,270 @@ +/* Includes ------------------------------------------------------------------*/ + +// Because of broken cmsis_os.h, we need to include arm_math first, +// otherwise chip specific defines are ommited +#include +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 +#include + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +/* Private defines -----------------------------------------------------------*/ + +// #define DEBUG_PRINT + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +// This value is updated by the DC-bus reading ADC. +// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late +float vbus_voltage = 12.0f; + +// TODO: Migrate to C++, clearly we are actually doing object oriented code here... + +float brake_resistance = 0.47f; // [ohm] + +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void start_adc_pwm() { + // Enable ADC and interrupts + __HAL_ADC_ENABLE(&hadc1); + __HAL_ADC_ENABLE(&hadc2); + __HAL_ADC_ENABLE(&hadc3); + // Warp field stabilize. + osDelay(2); + __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); + __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); + + // Ensure that debug halting of the core doesn't leave the motor PWM running + __HAL_DBGMCU_FREEZE_TIM1(); + __HAL_DBGMCU_FREEZE_TIM8(); + + start_pwm(&htim1); + start_pwm(&htim8); + // TODO: explain why this offset + sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); + + // Motor output starts in the disabled state + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); + + // Start brake resistor PWM in floating output configuration + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); + HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); +} + +void global_fault(Error_t error) { + // Disable motors NOW! + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->motor.disarm(); + } + // Set fault codes, etc. + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->motor.error = error; + // TODO: update axis_state + } + // disable brake resistor + set_brake_current(0.0f); +} + +void start_pwm(TIM_HandleTypeDef* htim) { + // Init PWM + int half_load = TIM_1_8_PERIOD_CLOCKS / 2; + htim->Instance->CCR1 = half_load; + htim->Instance->CCR2 = half_load; + htim->Instance->CCR3 = half_load; + + // This hardware obfustication layer really is getting on my nerves + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); + HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); + HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); + + htim->Instance->CCR4 = 1; + HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); +} + +void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, + uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { + // Store intial timer configs + uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); + uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); + uint16_t CR2_store = htim_a->Instance->CR2; + uint16_t SMCR_store = htim_b->Instance->SMCR; + // Turn off output + htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); + htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); + // Disable both timer counters + htim_a->Instance->CR1 &= ~TIM_CR1_CEN; + htim_b->Instance->CR1 &= ~TIM_CR1_CEN; + // Set first timer to send TRGO on counter enable + htim_a->Instance->CR2 &= ~TIM_CR2_MMS; + htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; + // Set Trigger Source of second timer to the TRGO of the first timer + htim_b->Instance->SMCR &= ~TIM_SMCR_TS; + htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; + // Set 2nd timer to start on trigger + htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; + htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; + // Dir bit is read only in center aligned mode, so we clear the mode for now + uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; + uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; + htim_a->Instance->CR1 &= ~TIM_CR1_CMS; + htim_b->Instance->CR1 &= ~TIM_CR1_CMS; + // Set both timers to up-counting state + htim_a->Instance->CR1 &= ~TIM_CR1_DIR; + htim_b->Instance->CR1 &= ~TIM_CR1_DIR; + // Restore center aligned mode + htim_a->Instance->CR1 |= CMS_store_a; + htim_b->Instance->CR1 |= CMS_store_b; + // set counter offset + htim_a->Instance->CNT = count_offset; + htim_b->Instance->CNT = 0; + // Start Timer a + htim_a->Instance->CR1 |= (TIM_CR1_CEN); + // Restore timer configs + htim_a->Instance->CR2 = CR2_store; + htim_b->Instance->SMCR = SMCR_store; + // restore output + htim_a->Instance->BDTR |= MOE_store_a; + htim_b->Instance->BDTR |= MOE_store_b; +} + +//-------------------------------- +// IRQ Callbacks +//-------------------------------- + + +void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { + static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); + // Only one conversion in sequence, so only rank1 + uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + vbus_voltage = ADCValue * voltage_scale; +} + +// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. +// TODO: Document how the phasing is done, link to timing diagram +void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { +#define calib_tau 0.2f //@TOTO make more easily configurable + static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; + + // Ensure ADCs are expected ones to simplify the logic below + if (!(hadc == &hadc2 || hadc == &hadc3)) { + global_fault(ERROR_ADC_FAILED); + return; + }; + + // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion + // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion + // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current + // If we are counting down, we just sampled in SVM vector 7, with zero current + Axis& axis = injected ? *axes[0] : *axes[1]; + Axis& other_axis = injected ? *axes[1] : *axes[0]; + bool counting_down = axis.motor.hw_config.timer->Instance->CR1 & TIM_CR1_DIR; + + bool current_meas_not_DC_CAL = !counting_down; + if (&axis == axes[1] && counting_down) { + // Load next timings for M0 (only once is sufficient) + if (hadc == &hadc2) { + other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; + other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; + other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; + } + } else if (&axis == axes[0] && !counting_down) { + // Load next timings for M1 (only once is sufficient) + if (hadc == &hadc2) { + other_axis.motor.hw_config.timer->Instance->CCR1 = other_axis.motor.next_timings[0]; + other_axis.motor.hw_config.timer->Instance->CCR2 = other_axis.motor.next_timings[1]; + other_axis.motor.hw_config.timer->Instance->CCR3 = other_axis.motor.next_timings[2]; + } + } + + // Check the timing of the sequencing + axis.motor.check_timing(); + + uint32_t ADCValue; + if (injected) { + ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); + } else { + ADCValue = HAL_ADC_GetValue(hadc); + } + float current = axis.motor.phase_current_from_adcval(ADCValue); + + if (current_meas_not_DC_CAL) { + // ADC2 and ADC3 record the phB and phC currents concurrently, + // and their interrupts should arrive on the same clock cycle. + // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. + // Therefore we store the value from ADC2 and signal the thread that the + // measurement is ready when we receive the ADC3 measurement + + // return or continue + if (hadc == &hadc2) { + axis.motor.current_meas.phB = current - axis.motor.DC_calib.phB; + return; + } else { + axis.motor.current_meas.phC = current - axis.motor.DC_calib.phC; + } + // Trigger axis thread + axis.signal_thread(Axis::thread_signals::M_SIGNAL_PH_CURRENT_MEAS); + } else { + // DC_CAL measurement + if (hadc == &hadc2) { + axis.motor.DC_calib.phB += (current - axis.motor.DC_calib.phB) * calib_filter_k; + } else { + axis.motor.DC_calib.phC += (current - axis.motor.DC_calib.phC) * calib_filter_k; + } + } +} + +void update_brake_current() { + float Ibus_sum = 0.0f; + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Ibus_sum += axes[i]->motor.current_control.Ibus; + } + // Note: set_brake_current will clip negative values to 0.0f + set_brake_current(-Ibus_sum); +} + +void set_brake_current(float brake_current) { + if (brake_current < 0.0f) brake_current = 0.0f; + float brake_duty = brake_current * brake_resistance / vbus_voltage; + + // Duty limit at 90% to allow bootstrap caps to charge + if (brake_duty > 0.9f) brake_duty = 0.9f; + int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); + int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; + if (low_off < 0) low_off = 0; + + // Safe update of low and high side timings + // To avoid race condition, first reset timings to safe state + // ch3 is low side, ch4 is high side + htim2.Instance->CCR3 = 0; + htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; + htim2.Instance->CCR3 = low_off; + htim2.Instance->CCR4 = high_on; +} diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e591e778..2b414786 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -8,191 +8,10 @@ extern "C" { /* Includes ------------------------------------------------------------------*/ #include -#include "drv8301.h" - -//default timeout waiting for phase measurement signals -#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] +#include +#include /* Exported types ------------------------------------------------------------*/ -typedef enum { - M_SIGNAL_PH_CURRENT_MEAS = 1u << 0 -} Motor_thread_signals_t; - -typedef struct { - int index; - float *cogging_map; - bool use_anticogging; - bool calib_anticogging; - float calib_pos_threshold; - float calib_vel_threshold; -} Anticogging_t; - -typedef enum { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_TIMING, - ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_TIMING, - ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ENCODER_RESPONSE, - ERROR_ENCODER_MEASUREMENT_TIMEOUT, - ERROR_ADC_FAILED, - ERROR_CALIBRATION_TIMING, - ERROR_FOC_TIMING, - ERROR_FOC_MEASUREMENT_TIMEOUT, - ERROR_SCAN_MOTOR_TIMING, - ERROR_FOC_VOLTAGE_TIMING, - ERROR_GATEDRIVER_INVALID_GAIN, - ERROR_PWM_SRC_FAIL, - ERROR_UNEXPECTED_STEP_SRC, - ERROR_POS_CTRL_DURING_SENSORLESS, - ERROR_SPIN_UP_TIMEOUT, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_ENCODER_CPR_OUT_OF_RANGE, - ERROR_DC_BUS_BROWNOUT, -} Error_t; - -// Note: these should be sorted from lowest level of control to -// highest level of control, to allow "<" style comparisons. -typedef enum { - CTRL_MODE_VOLTAGE_CONTROL = 0, - CTRL_MODE_CURRENT_CONTROL = 1, - CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3 -} Motor_control_mode_t; - -typedef enum { - MOTOR_TYPE_HIGH_CURRENT = 0, - // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented - MOTOR_TYPE_GIMBAL = 2 -} Motor_type_t; - -typedef struct { - float phB; - float phC; -} Iph_BC_t; - -typedef struct { - float current_lim; // [A] - float p_gain; // [V/A] - float i_gain; // [V/As] - float v_current_control_integral_d; // [V] - float v_current_control_integral_q; // [V] - float Ibus; // DC bus current [A] - // Voltage applied at end of cycle: - float final_v_alpha; // [V] - float final_v_beta; // [V] - float Iq_setpoint; - float Iq_measured; - float max_allowed_current; -} Current_control_t; - -typedef enum { - ROTOR_MODE_ENCODER, - ROTOR_MODE_SENSORLESS, - ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing -} Rotor_mode_t; - -typedef struct { - float phase; - float pll_pos; - float pll_vel; - float pll_kp; - float pll_ki; - float observer_gain; // [rad/s] - float flux_state[2]; // [Vs] - float V_alpha_beta_memory[2]; // [V] - float pm_flux_linkage; // [V / (rad/s)] - bool estimator_good; - float spin_up_current; // [A] - float spin_up_acceleration; // [rad/s^2] - float spin_up_target_vel; // [rad/s] -} Sensorless_t; - -typedef struct { - TIM_HandleTypeDef* encoder_timer; - bool use_index; - bool index_found; - bool calibrated; - float idx_search_speed; - int32_t encoder_cpr; - int32_t encoder_offset; - int32_t encoder_state; - int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder. - float encoder_calib_range; - float phase; - float pll_pos; - float pll_vel; - float pll_kp; - float pll_ki; -} Encoder_t; - -typedef struct { - bool* enable_control; -} Axis_legacy_t; - -#define TIMING_LOG_SIZE 16 -typedef struct { - Axis_legacy_t axis_legacy; - Motor_control_mode_t control_mode; - bool enable_step_dir; - float counts_per_step; - Error_t error; - int32_t pole_pairs; - float pos_setpoint; - float pos_gain; - float vel_setpoint; - float vel_gain; - float vel_integrator_gain; - float vel_integrator_current; - float vel_limit; - float current_setpoint; - float calibration_current; - float resistance_calib_max_voltage; - float dc_bus_brownout_trip_level; - float phase_inductance; - float phase_resistance; - osThreadId motor_thread; - bool thread_ready; - // bool enable_control; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true - // bool do_calibration; // trigger motor calibration. will be reset to false after self test - // bool calibration_ok; - TIM_HandleTypeDef* motor_timer; - uint16_t next_timings[3]; - uint16_t control_deadline; - uint16_t last_cpu_time; - Iph_BC_t current_meas; - Iph_BC_t DC_calib; - DRV8301_Obj gate_driver; - DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers - Motor_type_t motor_type; - float shunt_conductance; - float phase_current_rev_gain; //Reverse gain for ADC to Amps - Current_control_t current_control; - Rotor_mode_t rotor_mode; - Encoder_t encoder; - Sensorless_t sensorless; - uint32_t loop_counter; - int timing_log_index; - uint16_t timing_log[TIMING_LOG_SIZE]; - // Cache for remote procedure calls arguments - struct { - float pos_setpoint; - float vel_feed_forward; - float current_feed_forward; - } set_pos_setpoint_args; - struct { - float vel_setpoint; - float current_feed_forward; - } set_vel_setpoint_args; - struct { - float current_setpoint; - } set_current_setpoint_args; - Anticogging_t anticogging; - DRV8301_FaultType_e drv_fault; -} Motor_t; typedef struct{ int type; @@ -200,74 +19,26 @@ typedef struct{ } monitoring_slot; /* Exported constants --------------------------------------------------------*/ -extern const size_t num_motors; extern const float elec_rad_per_enc; /* Exported variables --------------------------------------------------------*/ -extern float vbus_voltage; -extern float brake_resistance; -extern Motor_t motors[]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ //Note: to control without feed forward, set feed forward terms to 0.0f. -void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward); -void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward); -void set_current_setpoint(Motor_t* motor, float current_setpoint); -void step_cb(uint16_t GPIO_Pin); -void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index); void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); -void safe_assert(int arg); -void init_motor_control(); -void setEncoderCount(Motor_t* motor, uint32_t count); - -bool anti_cogging_calibration(Motor_t* motor); - -bool motor_calibration(Motor_t* motor); - - -//// Old private: // Utility -uint16_t check_timing(Motor_t* motor); void global_fault(int error); -float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue); // Initalisation -void DRV8301_setup(Motor_t* motor); void start_adc_pwm(); void start_pwm(TIM_HandleTypeDef* htim); void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset); -// IRQ Callbacks (are all public) -// Measurement and calibrationa -bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage); -bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high); -bool calib_enc_offset(Motor_t* motor, float voltage_magnitude); -bool scan_for_enc_idx(Motor_t* motor, float v_d, float v_q); -bool anti_cogging_calibration(Motor_t* motor); -// Test functions -void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude); -// Main motor control -bool do_checks(Motor_t* motor); -bool loop_updates(Motor_t* motor); -void update_rotor(Motor_t* motor); -bool using_encoder(Motor_t* motor); -bool using_sensorless(Motor_t* motor); -float get_rotor_phase(Motor_t* motor); -float get_pll_vel(Motor_t* motor); -bool spin_up_sensorless(Motor_t* motor); void update_brake_current(); void set_brake_current(float brake_current); -void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta); -void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta); -bool FOC_voltage(Motor_t* motor, float v_d, float v_q); -bool FOC_current(Motor_t* motor, float Id_des, float Iq_des); -void control_motor_loop(Motor_t* motor); - -//motor thread moved to axis object -//void motor_thread(void const * argument); #ifdef __cplusplus } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp new file mode 100644 index 00000000..1a2bb3e9 --- /dev/null +++ b/Firmware/MotorControl/main.cpp @@ -0,0 +1,110 @@ + +#include +#include + + +EncoderConfig_t encoder_configs[AXIS_COUNT]; +ControllerConfig_t controller_configs[AXIS_COUNT]; +MotorConfig_t motor_configs[AXIS_COUNT]; +AxisConfig_t axis_configs[AXIS_COUNT]; +Axis *axes[AXIS_COUNT]; + +bool enable_uart; + +typedef Config ConfigFormat; + +void save_configuration(void) { + if (ConfigFormat::safe_store_config( + &axis_configs, + &motor_configs, + &brake_resistance, + &enable_uart)) { + //printf("saving configuration failed\r\n"); osDelay(5); + } +} + +void load_configuration() { + if (NVM_init() || + ConfigFormat::safe_load_config( + &axis_configs, + &motor_configs, + &brake_resistance, + &enable_uart)) { + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axis_configs[i] = AxisConfig_t(); + motor_configs[i] = MotorConfig_t(); + } + brake_resistance = 0.47f; + enable_uart = true; + } +} + +void erase_configuration(void) { + NVM_erase(); +} + +extern "C" { +int odrive_main(void); +} + +int odrive_main(void) { + // Load persistent configuration (or defaults) + load_configuration(); + + // Construct all objects. + for (size_t i = 0; i < AXIS_COUNT; ++i) { + Encoder *encoder = new Encoder(hw_configs[i].encoder_config, + encoder_configs[i]); + SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(); + Controller *controller = new Controller(controller_configs[i]); + Motor *motor = new Motor(hw_configs[i].motor_config, + hw_configs[i].gate_driver_config, + motor_configs[i]); + axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], + *encoder, *sensorless_estimator, *controller, *motor); + } + + // TODO: make dynamically reconfigurable + if (enable_uart) { + axes[0]->config.enable_step_dir_after_calibration = false; + axes[0]->set_step_dir_enabled(false); + SetGPIO12toUART(); + } +/* + // Init communications (this requires the axis objects to be constructed) + init_communication(); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512); + thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL); + + // Start USB interrupt handler thread + osThreadDef(task_usb_pump, usb_update_thread, osPriorityNormal, 0, 512); + thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL); + */ + + // Setup hardware for all components + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->setup(); + } + + // Start PWM and enable adc interrupts/callbacks + start_adc_pwm(); + + // This delay serves two purposes: + // - Let the current sense calibration converge (the current + // sense interrupts are firing in background by now) + // - Allow a user to interrupt the code, e.g. by flashing a new code, + // before it does anything crazy + // TODO make timing a function of calibration filter tau + osDelay(1500); + + // Start state machine threads. Each thread will go through various calibration + // procedures and then run the actual controller loops. + // TODO: generalize for AXIS_COUNT != 2 + for (size_t i = 0; i < AXIS_COUNT; ++i) { + axes[i]->start_thread(); + } + + return 0; +} diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp new file mode 100644 index 00000000..0e78235f --- /dev/null +++ b/Firmware/MotorControl/motor.cpp @@ -0,0 +1,322 @@ + +#include + +#include "drv8301.h" +//#include "motor.hpp" +#include + + +Motor::Motor(const MotorHardwareConfig_t& hw_config, + const GateDriverHardwareConfig_t& gate_driver_config, + MotorConfig_t& config) : + hw_config(hw_config), + gate_driver_config(gate_driver_config), + config(config), + gate_driver({ + .spiHandle = gate_driver_config.spi, + .EngpioHandle = gate_driver_config.enable_port, + .EngpioNumber = gate_driver_config.enable_pin, + .nCSgpioHandle = gate_driver_config.nCS_port, + .nCSgpioNumber = gate_driver_config.nCS_pin, + }) +{ +} + +void Motor::arm() { + __HAL_TIM_MOE_ENABLE(hw_config.timer); // enable pwm outputs +} + +void Motor::disarm() { + __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(hw_config.timer); // disables pwm outputs +} + +// Set up the gate drivers +void Motor::DRV8301_setup() { + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + + DRV8301_enable(&gate_driver); + DRV8301_setupSpi(&gate_driver, local_regs); + + // TODO we can use reporting only if we actually wire up the nOCTW pin + local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + // Overcurrent set to approximately 150A at 100degC. This may need tweaking. + local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + // 20V/V on 500uOhm gives a range of +/- 150A + // 40V/V on 500uOhm gives a range of +/- 75A + // 20V/V on 666uOhm gives a range of +/- 110A + // 40V/V on 666uOhm gives a range of +/- 55A + local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; + + switch (local_regs->Ctrl_Reg_2.GAIN) { + case DRV8301_ShuntAmpGain_10VpV: + phase_current_rev_gain = 1.0f / 10.0f; + break; + case DRV8301_ShuntAmpGain_20VpV: + phase_current_rev_gain = 1.0f / 20.0f; + break; + case DRV8301_ShuntAmpGain_40VpV: + phase_current_rev_gain = 1.0f / 40.0f; + break; + case DRV8301_ShuntAmpGain_80VpV: + phase_current_rev_gain = 1.0f / 80.0f; + break; + } + + float margin = 0.90f; + float max_input = margin * 0.3f * hw_config.shunt_conductance; + float max_swing = margin * 1.6f * hw_config.shunt_conductance * phase_current_rev_gain; + current_control.max_allowed_current = std::min(max_input, max_swing); + + local_regs->SndCmd = true; + DRV8301_writeData(&gate_driver, local_regs); + local_regs->RcvCmd = true; + DRV8301_readData(&gate_driver, local_regs); +} + +//Returns true if everything is OK (no fault) +bool Motor::check_DRV_fault() { + //TODO: make this pin configurable per motor ch + GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config.nFAULT_port, gate_driver_config.nFAULT_pin); + if (nFAULT_state == GPIO_PIN_RESET) { + // Update DRV Fault Code + drv_fault = DRV8301_getFaultType(&gate_driver); + // Update/Cache all SPI device registers + DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs; + local_regs->RcvCmd = true; + DRV8301_readData(&gate_driver, local_regs); + return false; + }; + return true; +} + +uint16_t Motor::check_timing() { + TIM_HandleTypeDef* htim = hw_config.timer; + uint16_t timing = htim->Instance->CNT; + bool down = htim->Instance->CR1 & TIM_CR1_DIR; + if (down) { + uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; + timing = TIM_1_8_PERIOD_CLOCKS + delta; + } + + if (++(timing_log_index) == TIMING_LOG_SIZE) { + timing_log_index = 0; + } + timing_log[timing_log_index] = timing; + + return timing; +} + +float Motor::phase_current_from_adcval(uint32_t ADCValue) { + int adcval_bal = (int)ADCValue - (1 << 11); + float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; + float shunt_volt = amp_out_volt * phase_current_rev_gain; + float current = shunt_volt * hw_config.shunt_conductance; + return current; +} + +//-------------------------------- +// Measurement and calibration +//-------------------------------- + +// TODO check Ibeta balance to verify good motor connection +bool Motor::measure_phase_resistance(float test_current, float max_voltage) { + static const float kI = 10.0f; // [(V/s)/A] + static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s + float test_voltage = 0.0f; + + size_t i = 0; + axis->run_control_loop([&](){ + float Ialpha = -(current_meas.phB + current_meas.phC); + test_voltage += (kI * current_meas_period) * (test_current - Ialpha); + if (test_voltage > max_voltage || test_voltage < -max_voltage) { + error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; + return false; + } + + // Test voltage along phase A + enqueue_voltage_timings(test_voltage, 0.0f); + + return ++i < num_test_cycles; + }); + + //// De-energize motor + //enqueue_voltage_timings(motor, 0.0f, 0.0f); + + float R = test_voltage / test_current; + config.phase_resistance = R; + return i == num_test_cycles; // if we ran to completion that means success +} + +bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) { + float test_voltages[2] = {voltage_low, voltage_high}; + float Ialphas[2] = {0.0f}; + static const int num_cycles = 5000; + + size_t t = 0; + axis->run_control_loop([&](){ + int i = t & 1; + Ialphas[i] += -current_meas.phB - current_meas.phC; + + // Test voltage along phase A + enqueue_voltage_timings(test_voltages[i], 0.0f); + + return ++t < (num_cycles << 1); + }); + + if (t != (num_cycles << 1)) + return false; // the loop aborted prematurely + + //// De-energize motor + //enqueue_voltage_timings(motor, 0.0f, 0.0f); + + float v_L = 0.5f * (voltage_high - voltage_low); + // Note: A more correct formula would also take into account that there is a finite timestep. + // However, the discretisation in the current control loop inverts the same discrepancy + float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); + float L = v_L / dI_by_dt; + + config.phase_inductance = L; + // TODO arbitrary values set for now + if (L < 1e-6f || L > 500e-6f) { + error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; + return false; + } + return true; +} + + +bool Motor::run_calibration() { + error = ERROR_NO_ERROR; + + float R_calib_max_voltage = config.resistance_calib_max_voltage; + if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if (!measure_phase_resistance(config.calibration_current, R_calib_max_voltage)) + return false; + if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage)) + return false; + } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + // no calibration needed + } else { + return false; + } + + // Calculate current control gains + float current_control_bandwidth = 1000.0f; // [rad/s] + current_control.p_gain = current_control_bandwidth * config.phase_inductance; + float plant_pole = config.phase_resistance / config.phase_inductance; + current_control.i_gain = plant_pole * current_control.p_gain; + + return true; +} + +void Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) { + float tA, tB, tC; + SVM(mod_alpha, mod_beta, &tA, &tB, &tC); + next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); + next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); +} + +void Motor::enqueue_voltage_timings(float v_alpha, float v_beta) { + float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); + float mod_alpha = vfactor * v_alpha; + float mod_beta = vfactor * v_beta; + enqueue_modulation_timings(mod_alpha, mod_beta); +} + +// TODO: This doesn't update brake current +// We should probably make FOC Current call FOC Voltage to avoid duplication. +bool Motor::FOC_voltage(float v_d, float v_q, float phase) { + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float v_alpha = c*v_d - s*v_q; + float v_beta = c*v_q + s*v_d; + enqueue_voltage_timings(v_alpha, v_beta); + return true; +} + +bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { + Current_control_t* ictrl = ¤t_control; + + // For Reporting + ictrl->Iq_setpoint = Iq_des; + + // Clarke transform + float Ialpha = -current_meas.phB - current_meas.phC; + float Ibeta = one_by_sqrt3 * (current_meas.phB - current_meas.phC); + + // Park transform + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; + ictrl->Iq_measured = Iq; + + // Current error + float Ierr_d = Id_des - Id; + float Ierr_q = Iq_des - Iq; + + // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) + // Apply PI control + float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; + float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + + float mod_to_V = (2.0f / 3.0f) * vbus_voltage; + float V_to_mod = 1.0f / mod_to_V; + float mod_d = V_to_mod * Vd; + float mod_q = V_to_mod * Vq; + + // Vector modulation saturation, lock integrator if saturated + // TODO make maximum modulation configurable + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { + mod_d *= mod_scalefactor; + mod_q *= mod_scalefactor; + // TODO make decayfactor configurable + ictrl->v_current_control_integral_d *= 0.99f; + ictrl->v_current_control_integral_q *= 0.99f; + } else { + ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); + ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + } + + // Compute estimated bus current + ictrl->Ibus = mod_d * Id + mod_q * Iq; + + // Inverse park transform + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; + + // Report final applied voltage in stationary frame (for sensorles estimator) + ictrl->final_v_alpha = mod_to_V * mod_alpha; + ictrl->final_v_beta = mod_to_V * mod_beta; + + // Apply SVM + enqueue_modulation_timings(mod_alpha, mod_beta); + + update_brake_current(); + return true; +} + + +bool Motor::update(float current_setpoint, float phase) { + current_setpoint *= config.direction; + phase *= config.direction; + + // Execute current command + // TODO: move this into the mot + if (config.motor_type == MOTOR_TYPE_HIGH_CURRENT) { + if(!FOC_current(0.0f, current_setpoint, phase)){ + return false; + } + } else if (config.motor_type == MOTOR_TYPE_GIMBAL) { + //In gimbal motor mode, current is reinterptreted as voltage. + if(!FOC_voltage(0.0f, current_setpoint, phase)) + return false; + } else { + error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; + return false; + } + return true; +} diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp new file mode 100644 index 00000000..3825b012 --- /dev/null +++ b/Firmware/MotorControl/motor.hpp @@ -0,0 +1,147 @@ +#ifndef __MOTOR_HPP +#define __MOTOR_HPP + +// The Motor declaration is needed in the axis header +//class Motor; +#include + +#include "drv8301.h" + +typedef enum { + ERROR_NO_ERROR, + ERROR_PHASE_RESISTANCE_TIMING, + ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT, + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, + ERROR_PHASE_INDUCTANCE_TIMING, + ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, + ERROR_ENCODER_RESPONSE, + ERROR_ENCODER_MEASUREMENT_TIMEOUT, + ERROR_ADC_FAILED, + ERROR_CALIBRATION_TIMING, + ERROR_FOC_TIMING, + ERROR_FOC_MEASUREMENT_TIMEOUT, + ERROR_SCAN_MOTOR_TIMING, + ERROR_FOC_VOLTAGE_TIMING, + ERROR_GATEDRIVER_INVALID_GAIN, + ERROR_PWM_SRC_FAIL, + ERROR_UNEXPECTED_STEP_SRC, + ERROR_POS_CTRL_DURING_SENSORLESS, + ERROR_SPIN_UP_TIMEOUT, + ERROR_DRV_FAULT, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, + ERROR_ENCODER_CPR_OUT_OF_RANGE, + ERROR_DC_BUS_BROWNOUT, +} Error_t; + +typedef enum { + MOTOR_TYPE_HIGH_CURRENT = 0, + // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented + MOTOR_TYPE_GIMBAL = 2 +} Motor_type_t; + +typedef struct { + float phB; + float phC; +} Iph_BC_t; + +typedef struct { + float p_gain; // [V/A] + float i_gain; // [V/As] + float v_current_control_integral_d; // [V] + float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] + // Voltage applied at end of cycle: + float final_v_alpha; // [V] + float final_v_beta; // [V] + float Iq_setpoint; + float Iq_measured; + float max_allowed_current; +} Current_control_t; + +// NOTE: for gimbal motors, all units of A are instead V. +// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] +// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. +typedef struct { + int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series. + float calibration_current = 10.0f; // [A] + float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. + float phase_inductance = 0.0f; // to be set by measure_phase_inductance + float phase_resistance = 0.0f; // to be set by measure_phase_resistance + int32_t direction = 1; // 1 or -1 + Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT; + + // Read out max_allowed_current to see max supported value for current_lim. + // You can change DRV8301_ShuntAmpGain to get a different range. + // float current_lim = 75.0f; //[A] + float current_lim = 10.0f; //[A] +} MotorConfig_t; + +#define TIMING_LOG_SIZE 16 + +class Motor { +public: + Motor(const MotorHardwareConfig_t& hw_config, + const GateDriverHardwareConfig_t& gate_driver_config, + MotorConfig_t& config); + + void arm(); + void disarm(); + void setup() { + DRV8301_setup(); + } + void DRV8301_setup(); + bool check_DRV_fault(); + uint16_t check_timing(); + float phase_current_from_adcval(uint32_t ADCValue); + bool measure_phase_resistance(float test_current, float max_voltage); + bool measure_phase_inductance(float voltage_low, float voltage_high); + bool run_calibration(); + void enqueue_modulation_timings(float mod_alpha, float mod_beta); + void enqueue_voltage_timings(float v_alpha, float v_beta); + bool FOC_voltage(float v_d, float v_q, float phase); + bool FOC_current(float Id_des, float Iq_des, float phase); + bool update(float current_setpoint, float phase); + + const MotorHardwareConfig_t& hw_config; + const GateDriverHardwareConfig_t gate_driver_config; + MotorConfig_t& config; + Axis* axis = nullptr; // set by Axis constructor + +//private: + + DRV8301_Obj gate_driver; // initialized in constructor + + Error_t error = ERROR_NO_ERROR; + // bool enable_control = true; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true + // bool do_calibration = true; // trigger motor calibration. will be reset to false after self test + // bool calibration_ok = false; + uint16_t next_timings[3] = { + TIM_1_8_PERIOD_CLOCKS / 2, + TIM_1_8_PERIOD_CLOCKS / 2, + TIM_1_8_PERIOD_CLOCKS / 2 + }; + uint16_t last_cpu_time = 0; + Iph_BC_t current_meas = {0.0f, 0.0f}; + Iph_BC_t DC_calib = {0.0f, 0.0f}; + DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers (initialized by DRV8301_setup) + float shunt_conductance = 1.0f / SHUNT_RESISTANCE; //[S] + float phase_current_rev_gain = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) + Current_control_t current_control = { + .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement + .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement + .v_current_control_integral_d = 0.0f, + .v_current_control_integral_q = 0.0f, + .Ibus = 0.0f, + .final_v_alpha = 0.0f, + .final_v_beta = 0.0f, + .Iq_setpoint = 0.0f, + .Iq_measured = 0.0f, + .max_allowed_current = 0.0f, + }; + int timing_log_index = 0; + uint16_t timing_log[TIMING_LOG_SIZE] = { 0 }; + DRV8301_FaultType_e drv_fault = DRV8301_FaultType_NoFault; +}; + +#endif // __MOTOR_HPP diff --git a/Firmware/MotorControl/nvm_config.hpp b/Firmware/MotorControl/nvm_config.hpp new file mode 100644 index 00000000..bf6c6415 --- /dev/null +++ b/Firmware/MotorControl/nvm_config.hpp @@ -0,0 +1,142 @@ +/* +* Convenience functions to load and store multiple objects from and to NVM. +* +* The NVM stores consecutive one-to-one copies of arbitrary objects. +* The types of these objects are passed as template arguments to Config. +*/ + +/* Includes ------------------------------------------------------------------*/ + +#include +#include +#include + +#include "nvm.h" +#include "crc.hpp" +#include "low_level.h" +#include "axis.hpp" + + +/* Private defines -----------------------------------------------------------*/ +#define CONFIG_CRC16_INIT 0xabcd + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ + +// IMPORTANT: if you change, reorder or otherwise modify any of the fields in +// the config structs, make sure to increment this number: +static constexpr uint16_t config_version = 0x0001; + +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + + +// @brief Manages configuration load and store operations from and to NVM +// +// The NVM stores consecutive one-to-one copies of arbitrary objects. +// The types of these objects are passed as template arguments to Config. +// +// Config has two template specializations to implement template recursion: +// - Config handles loading/storing of the first object (type T) and leaves +// the rest of the objects to an "inner" class Config. +// - Config<> represents the leaf of the recursion. +template +struct Config; + +template<> +struct Config<> { + static size_t get_size() { + return 0; + } + static int load_config(size_t offset, uint16_t* crc16) { + return 0; + } + static int store_config(size_t offset, uint16_t* crc16) { + return 0; + } +}; + +template +struct Config { + static size_t get_size() { + return sizeof(T) + Config::get_size(); + } + + // @brief Loads one or more consecutive objects from the NVM. + // During loading this function also calculates the CRC over the loaded data. + // @param offset: 0 means that the function should start reading at the beginning + // of the last comitted NVM block + // @param crc16: the result of the CRC calculation is written to this address + // @param val0, vals: the values to be loaded + static int load_config(size_t offset, uint16_t* crc16, T* val0, Ts* ... vals) { + size_t size = sizeof(T); + // save current CRC (in case val0 and crc16 point to the same address) + size_t previous_crc16 = *crc16; + if (NVM_read(offset, (uint8_t *)val0, size)) + return -1; + *crc16 = calc_crc16(previous_crc16, (uint8_t *)val0, size); + if (Config::load_config(offset + size, crc16, vals...)) + return -1; + return 0; + } + + // @brief Stores one or more consecutive objects to the NVM. + // During storing this function also calculates the CRC over the stored data. + // @param offset: 0 means that the function should start writing at the beginning + // of the currently active NVM write block + // @param crc16: the result of the CRC calculation is written to this address + // @param val0, vals: the values to be stored + static int store_config(size_t offset, uint16_t* crc16, const T* val0, const Ts* ... vals) { + size_t size = sizeof(T); + if (NVM_write(offset, (uint8_t *)val0, size)) + return -1; + // update CRC _after_ writing (in case val0 and crc16 point to the same address) + if (crc16) + *crc16 = calc_crc16(*crc16, (uint8_t *)val0, size); + if (Config::store_config(offset + size, crc16, vals...)) + return -1; + return 0; + } + + // @brief Loads one or more consecutive objects from the NVM. The loaded data + // is validated using a CRC value that is stored at the beginning of the data. + static int safe_load_config(T* val0, Ts* ... vals) { + //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5); + if (Config::get_size() > NVM_get_max_read_length()) + return -1; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; + if (Config::load_config(0, &crc16, val0, vals..., &crc16)) + return -1; + if (crc16) + return -1; + return 0; + } + + // @brief Stores one or more consecutive objects to the NVM. In addition to the + // provided objects, a CRC of the data is stored. + // + // The CRC includes a version number and thus adds some protection against + // changes of the config structs during firmware update. Note that if the total + // config data length changes, the CRC validation will fail even if the developer + // forgets to update the config version number. + static int safe_store_config(const T* val0, const Ts* ... vals) { + size_t size = Config::get_size() + 2; + //printf("config is %d bytes\r\n", size); osDelay(5); + if (size > NVM_get_max_write_length()) + return -1; + if (NVM_start_write(size)) + return -1; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; + if (Config::store_config(0, &crc16, val0, vals...)) + return -1; + if (Config::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16)) + return -1; + if (NVM_commit()) + return -1; + return 0; + } +}; diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp new file mode 100644 index 00000000..664ed9e7 --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -0,0 +1,101 @@ + +//#include "sensorless_estimator.hpp" +#include + +SensorlessEstimator::SensorlessEstimator() +{ + // Calculate pll gains + // This calculation is currently identical to the PLL in Encoder + float pll_bandwidth = 1000.0f; // [rad/s] + pll_kp = 2.0f * pll_bandwidth; + + // Critically damped + pll_ki = 0.25f * (pll_kp * pll_kp); +} + +bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) { + // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer + // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf + // In particular, equation 8 (and by extension eqn 4 and 6). + + // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle + // is the one computed two cycles ago. To get the correct measurement, it was stored twice: + // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. + + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp < 1.0f)) { + axis->motor.error = ERROR_CALIBRATION_TIMING; + return false; + } + + // Clarke transform + float I_alpha_beta[2] = { + -axis->motor.current_meas.phB - axis->motor.current_meas.phC, + one_by_sqrt3 * (axis->motor.current_meas.phB - axis->motor.current_meas.phC)}; + + // alpha-beta vector operations + float eta[2]; + for (int i = 0; i <= 1; ++i) { + // y is the total flux-driving voltage (see paper eqn 4) + float y = -axis->motor.config.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory[i]; + // flux dynamics (prediction) + float x_dot = y; + // integrate prediction to current timestep + flux_state[i] += x_dot * current_meas_period; + + // eta is the estimated permanent magnet flux (see paper eqn 6) + eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + } + + // Non-linear observer (see paper eqn 8): + float pm_flux_sqr = pm_flux_linkage * pm_flux_linkage; + float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; + float bandwidth_factor = 1.0f / (pm_flux_linkage * pm_flux_linkage); + float eta_factor = 0.5f * (observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); + + static float eta_factor_avg_test = 0.0f; + eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); + + // alpha-beta vector operations + for (int i = 0; i <= 1; ++i) { + // add observer action to flux estimate dynamics + float x_dot = eta_factor * eta[i]; + // convert action to discrete-time + flux_state[i] += x_dot * current_meas_period; + // update new eta + eta[i] = flux_state[i] - axis->motor.config.phase_inductance * I_alpha_beta[i]; + } + + // Flux state estimation done, store V_alpha_beta for next timestep + V_alpha_beta_memory[0] = axis->motor.current_control.final_v_alpha; + V_alpha_beta_memory[1] = axis->motor.current_control.final_v_beta; + + // PLL + // TODO: the PLL part has some code duplication with the encoder PLL + // predict PLL phase with velocity + pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_vel); + // update PLL phase with observer permanent magnet phase + phase = fast_atan2(eta[1], eta[0]); + float delta_phase = wrap_pm_pi(phase - pll_pos); + pll_pos = wrap_pm_pi(pll_pos + current_meas_period * pll_kp * delta_phase); + // update PLL velocity + pll_vel += current_meas_period * pll_ki * delta_phase; + + //TODO TEMP TEST HACK + // static int trigger_ctr = 0; + // if (++trigger_ctr >= 3*current_meas_hz) { + // trigger_ctr = 0; + + // //Change to sensorless units + // motor->vel_gain = 15.0f / 200.0f; + // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; + + // //Change mode + // motor->rotor_mode = ROTOR_MODE_SENSORLESS; + // } + + if (pos_estimate) *pos_estimate = pll_pos; + if (vel_estimate) *vel_estimate = pll_vel; + if (phase_output) *phase_output = phase; + return true; +}; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp new file mode 100644 index 00000000..9f276aa3 --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -0,0 +1,24 @@ +#ifndef __SENSORLESS_ESTIMATOR_HPP +#define __SENSORLESS_ESTIMATOR_HPP + +class SensorlessEstimator { +public: + SensorlessEstimator(); + + bool update(float* pos_estimate, float* vel_estimate, float* phase); + + Axis* axis = nullptr; // set by Axis constructor + + float phase = 0.0f; // [rad] + float pll_pos = 0.0f; // [rad] + float pll_vel = 0.0f; // [rad/s] + float pll_kp = 0.0f; // [rad/s / rad] + float pll_ki = 0.0f; // [(rad/s^2) / rad] + float observer_gain = 1000.0f; // [rad/s] + float flux_state[2] = {0.0f, 0.0f}; // [Vs] + float V_alpha_beta_memory[2] = {0.0f, 0.0f}; // [V] + float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } + bool estimator_good = false; +}; + +#endif /* __SENSORLESS_ESTIMATOR_HPP */ diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 032a996a..741810ed 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -4,8 +4,6 @@ #include #include -static const float one_by_sqrt3 = 0.57735026919f; -static const float two_by_sqrt3 = 1.15470053838f; int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { int Sextant; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index bf329605..a4d7a6e0 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -78,6 +78,10 @@ extern "C" { #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +static const float one_by_sqrt3 = 0.57735026919f; +static const float two_by_sqrt3 = 1.15470053838f; +static const float sqrt3_by_2 = 0.86602540378f; + // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index dec05271..e3908cde 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -65,12 +65,16 @@ build{ sources={ 'MotorControl/utils.c', 'MotorControl/legacy_commands.c', - 'MotorControl/low_level.c', + 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', 'MotorControl/commands.cpp', 'MotorControl/protocol.cpp', - 'MotorControl/config.cpp' + 'MotorControl/motor.cpp', + 'MotorControl/encoder.cpp', + 'MotorControl/controller.cpp', + 'MotorControl/sensorless_estimator.cpp', + 'MotorControl/main.cpp' }, includes={ 'MotorControl'