Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-18 01:09:22 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Refactor control loop.

Browse Source 
Please see https://github.com/madcowswe/ODrive/issues/472 for a detailed description.
This commit is contained in:
Samuel Sadok
2020-09-01 00:03:01 +02:00
parent 86be15086f
commit 5661a3b5ed
67 changed files with 2587 additions and 2075 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+34
View File
@@ -7,6 +7,20 @@ Please add a note of your changes below this heading if you make a Pull Request.
* Make NVM configuration code more dynamic so that the layout doesn't have to be known at compile time.
* GPIO initialization logic was changed. GPIOs now need to be explicitly set to the mode corresponding to the feature that they are used by. See `<odrv>.config.gpioX_mode`.
* Previously, if two components used the same interrupt pin (e.g. step input for axis0 and axis1) then the one that was configured later would override the other one. Now this is no longer the case (the old component remains the owner of the pin).
* New control loop architecture:
1. TIM8 update interrupt handler (CNT = 0) runs at a high priority and invokes the system level function `sample_cb()` to sample all timing critical inputs (currently only encoder state).
2. TIM8 update interrupt handler (CNT = 0) raises an NVIC flag to kick off a lower priority interrupt.
3. The control loop interrupt handler checks if all ADC measurements are ready and informs both motor objects about the current measurements.
4. The control loop interrupt handler invokes the system level function `control_loop_cb()` which updates all components (encoders, estimators, torque controllers, etc). The data paths between the components are configured by the Axis threads based on the requested state. This replaces the previous architecture where the components were updated inside the Axis threads in `Axis::run_control_loop()`.
5. Meanwhile the TIM1 and TIM8 updates for CNT = 3500 will have fired. The control loop interrupt handler thus reads the new ADC measurements and informs both motor objects that a DC calibration event has happened.
6. Finally, the control loop interrupt invokes `pwm_update_cb` on both motors to make them update their PWM timing registers.
* Components that need low level control over PWM timings are implemented by inheriting from the `PhaseControlLaw` interface. Three components currently inherit this interface: `FieldOrientedController`, `ResistanceMeasurementControlLaw` and `InductanceMeasurementControlLaw`.
* The FOC algorithm is now found in foc.cpp and and is presumably capable of running at a different frequency than the main control tasks (not relevant for ODrive v3).
* Async estimator was consolidated into a separate component `<odrv>.async_estimator`.
* The Automatic Output Enable (AOE) flag of TIM1/TIM8 is used to achieve glitch-free motor arming.
* Sensorless mode was merged into closed loop control mode. Use `<axis>.enable_sensorless_mode` to disable the use of an encoder.
* More informative profiling instrumentation was added.
* A system-level error property was introduced.
### API Miration Notes
@@ -14,6 +28,26 @@ Please add a note of your changes below this heading if you make a Pull Request.
* `enable_i2c_instead_of_can` was replaced by the separate settings `enable_i2c0` and `enable_can0`.
* `<axis>.motor.gate_driver` was moved to `<axis>.gate_driver`.
* `<axis>.min_endstop.pullup` and `<axis>.max_endstop.pullup` were removed. Use `<odrv>.config.gpioX_mode = GPIO_MODE_DIGITAL / GPIO_MODE_DIGITAL_PULL_UP / GPIO_MODE_DIGITAL_PULL_DOWN` instead.
* `<odrv>.get_oscilloscope_val()` was moved to `<odrv>.oscilloscope.get_val()`.
* Several error flags from `<odrv>.<axis>.error` were removed. Some were moved to `<odrv>.error` and some are no longer relevant because implementation details changed.
* Several error flags from `<odrv>.<axis>.motor.error` were removed. Some were moved to `<odrv>.error` and some are no longer relevant because implementation details changed.
* `<axis>.lockin_state` was removed as the lockin implementation was replaced by a more general open loop control block (currently not exposed on the API).
* `AXIS_STATE_SENSORLESS_CONTROL` was removed. Use `AXIS_STATE_CLOSED_LOOP_CONTROL` instead with `<odrv>.enable_sensorless_mode = True`.
* `<axis>.config.startup_sensorless_control` was removed. Use `<axis>.config.startup_closed_loop_control` instead with `<odrv>.enable_sensorless_mode = True`.
* `<axis>.clear_errors()` was replaced by the system-wide function `<odrv>.clear_errors()`.
* `<axis>.armed_state` was replaced by `<axis>.is_armed`.
* Several properties in `<axis>.motor.current_control` were changed to read-only.
* `<axis>.motor.current_control.Ibus` was moved to `<axis>.motor.I_bus`.
* `<axis>.motor.current_control.max_allowed_current` was moved to `<axis>.motor.max_allowed_current`.
* `<axis>.motor.current_control.overcurrent_trip_level` was removed.
* `<axis>.motor.current_control.acim_rotor_flux` was moved to `<axis>.async_estimator.rotor_flux`.
* `<axis>.motor.current_control.async_phase_vel` was moved to `<axis>.async_estimator.stator_phase_vel`.
* `<axis>.motor.current_control.async_phase_offset` was moved to `<axis>.async_estimator.phase`.
* `<axis>.motor.timing_log` was removed in favor of `<odrv>.task_times` and `<odrv>.<axis>.task_times`.
* `<axis>.motor.config.direction` was moved to `<axis>.encoder.config.direction`.
* `<axis>.motor.config.acim_slip_velocity` was moved to `<axis>.async_estimator.config.slip_velocity`.
* `<axis>.encoder.config.idx_search_unidirectional` was removed. Offset calibration direction is fully defined by the sign of `<axis>.encoder.config.calib_scan_omega` and how the motor is wired up.
* The unit of `<axis>.sensorless_estimator.vel_estimate` was changed from `rad/s` to `turns/s`.
# Release Candidate
## [0.5.1] - Date TBD
Firmware/Board/v3/Inc/board.h
+14 -1
View File
@@ -62,6 +62,15 @@
#define TIM_TIME_BASE TIM14
// Run control loop at the same frequency as the current measurements.
#define CONTROL_TIMER_PERIOD_TICKS (2 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1))
#define TIM1_INIT_COUNT (TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128) // TODO: explain why this offset
// The delta from the control loop timestamp to the current sense timestamp is
// exactly 0 for M0 and TIM1_INIT_COUNT for M1.
#define MAX_CONTROL_LOOP_UPDATE_TO_CURRENT_UPDATE_DELTA (TIM_1_8_PERIOD_CLOCKS / 2 + 1 * 128)
#ifdef __cplusplus
#include <Drivers/DRV8301/drv8301.hpp>
#include <Drivers/STM32/stm32_gpio.hpp>
@@ -84,7 +93,6 @@ extern Stm32Gpio gpios[GPIO_COUNT];
struct GpioFunction { int mode = 0; uint8_t alternate_function = 0xff; };
extern std::array<GpioFunction, 3> alternate_functions[GPIO_COUNT];
extern PCD_HandleTypeDef& usb_pcd_handle;
extern USBD_HandleTypeDef& usb_dev_handle;
extern Stm32SpiArbiter& ext_spi_arbiter;
@@ -112,6 +120,10 @@ static const int current_meas_hz = CURRENT_MEAS_HZ;
#error "unknown board voltage"
#endif
// Linear range of the DRV8301 opamp output: 0.3V...5.7V. We set the upper limit
// to 3.0V so that it's symmetric around the center point of 1.65V.
#define CURRENT_SENSE_MIN_VOLT 0.3f
#define CURRENT_SENSE_MAX_VOLT 3.0f
// This board has no board-specific user configurations
static inline bool board_read_config() { return true; }
@@ -121,5 +133,6 @@ static inline bool board_apply_config() { return true; }
void system_init();
bool board_init();
void start_timers();
#endif // __BOARD_CONFIG_H
Firmware/Board/v3/Inc/stm32f4xx_hal_conf.h
+1 -1
View File
@@ -158,7 +158,7 @@
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */
#define TICK_INT_PRIORITY ((uint32_t)6U) /*!< tick interrupt priority */
#define USE_RTOS 0U
#define PREFETCH_ENABLE 1U
#define INSTRUCTION_CACHE_ENABLE 1U
Firmware/Board/v3/Inc/stm32f4xx_it.h
+1 -1
View File
@@ -58,7 +58,7 @@ void DMA1_Stream0_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void);
void DMA1_Stream4_IRQHandler(void);
void DMA1_Stream5_IRQHandler(void);
void ADC_IRQHandler(void);
//void ADC_IRQHandler(void);
void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void);
Firmware/Board/v3/Src/adc.c
+2 -8
View File
@@ -278,9 +278,6 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
__HAL_LINKDMA(adcHandle,DMA_Handle,hdma_adc1);
/* ADC1 interrupt Init */
HAL_NVIC_SetPriority(ADC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
/* USER CODE BEGIN ADC1_MspInit 1 */
/* USER CODE END ADC1_MspInit 1 */
@@ -314,9 +311,6 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* ADC2 interrupt Init */
HAL_NVIC_SetPriority(ADC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
/* USER CODE BEGIN ADC2_MspInit 1 */
/* USER CODE END ADC2_MspInit 1 */
@@ -341,8 +335,8 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* ADC3 interrupt Init */
HAL_NVIC_SetPriority(ADC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
//HAL_NVIC_SetPriority(ADC_IRQn, 5, 0); // must be on the same level as control loop
//HAL_NVIC_EnableIRQ(ADC_IRQn);
/* USER CODE BEGIN ADC3_MspInit 1 */
/* USER CODE END ADC3_MspInit 1 */
Firmware/Board/v3/Src/can.c
+4 -4
View File
@@ -105,13 +105,13 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* CAN1 interrupt Init */
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 6, 0);
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 9, 0);
HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 6, 0);
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 9, 0);
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 6, 0);
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 9, 0);
HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn);
HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 6, 0);
HAL_NVIC_SetPriority(CAN1_SCE_IRQn, 9, 0);
HAL_NVIC_EnableIRQ(CAN1_SCE_IRQn);
/* USER CODE BEGIN CAN1_MspInit 1 */
Firmware/Board/v3/Src/dma.c
+6 -4
View File
@@ -72,16 +72,18 @@ void MX_DMA_Init(void)
/* DMA interrupt init */
/* DMA1_Stream0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 5, 0);
HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 4, 0); // SPI RX - must have lower priority than SPI TX
// and higher priority than the control loop handler
HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
/* DMA1_Stream2_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 5, 0);
HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 10, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
/* DMA1_Stream4_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 5, 0);
HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 10, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
/* DMA1_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 3, 0); // SPI TX - must have higher priority than SPI RX
// and higher priority than the control loop handler
HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
/* DMA2_Stream0_IRQn interrupt configuration */
// Dear STM, no we _don't_ want to fire an interrupt for this DMA
Firmware/Board/v3/Src/i2c.c
+2 -2
View File
@@ -131,9 +131,9 @@ void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle)
__HAL_LINKDMA(i2cHandle,hdmatx,hdma_i2c1_tx);
/* I2C1 interrupt Init */
HAL_NVIC_SetPriority(I2C1_EV_IRQn, 5, 0);
HAL_NVIC_SetPriority(I2C1_EV_IRQn, 9, 0);
HAL_NVIC_EnableIRQ(I2C1_EV_IRQn);
HAL_NVIC_SetPriority(I2C1_ER_IRQn, 5, 0);
HAL_NVIC_SetPriority(I2C1_ER_IRQn, 9, 0);
HAL_NVIC_EnableIRQ(I2C1_ER_IRQn);
/* USER CODE BEGIN I2C1_MspInit 1 */
Firmware/Board/v3/Src/spi.c
+3 -3
View File
@@ -125,7 +125,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
}
hdma_spi3_tx.Init.Mode = DMA_NORMAL;
hdma_spi3_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
hdma_spi3_tx.Init.Priority = DMA_PRIORITY_HIGH; // SPI TX must have higher priority than SPI RX
hdma_spi3_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_spi3_tx) != HAL_OK)
{
@@ -158,8 +158,8 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
__HAL_LINKDMA(spiHandle,hdmarx,hdma_spi3_rx);
/* SPI3 interrupt Init */
HAL_NVIC_SetPriority(SPI3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(SPI3_IRQn);
//HAL_NVIC_SetPriority(SPI3_IRQn, 3, 0);
//HAL_NVIC_EnableIRQ(SPI3_IRQn);
/* USER CODE BEGIN SPI3_MspInit 1 */
/* USER CODE END SPI3_MspInit 1 */
Firmware/Board/v3/Src/stm32f4xx_hal_msp.c
+1 -1
View File
@@ -74,7 +74,7 @@ void HAL_MspInit(void)
/* UsageFault_IRQn interrupt configuration */
HAL_NVIC_SetPriority(UsageFault_IRQn, 0, 0);
/* SVCall_IRQn interrupt configuration */
HAL_NVIC_SetPriority(SVCall_IRQn, 0, 0);
HAL_NVIC_SetPriority(SVCall_IRQn, 3, 0);
/* DebugMonitor_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DebugMonitor_IRQn, 0, 0);
/* PendSV_IRQn interrupt configuration */
Firmware/Board/v3/Src/stm32f4xx_it.c
+4
View File
@@ -85,6 +85,10 @@ void get_regs(void** stack_ptr) {
void* volatile pc __attribute__((unused)) = stack_ptr[6]; // Program counter
void* volatile psr __attribute__((unused)) = stack_ptr[7]; // Program status register
void* volatile cfsr __attribute__((unused)) = (void*)SCB->CFSR; // Configurable fault status register
void* volatile cpacr __attribute__((unused)) = (void*)SCB->CPACR;
void* volatile fpccr __attribute__((unused)) = (void*)FPU->FPCCR;
volatile int stay_looping = 1;
while(stay_looping);
}
Firmware/Board/v3/Src/tim.c
+1 -18
View File
@@ -383,10 +383,6 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
/* USER CODE END TIM1_MspInit 0 */
/* TIM1 clock enable */
__HAL_RCC_TIM1_CLK_ENABLE();
/* TIM1 interrupt Init */
HAL_NVIC_SetPriority(TIM1_UP_TIM10_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM1_UP_TIM10_IRQn);
/* USER CODE BEGIN TIM1_MspInit 1 */
/* USER CODE END TIM1_MspInit 1 */
@@ -398,10 +394,6 @@ void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
/* USER CODE END TIM13_MspInit 0 */
/* TIM13 clock enable */
__HAL_RCC_TIM13_CLK_ENABLE();
/* TIM13 interrupt Init */
HAL_NVIC_SetPriority(TIM8_UP_TIM13_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM8_UP_TIM13_IRQn);
/* USER CODE BEGIN TIM13_MspInit 1 */
/* USER CODE END TIM13_MspInit 1 */
@@ -429,12 +421,6 @@ void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
/* USER CODE END TIM8_MspInit 0 */
/* TIM8 clock enable */
__HAL_RCC_TIM8_CLK_ENABLE();
/* TIM8 interrupt Init */
HAL_NVIC_SetPriority(TIM8_UP_TIM13_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM8_UP_TIM13_IRQn);
HAL_NVIC_SetPriority(TIM8_TRG_COM_TIM14_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM8_TRG_COM_TIM14_IRQn);
/* USER CODE BEGIN TIM8_MspInit 1 */
/* USER CODE END TIM8_MspInit 1 */
@@ -482,7 +468,7 @@ void HAL_TIM_IC_MspInit(TIM_HandleTypeDef* tim_icHandle)
__HAL_RCC_TIM5_CLK_ENABLE();
/* TIM5 interrupt Init */
HAL_NVIC_SetPriority(TIM5_IRQn, 5, 0);
HAL_NVIC_SetPriority(TIM5_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(TIM5_IRQn);
/* USER CODE BEGIN TIM5_MspInit 1 */
@@ -599,7 +585,6 @@ void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
__HAL_RCC_TIM1_CLK_DISABLE();
/* TIM1 interrupt Deinit */
HAL_NVIC_DisableIRQ(TIM1_UP_TIM10_IRQn);
/* USER CODE BEGIN TIM1_MspDeInit 1 */
/* USER CODE END TIM1_MspDeInit 1 */
@@ -657,8 +642,6 @@ void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
*/
/* HAL_NVIC_DisableIRQ(TIM8_UP_TIM13_IRQn); */
/* USER CODE END TIM8:TIM8_UP_TIM13_IRQn disable */
HAL_NVIC_DisableIRQ(TIM8_TRG_COM_TIM14_IRQn);
/* USER CODE BEGIN TIM8_MspDeInit 1 */
/* USER CODE END TIM8_MspDeInit 1 */
Firmware/Board/v3/Src/usart.c
+1 -1
View File
@@ -129,7 +129,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmatx,hdma_uart4_tx);
/* UART4 interrupt Init */
HAL_NVIC_SetPriority(UART4_IRQn, 5, 0);
HAL_NVIC_SetPriority(UART4_IRQn, 10, 0);
HAL_NVIC_EnableIRQ(UART4_IRQn);
/* USER CODE BEGIN UART4_MspInit 1 */
Firmware/Board/v3/Src/usbd_conf.c
+1 -1
View File
@@ -114,7 +114,7 @@ void HAL_PCD_MspInit(PCD_HandleTypeDef* pcdHandle)
__HAL_RCC_USB_OTG_FS_CLK_ENABLE();
/* Peripheral interrupt init */
HAL_NVIC_SetPriority(OTG_FS_IRQn, 5, 0);
HAL_NVIC_SetPriority(OTG_FS_IRQn, 6, 0);
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
/* USER CODE BEGIN USB_OTG_FS_MspInit 1 */
Firmware/Board/v3/board.cpp
+199 -65
View File
@@ -15,8 +15,14 @@
#include <usart.h>
#include <freertos_vars.h>
// this should technically be in task_timer.cpp but let's not make a one-line file
bool TaskTimer::enabled = false;
extern "C" void SystemClock_Config(void); // defined in main.c generated by CubeMX
#define ControlLoop_IRQHandler OTG_HS_IRQHandler
#define ControlLoop_IRQn OTG_HS_IRQn
Stm32SpiArbiter spi3_arbiter{&hspi3};
Stm32SpiArbiter& ext_spi_arbiter = spi3_arbiter;
@@ -27,14 +33,14 @@ UART_HandleTypeDef* uart2 = nullptr;
Drv8301 m0_gate_driver{
&spi3_arbiter,
{M0_nCS_GPIO_Port, M0_nCS_Pin}, // nCS
{EN_GATE_GPIO_Port, EN_GATE_Pin}, // EN pin (shared between both motors)
{}, // EN pin (shared between both motors, therefore we actuate it outside of the drv8301 driver)
{nFAULT_GPIO_Port, nFAULT_Pin} // nFAULT pin (shared between both motors)
};
Drv8301 m1_gate_driver{
&spi3_arbiter,
{M1_nCS_GPIO_Port, M1_nCS_Pin}, // nCS
{EN_GATE_GPIO_Port, EN_GATE_Pin}, // EN pin (shared between both motors)
{}, // EN pin (shared between both motors, therefore we actuate it outside of the drv8301 driver)
{nFAULT_GPIO_Port, nFAULT_Pin} // nFAULT pin (shared between both motors)
};
@@ -61,14 +67,14 @@ OnboardThermistorCurrentLimiter fet_thermistors[AXIS_COUNT] = {
Motor motors[AXIS_COUNT] = {
{
&htim1, // timer
TIM_1_8_PERIOD_CLOCKS, // control_deadline
0b110, // current_sensor_mask
1.0f / SHUNT_RESISTANCE, // shunt_conductance [S]
m0_gate_driver, // gate_driver
m0_gate_driver // opamp
},
{
&htim8, // timer
(3 * TIM_1_8_PERIOD_CLOCKS) / 2, // control_deadline
0b110, // current_sensor_mask
1.0f / SHUNT_RESISTANCE, // shunt_conductance [S]
m1_gate_driver, // gate_driver
m1_gate_driver // opamp
@@ -244,8 +250,6 @@ PwmInput pwm0_input{&htim5, {0, 0, 0, 4}}; // 0 means not in use
PwmInput pwm0_input{&htim5, {1, 2, 3, 4}};
#endif
extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // defined in usbd_conf.c
PCD_HandleTypeDef& usb_pcd_handle = hpcd_USB_OTG_FS;
extern USBD_HandleTypeDef hUsbDeviceFS;
USBD_HandleTypeDef& usb_dev_handle = hUsbDeviceFS;
@@ -274,6 +278,28 @@ bool board_init() {
MX_TIM5_Init();
MX_TIM13_Init();
// External interrupt lines are individually enabled in stm32_gpio.cpp
HAL_NVIC_SetPriority(EXTI0_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
HAL_NVIC_SetPriority(EXTI1_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI1_IRQn);
HAL_NVIC_SetPriority(EXTI2_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI2_IRQn);
HAL_NVIC_SetPriority(EXTI3_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
HAL_NVIC_SetPriority(EXTI4_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI4_IRQn);
HAL_NVIC_SetPriority(EXTI9_5_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 1, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
HAL_NVIC_SetPriority(ControlLoop_IRQn, 5, 0); // must be on the same level as ADC interrupt
HAL_NVIC_EnableIRQ(ControlLoop_IRQn);
HAL_NVIC_SetPriority(TIM8_UP_TIM13_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(TIM8_UP_TIM13_IRQn);
HAL_UART_DeInit(uart0);
uart0->Init.BaudRate = odrv.config_.uart0_baudrate;
HAL_UART_Init(uart0);
@@ -308,29 +334,92 @@ bool board_init() {
__HAL_DBGMCU_FREEZE_TIM8();
__HAL_DBGMCU_FREEZE_TIM13();
/*
* Initial intention of the synchronization:
* Synchronize TIM1, TIM8 and TIM13 such that:
* 1. The triangle waveform of TIM1 leads the triangle waveform of TIM8 by a
* 90° phase shift.
* 2. The timer update events of TIM1 and TIM8 are symmetrically interleaved.
* 3. Each TIM13 reload coincides with a TIM1 lower update event.
*
* However right now this synchronization only ensures point (1) and (3) but because
* TIM1 and TIM3 only trigger an update on every third reload, this does not
* allow for (2).
*
* TODO: revisit the timing topic in general.
*
*/
Stm32Timer::start_synchronously<3>(
{&htim1, &htim8, &htim13},
{TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128 /* TODO: explain why this offset */, 0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128}
);
Stm32Gpio drv_enable_gpio = {EN_GATE_GPIO_Port, EN_GATE_Pin};
// Reset both DRV chips. The enable pin also controls the SPI interface, not
// only the driver stages.
drv_enable_gpio.write(false);
delay_us(40); // mimumum pull-down time for full reset: 20us
drv_enable_gpio.write(true);
delay_us(20000); // mimumum pull-down time for full reset: 20us
return true;
}
void start_timers() {
CRITICAL_SECTION() {
// Temporarily disable ADC triggers so they don't trigger as a side
// effect of starting the timers.
hadc1.Instance->CR2 &= ~(ADC_CR2_JEXTEN);
hadc2.Instance->CR2 &= ~(ADC_CR2_EXTEN | ADC_CR2_JEXTEN);
hadc3.Instance->CR2 &= ~(ADC_CR2_EXTEN | ADC_CR2_JEXTEN);
/*
* Initial intention of the synchronization:
* Synchronize TIM1, TIM8 and TIM13 such that:
* 1. The triangle waveform of TIM1 leads the triangle waveform of TIM8 by a
* 90° phase shift.
* 2. The timer update events of TIM1 and TIM8 are symmetrically interleaved.
* 3. Each TIM13 reload coincides with a TIM1 lower update event.
*
* However right now this synchronization only ensures point (1) and (3) but because
* TIM1 and TIM3 only trigger an update on every third reload, this does not
* allow for (2).
*
* TODO: revisit the timing topic in general.
*
*/
Stm32Timer::start_synchronously<3>(
{&htim1, &htim8, &htim13},
{TIM1_INIT_COUNT, 0, TIM1_INIT_COUNT / 2 /* TIM13 is on a clock that's only have as fast as TIM1 */}
);
hadc1.Instance->CR2 |= (ADC_EXTERNALTRIGINJECCONVEDGE_RISING);
hadc2.Instance->CR2 |= (ADC_EXTERNALTRIGCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONVEDGE_RISING);
hadc3.Instance->CR2 |= (ADC_EXTERNALTRIGCONVEDGE_RISING | ADC_EXTERNALTRIGINJECCONVEDGE_RISING);
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_JEOC);
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_JEOC);
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_JEOC);
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_EOC);
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_EOC);
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_EOC);
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_OVR);
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_OVR);
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_OVR);
__HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE);
// it's sufficient to enable interrupts for one ADC only because they all trigger simultaneously
//__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC);
//__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC);
__HAL_TIM_ENABLE_IT(&htim8, TIM_IT_UPDATE);
}
}
static bool fetch_and_reset_adcs(float* current0_phB, float* current0_phC, float* current1_phB, float* current1_phC) {
bool all_adcs_done = (ADC1->SR & ADC_SR_JEOC) == ADC_SR_JEOC
&& (ADC2->SR & (ADC_SR_EOC | ADC_SR_JEOC)) == (ADC_SR_EOC | ADC_SR_JEOC)
&& (ADC3->SR & (ADC_SR_EOC | ADC_SR_JEOC)) == (ADC_SR_EOC | ADC_SR_JEOC);
if (!all_adcs_done) {
return false;
}
bool m0_current_valid = m0_gate_driver.is_ready();
bool m1_current_valid = m1_gate_driver.is_ready();
vbus_sense_adc_cb(ADC1->JDR1);
*current0_phB = m0_current_valid ? motors[0].phase_current_from_adcval(ADC2->JDR1) : NAN;
*current0_phC = m0_current_valid ? motors[0].phase_current_from_adcval(ADC3->JDR1) : NAN;
*current1_phB = m1_current_valid ? motors[1].phase_current_from_adcval(ADC2->DR) : NAN;
*current1_phC = m1_current_valid ? motors[1].phase_current_from_adcval(ADC3->DR) : NAN;
ADC1->SR = ~(ADC_SR_JEOC);
ADC2->SR = ~(ADC_SR_EOC | ADC_SR_JEOC | ADC_SR_OVR);
ADC3->SR = ~(ADC_SR_EOC | ADC_SR_JEOC | ADC_SR_OVR);
return true;
}
extern "C" {
@@ -348,51 +437,98 @@ void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
}
}
void TIM1_UP_TIM10_IRQHandler(void) {
COUNT_IRQ(TIM1_UP_TIM10_IRQn);
__HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);
motors[0].tim_update_cb();
}
void TIM8_UP_TIM13_IRQHandler(void) {
COUNT_IRQ(TIM8_UP_TIM13_IRQn);
__HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE);
motors[1].tim_update_cb();
}
void TIM5_IRQHandler(void) {
COUNT_IRQ(TIM5_IRQn);
pwm0_input.on_capture();
}
void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, void(*callback)(ADC_HandleTypeDef* hadc, bool injected)) {
// Injected measurements
uint32_t JEOC = __HAL_ADC_GET_FLAG(hadc, ADC_FLAG_JEOC);
uint32_t JEOC_IT_EN = __HAL_ADC_GET_IT_SOURCE(hadc, ADC_IT_JEOC);
if (JEOC && JEOC_IT_EN) {
callback(hadc, true);
__HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_JSTRT | ADC_FLAG_JEOC));
volatile uint32_t timestamp_ = 0;
volatile bool counting_down_ = false;
void TIM8_UP_TIM13_IRQHandler(void) {
// Entry into this function happens at 21-23 clock cycles after the timer
// update event.
__HAL_TIM_CLEAR_IT(&htim8, TIM_IT_UPDATE);
// 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
bool counting_down = TIM8->CR1 & TIM_CR1_DIR;
bool timer_update_missed = (counting_down_ == counting_down);
if (timer_update_missed) {
motors[0].disarm_with_error(Motor::ERROR_TIMER_UPDATE_MISSED);
motors[1].disarm_with_error(Motor::ERROR_TIMER_UPDATE_MISSED);
return;
}
// Regular measurements
uint32_t EOC = __HAL_ADC_GET_FLAG(hadc, ADC_FLAG_EOC);
uint32_t EOC_IT_EN = __HAL_ADC_GET_IT_SOURCE(hadc, ADC_IT_EOC);
if (EOC && EOC_IT_EN) {
callback(hadc, false);
__HAL_ADC_CLEAR_FLAG(hadc, (ADC_FLAG_STRT | ADC_FLAG_EOC));
counting_down_ = counting_down;
timestamp_ += TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1);
if (!counting_down) {
TaskTimer::enabled = odrv.task_timers_armed_;
// Run sampling handlers and kick off control tasks when TIM8 is
// counting up.
odrv.sampling_cb();
NVIC->STIR = ControlLoop_IRQn;
} else {
// Tentatively reset all PWM outputs to 50% duty cycles. If the control
// loop handler finishes in time then these values will be overridden
// before they go into effect.
TIM1->CCR1 =
TIM1->CCR2 =
TIM1->CCR3 =
TIM8->CCR1 =
TIM8->CCR2 =
TIM8->CCR3 =
TIM_1_8_PERIOD_CLOCKS / 2;
}
}
void ADC_IRQHandler(void) {
COUNT_IRQ(ADC_IRQn);
// The HAL's ADC handling mechanism adds many clock cycles of overhead
// So we bypass it and handle the logic ourselves.
//@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);
void ControlLoop_IRQHandler(void) {
COUNT_IRQ(ControlLoop_IRQn);
uint32_t timestamp = timestamp_;
// Ensure that all the ADCs are done
float current0_phB;
float current0_phC;
float current1_phB;
float current1_phC;
if (!fetch_and_reset_adcs(&current0_phB, &current0_phC, &current1_phB, &current1_phC)) {
motors[0].disarm_with_error(Motor::ERROR_BAD_TIMING);
motors[1].disarm_with_error(Motor::ERROR_BAD_TIMING);
}
motors[0].current_meas_cb(timestamp - TIM1_INIT_COUNT, {-current0_phB - current0_phC, current0_phB, current0_phC});
motors[1].current_meas_cb(timestamp, {-current1_phB - current1_phC, current1_phB, current1_phC});
odrv.control_loop_cb(timestamp);
// By this time the ADCs for both M0 and M1 should have fired again. But
// let's wait for them just to be sure.
while (!(ADC2->SR & ADC_SR_EOC));
if (!fetch_and_reset_adcs(&current0_phB, &current0_phC, &current1_phB, &current1_phC)) {
motors[0].disarm_with_error(Motor::ERROR_BAD_TIMING);
motors[1].disarm_with_error(Motor::ERROR_BAD_TIMING);
}
motors[0].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT, {-current0_phB - current0_phC, current0_phB, current0_phC});
motors[1].dc_calib_cb(timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1), {-current1_phB - current1_phC, current1_phB, current1_phC});
motors[0].pwm_update_cb(timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1) - TIM1_INIT_COUNT);
motors[1].pwm_update_cb(timestamp + 3 * TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1));
// If we did everything right, the TIM8 update handler should have been
// called exactly once between the start of this function and now.
if (timestamp_ != timestamp + TIM_1_8_PERIOD_CLOCKS * (TIM_1_8_RCR + 1)) {
motors[0].disarm_with_error(Motor::ERROR_CONTROL_DEADLINE_MISSED);
motors[1].disarm_with_error(Motor::ERROR_CONTROL_DEADLINE_MISSED);
}
odrv.task_timers_armed_ = odrv.task_timers_armed_ && !TaskTimer::enabled;
TaskTimer::enabled = false;
}
void I2C1_EV_IRQHandler(void) {
@@ -405,12 +541,10 @@ void I2C1_ER_IRQHandler(void) {
HAL_I2C_ER_IRQHandler(&hi2c1);
}
extern PCD_HandleTypeDef hpcd_USB_OTG_FS; // defined in usbd_conf.c
void OTG_FS_IRQHandler(void) {
COUNT_IRQ(OTG_FS_IRQn);
// Mask interrupt, and signal processing of interrupt by usb_cmd_thread
// The thread will re-enable the interrupt when all pending irqs are clear.
HAL_NVIC_DisableIRQ(OTG_FS_IRQn);
osSemaphoreRelease(sem_usb_irq);
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
}
}
Firmware/Drivers/DRV8301/drv8301.cpp
+108 -206
View File
@@ -1,49 +1,8 @@
/* --COPYRIGHT--,BSD
* Copyright (c) 2015, Texas Instruments Incorporated
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
*
* * Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
* WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
* OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
* --/COPYRIGHT--*/
//! \file drivers/drvic/drv8301/src/32b/f28x/f2806x/drv8301.c
//! \brief Contains the various functions related to the DRV8301 object
//!
//! (C) Copyright 2015, Texas Instruments, Inc.
// **************************************************************************
// the includes
#include "drv8301.hpp"
#include "utils.hpp"
#include "cmsis_os.h"
#include <math.h>
#include <array>
#include <algorithm>
#include "board.h"
const SPI_InitTypeDef Drv8301::spi_config_ = {
.Mode = SPI_MODE_MASTER,
@@ -59,119 +18,135 @@ const SPI_InitTypeDef Drv8301::spi_config_ = {
.CRCPolynomial = 10,
};
bool Drv8301::init() {
enable_gpio_.write(true);
// Wait for driver to come online
osDelay(10);
bool Drv8301::config(float requested_gain, float* actual_gain) {
// Calculate gain setting: Snap down to have equal or larger range as
// requested or largest possible range otherwise
// Make sure the Fault bit is not set during startup
uint16_t reg;
while (!read_spi(RegName_Status_1, &reg) || (reg & DRV8301_STATUS1_FAULT_BITS))
; // TODO: don't spin
// Wait for the DRV8301 registers to update
osDelay(1);
return true;
}
Drv8301::FaultType_e Drv8301::get_error() {
uint16_t readWord;
FaultType_e faultType = FaultType_NoFault;
// read the data
if (!read_spi(RegName_Status_1, &readWord)) {
return (FaultType_e)0xffff;
}
if (readWord & DRV8301_STATUS1_FAULT_BITS) {
faultType = (FaultType_e)(readWord & DRV8301_FAULT_TYPE_MASK);
if (faultType == FaultType_NoFault) {
// read the data
if (!read_spi(RegName_Status_2, &readWord)) {
return (FaultType_e)0xffff;
}
if (readWord & DRV8301_STATUS2_GVDD_OV_BITS) {
faultType = FaultType_GVDD_OV;
}
}
}
return faultType;
}
bool Drv8301::set_gain(float requested_gain, float* actual_gain) {
// for reference:
// 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
// Snap down to have equal or larger range as requested or largest possible range otherwise
// Decoding array for snapping gain
std::array<std::pair<float, ShuntAmpGain_e>, 4> gain_choices = {
std::make_pair(10.0f, ShuntAmpGain_10VpV),
std::make_pair(20.0f, ShuntAmpGain_20VpV),
std::make_pair(40.0f, ShuntAmpGain_40VpV),
std::make_pair(80.0f, ShuntAmpGain_80VpV)
};
// We use lower_bound in reverse because it snaps up by default, we want to snap down.
auto gain_snap_down = std::lower_bound(gain_choices.crbegin(), gain_choices.crend(), requested_gain,
[](std::pair<float, ShuntAmpGain_e> pair, float val){
return (bool)(pair.first > val);
});
// If we snap to outside the array, clip to smallest val
if (gain_snap_down == gain_choices.crend())
--gain_snap_down;
Registers_t regs;
if (!read_regs(&regs)) {
return false;
}
regs.Ctrl_Reg_1.OC_MODE = OcMode_LatchShutDown;
// Overcurrent set to approximately 150A at 100degC. This may need tweaking.
regs.Ctrl_Reg_1.OC_ADJ_SET = VdsLevel_0p730_V;
regs.Ctrl_Reg_2.GAIN = gain_snap_down->second;
if (!write_regs(&regs)) {
return false;
uint16_t gain_setting = 3;
float gain_choices[] = {10.0f, 20.0f, 40.0f, 80.0f};
while (gain_setting && (gain_choices[gain_setting] > requested_gain)) {
gain_setting--;
}
if (actual_gain) {
*actual_gain = gain_snap_down->first;
*actual_gain = gain_choices[gain_setting];
}
RegisterFile new_config;
new_config.control_register_1 =
(21 << 6) // Overcurrent set to approximately 150A at 100degC. This may need tweaking.
| (0b01 << 4) // OCP_MODE: latch shut down
| (0b0 << 3) // 6x PWM mode
| (0b0 << 2) // don't reset latched faults
| (0b00 << 0); // gate-drive peak current: 1.7A
new_config.control_register_2 =
(0b0 << 6) // OC_TOFF: cycle by cycle
| (0b00 << 4) // calibration off (normal operation)
| (gain_setting << 2) // select gain
| (0b00 << 0); // report both over temperature and over current on nOCTW pin
bool regs_equal = (regs_.control_register_1 == new_config.control_register_1)
&& (regs_.control_register_2 == new_config.control_register_2);
if (!regs_equal) {
regs_ = new_config;
state_ = kStateUninitialized;
enable_gpio_.write(false);
}
return true;
}
bool Drv8301::check_fault() {
if (nfault_gpio_) {
return nfault_gpio_.read();
} else {
bool Drv8301::init() {
uint16_t val;
if (state_ == kStateReady) {
return true;
}
// Reset DRV chip. The enable pin also controls the SPI interface, not only
// the driver stages.
enable_gpio_.write(false);
delay_us(40); // mimumum pull-down time for full reset: 20us
state_ = kStateUninitialized; // make is_ready() ignore transient errors before registers are set up
enable_gpio_.write(true);
osDelay(20); // t_spi_ready, max = 10ms
// Write current configuration
bool did_write_regs = write_reg(kRegNameControl1, regs_.control_register_1)
&& write_reg(kRegNameControl1, regs_.control_register_1)
&& write_reg(kRegNameControl1, regs_.control_register_1)
&& write_reg(kRegNameControl1, regs_.control_register_1)
&& write_reg(kRegNameControl1, regs_.control_register_1) // the write operation tends to be ignored if only done once (not sure why)
&& write_reg(kRegNameControl2, regs_.control_register_2);
if (!did_write_regs) {
return false;
}
// Wait for configuration to be applied
delay_us(100);
state_ = kStateStartupChecks;
bool did_read_regs = read_reg(kRegNameControl1, &val) && (val == regs_.control_register_1)
&& read_reg(kRegNameControl2, &val) && (val == regs_.control_register_2);
if (!did_read_regs) {
return false;
}
if (get_error() != FaultType_NoFault) {
return false;
}
// There could have been an nFAULT edge meanwhile. In this case we shouldn't
// consider the driver ready.
CRITICAL_SECTION() {
if (state_ == kStateStartupChecks) {
state_ = kStateReady;
}
}
return state_ == kStateReady;
}
void Drv8301::do_checks() {
if (state_ != kStateUninitialized && !nfault_gpio_.read()) {
state_ = kStateUninitialized;
}
}
bool Drv8301::read_spi(const RegName_e regName, uint16_t* data) {
bool Drv8301::is_ready() {
return state_ == kStateReady;
}
Drv8301::FaultType_e Drv8301::get_error() {
uint16_t fault1, fault2;
if (!read_reg(kRegNameStatus1, &fault1) ||
!read_reg(kRegNameStatus2, &fault2)) {
return (FaultType_e)0xffffffff;
}
return (FaultType_e)((uint32_t)fault1 | ((uint32_t)(fault2 & 0x0080) << 16));
}
bool Drv8301::read_reg(const RegName_e regName, uint16_t* data) {
tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Read, regName, 0);
if (!spi_arbiter_->transfer(spi_config_, ncs_gpio_, (uint8_t *)(&tx_buf_), nullptr, 1, 1000)) {
return false;
}
// Datasheet says you don't have to pulse the nCS between transfers, (16
// clocks should commit the transfer) but for some reason you actually need
// to pulse it.
delay_us(1);
tx_buf_ = 0;
rx_buf_ = 0xbeef;
tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Read, regName, 0);
rx_buf_ = 0xffff;
if (!spi_arbiter_->transfer(spi_config_, ncs_gpio_, (uint8_t *)(&tx_buf_), (uint8_t *)(&rx_buf_), 1, 1000)) {
return false;
}
@@ -183,13 +158,13 @@ bool Drv8301::read_spi(const RegName_e regName, uint16_t* data) {
}
if (data) {
*data = rx_buf_ & DRV8301_DATA_MASK;
*data = rx_buf_ & 0x07FF;
}
return true;
}
bool Drv8301::write_spi(const RegName_e regName, const uint16_t data) {
bool Drv8301::write_reg(const RegName_e regName, const uint16_t data) {
// Do blocking write
tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Write, regName, data);
if (!spi_arbiter_->transfer(spi_config_, ncs_gpio_, (uint8_t *)(&tx_buf_), nullptr, 1, 1000)) {
@@ -199,76 +174,3 @@ bool Drv8301::write_spi(const RegName_e regName, const uint16_t data) {
return true;
}
bool Drv8301::write_regs(Registers_t *regs) {
uint16_t ctrl1 = regs->Ctrl_Reg_1.DRV8301_CURRENT |
regs->Ctrl_Reg_1.DRV8301_RESET |
regs->Ctrl_Reg_1.PWM_MODE |
regs->Ctrl_Reg_1.OC_MODE |
regs->Ctrl_Reg_1.OC_ADJ_SET;
uint16_t ctrl2 = regs->Ctrl_Reg_2.OCTW_SET |
regs->Ctrl_Reg_2.GAIN |
regs->Ctrl_Reg_2.DC_CAL_CH1p2 |
regs->Ctrl_Reg_2.OC_TOFF;
return write_spi(RegName_Control_1, ctrl1)
&& write_spi(RegName_Control_2, ctrl2);
}
bool Drv8301::read_regs(Registers_t *regs) {
bool success = true;
uint16_t drvDataNew;
// Update Status Register 1
if (read_spi(RegName_Status_1, &drvDataNew)) {
regs->Stat_Reg_1.FAULT = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FAULT_BITS);
regs->Stat_Reg_1.GVDD_UV = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_GVDD_UV_BITS);
regs->Stat_Reg_1.PVDD_UV = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_PVDD_UV_BITS);
regs->Stat_Reg_1.OTSD = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_OTSD_BITS);
regs->Stat_Reg_1.OTW = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_OTW_BITS);
regs->Stat_Reg_1.FETHA_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETHA_OC_BITS);
regs->Stat_Reg_1.FETLA_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETLA_OC_BITS);
regs->Stat_Reg_1.FETHB_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETHB_OC_BITS);
regs->Stat_Reg_1.FETLB_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETLB_OC_BITS);
regs->Stat_Reg_1.FETHC_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETHC_OC_BITS);
regs->Stat_Reg_1.FETLC_OC = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS1_FETLC_OC_BITS);
regs->Stat_Reg_1_Value = drvDataNew;
} else {
success = false;
}
// Update Status Register 2
if (read_spi(RegName_Status_2, &drvDataNew)) {
regs->Stat_Reg_2.GVDD_OV = (bool)(drvDataNew & (uint16_t)DRV8301_STATUS2_GVDD_OV_BITS);
regs->Stat_Reg_2.DeviceID = (uint16_t)(drvDataNew & (uint16_t)DRV8301_STATUS2_ID_BITS);
regs->Stat_Reg_2_Value = drvDataNew;
} else {
success = false;
}
// Update Control Register 1
if (read_spi(RegName_Control_1, &drvDataNew)) {
regs->Ctrl_Reg_1.DRV8301_CURRENT = (PeakCurrent_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_GATE_CURRENT_BITS);
regs->Ctrl_Reg_1.DRV8301_RESET = (Reset_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_GATE_RESET_BITS);
regs->Ctrl_Reg_1.PWM_MODE = (PwmMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_PWM_MODE_BITS);
regs->Ctrl_Reg_1.OC_MODE = (OcMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_OC_MODE_BITS);
regs->Ctrl_Reg_1.OC_ADJ_SET = (VdsLevel_e)(drvDataNew & (uint16_t)DRV8301_CTRL1_OC_ADJ_SET_BITS);
regs->Ctrl_Reg_1_Value = drvDataNew;
} else {
success = false;
}
// Update Control Register 2
if (read_spi(RegName_Control_2, &drvDataNew)) {
regs->Ctrl_Reg_2.OCTW_SET = (OcTwMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL2_OCTW_SET_BITS);
regs->Ctrl_Reg_2.GAIN = (ShuntAmpGain_e)(drvDataNew & (uint16_t)DRV8301_CTRL2_GAIN_BITS);
regs->Ctrl_Reg_2.DC_CAL_CH1p2 = (DcCalMode_e)(drvDataNew & (uint16_t)(DRV8301_CTRL2_DC_CAL_1_BITS | DRV8301_CTRL2_DC_CAL_2_BITS));
regs->Ctrl_Reg_2.OC_TOFF = (OcOffTimeMode_e)(drvDataNew & (uint16_t)DRV8301_CTRL2_OC_TOFF_BITS);
regs->Ctrl_Reg_2_Value = drvDataNew;
} else {
success = false;
}
return success;
}
Firmware/Drivers/DRV8301/drv8301.hpp
+75 -370
View File
File diff suppressed because it is too large Load Diff
Firmware/Drivers/STM32/stm32_gpio.cpp
+9 -9
View File
@@ -92,14 +92,11 @@ bool Stm32Gpio::subscribe(bool rising_edge, bool falling_edge, void (*callback)(
struct subscription_t& subscription = subscriptions[pin_number];
void (*no_port)(void*) = nullptr;
GPIO_TypeDef* no_port = nullptr;
if (!__atomic_compare_exchange_n(&subscription.port, &no_port, port_, false, __ATOMIC_SEQ_CST, __ATOMIC_SEQ_CST)) {
return false; // already in use
}
subscription.ctx = ctx;
subscription.callback = callback;
// The following code is mostly taken from HAL_GPIO_Init
__HAL_RCC_SYSCFG_CLK_ENABLE();
@@ -126,10 +123,9 @@ bool Stm32Gpio::subscribe(bool rising_edge, bool falling_edge, void (*callback)(
// Clear any previous triggers
__HAL_GPIO_EXTI_CLEAR_IT(pin_mask_);
// Enable interrupt
// TODO: use configurable priority
HAL_NVIC_SetPriority(get_irq_number(pin_number), 0, 0);
HAL_NVIC_EnableIRQ(get_irq_number(pin_number));
subscription.ctx = ctx;
subscription.callback = callback;
return true;
}
@@ -141,8 +137,12 @@ void Stm32Gpio::unsubscribe() {
struct subscription_t& subscription = subscriptions[pin_number];
HAL_NVIC_DisableIRQ(get_irq_number(pin_number));
if (subscription.port != port_) {
return; // the subscription was not for this GPIO
}
EXTI->IMR |= (uint32_t)pin_mask_;
__HAL_GPIO_EXTI_CLEAR_IT(pin_mask_);
// At this point no more interrupts will be triggered for this GPIO
Firmware/Drivers/STM32/stm32_gpio.hpp
+8
View File
@@ -37,6 +37,8 @@ public:
* Before calling this function the gpio should most likely be configured as
* input (however this is not mandatory, the interrupt works in output mode
* too).
* Also you need to enable the EXTIx_IRQn interrupt vectors in the NVIC,
* otherwise the subscription won't have any effect.
*
* Only one subscription is allowed per pin number. I.e. it is not possible
* to set up a subscription for both PA0 and PB0 at the same time.
@@ -51,9 +53,15 @@ public:
/**
* @brief Unsubscribes from external interrupt on the specified GPIO.
*
* If no subscription was active for this GPIO, calling this function has no
* effect.
*
* This function is thread-safe with respect to all other public functions
* of this class, however it must not be called from an interrupt routine
* running at a higher priority than the interrupt that is being unsubscribed.
*
* After this function returns the callback given to subscribe() will no
* longer be invoked.
*/
void unsubscribe();
Firmware/Drivers/STM32/stm32_spi_arbiter.cpp
+5 -1
View File
@@ -40,7 +40,11 @@ bool Stm32SpiArbiter::start() {
task.ncs_gpio.write(false);
HAL_StatusTypeDef status = HAL_ERROR;
if (task.tx_buf && task.rx_buf) {
if (hspi_->hdmatx->State != HAL_DMA_STATE_READY || hspi_->hdmarx->State != HAL_DMA_STATE_READY) {
// This can happen if the DMA or interrupt priorities are not configured properly.
status = HAL_BUSY;
} else if (task.tx_buf && task.rx_buf) {
status = HAL_SPI_TransmitReceive_DMA(hspi_, (uint8_t*)task.tx_buf, task.rx_buf, task.length);
} else if (task.tx_buf) {
status = HAL_SPI_Transmit_DMA(hspi_, (uint8_t*)task.tx_buf, task.length);
Firmware/Drivers/STM32/stm32_spi_arbiter.hpp
-1
View File
@@ -89,7 +89,6 @@ private:
SPI_HandleTypeDef* hspi_;
SpiTask* task_list_ = nullptr;
SpiTask* current_task_ = nullptr;
};
#endif // __STM32_SPI_ARBITER_HPP
Firmware/Drivers/gate_driver.hpp
+8 -5
View File
@@ -13,17 +13,20 @@ struct GateDriverBase {
virtual bool set_enabled(bool enabled) = 0;
/**
* @brief Checks for a fault condition. Returns false if the driver is in a
* fault state and true if it is in a nominal state.
* @brief Returns false if the gate driver is in a state where the output
* drive stages are disarmed or not properly configured (e.g. because they
* are not initialized or there was a fault condition).
*/
virtual bool check_fault() = 0;
virtual bool is_ready() = 0;
};
struct OpAmpBase {
/**
* @brief Tries to set the OpAmp gain to the specified value or lower.
* @brief Returns false if the opamp is in a state where it's not operating
* with the latest configured gain (e.g. because it was not initialized or
* there was a fault condition).
*/
virtual bool set_gain(float requested_gain, float* actual_gain) = 0;
virtual bool is_ready() = 0;
/**
* @brief Returns the neutral voltage of the OpAmp in Volts
Firmware/MotorControl/async_estimator.cpp
+48
View File
@@ -0,0 +1,48 @@
#include "async_estimator.hpp"
#include <board.h>
void AsyncEstimator::update(uint32_t timestamp) {
float rotor_phase = rotor_phase_src_ ? *rotor_phase_src_ : NAN;
float rotor_phase_vel = rotor_phase_vel_src_ ? *rotor_phase_vel_src_ : NAN;
float id = id_src_ ? *id_src_ : NAN;
float iq = iq_src_ ? *iq_src_ : NAN;
if (std::isnan(rotor_phase) || std::isnan(rotor_phase_vel)) {
stator_phase_vel_ = NAN;
stator_phase_ = NAN;
active_ = false;
return;
}
if (!active_) {
last_timestamp_ = timestamp;
stator_phase_vel_ = 0.0f;
stator_phase_ = 0.0f;
active_ = true;
return;
}
last_timestamp_ = timestamp;
float dt = (float)(timestamp - last_timestamp_) / (float)TIM_1_8_CLOCK_HZ;
// Note that the effect of the current commands on the real currents is actually 1.5 PWM cycles later
// However the rotor time constant is (usually) so slow that it doesn't matter
// So we elect to write it as if the effect is immediate, to have cleaner code
// acim_rotor_flux is normalized to units of [A] tracking Id; rotor inductance is unspecified
float dflux_by_dt = config_.slip_velocity * (id - rotor_flux_);
rotor_flux_ += dflux_by_dt * dt;
float slip_velocity = config_.slip_velocity * (iq / rotor_flux_);
// Check for issues with small denominator. Polarity of check to catch NaN too
bool acceptable_vel = fabsf(slip_velocity) <= 0.1f / dt;
if (!acceptable_vel)
slip_velocity = 0.0f;
slip_vel_ = slip_velocity; // reporting only
stator_phase_vel_ = rotor_phase_vel + slip_velocity;
phase_offset_ += slip_velocity * dt;
phase_offset_ = wrap_pm_pi(phase_offset_);
stator_phase_ = wrap_pm_pi(rotor_phase + phase_offset_);
}
Firmware/MotorControl/async_estimator.hpp
+36
View File
@@ -0,0 +1,36 @@
#ifndef __ASYNC_ESTIMATOR_HPP
#define __ASYNC_ESTIMATOR_HPP
#include <component.hpp>
#include <cmath>
class AsyncEstimator : public ComponentBase {
public:
struct Config_t {
float slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau
};
void update(uint32_t timestamp) final;
// Config
Config_t config_;
// Inputs
float* rotor_phase_src_ = nullptr;
float* rotor_phase_vel_src_ = nullptr;
float* id_src_ = nullptr;
float* iq_src_ = nullptr;
// State variables
float active_ = false;
uint32_t last_timestamp_ = 0;
float rotor_flux_ = 0.0f; // [A]
float slip_vel_ = 0.0f; // [rad/s electrical]
float phase_offset_ = 0.0f; // [rad electrical]
// Outputs
float stator_phase_vel_ = NAN; // [rad/s] rotor flux angular velocity estimate
float stator_phase_ = NAN; // [rad] rotor flux phase angle estimate
};
#endif // __ASYNC_ESTIMATOR_HPP
Firmware/MotorControl/axis.cpp
+184 -224
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/axis.hpp
+28 -88
View File
@@ -4,13 +4,15 @@
class Axis;
#include "encoder.hpp"
#include "async_estimator.hpp"
#include "sensorless_estimator.hpp"
#include "controller.hpp"
#include "open_loop_controller.hpp"
#include "trapTraj.hpp"
#include "endstop.hpp"
#include "low_level.h"
#include "utils.hpp"
#include "communication/interface_uart.h" // TODO: remove once uart_poll() is gone
#include "task_timer.hpp"
#include <array>
@@ -28,6 +30,22 @@ public:
bool finish_on_enc_idx = false;
};
struct TaskTimes {
TaskTimer thermistor_update;
TaskTimer encoder_update;
TaskTimer sensorless_estimator_update;
TaskTimer endstop_update;
TaskTimer can_heartbeat;
TaskTimer controller_update;
TaskTimer open_loop_controller_update;
TaskTimer async_estimator_update;
TaskTimer motor_update;
TaskTimer current_controller_update;
TaskTimer dc_calib;
TaskTimer current_sense;
TaskTimer pwm_update;
};
static LockinConfig_t default_calibration();
static LockinConfig_t default_sensorless();
static LockinConfig_t default_lockin();
@@ -38,7 +56,6 @@ public:
// this only has an effect if encoder.config.use_index is also true
bool startup_encoder_offset_calibration = false; //<! run encoder offset calibration after startup, skip otherwise
bool startup_closed_loop_control = false; //<! enable closed loop control after calibration/startup
bool startup_sensorless_control = false; //<! enable sensorless control after calibration/startup
bool startup_homing = false; //<! enable homing after calibration/startup
bool enable_step_dir = false; //<! enable step/dir input after calibration
@@ -48,6 +65,8 @@ public:
//<! This setting only takes effect on a state transition
//<! into idle or out of closed loop control.
bool enable_sensorless_mode = false;
float turns_per_step = 1.0f / 1024.0f;
float watchdog_timeout = 0.0f; // [s]
@@ -74,10 +93,6 @@ public:
bool is_homed = false;
};
enum thread_signals {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
};
Axis(int axis_num,
uint16_t default_step_gpio_pin,
uint16_t default_dir_gpio_pin,
@@ -95,10 +110,8 @@ public:
bool apply_config();
void clear_config();
bool setup();
void start_thread();
void signal_current_meas();
bool wait_for_current_meas();
bool wait_for_control_iteration();
void step_cb();
void set_step_dir_active(bool enable);
@@ -106,94 +119,19 @@ public:
bool check_DRV_fault();
bool check_PSU_brownout();
bool do_checks();
bool do_updates();
bool do_checks(uint32_t timestamp);
void watchdog_feed();
bool watchdog_check();
void clear_errors() {
motor_.error_ = Motor::ERROR_NONE;
controller_.error_ = Controller::ERROR_NONE;
sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
encoder_.error_ = Encoder::ERROR_NONE;
encoder_.spi_error_rate_ = 0.0f;
error_ = ERROR_NONE;
}
// True if there are no errors
bool inline check_for_errors() {
return error_ == ERROR_NONE;
}
// @brief Runs the specified update handler at the frequency of the current measurements.
//
// The loop runs until one of the following conditions:
// - update_handler returns false
// - the current measurement times out
// - the health checks fail (brownout, driver fault line)
// - update_handler doesn't update the modulation timings in time
// This criterion is ignored if current_state is AXIS_STATE_IDLE
//
// If update_handler is going to update the motor timings, you must call motor.arm()
// shortly before this function.
//
// If the function returns, it is guaranteed that error is non-zero, except if the cause
// for the exit was a negative return value of update_handler or an external
// state change request (requested_state != AXIS_STATE_DONT_CARE).
// Under all exit conditions the motor is disarmed and the brake current set to zero.
// Furthermore, if the update_handler does not set the phase voltages in time, they will
// go to zero.
//
// @tparam T Must be a callable type that takes no arguments and returns a bool
template<typename T>
void run_control_loop(const T& update_handler) {
while (requested_state_ == AXIS_STATE_UNDEFINED) {
// look for errors at axis level and also all subcomponents
bool checks_ok = do_checks();
// Update all estimators
// Note: updates run even if checks fail
bool updates_ok = do_updates();
// make sure the watchdog is being fed.
bool watchdog_ok = watchdog_check();
if (!checks_ok || !updates_ok || !watchdog_ok) {
// It's not useful to quit idle since that is the safe action
// Also leaving idle would rearm the motors
if (current_state_ != AXIS_STATE_IDLE)
break;
}
// Run main loop function, defer quitting for after wait
// TODO: change arming logic to arm after waiting
bool main_continue = update_handler();
if (axis_num_ == 0) {
uart_poll(); // TODO: move to board-level control loop once it exists
}
// Check we meet deadlines after queueing
++loop_counter_;
// Wait until the current measurement interrupt fires
if (!wait_for_current_meas()) {
// maybe the interrupt handler is dead, let's be
// safe and float the phases
safety_critical_disarm_motor_pwm(motor_);
update_brake_current();
error_ |= ERROR_CURRENT_MEASUREMENT_TIMEOUT;
break;
}
if (!main_continue)
break;
}
}
bool start_closed_loop_control();
bool stop_closed_loop_control();
bool run_lockin_spin(const LockinConfig_t &lockin_config);
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_homing();
bool run_idle_loop();
@@ -212,14 +150,17 @@ public:
Config_t config_;
Encoder& encoder_;
AsyncEstimator async_estimator_;
SensorlessEstimator& sensorless_estimator_;
Controller& controller_;
OpenLoopController open_loop_controller_;
OnboardThermistorCurrentLimiter& fet_thermistor_;
OffboardThermistorCurrentLimiter& motor_thermistor_;
Motor& motor_;
TrapezoidalTrajectory& trap_traj_;
Endstop& min_endstop_;
Endstop& max_endstop_;
TaskTimes task_times_;
// List of current_limiters and thermistors to
// provide easy iteration.
@@ -242,7 +183,6 @@ public:
std::array<AxisState, 10> task_chain_ = { AXIS_STATE_UNDEFINED };
AxisState& current_state_ = task_chain_.front();
uint32_t loop_counter_ = 0;
LockinState lockin_state_ = LOCKIN_STATE_INACTIVE;
Homing_t homing_;
uint32_t last_heartbeat_ = 0;
Firmware/MotorControl/component.hpp
+20
View File
@@ -0,0 +1,20 @@
#ifndef __COMPONENT_HPP
#define __COMPONENT_HPP
#include <stdint.h>
class ComponentBase {
public:
/**
* @brief Shall run the update action of this component.
*
* This function gets called in a low priority interrupt context and is
* allowed to call CMSIS functions.
*
* @param timestamp: The timestamp (in HCLK ticks) for which this update
* is run.
*/
virtual void update(uint32_t timestamp) = 0;
};
#endif // __COMPONENT_HPP
Firmware/MotorControl/controller.cpp
+28 -37
View File
@@ -19,7 +19,6 @@ void Controller::reset() {
void Controller::set_error(Error error) {
error_ |= error;
axis_->error_ |= Axis::ERROR_CONTROLLER_FAILED;
}
//--------------------------------
@@ -27,21 +26,6 @@ void Controller::set_error(Error error) {
//--------------------------------
bool Controller::select_encoder(size_t encoder_num) {
if (encoder_num < AXIS_COUNT) {
Axis* ax = &axes[encoder_num];
pos_estimate_circular_src_ = &ax->encoder_.pos_circular_;
pos_wrap_src_ = &config_.circular_setpoint_range;
pos_estimate_linear_src_ = &ax->encoder_.pos_estimate_;
pos_estimate_valid_src_ = &ax->encoder_.pos_estimate_valid_;
vel_estimate_src_ = &ax->encoder_.vel_estimate_;
vel_estimate_valid_src_ = &ax->encoder_.vel_estimate_valid_;
return true;
} else {
return set_error(Controller::ERROR_INVALID_LOAD_ENCODER), false;
}
}
void Controller::move_to_pos(float goal_point) {
axis_->trap_traj_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,
axis_->trap_traj_.config_.vel_limit,
@@ -114,18 +98,19 @@ static float limitVel(const float vel_limit, const float vel_estimate, const flo
return std::clamp(torque, Tmin, Tmax);
}
bool Controller::update(float* torque_setpoint_output) {
float* pos_estimate_linear = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
? pos_estimate_linear_src_ : nullptr;
float* pos_estimate_circular = (pos_estimate_valid_src_ && *pos_estimate_valid_src_)
? pos_estimate_circular_src_ : nullptr;
float* vel_estimate_src = (vel_estimate_valid_src_ && *vel_estimate_valid_src_)
? vel_estimate_src_ : nullptr;
bool Controller::update() {
float pos_estimate_linear = pos_estimate_linear_src_ ? *pos_estimate_linear_src_ : NAN;
float pos_estimate_circular = pos_estimate_circular_src_ ? *pos_estimate_circular_src_ : NAN;
float pos_wrap = pos_wrap_src_ ? *pos_wrap_src_ : NAN;
float vel_estimate = vel_estimate_src_ ? *vel_estimate_src_ : NAN;
// Reset output just in case the controller fails for any reason
torque_output_ = NAN;
// Calib_anticogging is only true when calibration is occurring, so we can't block anticogging_pos
float anticogging_pos = axis_->encoder_.pos_estimate_ / axis_->encoder_.getCoggingRatio();
if (config_.anticogging.calib_anticogging) {
if (!axis_->encoder_.pos_estimate_valid_ || !axis_->encoder_.vel_estimate_valid_) {
if (std::isnan(axis_->encoder_.pos_estimate_) || std::isnan(axis_->encoder_.vel_estimate_)) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
@@ -225,21 +210,21 @@ bool Controller::update(float* torque_setpoint_output) {
float pos_err;
if (config_.circular_setpoints) {
if(!pos_estimate_circular) {
if (std::isnan(pos_estimate_circular) || std::isnan(pos_wrap)) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
// Keep pos setpoint from drifting
pos_setpoint_ = fmodf_pos(pos_setpoint_, *pos_wrap_src_);
// Circular delta
pos_err = pos_setpoint_ - *pos_estimate_circular;
pos_err = wrap_pm(pos_err, 0.5f * *pos_wrap_src_);
pos_err = pos_setpoint_ - pos_estimate_circular;
pos_err = wrap_pm(pos_err, 0.5f * pos_wrap);
} else {
if(!pos_estimate_linear) {
if (std::isnan(pos_estimate_linear)) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
pos_err = pos_setpoint_ - *pos_estimate_linear;
pos_err = pos_setpoint_ - pos_estimate_linear;
}
vel_des += config_.pos_gain * pos_err;
@@ -258,11 +243,11 @@ bool Controller::update(float* torque_setpoint_output) {
// Check for overspeed fault (done in this module (controller) for cohesion with vel_lim)
if (config_.enable_overspeed_error) { // 0.0f to disable
if (!vel_estimate_src) {
if (std::isnan(vel_estimate)) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
if (std::abs(*vel_estimate_src) > config_.vel_limit_tolerance * vel_lim) {
if (std::abs(vel_estimate) > config_.vel_limit_tolerance * vel_lim) {
set_error(ERROR_OVERSPEED);
return false;
}
@@ -273,7 +258,7 @@ bool Controller::update(float* torque_setpoint_output) {
float vel_gain = config_.vel_gain;
float vel_integrator_gain = config_.vel_integrator_gain;
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_ACIM) {
float effective_flux = axis_->motor_.current_control_.acim_rotor_flux;
float effective_flux = axis_->async_estimator_.rotor_flux_;
float minflux = axis_->motor_.config_.acim_gain_min_flux;
if (fabsf(effective_flux) < minflux)
effective_flux = std::copysignf(minflux, effective_flux);
@@ -295,12 +280,12 @@ bool Controller::update(float* torque_setpoint_output) {
float v_err = 0.0f;
if (config_.control_mode >= CONTROL_MODE_VELOCITY_CONTROL) {
if (!vel_estimate_src) {
if (std::isnan(vel_estimate)) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
v_err = vel_des - *vel_estimate_src;
v_err = vel_des - vel_estimate;
torque += (vel_gain * gain_scheduling_multiplier) * v_err;
// Velocity integral action before limiting
@@ -309,11 +294,11 @@ bool Controller::update(float* torque_setpoint_output) {
// Velocity limiting in current mode
if (config_.control_mode < CONTROL_MODE_VELOCITY_CONTROL && config_.enable_current_mode_vel_limit) {
if (!vel_estimate_src) {
if (std::isnan(vel_estimate)) {
set_error(ERROR_INVALID_ESTIMATE);
return false;
}
torque = limitVel(config_.vel_limit, *vel_estimate_src, vel_gain, torque);
torque = limitVel(config_.vel_limit, vel_estimate, vel_gain, torque);
}
// Torque limiting
@@ -341,6 +326,12 @@ bool Controller::update(float* torque_setpoint_output) {
}
}
if (torque_setpoint_output) *torque_setpoint_output = torque;
torque_output_ = torque;
// TODO: this is inconsistent with the other errors which are sticky.
// However if we make ERROR_INVALID_ESTIMATE sticky then it will be
// confusing that a normal sequence of motor calibration + encoder
// calibration would leave the controller in an error state.
error_ &= ~ERROR_INVALID_ESTIMATE;
return true;
}
Firmware/MotorControl/controller.hpp
+6 -5
View File
@@ -38,7 +38,7 @@ public:
bool enable_current_mode_vel_limit = true; // enable velocity limit in current control mode (requires a valid velocity estimator)
uint8_t axis_to_mirror = -1;
float mirror_ratio = 1.0f;
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration()
uint8_t load_encoder_axis = -1; // default depends on Axis number and is set in load_configuration(). Set to -1 to select sensorless estimator.
// custom setters
Controller* parent;
@@ -67,21 +67,19 @@ public:
bool anticogging_calibration(float pos_estimate, float vel_estimate);
void update_filter_gains();
bool update(float* torque_setpoint);
bool update();
Config_t config_;
Axis* axis_ = nullptr; // set by Axis constructor
Error error_ = ERROR_NONE;
// Inputs
float* pos_estimate_linear_src_ = nullptr;
float* pos_estimate_circular_src_ = nullptr;
bool* pos_estimate_valid_src_ = nullptr;
float* vel_estimate_src_ = nullptr;
bool* vel_estimate_valid_src_ = nullptr;
float* pos_wrap_src_ = nullptr;
float pos_setpoint_ = 0.0f; // [turns]
float vel_setpoint_ = 0.0f; // [turn/s]
// float vel_setpoint = 800.0f; <sensorless example>
@@ -100,6 +98,9 @@ public:
bool anticogging_valid_ = false;
// Outputs
float torque_output_ = NAN;
// custom setters
void set_input_pos(float value) { input_pos_ = value; input_pos_updated(); }
Firmware/MotorControl/encoder.cpp
+113 -66
View File
@@ -64,7 +64,6 @@ void Encoder::set_error(Error error) {
vel_estimate_valid_ = false;
pos_estimate_valid_ = false;
error_ |= error;
axis_->error_ |= Axis::ERROR_ENCODER_FAILED;
}
bool Encoder::do_checks(){
@@ -166,9 +165,6 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
bool Encoder::run_index_search() {
config_.use_index = true;
index_found_ = false;
if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
axis_->motor_.config_.direction = 1;
}
set_idx_subscribe();
bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
@@ -177,7 +173,6 @@ bool Encoder::run_index_search() {
bool Encoder::run_direction_find() {
int32_t init_enc_val = shadow_count_;
axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
Axis::LockinConfig_t lockin_config = axis_->config_.calibration_lockin;
lockin_config.finish_distance = lockin_config.vel * 3.0f; // run for 3 seconds
@@ -190,12 +185,12 @@ bool Encoder::run_direction_find() {
// Check response and direction
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
config_.direction = 1;
} else if (shadow_count_ < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
config_.direction = -1;
} else {
axis_->motor_.config_.direction = 0;
config_.direction = 0;
}
}
@@ -205,10 +200,8 @@ bool Encoder::run_direction_find() {
// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
const float start_lock_duration = 1.0f;
const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz);
// Require index found if enabled
if (config_.use_index && !index_found_) {
@@ -220,55 +213,85 @@ bool Encoder::run_offset_calibration() {
// Therefore we have to sync them for calibration
shadow_count_ = count_in_cpr_;
float voltage_magnitude;
if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
voltage_magnitude = axis_->motor_.config_.calibration_current;
else
return false;
CRITICAL_SECTION() {
// Reset state variables
axis_->open_loop_controller_.Id_setpoint_ = NAN;
axis_->open_loop_controller_.Iq_setpoint_ = NAN;
axis_->open_loop_controller_.Vd_setpoint_ = NAN;
axis_->open_loop_controller_.Vq_setpoint_ = NAN;
axis_->open_loop_controller_.phase_ = 0.0f;
axis_->open_loop_controller_.phase_vel_ = NAN;
float max_current_ramp = axis_->motor_.config_.calibration_current / start_lock_duration * 2.0f;
axis_->open_loop_controller_.max_current_ramp_ = max_current_ramp;
axis_->open_loop_controller_.max_voltage_ramp_ = max_current_ramp;
axis_->open_loop_controller_.max_phase_vel_ramp_ = INFINITY;
axis_->open_loop_controller_.target_current_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? axis_->motor_.config_.calibration_current : 0.0f;
axis_->open_loop_controller_.target_voltage_ = axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL ? 0.0f : axis_->motor_.config_.calibration_current;
axis_->open_loop_controller_.target_vel_ = 0.0f;
axis_->open_loop_controller_.total_distance_ = 0.0f;
axis_->motor_.current_control_.enable_current_control_src_ = (axis_->motor_.config_.motor_type != Motor::MOTOR_TYPE_GIMBAL);
axis_->motor_.current_control_.Id_setpoint_src_ = &axis_->open_loop_controller_.Id_setpoint_;
axis_->motor_.current_control_.Iq_setpoint_src_ = &axis_->open_loop_controller_.Iq_setpoint_;
axis_->motor_.current_control_.Vd_setpoint_src_ = &axis_->open_loop_controller_.Vd_setpoint_;
axis_->motor_.current_control_.Vq_setpoint_src_ = &axis_->open_loop_controller_.Vq_setpoint_;
axis_->motor_.current_control_.phase_src_ =
axis_->async_estimator_.rotor_phase_src_ =
&axis_->open_loop_controller_.phase_;
axis_->motor_.phase_vel_src_ =
axis_->motor_.current_control_.phase_vel_src_ =
axis_->async_estimator_.rotor_phase_vel_src_ =
&axis_->open_loop_controller_.phase_vel_;
}
axis_->wait_for_control_iteration();
axis_->motor_.arm(&axis_->motor_.current_control_);
// go to motor zero phase for start_lock_duration to get ready to scan
int i = 0;
axis_->run_control_loop([&](){
if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
if (axis_->error_ != Axis::ERROR_NONE)
return false;
for (size_t i = 0; i < (size_t)(start_lock_duration * 1000.0f); ++i) {
if (!axis_->motor_.is_armed_) {
return false; // TODO: return "disarmed" error code
}
if (axis_->requested_state_ != Axis::AXIS_STATE_UNDEFINED) {
axis_->motor_.disarm();
return false; // TODO: return "aborted" error code
}
osDelay(1);
}
int32_t init_enc_val = shadow_count_;
uint32_t num_steps = 0;
int64_t encvaluesum = 0;
// scan forward
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
CRITICAL_SECTION() {
axis_->open_loop_controller_.target_vel_ = config_.calib_scan_omega;
axis_->open_loop_controller_.total_distance_ = 0.0f;
}
// scan forward
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
bool reached_target_dist = axis_->open_loop_controller_.total_distance_ >= config_.calib_scan_distance;
if (reached_target_dist) {
break;
}
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NONE)
return false;
num_steps++;
osDelay(1);
}
// Check response and direction
if (shadow_count_ > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
config_.direction = 1;
} else if (shadow_count_ < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
config_.direction = -1;
} else {
// Encoder response error
set_error(ERROR_NO_RESPONSE);
axis_->motor_.disarm();
return false;
}
@@ -279,25 +302,31 @@ bool Encoder::run_offset_calibration() {
calib_scan_response_ = std::abs(shadow_count_ - init_enc_val);
if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) {
set_error(ERROR_CPR_POLEPAIRS_MISMATCH);
axis_->motor_.disarm();
return false;
}
// scan backwards
i = 0;
axis_->run_control_loop([&]() {
float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f);
float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(TIMING_LOG_ENC_CALIB);
CRITICAL_SECTION() {
axis_->open_loop_controller_.target_vel_ = -config_.calib_scan_omega;
}
// scan backwards
while ((axis_->requested_state_ == Axis::AXIS_STATE_UNDEFINED) && axis_->motor_.is_armed_) {
bool reached_target_dist = axis_->open_loop_controller_.total_distance_ <= 0.0f;
if (reached_target_dist) {
break;
}
encvaluesum += shadow_count_;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NONE)
num_steps++;
osDelay(1);
}
// Motor disarmed because of an error
if (!axis_->motor_.is_armed_) {
return false;
}
axis_->motor_.disarm();
config_.offset = encvaluesum / (num_steps * 2);
int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2));
@@ -339,7 +368,7 @@ void Encoder::sample_now() {
case MODE_SPI_ABS_AEAT:
case MODE_SPI_ABS_RLS:
{
axis_->motor_.log_timing(TIMING_LOG_SAMPLE_NOW);
abs_spi_start_transaction();
// Do nothing
} break;
@@ -368,10 +397,8 @@ void Encoder::decode_hall_samples() {
| (read_sampled_gpio(hallC_gpio_) ? 4 : 0);
}
bool Encoder::abs_spi_start_transaction(){
bool Encoder::abs_spi_start_transaction() {
if (mode_ & MODE_FLAG_ABS){
axis_->motor_.log_timing(TIMING_LOG_SPI_START);
if (Stm32SpiArbiter::acquire_task(&spi_task_)) {
spi_task_.ncs_gpio = abs_spi_cs_gpio_;
spi_task_.tx_buf = (uint8_t*)abs_spi_dma_tx_;
@@ -411,8 +438,6 @@ void Encoder::abs_spi_cb(bool success) {
goto done;
}
axis_->motor_.log_timing(TIMING_LOG_SPI_END);
switch (mode_) {
case MODE_SPI_ABS_AMS: {
uint16_t rawVal = abs_spi_dma_rx_[0];
@@ -476,6 +501,7 @@ bool Encoder::update() {
} break;
case MODE_HALL: {
decode_hall_samples();
int32_t hall_cnt;
if (decode_hall(hall_state_, &hall_cnt)) {
delta_enc = hall_cnt - count_in_cpr_;
@@ -485,6 +511,11 @@ bool Encoder::update() {
} else {
if (!config_.ignore_illegal_hall_state) {
set_error(ERROR_ILLEGAL_HALL_STATE);
pos_estimate_ = NAN;
pos_cpr_ = NAN;
vel_estimate_ = NAN;
phase_ = NAN;
phase_vel_ = NAN;
return false;
}
}
@@ -508,8 +539,15 @@ bool Encoder::update() {
if (abs_spi_pos_updated_ == false) {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (1.0f - spi_error_rate_);
if (spi_error_rate_ > 0.005f)
if (spi_error_rate_ > 0.005f) {
set_error(ERROR_ABS_SPI_COM_FAIL);
pos_estimate_ = NAN;
pos_cpr_ = NAN;
vel_estimate_ = NAN;
phase_ = NAN;
phase_vel_ = NAN;
return false;
}
} else {
// Low pass filter the error
spi_error_rate_ += current_meas_period * (0.0f - spi_error_rate_);
@@ -524,7 +562,12 @@ bool Encoder::update() {
}break;
default: {
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
set_error(ERROR_UNSUPPORTED_ENCODER_MODE);
pos_estimate_ = NAN;
pos_cpr_ = NAN;
vel_estimate_ = NAN;
phase_ = NAN;
phase_vel_ = NAN;
return false;
} break;
}
@@ -589,9 +632,13 @@ bool Encoder::update() {
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float);
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
if (is_ready_) {
phase_ = wrap_pm_pi(ph) * config_.direction;
phase_vel_ = (2*M_PI) * vel_estimate_ * axis_->motor_.config_.pole_pairs * config_.direction;
} else {
phase_ = NAN;
phase_vel_ = NAN;
}
vel_estimate_valid_ = true;
pos_estimate_valid_ = true;
return true;
}
Firmware/MotorControl/encoder.hpp
+3 -2
View File
@@ -22,13 +22,13 @@ public:
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // Offset between encoder count and rotor electrical phase
float offset_float = 0.0f; // Sub-count phase alignment offset
int32_t direction = 0.0f; // direction with respect to motor
bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
float calib_scan_distance = 16.0f * M_PI; // rad electrical
float calib_scan_omega = 4.0f * M_PI; // rad/s electrical
float bandwidth = 1000.0f;
bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
bool idx_search_unidirectional = false; // Only allow index search in known direction
bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
uint16_t abs_spi_cs_gpio_pin = 1;
uint16_t sincos_gpio_pin_sin = 3;
@@ -85,7 +85,8 @@ public:
int32_t shadow_count_ = 0;
int32_t count_in_cpr_ = 0;
float interpolation_ = 0.0f;
float phase_ = 0.0f; // [count]
float phase_ = 0.0f; // [rad]
float phase_vel_ = 0.0f; // [rad/s]
float pos_estimate_counts_ = 0.0f; // [count]
float pos_cpr_counts_ = 0.0f; // [count]
float vel_estimate_counts_ = 0.0f; // [count/s]
Firmware/MotorControl/foc.cpp
+160
View File
@@ -0,0 +1,160 @@
#include "foc.hpp"
#include <board.h>
Motor::Error AlphaBetaFrameController::on_measurement(
float vbus_voltage, std::array<float, 3> currents,
uint32_t input_timestamp) {
// Clarke transform
float Ialpha = currents[0];
float Ibeta = one_by_sqrt3 * (currents[1] - currents[2]);
return on_measurement(vbus_voltage, Ialpha, Ibeta, input_timestamp);
}
Motor::Error AlphaBetaFrameController::get_output(
uint32_t output_timestamp, float (&pwm_timings)[3], float* ibus) {
float mod_alpha = NAN;
float mod_beta = NAN;
Motor::Error status = get_alpha_beta_output(output_timestamp, &mod_alpha, &mod_beta, ibus);
if (status != Motor::ERROR_NONE) {
return status;
} else if (std::isnan(mod_alpha) || std::isnan(mod_alpha)) {
return Motor::ERROR_MODULATION_IS_NAN;
} else if (SVM(mod_alpha, mod_beta, &pwm_timings[0], &pwm_timings[1], &pwm_timings[2]) != 0) {
return Motor::ERROR_MODULATION_MAGNITUDE;
}
return Motor::ERROR_NONE;
}
void FieldOrientedController::reset() {
v_current_control_integral_d_ = 0.0f;
v_current_control_integral_q_ = 0.0f;
vbus_voltage_measured_ = NAN;
Ialpha_measured_ = NAN;
Ibeta_measured_ = NAN;
}
Motor::Error FieldOrientedController::on_measurement(
float vbus_voltage, float Ialpha, float Ibeta,
uint32_t input_timestamp) {
// Store the measurements for later processing.
i_timestamp_ = input_timestamp;
vbus_voltage_measured_ = vbus_voltage;
Ialpha_measured_ = Ialpha;
Ibeta_measured_ = Ibeta;
return Motor::ERROR_NONE;
}
ODriveIntf::MotorIntf::Error FieldOrientedController::get_alpha_beta_output(
uint32_t output_timestamp, float* mod_alpha, float* mod_beta, float* ibus) {
if (std::isnan(vbus_voltage_measured_) || std::isnan(Ialpha_measured_) || std::isnan(Ibeta_measured_)) {
// FOC didn't receive a current measurement yet.
return Motor::ERROR_CONTROLLER_INITIALIZING;
} else if (abs((int32_t)(i_timestamp_ - ctrl_timestamp_)) > MAX_CONTROL_LOOP_UPDATE_TO_CURRENT_UPDATE_DELTA) {
// Data from control loop and current measurement are too far apart.
return Motor::ERROR_BAD_TIMING;
}
// TODO: improve efficiency in case PWM updates are requested at a higher
// rate than current sensor updates. In this case we can reuse mod_d and
// mod_q from a previous iteration.
// Fetch member variables into local variables to make the optimizer's life easier.
float vbus_voltage = vbus_voltage_measured_;
float Ialpha = Ialpha_measured_;
float Ibeta = Ibeta_measured_;
float Vd = Vd_setpoint_;
float Vq = Vq_setpoint_;
float Id_setpoint = Id_setpoint_;
float Iq_setpoint = Iq_setpoint_;
float phase = phase_;
float phase_vel = phase_vel_;
if (std::isnan(phase) || std::isnan(phase_vel)) {
return Motor::ERROR_UNKNOWN_PHASE;
}
// Park transform
float I_phase = phase + phase_vel * ((float)(int32_t)(i_timestamp_ - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
float c_I = our_arm_cos_f32(I_phase);
float s_I = our_arm_sin_f32(I_phase);
float Id = c_I * Ialpha + s_I * Ibeta;
float Iq = c_I * Ibeta - s_I * Ialpha;
Iq_measured_ += I_measured_report_filter_k_ * (Iq - Iq_measured_);
Id_measured_ += I_measured_report_filter_k_ * (Id - Id_measured_);
// Current error
float Ierr_d = Id_setpoint - Id;
float Ierr_q = Iq_setpoint - Iq;
if (enable_current_control_) {
// Check for current sense saturation
if (std::isnan(Ierr_d) || std::isnan(Ierr_q)) {
return Motor::ERROR_UNKNOWN_CURRENT;
}
// Apply PI control (V{d,q}_setpoint act as feed-forward terms in this mode)
Vd += v_current_control_integral_d_ + Ierr_d * p_gain_;
Vq += v_current_control_integral_q_ + Ierr_q * p_gain_;
}
if (std::isnan(vbus_voltage)) {
return Motor::ERROR_UNKNOWN_VBUS_VOLTAGE;
}
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;
if (enable_current_control_) {
// 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
v_current_control_integral_d_ *= 0.99f;
v_current_control_integral_q_ *= 0.99f;
} else {
v_current_control_integral_d_ += Ierr_d * (i_gain_ * current_meas_period);
v_current_control_integral_q_ += Ierr_q * (i_gain_ * current_meas_period);
}
}
// Inverse park transform
float pwm_phase = phase_ + phase_vel_ * ((float)(int32_t)(output_timestamp - ctrl_timestamp_) / (float)TIM_1_8_CLOCK_HZ);
float c_p = our_arm_cos_f32(pwm_phase);
float s_p = our_arm_sin_f32(pwm_phase);
float mod_alpha_temp = c_p * mod_d - s_p * mod_q;
float mod_beta_temp = c_p * mod_q + s_p * mod_d;
// Report final applied voltage in stationary frame (for sensorless estimator)
final_v_alpha_ = mod_to_V * mod_alpha_temp;
final_v_beta_ = mod_to_V * mod_beta_temp;
*mod_alpha = mod_alpha_temp;
*mod_beta = mod_beta_temp;
*ibus = mod_d * Id + mod_q * Iq;
return Motor::ERROR_NONE;
}
void FieldOrientedController::update(uint32_t timestamp) {
CRITICAL_SECTION() {
ctrl_timestamp_ = timestamp;
enable_current_control_ = enable_current_control_src_;
Id_setpoint_ = Id_setpoint_src_ ? *Id_setpoint_src_ : NAN;
Iq_setpoint_ = Iq_setpoint_src_ ? *Iq_setpoint_src_ : NAN;
Vd_setpoint_ = Vd_setpoint_src_ ? *Vd_setpoint_src_ : NAN;
Vq_setpoint_ = Vq_setpoint_src_ ? *Vq_setpoint_src_ : NAN;
phase_ = phase_src_ ? *phase_src_ : NAN;
phase_vel_ = phase_vel_src_ ? *phase_vel_src_ : NAN;
}
}
Firmware/MotorControl/foc.hpp
+67
View File
@@ -0,0 +1,67 @@
#ifndef __FOC_HPP
#define __FOC_HPP
#include "phase_control_law.hpp"
#include "component.hpp"
/**
* @brief Field oriented controller.
*
* This controller can run in either current control mode or voltage control
* mode.
*/
class FieldOrientedController : public AlphaBetaFrameController, public ComponentBase {
public:
void update(uint32_t timestamp) final;
void reset() final;
ODriveIntf::MotorIntf::Error on_measurement(
float vbus_voltage, float Ialpha, float Ibeta, uint32_t input_timestamp) final;
ODriveIntf::MotorIntf::Error get_alpha_beta_output(
uint32_t output_timestamp, float* mod_alpha, float* mod_beta, float* ibus) final;
// Config - these values are set while this controller is inactive
float p_gain_ = NAN; // [V/A] should be auto set after resistance and inductance measurement
float i_gain_ = NAN; // [V/As] should be auto set after resistance and inductance measurement
float I_measured_report_filter_k_ = 1.0f;
// Inputs
bool enable_current_control_src_ = false;
float* Id_setpoint_src_ = nullptr;
float* Iq_setpoint_src_ = nullptr;
float* Vd_setpoint_src_ = nullptr;
float* Vq_setpoint_src_ = nullptr;
float* phase_src_ = nullptr;
float* phase_vel_src_ = nullptr;
// These values are set atomically by the update() function and read by the
// calculate() function in an interrupt context.
uint32_t ctrl_timestamp_; // [HCLK ticks]
bool enable_current_control_ = false; // true: FOC runs in current control mode using I{dq}_setpoint, false: FOC runs in voltage control mode using V{dq}_setpoint
float Id_setpoint_; // [A] only used if enable_current_control_ == true
float Iq_setpoint_; // [A] only used if enable_current_control_ == true
float Vd_setpoint_; // [V] acts as input if enable_current_control_ == false and as output otherwise
float Vq_setpoint_; // [V] acts as input if enable_current_control_ == false and as output otherwise
float phase_; // [rad]
float phase_vel_; // [rad/s]
// These values (or some of them) are updated inside on_measurement() and get_alpha_beta_output()
uint32_t i_timestamp_;
float vbus_voltage_measured_ = NAN; // [V]
float Ialpha_measured_ = NAN; // [A]
float Ibeta_measured_ = NAN; // [A]
float Id_measured_ = 0.0f; // [A]
float Iq_measured_ = 0.0f; // [A]
float v_current_control_integral_d_ = 0.0f; // [V]
float v_current_control_integral_q_ = 0.0f; // [V]
//float mod_to_V_ = 0.0f;
//float mod_d_ = 0.0f;
//float mod_q_ = 0.0f;
//float ibus_ = 0.0f;
float final_v_alpha_ = 0.0f; // [V]
float final_v_beta_ = 0.0f; // [V]
};
#endif // __FOC_HPP
Firmware/MotorControl/low_level.cpp
+42 -223
View File
@@ -72,84 +72,14 @@ bool brake_resistor_saturated = false;
* at a high rate.
*/
// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i].motor_);
axes[i].motor_.error_ |= error;
}
safety_critical_disarm_brake_resistor();
}
// @brief Kicks off the arming process of the motor.
// All calls to this function must clearly originate
// from user input.
void safety_critical_arm_motor_pwm(Motor& motor) {
uint32_t mask = cpu_enter_critical();
if (brake_resistor_armed) {
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
}
cpu_exit_critical(mask);
}
// @brief Disarms the motor PWM.
// After calling this function, it is guaranteed that all three
// motor phases are floating and will not be enabled again until
// safety_critical_arm_motor_phases is called.
// @returns true if the motor was in a state other than disarmed before
bool safety_critical_disarm_motor_pwm(Motor& motor) {
uint32_t mask = cpu_enter_critical();
bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.timer_);
cpu_exit_critical(mask);
return was_armed;
}
// @brief Updates the phase timings unless the motor is disarmed.
//
// If this is called at a rate higher than the motor's timer period,
// the actual PMW timings on the pins can be undefined for up to one
// timer period.
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) {
uint32_t mask = cpu_enter_critical();
if (!brake_resistor_armed) {
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
}
motor.timer_->Instance->CCR1 = timings[0];
motor.timer_->Instance->CCR2 = timings[1];
motor.timer_->Instance->CCR3 = timings[2];
if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) {
// timings were just loaded into the timer registers
// the timer register are buffered, so they won't have an effect
// on the output just yet so we need to wait until the next
// interrupt before we actually enable the output
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE;
} else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) {
// now we waited long enough. Enter armed state and
// enable the actual PWM outputs.
motor.armed_state_ = Motor::ARMED_STATE_ARMED;
__HAL_TIM_MOE_ENABLE(motor.timer_); // enable pwm outputs
} else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) {
// nothing to do, PWM is running, all good
} else {
// unknown state oh no
safety_critical_disarm_motor_pwm(motor);
}
cpu_exit_critical(mask);
}
// @brief Arms the brake resistor
void safety_critical_arm_brake_resistor() {
uint32_t mask = cpu_enter_critical();
brake_resistor_armed = true;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
cpu_exit_critical(mask);
CRITICAL_SECTION() {
brake_resistor_armed = true;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
}
}
// @brief Disarms the brake resistor and by extension
@@ -157,40 +87,48 @@ void safety_critical_arm_brake_resistor() {
// After calling this, the brake resistor can only be armed again
// by calling safety_critical_arm_brake_resistor().
void safety_critical_disarm_brake_resistor() {
uint32_t mask = cpu_enter_critical();
brake_resistor_armed = false;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i].motor_);
bool brake_resistor_was_armed = brake_resistor_armed;
CRITICAL_SECTION() {
brake_resistor_armed = false;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
}
// Check necessary to prevent infinite recursion
if (brake_resistor_was_armed) {
for (auto& axis: axes) {
axis.motor_.disarm();
}
}
cpu_exit_critical(mask);
}
// @brief Updates the brake resistor PWM timings unless
// the brake resistor is disarmed.
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) {
if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS)
low_level_fault(Motor::ERROR_BRAKE_DEADTIME_VIOLATION);
uint32_t mask = cpu_enter_critical();
if (brake_resistor_armed) {
// Safe update of low and high side timings
// To avoid race condition, first reset timings to safe state
// ch3 is low side, ch4 is high side
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
htim2.Instance->CCR3 = low_off;
htim2.Instance->CCR4 = high_on;
if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS) {
odrv.disarm_with_error(ODrive::ERROR_BRAKE_DEADTIME_VIOLATION);
}
CRITICAL_SECTION() {
if (brake_resistor_armed) {
// Safe update of low and high side timings
// To avoid race condition, first reset timings to safe state
// ch3 is low side, ch4 is high side
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
htim2.Instance->CCR3 = low_off;
htim2.Instance->CCR4 = high_on;
}
}
cpu_exit_critical(mask);
}
/* Function implementations --------------------------------------------------*/
void start_adc_pwm() {
// Disarm motors
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i].motor_);
for (auto& axis: axes) {
axis.motor_.disarm();
}
for (Motor& motor: motors) {
@@ -215,26 +153,9 @@ void start_adc_pwm() {
__HAL_ADC_ENABLE(&hadc3);
// Warp field stabilize.
osDelay(2);
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_JEOC);
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_JEOC);
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_JEOC);
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_EOC);
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_EOC);
__HAL_ADC_CLEAR_FLAG(&hadc1, ADC_FLAG_OVR);
__HAL_ADC_CLEAR_FLAG(&hadc2, ADC_FLAG_OVR);
__HAL_ADC_CLEAR_FLAG(&hadc3, ADC_FLAG_OVR);
__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);
for (Motor& motor: motors) {
// Enable the update interrupt (used to coherently sample GPIO)
__HAL_TIM_CLEAR_IT(motor.timer_, TIM_IT_UPDATE);
__HAL_TIM_ENABLE_IT(motor.timer_, TIM_IT_UPDATE);
}
start_timers();
// Start brake resistor PWM in floating output configuration
@@ -372,111 +293,9 @@ float get_adc_voltage_channel(uint16_t channel)
// IRQ Callbacks
//--------------------------------
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
void vbus_sense_adc_cb(uint32_t adc_value) {
constexpr float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale;
// 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
constexpr float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
// Ensure ADCs are expected ones to simplify the logic below
if (!(hadc == &hadc2 || hadc == &hadc3)) {
low_level_fault(Motor::ERROR_ADC_FAILED);
return;
};
// Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion
// Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion
// If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current
// If we are counting down, we just sampled in SVM vector 7, with zero current
Axis& axis = injected ? axes[0] : axes[1];
int axis_num = injected ? 0 : 1;
Axis& other_axis = injected ? axes[1] : axes[0];
bool counting_down = axis.motor_.timer_->Instance->CR1 & TIM_CR1_DIR;
bool current_meas_not_DC_CAL = !counting_down;
// Check the timing of the sequencing
if (current_meas_not_DC_CAL)
axis.motor_.log_timing(TIMING_LOG_ADC_CB_I);
else
axis.motor_.log_timing(TIMING_LOG_ADC_CB_DC);
bool update_timings = false;
if (hadc == &hadc2) {
if (&axis == &axes[1] && counting_down)
update_timings = true; // update timings of M0
else if (&axis == &axes[0] && !counting_down)
update_timings = true; // update timings of M1
// TODO: this is out of place here. However when moving it somewhere
// else we have to consider the timing requirements to prevent the SPI
// transfers of axis0 and axis1 from conflicting.
// Also see comment on sync_timers.
if((current_meas_not_DC_CAL && !axis_num) ||
(axis_num && !current_meas_not_DC_CAL)){
axis.encoder_.abs_spi_start_transaction();
}
}
// Load next timings for the motor that we're not currently sampling
if (update_timings) {
if (!other_axis.motor_.next_timings_valid_) {
// the motor control loop failed to update the timings in time
// we must assume that it died and therefore float all phases
bool was_armed = safety_critical_disarm_motor_pwm(other_axis.motor_);
if (was_armed) {
other_axis.motor_.error_ |= Motor::ERROR_CONTROL_DEADLINE_MISSED;
}
} else {
other_axis.motor_.next_timings_valid_ = false;
safety_critical_apply_motor_pwm_timings(
other_axis.motor_, other_axis.motor_.next_timings_
);
}
update_brake_current();
}
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;
}
// Prepare hall readings
// TODO move this to inside encoder update function
axis.encoder_.decode_hall_samples();
// Trigger axis thread
axis.signal_current_meas();
} else {
// DC_CAL measurement
if (hadc == &hadc2) {
axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k;
} else {
axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
}
}
vbus_voltage = adc_value * voltage_scale;
}
// @brief Sums up the Ibus contribution of each motor and updates the
@@ -484,8 +303,8 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
void update_brake_current() {
float Ibus_sum = 0.0f;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
if (axes[i].motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
Ibus_sum += axes[i].motor_.current_control_.Ibus;
if (axes[i].motor_.is_armed_) {
Ibus_sum += axes[i].motor_.I_bus_;
}
}
@@ -499,7 +318,7 @@ void update_brake_current() {
if (std::isnan(brake_duty)) {
// Shuts off all motors AND brake resistor, sets error code on all motors.
low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN);
odrv.disarm_with_error(ODrive::ERROR_BRAKE_DUTY_CYCLE_NAN);
return;
}
@@ -516,11 +335,11 @@ void update_brake_current() {
ibus_ += odrv.ibus_report_filter_k_ * (Ibus_sum - ibus_);
if (Ibus_sum > odrv.config_.dc_max_positive_current) {
low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT);
odrv.disarm_with_error(ODrive::ERROR_DC_BUS_OVER_CURRENT);
return;
}
if (Ibus_sum < odrv.config_.dc_max_negative_current) {
low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT);
odrv.disarm_with_error(ODrive::ERROR_DC_BUS_OVER_REGEN_CURRENT);
return;
}
Firmware/MotorControl/low_level.h
+1 -5
View File
@@ -25,17 +25,13 @@ extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT];
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
void safety_critical_arm_motor_pwm(Motor& motor);
bool safety_critical_disarm_motor_pwm(Motor& motor);
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]);
void safety_critical_arm_brake_resistor();
void safety_critical_disarm_brake_resistor();
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on);
// called from STM platform code
extern "C" {
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(uint32_t adc_value);
void pwm_in_cb(TIM_HandleTypeDef *htim);
}
Firmware/MotorControl/main.cpp
+152 -41
View File
@@ -17,9 +17,6 @@ osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
osSemaphoreId sem_can;
osThreadId usb_irq_thread;
const uint32_t stack_size_usb_irq_thread = 2048; // Bytes
#if defined(STM32F405xx)
// Place FreeRTOS heap in core coupled memory for better performance
__attribute__((section(".ccmram")))
@@ -150,28 +147,25 @@ void ODrive::enter_dfu_mode() {
}
}
static void usb_deferred_interrupt_thread(void * ctx) {
(void) ctx; // unused parameter
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever);
if (semaphore_status == osOK) {
// We have a new incoming USB transmission: handle it
HAL_PCD_IRQHandler(&usb_pcd_handle);
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ((usb_pcd_handle.Instance == USB_OTG_FS) ? OTG_FS_IRQn : OTG_HS_IRQn);
}
void ODrive::clear_errors() {
for (auto& axis: axes) {
axis.motor_.error_ = Motor::ERROR_NONE;
axis.controller_.error_ = Controller::ERROR_NONE;
axis.sensorless_estimator_.error_ = SensorlessEstimator::ERROR_NONE;
axis.encoder_.error_ = Encoder::ERROR_NONE;
axis.encoder_.spi_error_rate_ = 0.0f;
axis.error_ = Axis::ERROR_NONE;
}
error_ = ERROR_NONE;
}
extern "C" {
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
for(auto& axis : axes){
safety_critical_disarm_motor_pwm(axis.motor_);
for(auto& axis: axes){
axis.motor_.disarm();
}
safety_critical_disarm_brake_resistor();
safety_critical_disarm_brake_resistor();
for (;;); // TODO: safe action
}
@@ -184,7 +178,6 @@ void vApplicationIdleHook(void) {
odrv.system_stats_.min_stack_space_axis = *std::min_element(std::begin(min_stack_space), std::end(min_stack_space));
odrv.system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
odrv.system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t);
@@ -192,7 +185,6 @@ void vApplicationIdleHook(void) {
odrv.system_stats_.stack_usage_axis = axes[0].stack_size_ - odrv.system_stats_.min_stack_space_axis;
odrv.system_stats_.stack_usage_usb = stack_size_usb_thread - odrv.system_stats_.min_stack_space_usb;
odrv.system_stats_.stack_usage_uart = stack_size_uart_thread - odrv.system_stats_.min_stack_space_uart;
odrv.system_stats_.stack_usage_usb_irq = stack_size_usb_irq_thread - odrv.system_stats_.min_stack_space_usb_irq;
odrv.system_stats_.stack_usage_startup = stack_size_default_task - odrv.system_stats_.min_stack_space_startup;
odrv.system_stats_.stack_usage_can = odCAN->stack_size_ - odrv.system_stats_.min_stack_space_can;
}
@@ -200,6 +192,140 @@ void vApplicationIdleHook(void) {
}
/**
* @brief Runs system-level checks that need to be as real-time as possible.
*
* This function is called after every current measurement of every motor.
* It should finish as quickly as possible.
*/
void ODrive::do_fast_checks() {
if (!(vbus_voltage >= config_.dc_bus_undervoltage_trip_level))
disarm_with_error(ERROR_DC_BUS_UNDER_VOLTAGE);
if (!(vbus_voltage <= config_.dc_bus_overvoltage_trip_level))
disarm_with_error(ERROR_DC_BUS_OVER_VOLTAGE);
}
/**
* @brief Floats all power phases on the system (all motors and brake resistors).
*
* This should be called if a system level exception ocurred that makes it
* unsafe to run power through the system in general.
*/
void ODrive::disarm_with_error(Error error) {
CRITICAL_SECTION() {
for (auto& axis: axes) {
axis.motor_.disarm_with_error(Motor::ERROR_SYSTEM_LEVEL);
}
safety_critical_disarm_brake_resistor();
error_ |= error;
}
}
/**
* @brief Runs the periodic sampling tasks
*
* All components that need to sample real-world data should do it in this
* function as it runs on a high interrupt priority and provides lowest possible
* timing jitter.
*
* All function called from this function should adhere to the following rules:
* - Try to use the same number of CPU cycles in every iteration.
* (reason: Tasks that run later in the function still want lowest possible timing jitter)
* - Use as few cycles as possible.
* (reason: The interrupt blocks other important interrupts (TODO: which ones?))
* - Not call any FreeRTOS functions.
* (reason: The interrupt priority is higher than the max allowed priority for syscalls)
*
* Time consuming and undeterministic logic/arithmetic should live on
* control_loop_cb() instead.
*/
void ODrive::sampling_cb() {
n_evt_sampling_++;
MEASURE_TIME(task_times_.sampling) {
for (auto& axis: axes) {
axis.encoder_.sample_now();
}
}
}
/**
* @brief Runs the periodic control loop.
*
* This function is executed in a low priority interrupt context and is allowed
* to call CMSIS functions.
*
* Yet it runs at a higher priority than communication workloads.
*
* @param update_cnt: The true count of update events (wrapping around at 16
* bits). This is used for timestamp calculation in the face of
* potentially missed timer update interrupts. Therefore this counter
* must not rely on any interrupts.
*/
void ODrive::control_loop_cb(uint32_t timestamp) {
last_update_timestamp_ = timestamp;
n_evt_control_loop_++;
// TODO: use a configurable component list for most of the following things
MEASURE_TIME(task_times_.control_loop_misc) {
uart_poll();
odrv.oscilloscope_.update();
}
MEASURE_TIME(task_times_.control_loop_checks) {
for (auto& axis: axes) {
// look for errors at axis level and also all subcomponents
bool checks_ok = axis.do_checks(timestamp);
// make sure the watchdog is being fed.
bool watchdog_ok = axis.watchdog_check();
if (!checks_ok || !watchdog_ok) {
axis.motor_.disarm();
}
}
}
for (auto& axis: axes) {
// Sub-components should use set_error which will propegate to this error_
MEASURE_TIME(axis.task_times_.thermistor_update) {
for (ThermistorCurrentLimiter* thermistor : axis.thermistors_) {
thermistor->update();
}
}
MEASURE_TIME(axis.task_times_.encoder_update)
axis.encoder_.update();
MEASURE_TIME(axis.task_times_.sensorless_estimator_update)
axis.sensorless_estimator_.update();
MEASURE_TIME(axis.task_times_.endstop_update) {
axis.min_endstop_.update();
axis.max_endstop_.update();
}
MEASURE_TIME(axis.task_times_.can_heartbeat)
odCAN->send_heartbeat(&axis);
MEASURE_TIME(axis.task_times_.controller_update)
axis.controller_.update(); // uses position and velocity from encoder
MEASURE_TIME(axis.task_times_.open_loop_controller_update)
axis.open_loop_controller_.update(timestamp);
MEASURE_TIME(axis.task_times_.async_estimator_update)
axis.async_estimator_.update(timestamp);
MEASURE_TIME(axis.task_times_.motor_update)
axis.motor_.update(); // uses torque from controller and phase_vel from encoder
MEASURE_TIME(axis.task_times_.current_controller_update)
axis.motor_.current_control_.update(timestamp); // uses the output of controller_ or open_loop_contoller_ and encoder_ or sensorless_estimator_ or async_estimator_
}
}
/** @brief For diagnostics only */
uint32_t ODrive::get_interrupt_status(int32_t irqn) {
@@ -259,30 +385,20 @@ static void rtos_main(void*) {
// must happen after communication is initialized
pwm0_input.init();
// Set up hardware for all components
for (size_t i = 0; i < AXIS_COUNT; ++i) {
if (!axes[i].setup()) {
for (;;) {
osDelay(10); // TODO: proper error handling
}
}
// Try to initialized gate drivers for fault-free startup.
// If this does not succeed, a fault will be raised and the idle loop will
// periodically attempt to reinit the gate driver.
for(auto& axis: axes){
axis.motor_.setup();
}
for(auto& axis : axes){
for(auto& axis: axes){
axis.encoder_.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
@@ -537,11 +653,6 @@ extern "C" int main(void) {
sem_can = osSemaphoreCreate(osSemaphore(sem_can), 1);
osSemaphoreWait(sem_can, 0);
// Start USB interrupt handler thread
osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, stack_size_usb_irq_thread / sizeof(StackType_t));
usb_irq_thread = osThreadCreate(osThread(task_usb_pump), NULL);
// Construct all objects.
odCAN = new ODriveCAN(can_config, &hcan1);
Firmware/MotorControl/motor.cpp
+480 -308
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/motor.hpp
+48 -91
View File
@@ -5,53 +5,17 @@ class Axis; // declared in axis.hpp
class Motor;
#include <board.h>
#include <autogen/interfaces.hpp>
enum TimingLog_t {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_SPI_START,
TIMING_LOG_SAMPLE_NOW,
TIMING_LOG_SPI_END,
TIMING_LOG_NUM_SLOTS
};
#include "foc.hpp"
class Motor : public ODriveIntf::MotorIntf {
public:
struct Iph_BC_t {
struct Iph_ABC_t {
float phA;
float phB;
float phC;
};
struct CurrentControl_t{
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 Id_setpoint; // [A]
float Iq_setpoint; // [A]
float Iq_measured; // [A]
float Id_measured; // [A]
float I_measured_report_filter_k;
float max_allowed_current; // [A]
float overcurrent_trip_level; // [A]
float acim_rotor_flux; // [A]
float async_phase_vel; // [rad/s electrical]
float async_phase_offset; // [rad electrical]
};
// NOTE: for gimbal motors, all units of Nm are instead V.
// example: vel_gain is [V/(turn/s)] instead of [Nm/(turn/s)]
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
@@ -63,7 +27,6 @@ public:
float phase_inductance = 0.0f; // to be set by measure_phase_inductance
float phase_resistance = 0.0f; // to be set by measure_phase_resistance
float torque_constant = 0.04f; // [Nm/A] for PM motors, [Nm/A^2] for induction motors. Equal to 8.27/Kv of the motor
int32_t direction = 0; // 1 or -1 (0 = unspecified)
MotorType motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// float current_lim = 70.0f; //[A]
@@ -75,15 +38,22 @@ public:
float current_control_bandwidth = 1000.0f; // [rad/s]
float inverter_temp_limit_lower = 100;
float inverter_temp_limit_upper = 120;
float acim_slip_velocity = 14.706f; // [rad/s electrical] = 1/rotor_tau
float acim_gain_min_flux = 10; // [A]
float acim_autoflux_min_Id = 10; // [A]
bool acim_autoflux_enable = false;
float acim_autoflux_attack_gain = 10.0f;
float acim_autoflux_decay_gain = 1.0f;
bool R_wL_FF_enable = false; // Enable feedforwards for R*I and w*L*I terms
bool bEMF_FF_enable = false; // Enable feedforward for bEMF
float I_bus_hard_min = -INFINITY;
float I_bus_hard_max = INFINITY;
float I_leak_max = 0.1f;
float dc_calib_tau = 0.2f;
// custom property setters
Motor* parent = nullptr;
void set_pre_calibrated(bool value) {
@@ -96,37 +66,36 @@ public:
};
Motor(TIM_HandleTypeDef* timer,
uint16_t control_deadline,
uint8_t current_sensor_mask,
float shunt_conductance,
TGateDriver& gate_driver,
TOpAmp& opamp);
bool arm();
void disarm();
bool arm(PhaseControlLaw<3>* control_law);
void apply_pwm_timings(uint16_t timings[3], bool tentative);
bool disarm(bool* was_armed = nullptr);
bool apply_config();
bool setup();
void reset_current_control();
void update_current_controller_gains();
void set_error(Error error);
bool do_checks();
void disarm_with_error(Error error);
bool do_checks(uint32_t timestamp);
float effective_current_lim();
float max_available_torque();
void log_timing(TimingLog_t log_idx);
float phase_current_from_adcval(uint32_t ADCValue);
bool measure_phase_resistance(float test_current, float max_voltage);
bool measure_phase_inductance(float voltage_low, float voltage_high);
bool measure_phase_inductance(float test_voltage);
bool run_calibration();
bool enqueue_modulation_timings(float mod_alpha, float mod_beta);
bool enqueue_voltage_timings(float v_alpha, float v_beta);
bool FOC_voltage(float v_d, float v_q, float pwm_phase);
bool FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_phase, float phase_vel);
bool update(float current_setpoint, float phase, float phase_vel);
void tim_update_cb();
void update();
// These functions are called as appropriate from the board.cpp file.
void current_meas_cb(uint32_t timestamp, Iph_ABC_t current);
void dc_calib_cb(uint32_t timestamp, Iph_ABC_t current);
void pwm_update_cb(uint32_t output_timestamp);
// hardware config
TIM_HandleTypeDef* const timer_;
const uint16_t control_deadline_;
const uint8_t current_sensor_mask_;
const float shunt_conductance_;
TGateDriver& gate_driver_;
TOpAmp& opamp_;
@@ -136,49 +105,37 @@ public:
//private:
uint16_t next_timings_[3] = {
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2
};
bool next_timings_valid_ = false;
uint16_t last_cpu_time_ = 0;
int timing_log_index_ = 0;
struct {
uint16_t& operator[](size_t idx) { return content[idx]; }
uint16_t& get(size_t idx) { return content[idx]; }
uint16_t content[TIMING_LOG_NUM_SLOTS];
} timing_log_;
uint32_t n_evt_current_measurement_ = 0;
uint32_t n_evt_pwm_update_ = 0;
// variables exposed on protocol
Error error_ = ERROR_NONE;
// Do not write to this variable directly!
// It is for exclusive use by the safety_critical_... functions.
ArmedState armed_state_ = ARMED_STATE_DISARMED;
bool is_armed_ = false;
bool is_calibrated_ = config_.pre_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
Iph_ABC_t current_meas_ = {NAN, NAN, NAN};
Iph_ABC_t DC_calib_ = {0.0f, 0.0f, 0.0f};
float dc_calib_running_since_ = 0.0f; // current sensor calibration needs some time to settle
float I_leak_ = NAN; // close to zero if only two current sensors are available
float I_bus_ = 0.0f; // this motors contribution to the bus current
bool current_meas_valid_ = false; // if false, the measured current values must not be used for control
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
CurrentControl_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,
.Id_setpoint = 0.0f,
.Iq_setpoint = 0.0f,
.Iq_measured = 0.0f,
.Id_measured = 0.0f,
.I_measured_report_filter_k = 1.0f,
.max_allowed_current = 0.0f,
.overcurrent_trip_level = 0.0f,
.acim_rotor_flux = 0.0f,
.async_phase_vel = 0.0f,
.async_phase_offset = 0.0f,
};
FieldOrientedController current_control_;
float effective_current_lim_ = 10.0f; // [A]
float max_allowed_current_ = 0.0f; // [A] set in setup()
float max_dc_calib_ = 0.0f; // [A] set in setup()
float* torque_setpoint_src_ = nullptr; // Usually points to the Controller object's output
float* phase_vel_src_ = nullptr; // Usually points to the Encoder object's output
float direction_ = 0.0f; // if -1 then positive torque is converted to negative Iq
float Vd_setpoint_ = NAN; // fed to the FOC
float Vq_setpoint_ = NAN; // fed to the FOC
float Id_setpoint_ = 0.0f; // fed to the FOC
float Iq_setpoint_ = NAN; // fed to the FOC
PhaseControlLaw<3>* control_law_;
};
#endif // __MOTOR_HPP
Firmware/MotorControl/odrive_main.h
+27 -14
View File
@@ -9,15 +9,13 @@
#include <communication/interface_usb.h>
#include <communication/interface_i2c.h>
#include <communication/interface_uart.h>
#include <task_timer.hpp>
extern "C" {
#endif
// OS includes
#include <cmsis_os.h>
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
// extern const float elec_rad_per_enc;
extern uint32_t _reboot_cookie;
@@ -34,14 +32,12 @@ typedef struct {
uint32_t min_stack_space_axis; // minimum remaining space since startup [Bytes]
uint32_t min_stack_space_usb;
uint32_t min_stack_space_uart;
uint32_t min_stack_space_usb_irq;
uint32_t min_stack_space_startup;
uint32_t min_stack_space_can;
uint32_t stack_usage_axis;
uint32_t stack_usage_usb;
uint32_t stack_usage_uart;
uint32_t stack_usage_usb_irq;
uint32_t stack_usage_startup;
uint32_t stack_usage_can;
@@ -105,6 +101,13 @@ struct BoardConfig_t {
PWMMapping_t analog_mappings[GPIO_COUNT];
};
struct TaskTimes {
TaskTimer sampling;
TaskTimer control_loop_misc;
TaskTimer control_loop_checks;
};
// Forward Declarations
class Axis;
class Motor;
@@ -112,11 +115,6 @@ class ODriveCAN;
extern ODriveCAN *odCAN;
// if you use the oscilloscope feature you can bump up this value
#define OSCILLOSCOPE_SIZE 4096
extern float oscilloscope[OSCILLOSCOPE_SIZE];
extern size_t oscilloscope_pos;
// TODO: move
// this is technically not thread-safe but practically it might be
#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \
@@ -143,6 +141,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
#include <trapTraj.hpp>
#include <endstop.hpp>
#include <axis.hpp>
#include <oscilloscope.hpp>
#include <communication/communication.h>
// Defined in autogen/version.c based on git-derived version numbers
@@ -164,10 +163,7 @@ public:
void erase_configuration() override;
void reboot() override { NVIC_SystemReset(); }
void enter_dfu_mode() override;
float get_oscilloscope_val(uint32_t index) override {
return oscilloscope[index];
}
void clear_errors() override;
float get_adc_voltage(uint32_t gpio) override {
return ::get_adc_voltage(get_gpio(gpio));
@@ -178,12 +174,18 @@ public:
return cnt += delta;
}
void do_fast_checks();
void sampling_cb();
void control_loop_cb(uint32_t timestamp);
Axis& get_axis(int num) { return axes[num]; }
ODriveCAN& get_can() { return *odCAN; }
uint32_t get_interrupt_status(int32_t irqn);
uint32_t get_dma_status(uint8_t stream_num);
void disarm_with_error(Error error);
Error error_ = ERROR_NONE;
float& vbus_voltage_ = ::vbus_voltage; // TODO: make this the actual variable
float& ibus_ = ::ibus_; // TODO: make this the actual variable
float ibus_report_filter_k_ = 1.0f;
@@ -222,12 +224,23 @@ public:
bool& brake_resistor_saturated_ = ::brake_resistor_saturated; // TODO: make this the actual variable
SystemStats_t system_stats_;
Oscilloscope oscilloscope_{
&axes[0].motor_.current_control_.v_current_control_integral_d_, // trigger_src
0.5f, // trigger_threshold
&axes[0].motor_.current_control_.Ialpha_measured_ // data_src
};
BoardConfig_t config_;
uint32_t user_config_loaded_ = 0;
bool misconfigured_ = false;
uint32_t test_property_ = 0;
uint32_t last_update_timestamp_ = 0;
uint32_t n_evt_sampling_ = 0;
uint32_t n_evt_control_loop_ = 0;
bool task_timers_armed_ = false;
TaskTimes task_times_;
};
extern ODrive odrv; // defined in main.cpp
Firmware/MotorControl/open_loop_controller.cpp
+27
View File
@@ -0,0 +1,27 @@
#include "open_loop_controller.hpp"
#include <board.h>
void OpenLoopController::update(uint32_t timestamp) {
if (std::isnan(Id_setpoint_) || std::isnan(Id_setpoint_) || std::isnan(phase_) || std::isnan(phase_vel_)) {
Id_setpoint_ = 0.0f;
Iq_setpoint_ = 0.0f;
Vd_setpoint_ = 0.0f;
Vq_setpoint_ = 0.0f;
phase_ = 0.0f;
phase_vel_ = 0.0f;
timestamp_ = timestamp;
}
float dt = (float)(timestamp - timestamp_) / (float)TIM_1_8_CLOCK_HZ;
Id_setpoint_ = std::clamp(target_current_, Id_setpoint_ - max_current_ramp_ * dt, Id_setpoint_ + max_current_ramp_ * dt);
Iq_setpoint_ = 0.0f;
Vd_setpoint_ = std::clamp(target_voltage_, Vd_setpoint_ - max_voltage_ramp_ * dt, Vd_setpoint_ + max_voltage_ramp_ * dt);
Vq_setpoint_ = 0.0f;
phase_vel_ = std::clamp(target_vel_, phase_vel_ - max_phase_vel_ramp_ * dt, phase_vel_ + max_phase_vel_ramp_ * dt);
phase_ = wrap_pm_pi(phase_ + phase_vel_ * dt);
total_distance_ += phase_vel_ * dt;
timestamp_ = timestamp;
}
Firmware/MotorControl/open_loop_controller.hpp
+32
View File
@@ -0,0 +1,32 @@
#ifndef __OPEN_LOOP_CONTROLLER_HPP
#define __OPEN_LOOP_CONTROLLER_HPP
#include "component.hpp"
#include <cmath>
class OpenLoopController : public ComponentBase {
public:
void update(uint32_t timestamp) final;
// Config
float max_current_ramp_ = INFINITY; // [A/s]
float max_voltage_ramp_ = INFINITY; // [V/s]
float max_phase_vel_ramp_ = INFINITY; // [rad/s^2]
// Inputs
float target_vel_ = NAN;
float target_current_ = NAN;
float target_voltage_ = NAN;
// State/Outputs
uint32_t timestamp_ = 0;
float Id_setpoint_ = NAN;
float Iq_setpoint_ = NAN;
float Vd_setpoint_ = NAN;
float Vq_setpoint_ = NAN;
float phase_ = NAN;
float phase_vel_ = NAN;
float total_distance_ = NAN;
};
#endif // __OPEN_LOOP_CONTROLLER_HPP
Firmware/MotorControl/oscilloscope.cpp
+29
View File
@@ -0,0 +1,29 @@
#include "oscilloscope.hpp"
// if you use the oscilloscope feature you can bump up this value
#define OSCILLOSCOPE_SIZE 4096
void Oscilloscope::update() {
// Edit these to suit your capture needs
float trigger_data = trigger_src_ ? *trigger_src_ : 0.0f;
float trigger_threshold = trigger_threshold_;
float sample_data = data_src_ ? *data_src_ : 0.0f;
static bool ready = false;
static bool capturing = false;
if (trigger_data < trigger_threshold) {
ready = true;
}
if (ready && trigger_data >= trigger_threshold) {
capturing = true;
ready = false;
}
if (capturing) {
data_[pos_] = sample_data;
if (++pos_ >= OSCILLOSCOPE_SIZE) {
pos_ = 0;
capturing = false;
}
}
}
Firmware/MotorControl/oscilloscope.hpp
+29
View File
@@ -0,0 +1,29 @@
#ifndef __OSCILLOSCOPE_HPP
#define __OSCILLOSCOPE_HPP
#include <autogen/interfaces.hpp>
// if you use the oscilloscope feature you can bump up this value
#define OSCILLOSCOPE_SIZE 4096
class Oscilloscope : public ODriveIntf::OscilloscopeIntf {
public:
Oscilloscope(float* trigger_src, float trigger_threshold, float* data_src)
: trigger_src_(trigger_src), trigger_threshold_(trigger_threshold), data_src_(data_src) {}
float get_val(uint32_t index) override {
return index < OSCILLOSCOPE_SIZE ? data_[index] : NAN;
}
void update();
const uint32_t size_ = OSCILLOSCOPE_SIZE;
const float* trigger_src_;
const float trigger_threshold_;
const float* data_src_;
float data_[OSCILLOSCOPE_SIZE] = {0};
size_t pos_ = 0;
};
#endif // __OSCILLOSCOPE_HPP
Firmware/MotorControl/phase_control_law.hpp
+87
View File
@@ -0,0 +1,87 @@
#ifndef __PHASE_CONTROL_LAW_HPP
#define __PHASE_CONTROL_LAW_HPP
#include <autogen/interfaces.hpp>
#include <variant>
template<size_t N_PHASES>
class PhaseControlLaw {
public:
/**
* @brief Called when this controller becomes the active controller.
*/
virtual void reset() = 0;
/**
* @brief Informs the control law about a new set of measurements.
*
* This function gets called in a high priority interrupt context and should
* run fast.
*
* Beware that all inputs can be NAN.
*
* @param vbus_voltage: The most recently measured DC link voltage. NAN if
* the measurement is not available or valid for some reason.
* @param currents: The most recently measured (or inferred) phase currents
* in Amps. Any of the values can be NAN if the measurement is not
* available or valid for some reason.
* @param input_timestamp: The timestamp (in HCLK ticks) corresponding to
* the vbus_voltage and current measurement.
*/
virtual ODriveIntf::MotorIntf::Error on_measurement(float vbus_voltage,
std::array<float, N_PHASES> currents, uint32_t input_timestamp) = 0;
/**
* @brief Shall calculate the PWM timings for the specified target time.
*
* This function gets called in a high priority interrupt context and should
* run fast.
*
* Beware that this function can be called before a call to on_measurement().
*
* @param output_timestamp: The timestamp (in HCLK ticks) corresponding to
* the middle of the time span during which the output will be
* active.
* @param pwm_timings: This array referenced by this argument shall be
* filled with the desired PWM timings. Each item corresponds to one
* phase and must lie in [0.0f, 1.0f].
* The function is not required to return valid PWM timings in case
* of an error.
* @param ibus: The variable pointed to by this argument is set to the
* estimated DC current around the output timestamp when the desired
* PWM timings get applied.
* The function is not required to return a valid I_bus estimate in
* case of an error.
*
* @returns: An error code or ERROR_NONE. If the function returns an error
* the motor gets disarmed with one exception: If the controller
* never returned valid PWM timings since it became active then it
* is allowed to return ERROR_CONTROLLER_INITIALIZING without
* triggering a motor disarm. In this phase the PWMs will not yet
* be truly active.
*/
virtual ODriveIntf::MotorIntf::Error get_output(uint32_t output_timestamp,
float (&pwm_timings)[N_PHASES],
float* ibus) = 0;
};
class AlphaBetaFrameController : public PhaseControlLaw<3> {
private:
ODriveIntf::MotorIntf::Error on_measurement(float vbus_voltage,
std::array<float, 3> currents, uint32_t input_timestamp) final;
ODriveIntf::MotorIntf::Error get_output(uint32_t output_timestamp,
float (&pwm_timings)[3],
float* ibus) final;
protected:
virtual ODriveIntf::MotorIntf::Error on_measurement(
float vbus_voltage, float Ialpha, float Ibeta, uint32_t input_timestamp) = 0;
virtual ODriveIntf::MotorIntf::Error get_alpha_beta_output(
uint32_t output_timestamp,
float* mod_alpha, float* mod_beta,
float* ibus) = 0;
};
#endif // __PHASE_CONTROL_LAW_HPP
Firmware/MotorControl/sensorless_estimator.cpp
+19 -9
View File
@@ -10,14 +10,21 @@ bool SensorlessEstimator::update() {
// 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.
if (std::isnan(flux_state_[0]) || std::isnan(flux_state_[1]) || std::isnan(pll_pos_)) {
// Automatically reset state if it becomes NAN. The state becomes NAN
// when invalid current measurements are processed (e.g. because of the
// opamp being uninitialized).
flux_state_[0] = 0.0f;
flux_state_[1] = 0.0f;
pll_pos_ = 0.0f;
phase_vel_ = 0.0f;
}
// 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)};
// Swap sign of I_beta if motor is reversed
I_alpha_beta[1] *= axis_->motor_.config_.direction;
// alpha-beta vector operations
float eta[2];
for (int i = 0; i <= 1; ++i) {
@@ -49,8 +56,8 @@ bool SensorlessEstimator::update() {
}
// 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 * axis_->motor_.config_.direction;
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
@@ -61,19 +68,22 @@ bool SensorlessEstimator::update() {
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp < 1.0f)) {
error_ |= ERROR_UNSTABLE_GAIN;
vel_estimate_valid_ = false;
pll_pos_ = NAN;
phase_ = NAN;
vel_estimate_ = NAN;
return false;
}
// predict PLL phase with velocity
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * vel_estimate_);
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * phase_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
vel_estimate_ += current_meas_period * pll_ki * delta_phase;
phase_vel_ += current_meas_period * pll_ki * delta_phase;
vel_estimate_ = phase_vel_ / (2 * M_PI);
vel_estimate_valid_ = true;
return true;
};
Firmware/MotorControl/sensorless_estimator.hpp
+2 -2
View File
@@ -18,8 +18,8 @@ public:
Error error_ = ERROR_NONE;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float vel_estimate_ = 0.0f; // [rad/s]
bool vel_estimate_valid_ = false;
float phase_vel_ = 0.0f; // [rad/s]
float vel_estimate_ = 0.0f; // [turns/s]
// float pll_kp_ = 0.0f; // [rad/s / rad]
// float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
Firmware/MotorControl/task_timer.hpp
+65
View File
@@ -0,0 +1,65 @@
#ifndef __TASK_TIMER_HPP
#define __TASK_TIMER_HPP
#include <stdint.h>
#include <board.h>
#define MEASURE_START_TIME
#define MEASURE_END_TIME
#define MEASURE_LENGTH
#define MEASURE_MAX_LENGTH
inline uint16_t sample_TIM13() {
constexpr uint16_t clocks_per_cnt = (uint16_t)((float)TIM_1_8_CLOCK_HZ / (float)TIM_APB1_CLOCK_HZ);
return clocks_per_cnt * TIM13->CNT; // TODO: Use a hw_config
}
struct TaskTimer {
uint32_t start_time_ = 0;
uint32_t end_time_ = 0;
uint32_t length_ = 0;
uint32_t max_length_ = 0;
static bool enabled;
uint32_t start() {
return sample_TIM13();
}
void stop(uint32_t start_time) {
uint32_t end_time = sample_TIM13();
uint32_t length = end_time - start_time;
if (enabled) {
#ifdef MEASURE_START_TIME
start_time_ = start_time;
#endif
#ifdef MEASURE_END_TIME
end_time_ = end_time;
#endif
#ifdef MEASURE_LENGTH
length_ = length;
#endif
}
#ifdef MEASURE_MAX_LENGTH
max_length_ = std::max(max_length_, length);
#endif
}
};
struct TaskTimerContext {
TaskTimerContext(const TaskTimerContext&) = delete;
TaskTimerContext(const TaskTimerContext&&) = delete;
void operator=(const TaskTimerContext&) = delete;
void operator=(const TaskTimerContext&&) = delete;
TaskTimerContext(TaskTimer& timer) : timer_(timer), start_time(timer.start()) {}
~TaskTimerContext() { timer_.stop(start_time); }
TaskTimer& timer_;
uint32_t start_time;
bool exit_ = false;
};
#define MEASURE_TIME(timer) for (TaskTimerContext __task_timer_ctx{timer}; !__task_timer_ctx.exit_; __task_timer_ctx.exit_ = true)
#endif // __TASK_TIMER_HPP
Firmware/Tupfile.lua
+4
View File
@@ -189,7 +189,11 @@ sources = {
'MotorControl/thermistor.cpp',
'MotorControl/encoder.cpp',
'MotorControl/endstop.cpp',
'MotorControl/async_estimator.cpp',
'MotorControl/controller.cpp',
'MotorControl/foc.cpp',
'MotorControl/open_loop_controller.cpp',
'MotorControl/oscilloscope.cpp',
'MotorControl/sensorless_estimator.cpp',
'MotorControl/trapTraj.cpp',
'MotorControl/pwm_input.cpp',
Firmware/communication/can_simple.cpp
+5 -5
View File
@@ -329,16 +329,16 @@ void CANSimple::get_iq_callback(Axis* axis, can_Message_t& msg) {
txmsg.len = 8;
uint32_t floatBytes;
static_assert(sizeof axis->motor_.current_control_.Iq_setpoint == sizeof floatBytes);
std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_setpoint, sizeof floatBytes);
static_assert(sizeof axis->motor_.current_control_.Iq_setpoint_ == sizeof floatBytes);
std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_setpoint_, sizeof floatBytes);
txmsg.buf[0] = floatBytes;
txmsg.buf[1] = floatBytes >> 8;
txmsg.buf[2] = floatBytes >> 16;
txmsg.buf[3] = floatBytes >> 24;
static_assert(sizeof floatBytes == sizeof axis->motor_.current_control_.Iq_measured);
std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_measured, sizeof floatBytes);
static_assert(sizeof floatBytes == sizeof axis->motor_.current_control_.Iq_measured_);
std::memcpy(&floatBytes, &axis->motor_.current_control_.Iq_measured_, sizeof floatBytes);
txmsg.buf[4] = floatBytes;
txmsg.buf[5] = floatBytes >> 8;
txmsg.buf[6] = floatBytes >> 16;
@@ -379,7 +379,7 @@ void CANSimple::get_vbus_voltage_callback(Axis* axis, can_Message_t& msg) {
}
void CANSimple::clear_errors_callback(Axis* axis, can_Message_t& msg) {
axis->clear_errors();
odrv.clear_errors(); // TODO: might want to clear axis errors only
}
void CANSimple::send_heartbeat(Axis* axis) {
Firmware/communication/communication.cpp
-3
View File
@@ -30,9 +30,6 @@
uint64_t serial_number;
char serial_number_str[13]; // 12 digits + null termination
float oscilloscope[OSCILLOSCOPE_SIZE] = {0};
size_t oscilloscope_pos = 0;
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/

Some files were not shown because too many files have changed in this diff Show More