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

Merge pull request #161 from madcowswe/sam_oskar_testing

Browse Source 
The Future
This commit is contained in:
Oskar Weigl
2018-05-12 21:12:07 -07:00
committed by GitHub
parent aa6c4dddd5 e19c477875
commit 50e3969430
88 changed files with 6662 additions and 4046 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.travis.yml
+1
View File
@@ -39,6 +39,7 @@ env:
# Various protocol combinations
- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=native-stream CONFIG_UART_PROTOCOL=native
- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=stdout CONFIG_UART_PROTOCOL=ascii
- CONFIG_BOARD_VERSION=v3.4-24V CONFIG_USB_PROTOCOL=none CONFIG_UART_PROTOCOL=none
script:
Firmware/.gitignore
+3
View File
@@ -17,6 +17,9 @@ Odrive.xml
.settings/
.project
# VSCode stuff
/.vscode/.cortex-debug.*.state.json
# STM32CubeMX (in case you put it in this folder, or a symlink)
STM32CubeMX
Firmware/.vscode/c_cpp_properties.json
Vendored
+9
View File
@@ -26,6 +26,9 @@
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=5",
"HW_VERSION_VOLTAGE=48",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -63,6 +66,9 @@
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=4",
"HW_VERSION_VOLTAGE=24",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -105,6 +111,9 @@
"defines": [
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=4",
"HW_VERSION_VOLTAGE=24",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch
+5 -20
View File
@@ -4,22 +4,14 @@ Date: Mon, 12 Mar 2018 23:49:32 -0700
Subject: [PATCH] expose correct serial number on USB
---
Firmware/Board/v3/Src/usbd_desc.c | 16 +++++++++-------
1 file changed, 9 insertions(+), 7 deletions(-)
Firmware/Board/v3/Src/usbd_desc.c | 9 +++++++++-------
1 file changed, 1 insertions(+), 8 deletions(-)
diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c
index b9c7bd0..94dc49b 100644
--- a/Firmware/Board/v3/Src/usbd_desc.c
+++ b/Firmware/Board/v3/Src/usbd_desc.c
@@ -51,6 +51,7 @@
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_conf.h"
+#include "commands.h"
/* USER CODE BEGIN INCLUDE */
@@ -327,14 +328,15 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l
@@ -327,14 +327,7 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l
*/
uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
{
@@ -30,15 +22,8 @@ index b9c7bd0..94dc49b 100644
- else
- {
- USBD_GetString((uint8_t *)USBD_SERIALNUMBER_STRING_FS, USBD_StrDesc, length);
+ uint8_t str[13]; // 12 digits + null termination
+ uint64_t val = serial_number;
+ for (size_t i = 0; i < 12; ++i) {
+ str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf];
+ val <<= 4;
}
+ str[12] = 0;
+
+ USBD_GetString ((uint8_t *)str, USBD_StrDesc, length);
- }
+ USBD_GetString ((uint8_t *)serial_number_str, USBD_StrDesc, length);
return USBD_StrDesc;
}
Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch
+64
View File
@@ -0,0 +1,64 @@
From 510ead2b159e1d8116e5241066c54a7bf8b7bfbe Mon Sep 17 00:00:00 2001
From: Samuel Sadok <samuel.sadok@bluewin.ch>
Date: Mon, 26 Mar 2018 15:29:44 -0700
Subject: [PATCH] FreeRTOS constness fixes
- make thread names const char *
- make thread argument non-const void*
---
.../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h | 6 +++---
Firmware/Board/v3/Src/freertos.c | 4 ++--
2 files changed, 5 insertions(+), 5 deletions(-)
diff --git a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
index 09cdf27..754be24 100644
--- a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
+++ b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
@@ -270,11 +270,11 @@ typedef enum {
/// Entry point of a thread.
/// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS.
-typedef void (*os_pthread) (void const *argument);
+typedef void (*os_pthread) (void *argument);
/// Entry point of a timer call back function.
/// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS.
-typedef void (*os_ptimer) (void const *argument);
+typedef void (*os_ptimer) (void *argument);
// >>> the following data type definitions may shall adapted towards a specific RTOS
@@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t;
/// Thread Definition structure contains startup information of a thread.
/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
typedef struct os_thread_def {
- char *name; ///< Thread name
+ const char *name; ///< Thread name
os_pthread pthread; ///< start address of thread function
osPriority tpriority; ///< initial thread priority
uint32_t instances; ///< maximum number of instances of that thread function
diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c
index 6eaea82..b247994 100644
--- a/Firmware/Board/v3/Src/freertos.c
+++ b/Firmware/Board/v3/Src/freertos.c
@@ -75,7 +75,7 @@ uint8_t ucHeap[configTOTAL_HEAP_SIZE];
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
-void StartDefaultTask(void const * argument);
+void StartDefaultTask(void * argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
@@ -147,7 +147,7 @@ void MX_FREERTOS_Init(void) {
}
/* StartDefaultTask function */
-void StartDefaultTask(void const * argument)
+void StartDefaultTask(void * argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
--
2.16.2
Firmware/Board/v3/Inc/FreeRTOSConfig.h
+3 -1
View File
@@ -96,7 +96,7 @@
#define configUSE_PREEMPTION 1
#define configSUPPORT_STATIC_ALLOCATION 0
#define configSUPPORT_DYNAMIC_ALLOCATION 1
#define configUSE_IDLE_HOOK 0
#define configUSE_IDLE_HOOK 1
#define configUSE_TICK_HOOK 0
#define configCPU_CLOCK_HZ ( SystemCoreClock )
#define configTICK_RATE_HZ ((TickType_t)1000)
@@ -107,6 +107,7 @@
#define configUSE_16_BIT_TICKS 0
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configCHECK_FOR_STACK_OVERFLOW 1
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 1
/* Co-routine definitions. */
@@ -123,6 +124,7 @@ to exclude the API function. */
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
/* Cortex-M specific definitions. */
#ifdef __NVIC_PRIO_BITS
Firmware/Board/v3/Inc/freertos_vars.h
+6 -9
View File
@@ -3,15 +3,12 @@
#define __FREERTOS_H
// List of semaphores
osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
extern osSemaphoreId sem_usb_irq;
extern osSemaphoreId sem_uart_dma;
extern osSemaphoreId sem_usb_rx;
extern osSemaphoreId sem_usb_tx;
// List of threads
osThreadId thread_motor_0;
osThreadId thread_motor_1;
osThreadId thread_cmd_parse;
osThreadId thread_usb_pump;
extern osThreadId defaultTaskHandle;
extern osThreadId usb_irq_thread;
#endif /* __FREERTOS_H */
Firmware/Board/v3/Inc/gpio.h
+6 -2
View File
@@ -59,7 +59,7 @@
#include "main.h"
/* USER CODE BEGIN Includes */
#include <stdbool.h>
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
@@ -71,8 +71,12 @@ void MX_GPIO_Init(void);
/* USER CODE BEGIN Prototypes */
void SetGPIO12toUART();
void SetGPIO12toStepDir();
void SetupENCIndexGPIO();
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down,
void (*callback)(void*), void* ctx);
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin);
/* USER CODE END Prototypes */
Firmware/Board/v3/Inc/main.h
+17 -15
View File
@@ -58,6 +58,9 @@
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2
#include "prev_board_ver/main_V3_2.h"
#elif HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 3 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 4
#include "prev_board_ver/main_V3_4.h"
#else
/* USER CODE END Includes */
@@ -74,8 +77,8 @@
#define M0_nCS_GPIO_Port GPIOC
#define M1_nCS_Pin GPIO_PIN_14
#define M1_nCS_GPIO_Port GPIOC
#define M1_DC_CAL_Pin GPIO_PIN_15
#define M1_DC_CAL_GPIO_Port GPIOC
#define M1_ENC_Z_Pin GPIO_PIN_15
#define M1_ENC_Z_GPIO_Port GPIOC
#define M0_IB_Pin GPIO_PIN_0
#define M0_IB_GPIO_Port GPIOC
#define M0_IC_Pin GPIO_PIN_1
@@ -90,27 +93,26 @@
#define GPIO_2_GPIO_Port GPIOA
#define GPIO_3_Pin GPIO_PIN_2
#define GPIO_3_GPIO_Port GPIOA
#define GPIO_3_EXTI_IRQn EXTI2_IRQn
#define GPIO_4_Pin GPIO_PIN_3
#define GPIO_4_GPIO_Port GPIOA
#define M1_TEMP_Pin GPIO_PIN_4
#define M1_TEMP_GPIO_Port GPIOA
#define AUX_I_Pin GPIO_PIN_5
#define AUX_I_GPIO_Port GPIOA
#define AUX_TEMP_Pin GPIO_PIN_5
#define AUX_TEMP_GPIO_Port GPIOA
#define VBUS_S_Pin GPIO_PIN_6
#define VBUS_S_GPIO_Port GPIOA
#define M1_AL_Pin GPIO_PIN_7
#define M1_AL_GPIO_Port GPIOA
#define AUX_TEMP_Pin GPIO_PIN_4
#define AUX_TEMP_GPIO_Port GPIOC
#define GPIO_5_Pin GPIO_PIN_4
#define GPIO_5_GPIO_Port GPIOC
#define M0_TEMP_Pin GPIO_PIN_5
#define M0_TEMP_GPIO_Port GPIOC
#define M1_BL_Pin GPIO_PIN_0
#define M1_BL_GPIO_Port GPIOB
#define M1_CL_Pin GPIO_PIN_1
#define M1_CL_GPIO_Port GPIOB
#define GPIO_5_Pin GPIO_PIN_2
#define GPIO_5_GPIO_Port GPIOB
#define GPIO_6_Pin GPIO_PIN_2
#define GPIO_6_GPIO_Port GPIOB
#define AUX_L_Pin GPIO_PIN_10
#define AUX_L_GPIO_Port GPIOB
#define AUX_H_Pin GPIO_PIN_11
@@ -129,20 +131,20 @@
#define M1_BH_GPIO_Port GPIOC
#define M1_CH_Pin GPIO_PIN_8
#define M1_CH_GPIO_Port GPIOC
#define M0_DC_CAL_Pin GPIO_PIN_9
#define M0_DC_CAL_GPIO_Port GPIOC
#define M0_ENC_Z_Pin GPIO_PIN_9
#define M0_ENC_Z_GPIO_Port GPIOC
#define M0_AH_Pin GPIO_PIN_8
#define M0_AH_GPIO_Port GPIOA
#define M0_BH_Pin GPIO_PIN_9
#define M0_BH_GPIO_Port GPIOA
#define M0_CH_Pin GPIO_PIN_10
#define M0_CH_GPIO_Port GPIOA
#define M0_ENC_Z_Pin GPIO_PIN_15
#define M0_ENC_Z_GPIO_Port GPIOA
#define GPIO_7_Pin GPIO_PIN_15
#define GPIO_7_GPIO_Port GPIOA
#define nFAULT_Pin GPIO_PIN_2
#define nFAULT_GPIO_Port GPIOD
#define M1_ENC_Z_Pin GPIO_PIN_3
#define M1_ENC_Z_GPIO_Port GPIOB
#define GPIO_8_Pin GPIO_PIN_3
#define GPIO_8_GPIO_Port GPIOB
#define M0_ENC_A_Pin GPIO_PIN_4
#define M0_ENC_A_GPIO_Port GPIOB
#define M0_ENC_B_Pin GPIO_PIN_5
Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h
+1
View File
@@ -6,6 +6,7 @@
#define TIM_APB1_CLOCK_HZ 84000000
#define TIM_APB1_PERIOD_CLOCKS 4096
#define TIM_APB1_DEADTIME_CLOCKS 40
#define configAPPLICATION_ALLOCATED_HEAP 1
#define M0_nCS_Pin GPIO_PIN_13
#define M0_nCS_GPIO_Port GPIOC
Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h
+91
View File
@@ -0,0 +1,91 @@
/* Private define ------------------------------------------------------------*/
#define TIM_1_8_CLOCK_HZ 168000000
#define TIM_1_8_PERIOD_CLOCKS 10192
#define TIM_1_8_DEADTIME_CLOCKS 20
#define TIM_APB1_CLOCK_HZ 84000000
#define TIM_APB1_PERIOD_CLOCKS 4096
#define TIM_APB1_DEADTIME_CLOCKS 40
#define configAPPLICATION_ALLOCATED_HEAP 1
#define M0_nCS_Pin GPIO_PIN_13
#define M0_nCS_GPIO_Port GPIOC
#define M1_nCS_Pin GPIO_PIN_14
#define M1_nCS_GPIO_Port GPIOC
#define M1_DC_CAL_Pin GPIO_PIN_15
#define M1_DC_CAL_GPIO_Port GPIOC
#define M0_IB_Pin GPIO_PIN_0
#define M0_IB_GPIO_Port GPIOC
#define M0_IC_Pin GPIO_PIN_1
#define M0_IC_GPIO_Port GPIOC
#define M1_IC_Pin GPIO_PIN_2
#define M1_IC_GPIO_Port GPIOC
#define M1_IB_Pin GPIO_PIN_3
#define M1_IB_GPIO_Port GPIOC
#define GPIO_1_Pin GPIO_PIN_0
#define GPIO_1_GPIO_Port GPIOA
#define GPIO_2_Pin GPIO_PIN_1
#define GPIO_2_GPIO_Port GPIOA
#define GPIO_3_Pin GPIO_PIN_2
#define GPIO_3_GPIO_Port GPIOA
#define GPIO_3_EXTI_IRQn EXTI2_IRQn
#define GPIO_4_Pin GPIO_PIN_3
#define GPIO_4_GPIO_Port GPIOA
#define M1_TEMP_Pin GPIO_PIN_4
#define M1_TEMP_GPIO_Port GPIOA
#define AUX_I_Pin GPIO_PIN_5
#define AUX_I_GPIO_Port GPIOA
#define VBUS_S_Pin GPIO_PIN_6
#define VBUS_S_GPIO_Port GPIOA
#define M1_AL_Pin GPIO_PIN_7
#define M1_AL_GPIO_Port GPIOA
#define AUX_TEMP_Pin GPIO_PIN_4
#define AUX_TEMP_GPIO_Port GPIOC
#define M0_TEMP_Pin GPIO_PIN_5
#define M0_TEMP_GPIO_Port GPIOC
#define M1_BL_Pin GPIO_PIN_0
#define M1_BL_GPIO_Port GPIOB
#define M1_CL_Pin GPIO_PIN_1
#define M1_CL_GPIO_Port GPIOB
#define GPIO_5_Pin GPIO_PIN_2
#define GPIO_5_GPIO_Port GPIOB
#define AUX_L_Pin GPIO_PIN_10
#define AUX_L_GPIO_Port GPIOB
#define AUX_H_Pin GPIO_PIN_11
#define AUX_H_GPIO_Port GPIOB
#define EN_GATE_Pin GPIO_PIN_12
#define EN_GATE_GPIO_Port GPIOB
#define M0_AL_Pin GPIO_PIN_13
#define M0_AL_GPIO_Port GPIOB
#define M0_BL_Pin GPIO_PIN_14
#define M0_BL_GPIO_Port GPIOB
#define M0_CL_Pin GPIO_PIN_15
#define M0_CL_GPIO_Port GPIOB
#define M1_AH_Pin GPIO_PIN_6
#define M1_AH_GPIO_Port GPIOC
#define M1_BH_Pin GPIO_PIN_7
#define M1_BH_GPIO_Port GPIOC
#define M1_CH_Pin GPIO_PIN_8
#define M1_CH_GPIO_Port GPIOC
#define M0_DC_CAL_Pin GPIO_PIN_9
#define M0_DC_CAL_GPIO_Port GPIOC
#define M0_AH_Pin GPIO_PIN_8
#define M0_AH_GPIO_Port GPIOA
#define M0_BH_Pin GPIO_PIN_9
#define M0_BH_GPIO_Port GPIOA
#define M0_CH_Pin GPIO_PIN_10
#define M0_CH_GPIO_Port GPIOA
#define M0_ENC_Z_Pin GPIO_PIN_15
#define M0_ENC_Z_GPIO_Port GPIOA
#define nFAULT_Pin GPIO_PIN_2
#define nFAULT_GPIO_Port GPIOD
#define M1_ENC_Z_Pin GPIO_PIN_3
#define M1_ENC_Z_GPIO_Port GPIOB
#define M0_ENC_A_Pin GPIO_PIN_4
#define M0_ENC_A_GPIO_Port GPIOB
#define M0_ENC_B_Pin GPIO_PIN_5
#define M0_ENC_B_GPIO_Port GPIOB
#define M1_ENC_A_Pin GPIO_PIN_6
#define M1_ENC_A_GPIO_Port GPIOB
#define M1_ENC_B_Pin GPIO_PIN_7
#define M1_ENC_B_GPIO_Port GPIOB
Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h
Vendored
+3 -3
View File
@@ -270,11 +270,11 @@ typedef enum {
/// Entry point of a thread.
/// \note MUST REMAIN UNCHANGED: \b os_pthread shall be consistent in every CMSIS-RTOS.
typedef void (*os_pthread) (void const *argument);
typedef void (*os_pthread) (void *argument);
/// Entry point of a timer call back function.
/// \note MUST REMAIN UNCHANGED: \b os_ptimer shall be consistent in every CMSIS-RTOS.
typedef void (*os_ptimer) (void const *argument);
typedef void (*os_ptimer) (void *argument);
// >>> the following data type definitions may shall adapted towards a specific RTOS
@@ -323,7 +323,7 @@ typedef StaticQueue_t osStaticMessageQDef_t;
/// Thread Definition structure contains startup information of a thread.
/// \note CAN BE CHANGED: \b os_thread_def is implementation specific in every CMSIS-RTOS.
typedef struct os_thread_def {
char *name; ///< Thread name
const char *name; ///< Thread name
os_pthread pthread; ///< start address of thread function
osPriority tpriority; ///< initial thread priority
uint32_t instances; ///< maximum number of instances of that thread function
Firmware/Board/v3/Odrive.ioc
+17 -21
View File
@@ -102,9 +102,11 @@ Dma.UART4_TX.1.PeriphInc=DMA_PINC_DISABLE
Dma.UART4_TX.1.Priority=DMA_PRIORITY_LOW
Dma.UART4_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode
FREERTOS.FootprintOK=true
FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark=1
FREERTOS.INCLUDE_vTaskDelayUntil=1
FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK
FREERTOS.Tasks01=defaultTask,-3,256,StartDefaultTask,Default
FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,INCLUDE_uxTaskGetStackHighWaterMark
FREERTOS.Tasks01=defaultTask,0,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1
FREERTOS.configTOTAL_HEAP_SIZE=65536
File.Version=6
KeepUserPlacement=true
@@ -197,7 +199,6 @@ NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:true
NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.EXTI2_IRQn=true\:0\:0\:false\:false\:false\:false\:true
NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:true
NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:true
@@ -236,14 +237,13 @@ PA13.Signal=SYS_JTMS-SWDIO
PA14.Mode=Serial_Wire
PA14.Signal=SYS_JTCK-SWCLK
PA15.GPIOParameters=GPIO_Label
PA15.GPIO_Label=M0_ENC_Z
PA15.GPIO_Label=GPIO_7
PA15.Locked=true
PA15.Signal=GPIO_Input
PA2.GPIOParameters=GPIO_PuPd,GPIO_Label
PA2.GPIOParameters=GPIO_Label
PA2.GPIO_Label=GPIO_3
PA2.GPIO_PuPd=GPIO_PULLDOWN
PA2.Locked=true
PA2.Signal=GPXTI2
PA2.Signal=GPIO_Input
PA3.GPIOParameters=GPIO_PuPd,GPIO_Label
PA3.GPIO_Label=GPIO_4
PA3.GPIO_PuPd=GPIO_NOPULL
@@ -254,7 +254,7 @@ PA4.GPIO_Label=M1_TEMP
PA4.Locked=true
PA4.Signal=ADCx_IN4
PA5.GPIOParameters=GPIO_Label
PA5.GPIO_Label=AUX_I
PA5.GPIO_Label=AUX_TEMP
PA5.Locked=true
PA5.Signal=ADCx_IN5
PA6.GPIOParameters=GPIO_Label
@@ -312,11 +312,11 @@ PB15.Locked=true
PB15.Mode=PWM Generation3 CH3 CH3N
PB15.Signal=TIM1_CH3N
PB2.GPIOParameters=GPIO_Label
PB2.GPIO_Label=GPIO_5
PB2.GPIO_Label=GPIO_6
PB2.Locked=true
PB2.Signal=GPIO_Input
PB3.GPIOParameters=GPIO_Label
PB3.GPIO_Label=M1_ENC_Z
PB3.GPIO_Label=GPIO_8
PB3.Locked=true
PB3.Signal=GPIO_Input
PB4.GPIOParameters=GPIO_Label
@@ -358,9 +358,9 @@ PC14-OSC32_IN.Locked=true
PC14-OSC32_IN.PinState=GPIO_PIN_SET
PC14-OSC32_IN.Signal=GPIO_Output
PC15-OSC32_OUT.GPIOParameters=GPIO_Label
PC15-OSC32_OUT.GPIO_Label=M1_DC_CAL
PC15-OSC32_OUT.GPIO_Label=M1_ENC_Z
PC15-OSC32_OUT.Locked=true
PC15-OSC32_OUT.Signal=GPIO_Output
PC15-OSC32_OUT.Signal=GPIO_Input
PC2.GPIOParameters=GPIO_Label
PC2.GPIO_Label=M1_IC
PC2.Signal=ADCx_IN12
@@ -368,8 +368,9 @@ PC3.GPIOParameters=GPIO_Label
PC3.GPIO_Label=M1_IB
PC3.Signal=ADCx_IN13
PC4.GPIOParameters=GPIO_Label
PC4.GPIO_Label=AUX_TEMP
PC4.Signal=ADCx_IN14
PC4.GPIO_Label=GPIO_5
PC4.Locked=true
PC4.Signal=GPIO_Input
PC5.GPIOParameters=GPIO_Label
PC5.GPIO_Label=M0_TEMP
PC5.Signal=ADCx_IN15
@@ -386,9 +387,9 @@ PC8.GPIO_Label=M1_CH
PC8.Locked=true
PC8.Signal=S_TIM8_CH3
PC9.GPIOParameters=GPIO_Label
PC9.GPIO_Label=M0_DC_CAL
PC9.GPIO_Label=M0_ENC_Z
PC9.Locked=true
PC9.Signal=GPIO_Output
PC9.Signal=GPIO_Input
PCC.Checker=false
PCC.Line=STM32F405/415
PCC.MCU=STM32F405RGTx
@@ -482,9 +483,6 @@ SH.ADCx_IN13.0=ADC1_IN13,IN13
SH.ADCx_IN13.1=ADC2_IN13,IN13
SH.ADCx_IN13.2=ADC3_IN13,IN13
SH.ADCx_IN13.ConfNb=3
SH.ADCx_IN14.0=ADC1_IN14,IN14
SH.ADCx_IN14.1=ADC2_IN14,IN14
SH.ADCx_IN14.ConfNb=2
SH.ADCx_IN15.0=ADC1_IN15,IN15
SH.ADCx_IN15.1=ADC2_IN15,IN15
SH.ADCx_IN15.ConfNb=2
@@ -497,8 +495,6 @@ SH.ADCx_IN5.ConfNb=2
SH.ADCx_IN6.0=ADC1_IN6,IN6
SH.ADCx_IN6.1=ADC2_IN6,IN6
SH.ADCx_IN6.ConfNb=2
SH.GPXTI2.0=GPIO_EXTI2
SH.GPXTI2.ConfNb=1
SH.S_TIM1_CH1.0=TIM1_CH1,PWM Generation1 CH1 CH1N
SH.S_TIM1_CH1.ConfNb=1
SH.S_TIM1_CH2.0=TIM1_CH2,PWM Generation2 CH2 CH2N
Firmware/Board/v3/Src/adc.c
+11 -12
View File
@@ -57,6 +57,9 @@
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2
#include "prev_board_ver/adc_V3_2.c"
#elif HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 3 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 4
#include "prev_board_ver/adc_V3_4.c"
#else
/* USER CODE END 0 */
@@ -241,16 +244,15 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
PA4 ------> ADC1_IN4
PA5 ------> ADC1_IN5
PA6 ------> ADC1_IN6
PC4 ------> ADC1_IN14
PC5 ------> ADC1_IN15
*/
GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin;
|M0_TEMP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin;
GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
@@ -278,16 +280,15 @@ void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
PA4 ------> ADC2_IN4
PA5 ------> ADC2_IN5
PA6 ------> ADC2_IN6
PC4 ------> ADC2_IN14
PC5 ------> ADC2_IN15
*/
GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin;
|M0_TEMP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin;
GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
@@ -346,13 +347,12 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
PA4 ------> ADC1_IN4
PA5 ------> ADC1_IN5
PA6 ------> ADC1_IN6
PC4 ------> ADC1_IN14
PC5 ------> ADC1_IN15
*/
HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin);
|M0_TEMP_Pin);
HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin);
HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin);
/* ADC1 interrupt Deinit */
/* USER CODE BEGIN ADC1:ADC_IRQn disable */
@@ -383,13 +383,12 @@ void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
PA4 ------> ADC2_IN4
PA5 ------> ADC2_IN5
PA6 ------> ADC2_IN6
PC4 ------> ADC2_IN14
PC5 ------> ADC2_IN15
*/
HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin);
|M0_TEMP_Pin);
HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin);
HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_TEMP_Pin|VBUS_S_Pin);
/* ADC2 interrupt Deinit */
/* USER CODE BEGIN ADC2:ADC_IRQn disable */
Firmware/Board/v3/Src/freertos.c
+45 -34
View File
@@ -53,10 +53,9 @@
/* USER CODE BEGIN Includes */
#include "freertos_vars.h"
#include "low_level.h"
#include "axis_c_interface.h"
#include "commands.h"
#include "config.h"
#include "usb_device.h"
extern PCD_HandleTypeDef hpcd_USB_OTG_FS;
int odrive_main(void);
/* USER CODE END Includes */
/* Variables -----------------------------------------------------------------*/
@@ -65,11 +64,11 @@ osThreadId defaultTaskHandle;
/* USER CODE BEGIN Variables */
// List of semaphores
osSemaphoreId sem_usb_irq;
osSemaphoreId sem_uart_dma;
osSemaphoreId sem_usb_rx;
osSemaphoreId sem_usb_tx;
// List of threads
osThreadId thread_motor_0;
osThreadId thread_motor_1;
osThreadId thread_cmd_parse;
osThreadId usb_irq_thread;
// Place FreeRTOS heap in core coupled memory for better performance
__attribute__((section(".ccmram")))
@@ -77,7 +76,7 @@ uint8_t ucHeap[configTOTAL_HEAP_SIZE];
/* USER CODE END Variables */
/* Function prototypes -------------------------------------------------------*/
void StartDefaultTask(void const * argument);
void StartDefaultTask(void * argument);
extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
@@ -87,6 +86,38 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* USER CODE END FunctionPrototypes */
/* Hook prototypes */
void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName);
/* USER CODE BEGIN 4 */
__weak void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName)
{
/* Run time stack overflow checking is performed if
configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
called if a stack overflow is detected. */
}
void usb_deferred_interrupt_thread(void * ctx) {
(void) ctx; // unused parameter
for (;;) {
// Wait for signalling from USB interrupt (OTG_FS_IRQHandler)
osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever);
if (semaphore_status == osOK) {
// We have a new incoming USB transmission: handle it
HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS);
// Let the irq (OTG_FS_IRQHandler) fire again.
HAL_NVIC_EnableIRQ(OTG_FS_IRQn);
}
}
}
void init_deferred_interrupts(void) {
// Start USB interrupt handler thread
osThreadDef(task_usb_pump, usb_deferred_interrupt_thread, osPriorityAboveNormal, 0, 512);
usb_irq_thread = osThreadCreate(osThread(task_usb_pump), NULL);
}
/* USER CODE END 4 */
/* Init FreeRTOS */
@@ -114,10 +145,11 @@ void MX_FREERTOS_Init(void) {
sem_usb_rx = osSemaphoreCreate(osSemaphore(sem_usb_rx), 1);
osSemaphoreWait(sem_usb_rx, 0); // Remove a token.
// Create a semaphore for USB RX
// Create a semaphore for USB TX
osSemaphoreDef(sem_usb_tx);
sem_usb_tx = osSemaphoreCreate(osSemaphore(sem_usb_tx), 1);
init_deferred_interrupts();
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
@@ -126,7 +158,7 @@ void MX_FREERTOS_Init(void) {
/* Create the thread(s) */
/* definition and creation of defaultTask */
osThreadDef(defaultTask, StartDefaultTask, osPriorityIdle, 0, 256);
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 256);
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
/* USER CODE BEGIN RTOS_THREADS */
@@ -139,35 +171,14 @@ void MX_FREERTOS_Init(void) {
}
/* StartDefaultTask function */
void StartDefaultTask(void const * argument)
void StartDefaultTask(void * argument)
{
/* init code for USB_DEVICE */
MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */
// Init and load persistent configuration
init_configuration();
// Init communications
init_communication();
// Init motor control
init_motor_control();
// Start motor threads
osThreadDef(task_motor_0, axis_thread_entry, osPriorityHigh+1, 0, 512);
osThreadDef(task_motor_1, axis_thread_entry, osPriorityHigh, 0, 512);
thread_motor_0 = osThreadCreate(osThread(task_motor_0), &motors[0]);
thread_motor_1 = osThreadCreate(osThread(task_motor_1), &motors[1]);
// Start command handling thread
osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 512);
thread_cmd_parse = osThreadCreate(osThread(task_cmd_parse), NULL);
// Start USB interrupt handler thread
osThreadDef(task_usb_pump, usb_update_thread, osPriorityAboveNormal, 0, 512);
thread_usb_pump = osThreadCreate(osThread(task_usb_pump), NULL);
odrive_main();
//If we get to here, then the default task is done.
vTaskDelete(defaultTaskHandle);
Firmware/Board/v3/Src/gpio.c
+114 -64
View File
@@ -50,11 +50,14 @@
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
#include "low_level.h"
#include <stdbool.h>
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2
#include "prev_board_ver/gpio_V3_2.c"
#elif HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 3 \
|| HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 4
#include "prev_board_ver/gpio_V3_4.c"
#else
/* USER CODE END 0 */
@@ -87,33 +90,30 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, M0_nCS_Pin|M1_nCS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, M1_DC_CAL_Pin|M0_DC_CAL_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(EN_GATE_GPIO_Port, EN_GATE_Pin, GPIO_PIN_RESET);
/*Configure GPIO pins : PCPin PCPin PCPin PCPin */
GPIO_InitStruct.Pin = M0_nCS_Pin|M1_nCS_Pin|M1_DC_CAL_Pin|M0_DC_CAL_Pin;
/*Configure GPIO pins : PCPin PCPin */
GPIO_InitStruct.Pin = M0_nCS_Pin|M1_nCS_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GPIO_3_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : PCPin PCPin PCPin */
GPIO_InitStruct.Pin = M1_ENC_Z_Pin|GPIO_5_Pin|M0_ENC_Z_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : PAPin PAPin */
GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin;
/*Configure GPIO pins : PAPin PAPin PAPin */
GPIO_InitStruct.Pin = GPIO_3_Pin|GPIO_4_Pin|GPIO_7_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PBPin PBPin */
GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin;
GPIO_InitStruct.Pin = GPIO_6_Pin|GPIO_8_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
@@ -131,19 +131,50 @@ void MX_GPIO_Init(void)
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(nFAULT_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI2_IRQn);
}
/* USER CODE BEGIN 2 */
#endif // End GPIO Include
// @brief Returns the IRQ number associated with a certain pin.
// Note that all GPIOs with the same pin number map to the same IRQn,
// no matter which port they belong to.
IRQn_Type get_irq_number(uint16_t pin) {
uint16_t pin_number = 0;
pin >>= 1;
while (pin) {
pin >>= 1;
pin_number++;
}
switch (pin_number) {
case 0: return EXTI0_IRQn;
case 1: return EXTI1_IRQn;
case 2: return EXTI2_IRQn;
case 3: return EXTI3_IRQn;
case 4: return EXTI4_IRQn;
case 5:
case 6:
case 7:
case 8:
case 9: return EXTI9_5_IRQn;
case 10:
case 11:
case 12:
case 13:
case 14:
case 15: return EXTI15_10_IRQn;
default: return 0; // impossible
}
}
// @brief Puts the GPIO's 1 and 2 into UART mode.
// This will disable any interrupt subscribers of these GPIOs.
void SetGPIO12toUART() {
GPIO_InitTypeDef GPIO_InitStruct;
HAL_NVIC_DisableIRQ(EXTI0_IRQn);
// make sure nothing is hogging the GPIO's
GPIO_unsubscribe(GPIO_1_GPIO_Port, GPIO_1_Pin);
GPIO_unsubscribe(GPIO_2_GPIO_Port, GPIO_2_Pin);
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
@@ -160,59 +191,78 @@ void SetGPIO12toUART() {
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
}
void SetGPIO12toStepDir() {
// Expected subscriptions: 2x step signal + 2x encoder index signal
#define MAX_SUBSCRIPTIONS 10
struct subscription_t {
GPIO_TypeDef* GPIO_port;
uint16_t GPIO_pin;
void (*callback)(void*);
void* ctx;
} subscriptions[MAX_SUBSCRIPTIONS] = { 0 };
size_t n_subscriptions = 0;
// Sets up the specified GPIO to trigger the specified callback
// on a rising edge of the GPIO.
// @param pull_up_down: one of GPIO_NOPULL, GPIO_PULLUP or GPIO_PULLDOWN
bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin,
uint32_t pull_up_down,
void (*callback)(void*), void* ctx) {
// Register handler (or reuse existing registration)
// TODO: make thread safe
struct subscription_t* subscription = NULL;
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_port == GPIO_port &&
subscriptions[i].GPIO_pin == GPIO_pin)
subscription = &subscriptions[i];
}
if (!subscription) {
if (n_subscriptions >= MAX_SUBSCRIPTIONS)
return false;
subscription = &subscriptions[n_subscriptions++];
}
*subscription = (struct subscription_t){
.GPIO_port = GPIO_port,
.GPIO_pin = GPIO_pin,
.callback = callback,
.ctx = ctx
};
// Set up GPIO
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = GPIO_1_Pin;
GPIO_InitStruct.Pin = GPIO_pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIO_1_GPIO_Port, &GPIO_InitStruct);
GPIO_InitStruct.Pull = pull_up_down;
HAL_GPIO_Init(GPIO_port, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_2_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIO_2_GPIO_Port, &GPIO_InitStruct);
//TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default
HAL_NVIC_SetPriority(EXTI0_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI0_IRQn);
// Enable interrupt
HAL_NVIC_SetPriority(get_irq_number(GPIO_pin), 0, 0);
HAL_NVIC_EnableIRQ(get_irq_number(GPIO_pin));
return true;
}
//TODO: Enable index on only one channel
void SetupENCIndexGPIO(){
GPIO_InitTypeDef GPIO_InitStruct;
/*Configure GPIO pins : PAPin PAPin */
GPIO_InitStruct.Pin = M0_ENC_Z_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(M0_ENC_Z_GPIO_Port, &GPIO_InitStruct);
//TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
/*Configure GPIO pins : PBPin PBPin */
GPIO_InitStruct.Pin = M1_ENC_Z_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(M1_ENC_Z_GPIO_Port, &GPIO_InitStruct);
//TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default
HAL_NVIC_SetPriority(EXTI3_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI3_IRQn);
void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) {
bool is_pin_in_use = false;
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_port == GPIO_port &&
subscriptions[i].GPIO_pin == GPIO_pin) {
subscriptions[i].callback = NULL;
subscriptions[i].ctx = NULL;
} else if (subscriptions[i].GPIO_pin == GPIO_pin) {
is_pin_in_use = true;
}
}
if (!is_pin_in_use)
HAL_NVIC_DisableIRQ(get_irq_number(GPIO_pin));
}
//Dispatch processing of external interrupts based on source
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
//Step signals for M0 and M1
if (GPIO_Pin & GPIO_1_Pin || GPIO_Pin & GPIO_3_Pin) {
step_cb(GPIO_Pin);
} else if(GPIO_Pin & M0_ENC_Z_Pin){
enc_index_cb(GPIO_Pin, 0);
} else if(GPIO_Pin & M1_ENC_Z_Pin){
enc_index_cb(GPIO_Pin, 1);
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) {
for (size_t i = 0; i < n_subscriptions; ++i) {
if (subscriptions[i].GPIO_pin == GPIO_pin) // TODO: check for port
if (subscriptions[i].callback)
subscriptions[i].callback(subscriptions[i].ctx);
}
}
Firmware/Board/v3/Src/main.c
+38 -31
View File
@@ -59,8 +59,8 @@
#include "gpio.h"
/* USER CODE BEGIN Includes */
#include "utils.h"
#include "commands.h"
#include <MotorControl/odrive_main.h>
#include "freertos_vars.h"
/* USER CODE END Includes */
/* Private variables ---------------------------------------------------------*/
@@ -81,26 +81,12 @@ void MX_FREERTOS_Init(void);
/* USER CODE BEGIN 0 */
void jump_to_builtin_bootloader(void) {
__set_MSP(0x20001000);
// http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf
void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004));
builtin_bootloader();
for (;;);
}
uint32_t _reboot_cookie __attribute__ ((section (".noinit")));
extern char _estack; // provided by the linker script
/* USER CODE END 0 */
/**
* @brief The application entry point.
*
* @retval None
*/
int main(void)
{
/* USER CODE BEGIN 1 */
if(*((unsigned long *)0x2001C000) == 0xDEADFE75) {
// Gets called from the startup assembly code
void early_start_checks(void) {
if(_reboot_cookie == 0xDEADFE75) {
/* The STM DFU bootloader enables internal pull-up resistors on PB10 (AUX_H)
* and PB11 (AUX_L), thereby causing shoot-through on the brake resistor
* FETs and obliterating them unless external 3.3k pull-down resistors are
@@ -119,34 +105,56 @@ int main(void)
for (size_t i = 0; i < (16000000UL / 5UL * 2UL); ++i) {
__NOP();
}
*((unsigned long *)0x2001C000) = 0xDEADBEEF;
_reboot_cookie = 0xDEADBEEF;
}
/* We could jump to the bootloader directly on demand without rebooting
but that requires us to reset several peripherals and interrupts for it
to function correctly. Therefore it's easier to just reset the entire chip. */
if(*((unsigned long *)0x2001C000) == 0xDEADBEEF) {
*((unsigned long *)0x2001C000) = 0xCAFEFEED; //Reset bootloader trigger
jump_to_builtin_bootloader();
if(_reboot_cookie == 0xDEADBEEF) {
_reboot_cookie = 0xCAFEFEED; //Reset bootloader trigger
__set_MSP((uintptr_t)&_estack);
// http://www.st.com/content/ccc/resource/technical/document/application_note/6a/17/92/02/58/98/45/0c/CD00264379.pdf/files/CD00264379.pdf
void (*builtin_bootloader)(void) = (void (*)(void))(*((uint32_t *)0x1FFF0004));
builtin_bootloader();
}
/* The bootloader might fail to properly clean up after itself,
so if we're not sure that the system is in a clean state we
just reset it again */
if(*((unsigned long *)0x2001C000) != 42) {
*((unsigned long *)0x2001C000) = 42;
if(_reboot_cookie != 42) {
_reboot_cookie = 42;
NVIC_SystemReset();
}
}
/* USER CODE END 0 */
/**
* @brief The application entry point.
*
* @retval None
*/
int main(void)
{
/* USER CODE BEGIN 1 */
// This procedure of building a USB serial number should be identical
// to the way the STM's built-in USB bootloader does it. This means
// that the device will have the same serial number in normal and DFU mode.
uint32_t uuid0 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 0);
uint32_t uuid1 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 4);
uint32_t uuid2 = *(uint32_t *) (ID_UNIQUE_ADDRESS + 8);
uint32_t uuid0 = *(uint32_t *)(UID_BASE + 0);
uint32_t uuid1 = *(uint32_t *)(UID_BASE + 4);
uint32_t uuid2 = *(uint32_t *)(UID_BASE + 8);
uint32_t uuid_mixed_part = uuid0 + uuid2;
serial_number = ((uint64_t)uuid_mixed_part << 16) | (uint64_t)(uuid1 >> 16);
uint64_t val = serial_number;
for (size_t i = 0; i < 12; ++i) {
serial_number_str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf];
val <<= 4;
}
serial_number_str[12] = 0;
/* USER CODE END 1 */
/* MCU Configuration----------------------------------------------------------*/
@@ -170,7 +178,6 @@ int main(void)
MX_DMA_Init();
MX_ADC1_Init();
MX_ADC2_Init();
MX_CAN1_Init();
MX_TIM1_Init();
MX_TIM8_Init();
MX_TIM3_Init();
Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c
+375
View File
@@ -0,0 +1,375 @@
ADC_HandleTypeDef hadc1;
ADC_HandleTypeDef hadc2;
ADC_HandleTypeDef hadc3;
/* ADC1 init function */
void MX_ADC1_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
ADC_InjectionConfTypeDef sConfigInjected;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = DISABLE;
hadc1.Init.ContinuousConvMode = DISABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 1;
hadc1.Init.DMAContinuousRequests = DISABLE;
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_6;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedChannel = ADC_CHANNEL_6;
sConfigInjected.InjectedRank = 1;
sConfigInjected.InjectedNbrOfConversion = 1;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING;
sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffset = 0;
if (HAL_ADCEx_InjectedConfigChannel(&hadc1, &sConfigInjected) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
/* ADC2 init function */
void MX_ADC2_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
ADC_InjectionConfTypeDef sConfigInjected;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc2.Instance = ADC2;
hadc2.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc2.Init.Resolution = ADC_RESOLUTION_12B;
hadc2.Init.ScanConvMode = DISABLE;
hadc2.Init.ContinuousConvMode = DISABLE;
hadc2.Init.DiscontinuousConvMode = DISABLE;
hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO;
hadc2.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc2.Init.NbrOfConversion = 1;
hadc2.Init.DMAContinuousRequests = DISABLE;
hadc2.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&hadc2) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_13;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
if (HAL_ADC_ConfigChannel(&hadc2, &sConfig) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedChannel = ADC_CHANNEL_10;
sConfigInjected.InjectedRank = 1;
sConfigInjected.InjectedNbrOfConversion = 1;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING;
sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffset = 0;
if (HAL_ADCEx_InjectedConfigChannel(&hadc2, &sConfigInjected) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
/* ADC3 init function */
void MX_ADC3_Init(void)
{
ADC_ChannelConfTypeDef sConfig;
ADC_InjectionConfTypeDef sConfigInjected;
/**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc3.Instance = ADC3;
hadc3.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadc3.Init.Resolution = ADC_RESOLUTION_12B;
hadc3.Init.ScanConvMode = DISABLE;
hadc3.Init.ContinuousConvMode = DISABLE;
hadc3.Init.DiscontinuousConvMode = DISABLE;
hadc3.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING;
hadc3.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONV_T8_TRGO;
hadc3.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc3.Init.NbrOfConversion = 1;
hadc3.Init.DMAContinuousRequests = DISABLE;
hadc3.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&hadc3) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_12;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
if (HAL_ADC_ConfigChannel(&hadc3, &sConfig) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
/**Configures for the selected ADC injected channel its corresponding rank in the sequencer and its sample time
*/
sConfigInjected.InjectedChannel = ADC_CHANNEL_11;
sConfigInjected.InjectedRank = 1;
sConfigInjected.InjectedNbrOfConversion = 1;
sConfigInjected.InjectedSamplingTime = ADC_SAMPLETIME_3CYCLES;
sConfigInjected.ExternalTrigInjecConvEdge = ADC_EXTERNALTRIGINJECCONVEDGE_RISING;
sConfigInjected.ExternalTrigInjecConv = ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
sConfigInjected.AutoInjectedConv = DISABLE;
sConfigInjected.InjectedDiscontinuousConvMode = DISABLE;
sConfigInjected.InjectedOffset = 0;
if (HAL_ADCEx_InjectedConfigChannel(&hadc3, &sConfigInjected) != HAL_OK)
{
_Error_Handler(__FILE__, __LINE__);
}
}
void HAL_ADC_MspInit(ADC_HandleTypeDef* adcHandle)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(adcHandle->Instance==ADC1)
{
/* USER CODE BEGIN ADC1_MspInit 0 */
/* USER CODE END ADC1_MspInit 0 */
/* ADC1 clock enable */
__HAL_RCC_ADC1_CLK_ENABLE();
/**ADC1 GPIO Configuration
PC0 ------> ADC1_IN10
PC1 ------> ADC1_IN11
PC2 ------> ADC1_IN12
PC3 ------> ADC1_IN13
PA4 ------> ADC1_IN4
PA5 ------> ADC1_IN5
PA6 ------> ADC1_IN6
PC4 ------> ADC1_IN14
PC5 ------> ADC1_IN15
*/
GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* ADC1 interrupt Init */
HAL_NVIC_SetPriority(ADC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
/* USER CODE BEGIN ADC1_MspInit 1 */
/* USER CODE END ADC1_MspInit 1 */
}
else if(adcHandle->Instance==ADC2)
{
/* USER CODE BEGIN ADC2_MspInit 0 */
/* USER CODE END ADC2_MspInit 0 */
/* ADC2 clock enable */
__HAL_RCC_ADC2_CLK_ENABLE();
/**ADC2 GPIO Configuration
PC0 ------> ADC2_IN10
PC1 ------> ADC2_IN11
PC2 ------> ADC2_IN12
PC3 ------> ADC2_IN13
PA4 ------> ADC2_IN4
PA5 ------> ADC2_IN5
PA6 ------> ADC2_IN6
PC4 ------> ADC2_IN14
PC5 ------> ADC2_IN15
*/
GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
GPIO_InitStruct.Pin = M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* ADC2 interrupt Init */
HAL_NVIC_SetPriority(ADC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
/* USER CODE BEGIN ADC2_MspInit 1 */
/* USER CODE END ADC2_MspInit 1 */
}
else if(adcHandle->Instance==ADC3)
{
/* USER CODE BEGIN ADC3_MspInit 0 */
/* USER CODE END ADC3_MspInit 0 */
/* ADC3 clock enable */
__HAL_RCC_ADC3_CLK_ENABLE();
/**ADC3 GPIO Configuration
PC0 ------> ADC3_IN10
PC1 ------> ADC3_IN11
PC2 ------> ADC3_IN12
PC3 ------> ADC3_IN13
*/
GPIO_InitStruct.Pin = M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* ADC3 interrupt Init */
HAL_NVIC_SetPriority(ADC_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(ADC_IRQn);
/* USER CODE BEGIN ADC3_MspInit 1 */
/* USER CODE END ADC3_MspInit 1 */
}
}
void HAL_ADC_MspDeInit(ADC_HandleTypeDef* adcHandle)
{
if(adcHandle->Instance==ADC1)
{
/* USER CODE BEGIN ADC1_MspDeInit 0 */
/* USER CODE END ADC1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_ADC1_CLK_DISABLE();
/**ADC1 GPIO Configuration
PC0 ------> ADC1_IN10
PC1 ------> ADC1_IN11
PC2 ------> ADC1_IN12
PC3 ------> ADC1_IN13
PA4 ------> ADC1_IN4
PA5 ------> ADC1_IN5
PA6 ------> ADC1_IN6
PC4 ------> ADC1_IN14
PC5 ------> ADC1_IN15
*/
HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin);
HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin);
/* ADC1 interrupt Deinit */
/* USER CODE BEGIN ADC1:ADC_IRQn disable */
/**
* Uncomment the line below to disable the "ADC_IRQn" interrupt
* Be aware, disabling shared interrupt may affect other IPs
*/
/* HAL_NVIC_DisableIRQ(ADC_IRQn); */
/* USER CODE END ADC1:ADC_IRQn disable */
/* USER CODE BEGIN ADC1_MspDeInit 1 */
/* USER CODE END ADC1_MspDeInit 1 */
}
else if(adcHandle->Instance==ADC2)
{
/* USER CODE BEGIN ADC2_MspDeInit 0 */
/* USER CODE END ADC2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_ADC2_CLK_DISABLE();
/**ADC2 GPIO Configuration
PC0 ------> ADC2_IN10
PC1 ------> ADC2_IN11
PC2 ------> ADC2_IN12
PC3 ------> ADC2_IN13
PA4 ------> ADC2_IN4
PA5 ------> ADC2_IN5
PA6 ------> ADC2_IN6
PC4 ------> ADC2_IN14
PC5 ------> ADC2_IN15
*/
HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin
|AUX_TEMP_Pin|M0_TEMP_Pin);
HAL_GPIO_DeInit(GPIOA, M1_TEMP_Pin|AUX_I_Pin|VBUS_S_Pin);
/* ADC2 interrupt Deinit */
/* USER CODE BEGIN ADC2:ADC_IRQn disable */
/**
* Uncomment the line below to disable the "ADC_IRQn" interrupt
* Be aware, disabling shared interrupt may affect other IPs
*/
/* HAL_NVIC_DisableIRQ(ADC_IRQn); */
/* USER CODE END ADC2:ADC_IRQn disable */
/* USER CODE BEGIN ADC2_MspDeInit 1 */
/* USER CODE END ADC2_MspDeInit 1 */
}
else if(adcHandle->Instance==ADC3)
{
/* USER CODE BEGIN ADC3_MspDeInit 0 */
/* USER CODE END ADC3_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_ADC3_CLK_DISABLE();
/**ADC3 GPIO Configuration
PC0 ------> ADC3_IN10
PC1 ------> ADC3_IN11
PC2 ------> ADC3_IN12
PC3 ------> ADC3_IN13
*/
HAL_GPIO_DeInit(GPIOC, M0_IB_Pin|M0_IC_Pin|M1_IC_Pin|M1_IB_Pin);
/* ADC3 interrupt Deinit */
/* USER CODE BEGIN ADC3:ADC_IRQn disable */
/**
* Uncomment the line below to disable the "ADC_IRQn" interrupt
* Be aware, disabling shared interrupt may affect other IPs
*/
/* HAL_NVIC_DisableIRQ(ADC_IRQn); */
/* USER CODE END ADC3:ADC_IRQn disable */
/* USER CODE BEGIN ADC3_MspDeInit 1 */
/* USER CODE END ADC3_MspDeInit 1 */
}
}
Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c
+71
View File
@@ -0,0 +1,71 @@
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, M0_nCS_Pin|M1_nCS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, M1_DC_CAL_Pin|M0_DC_CAL_Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(EN_GATE_GPIO_Port, EN_GATE_Pin, GPIO_PIN_RESET);
/*Configure GPIO pins : PCPin PCPin PCPin PCPin */
GPIO_InitStruct.Pin = M0_nCS_Pin|M1_nCS_Pin|M1_DC_CAL_Pin|M0_DC_CAL_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = GPIO_3_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
HAL_GPIO_Init(GPIO_3_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pins : PAPin PAPin */
GPIO_InitStruct.Pin = GPIO_4_Pin|M0_ENC_Z_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/*Configure GPIO pins : PBPin PBPin */
GPIO_InitStruct.Pin = GPIO_5_Pin|M1_ENC_Z_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = EN_GATE_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(EN_GATE_GPIO_Port, &GPIO_InitStruct);
/*Configure GPIO pin : PtPin */
GPIO_InitStruct.Pin = nFAULT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_PULLUP;
HAL_GPIO_Init(nFAULT_GPIO_Port, &GPIO_InitStruct);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI2_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(EXTI2_IRQn);
}
Firmware/Board/v3/Src/stm32f4xx_it.c
+25 -2
View File
@@ -38,11 +38,15 @@
/* USER CODE BEGIN 0 */
#include "freertos_vars.h"
#include "low_level.h"
#include <stdbool.h>
typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected);
void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback);
// TODO: move somewhere else
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected);
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
@@ -217,7 +221,7 @@ void ADC_IRQHandler(void)
// The HAL's ADC handling mechanism adds many clock cycles of overhead
// So we bypass it and handle the logic ourselves.
//@TODO add vbus meaasurement on adc1 here
//@TODO add vbus measurement on adc1 here
ADC_IRQ_Dispatch(&hadc1, &vbus_sense_adc_cb);
ADC_IRQ_Dispatch(&hadc2, &pwm_trig_adc_cb);
ADC_IRQ_Dispatch(&hadc3, &pwm_trig_adc_cb);
@@ -337,11 +341,30 @@ void EXTI4_IRQHandler(void)
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4);
}
/**
* @brief This function handles EXTI lines 5-9 interrupt.
*/
void EXTI9_5_IRQHandler(void)
{
// The true source of the interrupt is checked inside HAL_GPIO_EXTI_IRQHandler()
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_6);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_7);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_8);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_9);
}
/**
* @brief This function handles EXTI lines 10-15 interrupt.
*/
void EXTI15_10_IRQHandler(void)
{
// The true source of the interrupt is checked inside HAL_GPIO_EXTI_IRQHandler()
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_10);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_11);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_12);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_13);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_14);
HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_15);
}
Firmware/Board/v3/Src/syscalls.c
+1 -42
View File
@@ -10,7 +10,6 @@
#include <sys/unistd.h>
#include <usart.h>
#include <usbd_cdc_if.h>
#include <legacy_commands.h> // TODO: make serial_printf_select constant
//int _read(int file, char *data, int len) {}
@@ -57,46 +56,6 @@ intptr_t _sbrk(size_t size) {
return ptr;
}
#define UART_TX_BUFFER_SIZE 64
static uint8_t uart_tx_buf[UART_TX_BUFFER_SIZE];
// _write is defined in communication.cpp
int _write(int file, char* data, int len) {
//number of bytes written
int written = 0;
switch (serial_printf_select) {
case SERIAL_PRINTF_IS_USB: {
// Wait on semaphore for the interface to be available
// Note that the USB driver will release the interface again when the TX completes
const uint32_t usb_tx_timeout = 100; // ms
osStatus sem_stat = osSemaphoreWait(sem_usb_tx, usb_tx_timeout);
if (sem_stat == osOK) {
uint8_t status = CDC_Transmit_FS((uint8_t*)data, len); // transmit over CDC
written = (status == USBD_OK) ? len : 0;
} // If the semaphore times out, we simply leave "written" as 0
} break;
case SERIAL_PRINTF_IS_UART: {
//Check length
if (len > UART_TX_BUFFER_SIZE)
return 0;
// Wait on semaphore for the interface to be available
// Note that HAL_UART_TxCpltCallback will release the interface again when the TX completes
const uint32_t uart_tx_timeout = 100; // ms
osStatus sem_stat = osSemaphoreWait(sem_uart_dma, uart_tx_timeout);
if (sem_stat == osOK) {
memcpy(uart_tx_buf, data, len); // memcpy data into uart_tx_buf
HAL_UART_Transmit_DMA(&huart4, uart_tx_buf, len); // Start DMA background transfer
} // If the semaphore times out, we simply leave "written" as 0
} break;
default: {
written = 0;
} break;
}
return written;
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) {
osSemaphoreRelease(sem_uart_dma);
}
Firmware/Board/v3/Src/usbd_cdc_if.c
+2 -6
View File
@@ -52,9 +52,7 @@
/* USER CODE BEGIN INCLUDE */
#include "cmsis_os.h"
#include "freertos_vars.h"
#include "utils.h"
#include "commands.h"
#include <communication/interface_usb.h>
#include <freertos_vars.h>
/* USER CODE END INCLUDE */
@@ -292,9 +290,7 @@ static int8_t CDC_Control_FS(uint8_t cmd, uint8_t* pbuf, uint16_t length)
static int8_t CDC_Receive_FS(uint8_t* Buf, uint32_t *Len)
{
/* USER CODE BEGIN 6 */
set_cmd_buffer(Buf, *Len);
osSemaphoreRelease(sem_usb_rx);
usb_process_packet(Buf, *Len);
return (USBD_OK);
/* USER CODE END 6 */
Firmware/Board/v3/Src/usbd_desc.c
+2 -11
View File
@@ -51,10 +51,9 @@
#include "usbd_core.h"
#include "usbd_desc.h"
#include "usbd_conf.h"
#include "commands.h"
/* USER CODE BEGIN INCLUDE */
#include <MotorControl/odrive_main.h>
/* USER CODE END INCLUDE */
/* Private typedef -----------------------------------------------------------*/
@@ -330,15 +329,7 @@ uint8_t * USBD_FS_ManufacturerStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *l
*/
uint8_t * USBD_FS_SerialStrDescriptor(USBD_SpeedTypeDef speed, uint16_t *length)
{
uint8_t str[13]; // 12 digits + null termination
uint64_t val = serial_number;
for (size_t i = 0; i < 12; ++i) {
str[i] = "0123456789ABCDEF"[(val >> (48-4)) & 0xf];
val <<= 4;
}
str[12] = 0;
USBD_GetString ((uint8_t *)str, USBD_StrDesc, length);
USBD_GetString ((uint8_t *)serial_number_str, USBD_StrDesc, length);
return USBD_StrDesc;
}
Firmware/Board/v3/startup_stm32f405xx.s
+2
View File
@@ -107,6 +107,8 @@ LoopFillZerobss:
/* Call the clock system intitialization function.*/
bl SystemInit
bl early_start_checks
/* Call static constructors */
bl __libc_init_array
/* Call the application's entry point.*/
Firmware/CHANGELOG.md
+11 -4
View File
@@ -2,9 +2,13 @@
Please add a note of your changes below this heading if you make a Pull Request.
### Added
* Encoder can now go forever in velocity/torque mode due to using circular encoder space.
* `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you should run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `hw_version_[...]` properties.
* bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_version_[...]` properties.
* infrastructure to publish the python tools to PyPi. See `tools/setup.py` for details.
* Automated test script `run_tests.py`
* Protocol supports function return values
* System stats (e.g. stack usage) are exposed under `<odrv>.system_stats`
### Changed
* The DFU script now verifies the flash after writing
@@ -16,15 +20,14 @@ Please add a note of your changes below this heading if you make a Pull Request.
* ODrive accesses from within python tools are now thread-safe. That means you can read from the same remote property from multiple threads concurrently.
* The liveplotter (`odrivetool liveplotter`, formerly `liveplotter.py`) does no longer steal focus and closes as expected
* (experimental: start liveplotter from `odrivetool` shell by typing `start_liveplotter(lambda: odrv0.motor0.encoder.encoder_state)`)
* `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you can run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `board_version_[...]` properties.
* `make write_otp` command to burn the board version onto the ODrive's one-time programmable memory. If you have an ODrive v3.4 or older, you can run this once for a better firmware update user experience in the future. Run the command without any options for more details. Once set, the board version is exposed through the `hw_version_[...]` properties.
* bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_version_[...]` properties.
* Set thread priority of USB pump thread above protocol thread
### Changed
* The DFU script now verifies the flash after writing
* GPIO3 not sensitive to edges by default
### Fixed
* Enums now transported with correct underlying type on native protocol
* USB issue where the device would stop responding when the host script would quit abruptly or reset the device during operation
# Releases
@@ -37,6 +40,10 @@ Please add a note of your changes below this heading if you make a Pull Request.
* Travis-CI builds firmware for all board versions and deploys the binaries when a tag is pushed to master
### Changed
* Most of the code from `lowlevel.c` moved to `axis.cpp`, `encoder.cpp`, `controller.cpp`, `sensorless_estimator.cpp`, `motor.cpp` and the corresponding header files
* Refactoring of the developer-facing communication protocol interface. See e.g. `axis.hpp` or `controller.hpp` for examples on how to add your own fields and functions
* Change of the user-facing field paths. E.g. `my_odrive.motor0.pos_setpoint` is now at `my_odrive.axis0.controller.pos_setpoint`. Names are mostly unchanged.
* Rewrite of the top-level per-axis state-machine
* The build is now configured using the `tup.config` file instead of editing source files. Make sure you set your board version correctly. See [here](README.md#configuring-the-build) for details.
* The toplevel directory for tup is now `Firmware`. If you used tup before, go to `Firmware` and run `rm -rd ../.tup; rm -rd build/*; make`.
* Update CubeMX generated STM platform code to version 1.19.0
Firmware/Makefile
+16 -4
View File
@@ -5,18 +5,26 @@
BUILD_DIR = build
FIRMWARE = $(BUILD_DIR)/ODriveFirmware.elf
FIRMWARE_HEX = $(BUILD_DIR)/ODriveFirmware.hex
OPENOCD := openocd -f interface/stlink-v2.cfg \
$(if $(value PROGRAMMER),-c 'hla_serial $(PROGRAMMER)',) \
-f target/stm32f4x.cfg
all:
@tup --quiet --no-environ-check
flash: all
openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ write_image\ erase\ $(FIRMWARE) -c reset\ run -c exit
$(OPENOCD) -c init \
-c 'reset halt' \
-c 'flash write_image erase $(FIRMWARE)' \
-c 'reset run' \
-c exit
gdb: all
arm-none-eabi-gdb $(FIRMWARE) -x openocd.gdbinit
dfu: all
../tools/odrivetool $(if $(value SERIAL_NUMBER),--serial-number $(SERIAL_NUMBER),) dfu $(FIRMWARE_HEX)
python ../tools/odrivetool $(if $(value SERIAL_NUMBER),--serial-number $(SERIAL_NUMBER),) dfu $(FIRMWARE_HEX)
bmp: all
arm-none-eabi-gdb --ex 'target extended-remote /dev/stlink' \
@@ -24,9 +32,13 @@ bmp: all
--ex 'attach 1' \
--ex 'load' $(FIRMWARE)
# Erase entire STM32
erase:
$(OPENOCD) -c init -c reset\ halt -c flash\ erase_address\ 0x8000000\ 0x100000 -c reset\ run -c exit
# Erase all configuration from the ODrive
erase_config:
openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ run -c exit
$(OPENOCD) -c init -c reset\ halt -c flash\ erase_address\ 0x80C0000\ 0x40000 -c reset\ init -c reset\ run -c exit
# The one-time programmable memory stores the board version
# has the following format:
@@ -48,7 +60,7 @@ erase_config:
write_otp:
ifeq ($(OTP_CONFIRM),TRUE)
# Data:
openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg \
$(OPENOCD) \
-c init \
-c 'reset halt' \
-c 'mww 0x40023C04 0x45670123' \
Firmware/MotorControl/axis.cpp
+279 -80
View File
@@ -1,97 +1,296 @@
#include "axis.h"
#include <stdlib.h>
#include "legacy_commands.h"
#include <functional>
#include "gpio.h"
//TODO: goal of refactor is to kick this out completely
extern "C" {
#include "low_level.h"
}
#include "utils.h"
#include "odrive_main.h"
//TODO: Make it really clear where this is loaded.
AxisConfig axis_configs[2]; //TODO: get a constexpr for num motors
// C interface
extern "C" {
void axis_thread_entry(void const* temp_motor_ptr) {
Motor_t* motor = (Motor_t*)temp_motor_ptr;
//TODO: explicit axis number assignment
//for now we search for it
uint8_t ax_number = 0;
while (&motors[ax_number] != motor)
++ax_number;
Axis axis(axis_configs[ax_number], ax_number, motor);
axis.StateMachineLoop();
}
} // extern "C"
void Axis::SetupLegacyMappings() {
// Legacy reachability from C
legacy_motor_ref_->axis_legacy.enable_control = &enable_control_;
// override for compatibility with legacy comms paradigm
// TODO next gen comms
exposed_bools[4 * axis_number_ + 1] = &enable_control_;
exposed_bools[4 * axis_number_ + 2] = &do_calibration_;
}
Axis::Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref)
: axis_number_(axis_number),
enable_control_(config.enable_control_at_start),
do_calibration_(config.do_calibration_at_start),
Axis::Axis(const AxisHardwareConfig_t& hw_config,
AxisConfig_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor)
: hw_config_(hw_config),
config_(config),
legacy_motor_ref_(legacy_motor_ref) {
SetupLegacyMappings();
encoder_(encoder),
sensorless_estimator_(sensorless_estimator),
controller_(controller),
motor_(motor)
{
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
}
void Axis::StateMachineLoop() {
static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
// @brief Sets up all components of the axis,
// such as gate driver and encoder hardware.
void Axis::setup() {
encoder_.setup();
motor_.setup();
}
static void run_state_machine_loop_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
}
// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4*512);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
// @brief Unblocks the control loop thread.
// This is called from the current sense interrupt handler.
void Axis::signal_current_meas() {
if (thread_id_valid_)
osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS);
}
// @brief Blocks until a current measurement is completed
// @returns True on success, false otherwise
bool Axis::wait_for_current_meas() {
return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal;
}
// step/direction interface
void Axis::step_cb() {
if (enable_step_dir_) {
GPIO_PinState dir_pin = HAL_GPIO_ReadPin(hw_config_.dir_port, hw_config_.dir_pin);
float dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f;
controller_.pos_setpoint_ += dir * config_.counts_per_step;
}
};
// @brief Enables or disables step/dir input
void Axis::set_step_dir_enabled(bool enable) {
if (enable) {
// Set up the direction GPIO as input
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = hw_config_.dir_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(hw_config_.dir_port, &GPIO_InitStruct);
// Subscribe to rising edges of the step GPIO
GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN,
step_cb_wrapper, this);
enable_step_dir_ = true;
} else {
enable_step_dir_ = false;
// Unsubscribe from step GPIO
GPIO_unsubscribe(hw_config_.step_port, hw_config_.step_pin);
}
}
// @brief Returns true if everything is ok.
// Sets error and returns false otherwise.
bool Axis::do_checks() {
if (!motor_.do_checks())
return error_ |= ERROR_MOTOR_FAILED, false;
if (!(vbus_voltage >= board_config.dc_bus_undervoltage_trip_level))
return error_ |= ERROR_DC_BUS_UNDER_VOLTAGE, false;
if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level))
return error_ |= ERROR_DC_BUS_OVER_VOLTAGE, false;
return true;
}
bool Axis::run_sensorless_spin_up() {
// Early Spin-up: spiral up current
float x = 0.0f;
run_control_loop([&](){
float phase = wrap_pm_pi(config_.ramp_up_distance * x);
float I_mag = config_.spin_up_current * x;
x += current_meas_period / config_.ramp_up_time;
if (!motor_.update(I_mag, phase))
return error_ |= ERROR_MOTOR_FAILED, false;
return x < 1.0f;
});
if (error_ != ERROR_NO_ERROR)
return false;
// Late Spin-up: accelerate
float vel = config_.ramp_up_distance / config_.ramp_up_time;
float phase = wrap_pm_pi(config_.ramp_up_distance);
run_control_loop([&](){
vel += config_.spin_up_acceleration * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
float I_mag = config_.spin_up_current;
if (!motor_.update(I_mag, phase))
return error_ |= ERROR_MOTOR_FAILED, false;
return vel < config_.spin_up_target_vel;
});
return error_ == ERROR_NO_ERROR;
}
// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
bool Axis::run_sensorless_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
// We update the encoder just in case someone needs the output for testing
encoder_.update(nullptr, nullptr, nullptr);
if (!sensorless_estimator_.update(&pos_estimate, &vel_estimate, &phase))
return error_ |= ERROR_SENSORLESS_ESTIMATOR_FAILED, false;
if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, phase))
return error_ |= ERROR_MOTOR_FAILED, false;
return true;
});
set_step_dir_enabled(false);
return error_ == ERROR_NO_ERROR;
}
bool Axis::run_closed_loop_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
float pos_estimate, vel_estimate, phase, current_setpoint;
// We update the sensorless estimator just in case someone needs the output for testing
sensorless_estimator_.update(nullptr, nullptr, nullptr);
if (!encoder_.update(&pos_estimate, &vel_estimate, &phase))
return error_ |= ERROR_ENCODER_FAILED, false;
if (!controller_.update(pos_estimate, vel_estimate, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, phase))
return error_ |= ERROR_MOTOR_FAILED, false;
return true;
});
set_step_dir_enabled(false);
return error_ == ERROR_NO_ERROR;
}
bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
// if and only if we're in AXIS_STATE_IDLE
safety_critical_disarm_motor_pwm(motor_);
run_control_loop([this](){
sensorless_estimator_.update(nullptr, nullptr, nullptr);
encoder_.update(nullptr, nullptr, nullptr);
return true;
});
return error_ == ERROR_NO_ERROR;
}
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
//TODO: Move this somewhere else
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
int encoder_cpr = legacy_motor_ref_->encoder.encoder_cpr;
legacy_motor_ref_->anticogging.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
if (legacy_motor_ref_->anticogging.cogging_map != NULL) {
// TODO: Move this somewhere else
// TODO: respect changes of CPR
int encoder_cpr = encoder_.config_.cpr;
controller_.anticogging_.cogging_map = (float*)malloc(encoder_cpr * sizeof(float));
if (controller_.anticogging_.cogging_map != NULL) {
for (int i = 0; i < encoder_cpr; i++) {
legacy_motor_ref_->anticogging.cogging_map[i] = 0.0f;
controller_.anticogging_.cogging_map[i] = 0.0f;
}
}
legacy_motor_ref_->motor_thread = osThreadGetId();
legacy_motor_ref_->thread_ready = true;
bool calibration_ok = false;
// arm!
motor_.arm();
for (;;) {
// Keep rotor estimation up to date while idling
osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever);
loop_updates(legacy_motor_ref_);
if (do_calibration_) {
do_calibration_ = false;
__HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer); // enable pwm outputs
calibration_ok = motor_calibration(legacy_motor_ref_);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer); // disables pwm outputs
}
if (calibration_ok && enable_control_) {
legacy_motor_ref_->enable_step_dir = true;
__HAL_TIM_MOE_ENABLE(legacy_motor_ref_->motor_timer);
bool spin_up_ok = true;
if (legacy_motor_ref_->rotor_mode == ROTOR_MODE_SENSORLESS)
spin_up_ok = spin_up_sensorless(legacy_motor_ref_);
if (spin_up_ok)
control_motor_loop(legacy_motor_ref_);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(legacy_motor_ref_->motor_timer);
legacy_motor_ref_->enable_step_dir = false;
if (enable_control_) { // if control is still enabled, we exited because of error
calibration_ok = false;
enable_control_ = false;
// Load the task chain if a specific request is pending
if (requested_state_ != AXIS_STATE_UNDEFINED) {
size_t pos = 0;
if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) {
if (config_.startup_motor_calibration)
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
if (config_.startup_encoder_index_search && encoder_.config_.use_index)
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
if (config_.startup_encoder_offset_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
if (config_.startup_closed_loop_control)
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
else if (config_.startup_sensorless_control)
task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
if (encoder_.config_.use_index)
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ != AXIS_STATE_UNDEFINED) {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED;
// TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
}
// Note that current_state is a reference to task_chain_[0]
// Validate the state before running it
if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_)
current_state_ = AXIS_STATE_UNDEFINED;
if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_)
current_state_ = AXIS_STATE_UNDEFINED;
// Run the specified state
// Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
bool status;
switch (current_state_) {
case AXIS_STATE_MOTOR_CALIBRATION:
status = motor_.run_calibration();
if (!status)
error_ |= ERROR_MOTOR_FAILED;
break;
case AXIS_STATE_ENCODER_INDEX_SEARCH:
status = encoder_.run_index_search();
if (!status)
error_ |= ERROR_ENCODER_FAILED;
break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
status = encoder_.run_offset_calibration();
if (!status)
error_ |= ERROR_ENCODER_FAILED;
break;
case AXIS_STATE_SENSORLESS_CONTROL:
status = run_sensorless_spin_up(); // TODO: restart if desired
if (status)
status = run_sensorless_control_loop();
break;
case AXIS_STATE_CLOSED_LOOP_CONTROL:
status = run_closed_loop_control_loop();
break;
case AXIS_STATE_IDLE:
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
break;
default:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
break;
}
// If the state failed, go to idle, else advance task chain
if (!status)
current_state_ = AXIS_STATE_IDLE;
else
memcpy(task_chain_, task_chain_ + 1, sizeof(task_chain_) - sizeof(task_chain_[0]));
}
legacy_motor_ref_->thread_ready = false;
}
thread_id_valid_ = false;
}
Firmware/MotorControl/axis.h
-63
View File
@@ -1,63 +0,0 @@
#ifndef __AXIS_HPP
#define __AXIS_HPP
//TODO: goal of refactor is to kick this out completely
extern "C" {
#include "low_level.h"
}
//Outside axis:
//command handler
//callback dispatch
// TODO: decide if we want to consolidate all default configs in one file for ease of use?
struct AxisConfig {
bool enable_control_at_start = true;
bool do_calibration_at_start = true;
};
extern AxisConfig axis_configs[];
class Axis {
public:
//thread/os/system management
//timing log
//thread id
//etc.
//state machine
//control mode
//control_en/calib_ok
//error state
//motor
//current controller
//contains rotor phase logic
//motor level calibration routines
//low_level (implementation specifics)
//DRV driver
//adc callback handling
//pwm queueing
//rotor estimator
//kick out rotor phase logic
//pos/vel controller
//step/dir handler
// Object operation requires ptr to legacy object for now, TODO: get rid of this dep
Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref);
// Infinite loop that does calibration and enters main control loop as appropriate
void StateMachineLoop();
uint8_t axis_number_;
bool enable_control_;
bool do_calibration_;
AxisConfig& config_;
Motor_t* legacy_motor_ref_;
private:
void SetupLegacyMappings();
};
#endif /* __AXIS_HPP */
Firmware/MotorControl/axis.hpp
+203
View File
@@ -0,0 +1,203 @@
#ifndef __AXIS_HPP
#define __AXIS_HPP
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
// Warning: Do not reorder these enum values.
// The state machine uses ">" comparision on them.
enum AxisState_t {
AXIS_STATE_UNDEFINED = 0, //<! will fall through to idle
AXIS_STATE_IDLE = 1, //<! disable PWM and do nothing
AXIS_STATE_STARTUP_SEQUENCE = 2, //<! the actual sequence is defined by the config.startup_... flags
AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, //<! run all calibration procedures, then idle
AXIS_STATE_MOTOR_CALIBRATION = 4, //<! run motor calibration
AXIS_STATE_SENSORLESS_CONTROL = 5, //<! run sensorless control
AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
AXIS_STATE_CLOSED_LOOP_CONTROL = 8 //<! run closed loop control
};
struct AxisConfig_t {
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
// 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 enable_step_dir = true; //<! enable step/dir input after calibration
// For M0 this has no effect if enable_uart is true
float counts_per_step = 2.0f;
// Spinup settings
float ramp_up_time = 0.4f; // [s]
float ramp_up_distance = 4 * M_PI; // [rad]
float spin_up_current = 10.0f; // [A]
float spin_up_acceleration = 400.0f; // [rad/s^2]
float spin_up_target_vel = 400.0f; // [rad/s]
};
class Axis {
public:
enum Error_t {
ERROR_NO_ERROR = 0x00,
ERROR_INVALID_STATE = 0x01, //<! an invalid state was requested
ERROR_DC_BUS_UNDER_VOLTAGE = 0x02,
ERROR_DC_BUS_OVER_VOLTAGE = 0x04,
ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08,
ERROR_BRAKE_RESISTOR_DISARMED = 0x10, //<! the brake resistor was unexpectedly disarmed
ERROR_MOTOR_DISARMED = 0x20, //<! the motor was unexpectedly disarmed
ERROR_MOTOR_FAILED = 0x40,
ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80,
ERROR_ENCODER_FAILED = 0x100,
ERROR_CONTROLLER_FAILED = 0x200,
ERROR_POS_CTRL_DURING_SENSORLESS = 0x400,
};
enum thread_signals {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
};
Axis(const AxisHardwareConfig_t& hw_config,
AxisConfig_t& config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor);
void setup();
void start_thread();
void signal_current_meas();
bool wait_for_current_meas();
void step_cb();
void set_step_dir_enabled(bool enable);
bool check_DRV_fault();
bool check_PSU_brownout();
bool do_checks();
// @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) {
if (!brake_resistor_armed_) {
error_ |= ERROR_BRAKE_RESISTOR_DISARMED;
break;
}
if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED)) {
// motor got disarmed in something other than the idle loop
error_ |= ERROR_MOTOR_DISARMED;
break;
}
if (motor_.error_ != Motor::ERROR_NO_ERROR) {
error_ |= ERROR_MOTOR_FAILED;
break;
}
if (!do_checks()) // error set during function call
break;
// Run main loop function, defer quitting for after wait
// TODO: change arming logic to arm after waiting
bool main_continue = update_handler();
// Check we meet deadlines after queueing
++loop_counter_;
// 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 run_sensorless_spin_up();
bool run_sensorless_control_loop();
bool run_closed_loop_control_loop();
bool run_idle_loop();
void run_state_machine_loop();
const AxisHardwareConfig_t& hw_config_;
AxisConfig_t& config_;
Encoder& encoder_;
SensorlessEstimator& sensorless_estimator_;
Controller& controller_;
Motor& motor_;
osThreadId thread_id_;
volatile bool thread_id_valid_ = false;
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
bool enable_step_dir_ = false; // auto enabled after calibration, based on config.enable_step_dir
AxisState_t requested_state_ = AXIS_STATE_STARTUP_SEQUENCE;
AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
AxisState_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_property("enable_step_dir", &enable_step_dir_),
make_protocol_ro_property("current_state", &current_state_),
make_protocol_property("requested_state", &requested_state_),
make_protocol_ro_property("loop_counter", &loop_counter_),
make_protocol_object("config",
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
make_protocol_property("startup_encoder_offset_calibration", &config_.startup_encoder_offset_calibration),
make_protocol_property("startup_closed_loop_control", &config_.startup_closed_loop_control),
make_protocol_property("startup_sensorless_control", &config_.startup_sensorless_control),
make_protocol_property("enable_step_dir", &config_.enable_step_dir),
make_protocol_property("counts_per_step", &config_.counts_per_step),
make_protocol_property("ramp_up_time", &config_.ramp_up_time),
make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
make_protocol_property("spin_up_current", &config_.spin_up_current),
make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel)
),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
make_protocol_object("encoder", encoder_.make_protocol_definitions()),
make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions())
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(Axis::Error_t)
#endif /* __AXIS_HPP */
Firmware/MotorControl/axis_c_interface.h
-14
View File
@@ -1,14 +0,0 @@
#ifndef __AXIS_C_INTERFACE_H
#define __AXIS_C_INTERFACE_H
#ifdef __cplusplus
extern "C" {
#endif
void axis_thread_entry(void const * temp_motor_ptr);
#ifdef __cplusplus
}
#endif
#endif /* __AXIS_C_INTERFACE_H */
Firmware/MotorControl/board_config_v3.h
+120
View File
@@ -0,0 +1,120 @@
/*
* @brief Contains board specific configuration for ODrive v3.x
*/
#ifndef __BOARD_CONFIG_H
#define __BOARD_CONFIG_H
// STM specific includes
#include <gpio.h>
#include <spi.h>
#include <tim.h>
#include <main.h>
#if HW_VERSION_MAJOR == 3
#if HW_VERSION_MINOR <= 3
#define SHUNT_RESISTANCE (675e-6f)
#else
#define SHUNT_RESISTANCE (500e-6f)
#endif
#endif
typedef struct {
GPIO_TypeDef* step_port;
uint16_t step_pin;
GPIO_TypeDef* dir_port;
uint16_t dir_pin;
osPriority thread_priority;
} AxisHardwareConfig_t;
typedef struct {
TIM_HandleTypeDef* timer;
GPIO_TypeDef* index_port;
uint16_t index_pin;
} EncoderHardwareConfig_t;
typedef struct {
TIM_HandleTypeDef* timer;
uint16_t control_deadline;
float shunt_conductance;
} MotorHardwareConfig_t;
typedef struct {
SPI_HandleTypeDef* spi;
GPIO_TypeDef* enable_port;
uint16_t enable_pin;
GPIO_TypeDef* nCS_port;
uint16_t nCS_pin;
GPIO_TypeDef* nFAULT_port;
uint16_t nFAULT_pin;
} GateDriverHardwareConfig_t;
typedef struct {
AxisHardwareConfig_t axis_config;
EncoderHardwareConfig_t encoder_config;
MotorHardwareConfig_t motor_config;
GateDriverHardwareConfig_t gate_driver_config;
} BoardHardwareConfig_t;
extern const BoardHardwareConfig_t hw_configs[2];
#ifdef __MAIN_CPP__
const BoardHardwareConfig_t hw_configs[2] = { {
.axis_config = {
.step_port = GPIO_1_GPIO_Port,
.step_pin = GPIO_1_Pin,
.dir_port = GPIO_2_GPIO_Port,
.dir_pin = GPIO_2_Pin,
.thread_priority = (osPriority)(osPriorityHigh + (osPriority)1),
},
.encoder_config = {
.timer = &htim3,
.index_port = M0_ENC_Z_GPIO_Port,
.index_pin = M0_ENC_Z_Pin,
},
.motor_config = {
.timer = &htim1,
.control_deadline = TIM_1_8_PERIOD_CLOCKS,
.shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S]
},
.gate_driver_config = {
.spi = &hspi3,
// Note: this board has the EN_Gate pin shared!
.enable_port = EN_GATE_GPIO_Port,
.enable_pin = EN_GATE_Pin,
.nCS_port = M0_nCS_GPIO_Port,
.nCS_pin = M0_nCS_Pin,
.nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors
.nFAULT_pin = nFAULT_Pin,
}
},{
.axis_config = {
.step_port = GPIO_3_GPIO_Port,
.step_pin = GPIO_3_Pin,
.dir_port = GPIO_4_GPIO_Port,
.dir_pin = GPIO_4_Pin,
.thread_priority = osPriorityHigh,
},
.encoder_config = {
.timer = &htim4,
.index_port = M1_ENC_Z_GPIO_Port,
.index_pin = M1_ENC_Z_Pin,
},
.motor_config = {
.timer = &htim8,
.control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2,
.shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S]
},
.gate_driver_config = {
.spi = &hspi3,
// Note: this board has the EN_Gate pin shared!
.enable_port = EN_GATE_GPIO_Port,
.enable_pin = EN_GATE_Pin,
.nCS_port = M1_nCS_GPIO_Port,
.nCS_pin = M1_nCS_Pin,
.nFAULT_port = nFAULT_GPIO_Port, // the nFAULT pin is shared between both motors
.nFAULT_pin = nFAULT_Pin,
}
} };
#endif
#endif // __BOARD_CONFIG_H
Firmware/MotorControl/commands.cpp
-601
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/config.h
-17
View File
@@ -1,17 +0,0 @@
#ifndef __CONFIG_H
#define __CONFIG_H
#ifdef __cplusplus
extern "C" {
#endif
extern bool user_config_loaded;
void init_configuration(void);
void save_configuration(void);
void erase_configuration(void);
#ifdef __cplusplus
}
#endif
#endif /* __CONFIG_H */
Firmware/MotorControl/controller.cpp
+144
View File
@@ -0,0 +1,144 @@
#include "odrive_main.h"
Controller::Controller(ControllerConfig_t& config) :
config_(config)
{}
void Controller::reset() {
pos_setpoint_ = 0.0f;
vel_setpoint_ = 0.0f;
vel_integrator_current_ = 0.0f;
current_setpoint_ = 0.0f;
}
//--------------------------------
// Command Handling
//--------------------------------
void Controller::set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward) {
pos_setpoint_ = pos_setpoint;
vel_setpoint_ = vel_feed_forward;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_POSITION_CONTROL;
#ifdef DEBUG_PRINT
printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", pos_setpoint, vel_setpoint_, current_setpoint_);
#endif
}
void Controller::set_vel_setpoint(float vel_setpoint, float current_feed_forward) {
vel_setpoint_ = vel_setpoint;
current_setpoint_ = current_feed_forward;
config_.control_mode = CTRL_MODE_VELOCITY_CONTROL;
#ifdef DEBUG_PRINT
printf("VELOCITY_CONTROL %3.3f %3.3f\n", vel_setpoint_, motor->current_setpoint_);
#endif
}
void Controller::set_current_setpoint(float current_setpoint) {
current_setpoint_ = current_setpoint;
config_.control_mode = CTRL_MODE_CURRENT_CONTROL;
#ifdef DEBUG_PRINT
printf("CURRENT_CONTROL %3.3f\n", current_setpoint_);
#endif
}
void Controller::start_anticogging_calibration() {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NO_ERROR) {
anticogging_.calib_anticogging = true;
}
}
/*
* This anti-cogging implementation iterates through each encoder position,
* waits for zero velocity & position error,
* then samples the current required to maintain that position.
*
* This holding current is added as a feedforward term in the control loop.
*/
bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) {
if (anticogging_.calib_anticogging && anticogging_.cogging_map != NULL) {
float pos_err = anticogging_.index - pos_estimate;
if (fabsf(pos_err) <= anticogging_.calib_pos_threshold &&
fabsf(vel_estimate) < anticogging_.calib_vel_threshold) {
anticogging_.cogging_map[anticogging_.index++] = vel_integrator_current_;
}
if (anticogging_.index < axis_->encoder_.config_.cpr) { // TODO: remove the dependency on encoder CPR
set_pos_setpoint(anticogging_.index, 0.0f, 0.0f);
return false;
} else {
anticogging_.index = 0;
set_pos_setpoint(0.0f, 0.0f, 0.0f); // Send the motor home
anticogging_.use_anticogging = true; // We're good to go, enable anti-cogging
anticogging_.calib_anticogging = false;
return true;
}
}
return false;
}
bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) {
// Only runs if anticogging_.calib_anticogging is true; non-blocking
anticogging_calibration(pos_estimate, vel_estimate);
// Position control
// TODO Decide if we want to use encoder or pll position here
float vel_des = vel_setpoint_;
if (config_.control_mode >= CTRL_MODE_POSITION_CONTROL) {
float pos_err = pos_setpoint_ - pos_estimate;
vel_des += config_.pos_gain * pos_err;
}
// Velocity limiting
float vel_lim = config_.vel_limit;
if (vel_des > vel_lim) vel_des = vel_lim;
if (vel_des < -vel_lim) vel_des = -vel_lim;
// Velocity control
float Iq = current_setpoint_;
// Anti-cogging is enabled after calibration
// We get the current position and apply a current feed-forward
// ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1)
if (anticogging_.use_anticogging) {
Iq += anticogging_.cogging_map[mod(static_cast<int>(pos_estimate), axis_->encoder_.config_.cpr)];
}
float v_err = vel_des - vel_estimate;
if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) {
Iq += config_.vel_gain * v_err;
}
// Velocity integral action before limiting
Iq += vel_integrator_current_;
// Current limiting
float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current);
bool limited = false;
if (Iq > Ilim) {
limited = true;
Iq = Ilim;
}
if (Iq < -Ilim) {
limited = true;
Iq = -Ilim;
}
// Velocity integrator (behaviour dependent on limiting)
if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) {
// reset integral if not in use
vel_integrator_current_ = 0.0f;
} else {
if (limited) {
// TODO make decayfactor configurable
vel_integrator_current_ *= 0.99f;
} else {
vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err;
}
}
if (current_setpoint_output) *current_setpoint_output = Iq;
return true;
}
Firmware/MotorControl/controller.hpp
+102
View File
@@ -0,0 +1,102 @@
#ifndef __CONTROLLER_HPP
#define __CONTROLLER_HPP
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
// Note: these should be sorted from lowest level of control to
// highest level of control, to allow "<" style comparisons.
typedef enum {
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3
} Motor_control_mode_t;
struct ControllerConfig_t {
Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t
float pos_gain = 20.0f; // [(counts/s) / counts]
float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)]
// float vel_gain = 15.0f / 200.0f, // [A/(rad/s)] <sensorless example>
float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)]
float vel_limit = 20000.0f; // [counts/s]
};
class Controller {
public:
Controller(ControllerConfig_t& config);
void reset();
void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward);
void set_vel_setpoint(float vel_setpoint, float current_feed_forward);
void set_current_setpoint(float current_setpoint);
// TODO: make this more similar to other calibration loops
void start_anticogging_calibration();
bool anticogging_calibration(float pos_estimate, float vel_estimate);
bool update(float pos_estimate, float vel_estimate, float* current_setpoint);
ControllerConfig_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
// TODO: anticogging overhaul:
// - expose selected (all?) variables on protocol
// - make calibration user experience similar to motor & encoder calibration
// - use python tools to Fourier transform and write back the smoothed map or Fourier coefficients
// - make the calibration persistent
typedef struct {
int index;
float *cogging_map;
bool use_anticogging;
bool calib_anticogging;
float calib_pos_threshold;
float calib_vel_threshold;
} Anticogging_t;
Anticogging_t anticogging_ = {
.index = 0,
.cogging_map = nullptr,
.use_anticogging = false,
.calib_anticogging = false,
.calib_pos_threshold = 1.0f,
.calib_vel_threshold = 1.0f,
};
// variables exposed on protocol
float pos_setpoint_ = 0.0f;
float vel_setpoint_ = 0.0f;
// float vel_setpoint = 800.0f; <sensorless example>
float vel_integrator_current_ = 0.0f; // [A]
float current_setpoint_ = 0.0f; // [A]
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("pos_setpoint", &pos_setpoint_),
make_protocol_property("vel_setpoint", &vel_setpoint_),
make_protocol_property("vel_integrator_current", &vel_integrator_current_),
make_protocol_property("current_setpoint", &current_setpoint_),
make_protocol_object("config",
make_protocol_property("control_mode", &config_.control_mode),
make_protocol_property("pos_gain", &config_.pos_gain),
make_protocol_property("vel_gain", &config_.vel_gain),
make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain),
make_protocol_property("vel_limit", &config_.vel_limit)
),
make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint,
"pos_setpoint",
"vel_feed_forward",
"current_feed_forward"),
make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
"vel_setpoint",
"current_feed_forward"),
make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
"current_setpoint"),
make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
);
}
};
#endif // __CONTROLLER_HPP
Firmware/MotorControl/encoder.cpp
+254
View File
@@ -0,0 +1,254 @@
#include "odrive_main.h"
Encoder::Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config) :
hw_config_(hw_config),
config_(config)
{
// Calculate encoder pll gains
// This calculation is currently identical to the PLL in SensorlessEstimator
float pll_bandwidth = 1000.0f; // [rad/s]
pll_kp_ = 2.0f * pll_bandwidth;
// Critically damped
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
}
static void enc_index_cb_wrapper(void* ctx) {
reinterpret_cast<Encoder*>(ctx)->enc_index_cb();
}
void Encoder::setup() {
HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
enc_index_cb_wrapper, this);
}
//--------------------
// Hardware Dependent
//--------------------
// Triggered when an encoder passes over the "Index" pin
// TODO: only arm index edge interrupt when we know encoder has powered up
// TODO: disable interrupt once we found the index
void Encoder::enc_index_cb() {
if (config_.use_index && !index_found_) {
set_circular_count(0);
if (config_.pre_calibrated) {
offset_ = config_.offset;
is_ready_ = true;
}
index_found_ = true;
}
}
// Function that sets the current encoder count to a desired 32-bit value.
void Encoder::set_linear_count(int32_t count) {
// Disable interrupts to make a critical section to avoid race condition
uint32_t prim = __get_PRIMASK();
__disable_irq();
// Update states
shadow_count_ = count;
pos_estimate_ = (float)count;
//Write hardware last
hw_config_.timer->Instance->CNT = count;
__set_PRIMASK(prim);
}
// Function that sets the CPR circular tracking encoder count to a desired 32-bit value.
// Note that this will get mod'ed down to [0, cpr)
void Encoder::set_circular_count(int32_t count) {
// Disable interrupts to make a critical section to avoid race condition
uint32_t prim = __get_PRIMASK();
__disable_irq();
// Offset and state must be shifted by the same amount
offset_ += count - count_in_cpr_;
offset_ = mod(offset_, config_.cpr);
// Update states
count_in_cpr_ = mod(count, config_.cpr);
pos_cpr_ = (float)count_in_cpr_;
__set_PRIMASK(prim);
}
// @brief Slowly turns the motor in one direction until the
// encoder index is found.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_index_search() {
float voltage_magnitude;
if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
voltage_magnitude = axis_->motor_.config_.calibration_current;
else
return false;
float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed;
index_found_ = false;
float phase = 0.0f;
axis_->run_control_loop([&](){
phase = wrap_pm_pi(phase + omega * current_meas_period);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);
// continue until the index is found
return !index_found_;
});
return axis_->error_ != Axis::ERROR_NO_ERROR;
}
// @brief Turns the motor in one direction for a bit and then in the other
// direction in order to find the offset between the electrical phase 0
// and the encoder state 0.
// TODO: Do the scan with current, not voltage!
bool Encoder::run_offset_calibration() {
static const float start_lock_duration = 1.0f;
static const float scan_omega = 4.0f * M_PI;
static const float scan_distance = 16.0f * M_PI;
static const int num_steps = scan_distance / scan_omega * current_meas_hz;
// Temporarily disable index search so it doesn't mess
// with the offset calibration
bool old_use_index = config_.use_index;
config_.use_index = false;
float voltage_magnitude;
if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT)
voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
else if (axis_->motor_.config_.motor_type == MOTOR_TYPE_GIMBAL)
voltage_magnitude = axis_->motor_.config_.calibration_current;
else
return false;
// go to motor zero phase for start_lock_duration to get ready to scan
int i = 0;
axis_->run_control_loop([&](){
if (!axis_->motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
return ++i < start_lock_duration * current_meas_hz;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
int32_t init_enc_val = (int16_t)hw_config_.timer->Instance->CNT;
int64_t encvaluesum = 0;
// scan forward
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(scan_distance * (float)i / (float)num_steps - scan_distance / 2.0f);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float expected_encoder_delta = scan_distance / elec_rad_per_enc;
float actual_encoder_delta_abs = fabsf((int16_t)hw_config_.timer->Instance->CNT-init_enc_val);
if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > config_.calib_range)
{
error_ |= ERROR_CPR_OUT_OF_RANGE;
return false;
}
// check direction
if ((int16_t)hw_config_.timer->Instance->CNT > init_enc_val + 8) {
// motor same dir as encoder
axis_->motor_.config_.direction = 1;
} else if ((int16_t)hw_config_.timer->Instance->CNT < init_enc_val - 8) {
// motor opposite dir as encoder
axis_->motor_.config_.direction = -1;
} else {
// Encoder response error
error_ |= ERROR_RESPONSE;
return false;
}
// scan backwards
i = 0;
axis_->run_control_loop([&](){
float phase = wrap_pm_pi(-scan_distance * (float)i / (float)num_steps + scan_distance / 2.0f);
float v_alpha = voltage_magnitude * arm_cos_f32(phase);
float v_beta = voltage_magnitude * arm_sin_f32(phase);
if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
return false; // error set inside enqueue_voltage_timings
axis_->motor_.log_timing(Motor::TIMING_LOG_ENC_CALIB);
encvaluesum += (int16_t)hw_config_.timer->Instance->CNT;
return ++i < num_steps;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
offset_ = encvaluesum / (num_steps * 2);
is_ready_ = true;
config_.use_index = old_use_index;
return true;
}
bool Encoder::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
error_ |= ERROR_NUMERICAL;
return false;
}
// update internal encoder state
int16_t delta_enc_16 = (int16_t)hw_config_.timer->Instance->CNT - (int16_t)shadow_count_;
int32_t delta_enc = (int32_t)delta_enc_16; //sign extend
shadow_count_ += delta_enc;
count_in_cpr_ += delta_enc;
count_in_cpr_ = mod(count_in_cpr_, config_.cpr);
// compute electrical phase
int corrected_enc = count_in_cpr_ - offset_;
//TODO avoid recomputing elec_rad_per_enc every time
float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr));
float ph = elec_rad_per_enc * (float)corrected_enc;
// ph = fmodf(ph, 2*M_PI);
phase_ = wrap_pm_pi(ph);
// run pll (for now pll is in units of encoder counts)
// Predict current pos
pos_estimate_ += current_meas_period * pll_vel_;
pos_cpr_ += current_meas_period * pll_vel_;
// discrete phase detector
float delta_pos = (float)(shadow_count_ - (int32_t)floorf(pos_estimate_));
float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)floorf(pos_cpr_));
delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr));
// pll feedback
pos_estimate_ += current_meas_period * pll_kp_ * delta_pos;
pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr;
pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr));
pll_vel_ += current_meas_period * pll_ki_ * delta_pos_cpr;
if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_)
pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter
// Assign output arguments
if (pos_estimate) *pos_estimate = pos_estimate_;
if (vel_estimate) *vel_estimate = pll_vel_;
if (phase_output) *phase_output = phase_;
return true;
}
Firmware/MotorControl/encoder.hpp
+93
View File
@@ -0,0 +1,93 @@
#ifndef __ENCODER_HPP
#define __ENCODER_HPP
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
struct EncoderConfig_t {
bool use_index = false;
bool pre_calibrated = false; // If true, this means the offset stored in
// configuration is valid and does not need
// be determined by run_offset_calibration.
// In this case the encoder will enter ready
// state as soon as the index is found.
float idx_search_speed = 10.0f; // [rad/s electrical]
int32_t cpr = (2048 * 4); // Default resolution of CUI-AMT102 encoder,
int32_t offset = 0; // If pre_calibrated is true, this is copied into encoder.offset_ once
// index search succeeds
float calib_range = 0.02f;
};
class Encoder {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_NUMERICAL = 0x01,
ERROR_CPR_OUT_OF_RANGE = 0x02,
ERROR_RESPONSE = 0x04,
};
Encoder(const EncoderHardwareConfig_t& hw_config,
EncoderConfig_t& config);
void setup();
void enc_index_cb();
void set_linear_count(int32_t count);
void set_circular_count(int32_t count);
bool calib_enc_offset(float voltage_magnitude);
bool scan_for_enc_idx(float omega, float voltage_magnitude);
bool run_index_search();
bool run_offset_calibration();
bool update(float* pos_estimate, float* vel_estimate, float* phase);
const EncoderHardwareConfig_t& hw_config_;
EncoderConfig_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
Error_t error_ = ERROR_NONE;
bool index_found_ = false;
bool is_ready_ = false;
int32_t shadow_count_ = 0;
int32_t count_in_cpr_ = 0;
int32_t offset_ = 0;
float phase_ = 0.0f; // [rad]
float pos_estimate_ = 0.0f; // [rad]
float pos_cpr_ = 0.0f; // [rad]
float pll_vel_ = 0.0f; // [rad/s]
float pll_kp_ = 0.0f; // [rad/s / rad]
float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("is_ready", &is_ready_),
make_protocol_ro_property("index_found", const_cast<bool*>(&index_found_)),
make_protocol_property("shadow_count", &shadow_count_),
make_protocol_property("count_in_cpr", &count_in_cpr_),
make_protocol_property("offset", &offset_),
make_protocol_property("phase", &phase_),
make_protocol_property("pos_estimate", &pos_estimate_),
make_protocol_property("pos_cpr", &pos_cpr_),
make_protocol_property("pll_vel", &pll_vel_),
make_protocol_property("pll_kp", &pll_kp_),
make_protocol_property("pll_ki", &pll_ki_),
make_protocol_object("config",
make_protocol_property("use_index", &config_.use_index),
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("idx_search_speed", &config_.idx_search_speed),
make_protocol_property("cpr", &config_.cpr),
make_protocol_property("offset", &config_.offset),
make_protocol_property("calib_range", &config_.calib_range)
)
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(Encoder::Error_t)
#endif // __ENCODER_HPP
Firmware/MotorControl/example.json
+37
View File
@@ -0,0 +1,37 @@
[
{
"name": "",
"id": 0,
"type": "json"
},
{
"name": "subscriptions",
"id": 1,
"type": "int32[]"
},
{
"name": "motor0",
"id": 2,
"type": "tree",
"content": [
{
"name": "pos_setpoint",
"id": 3,
"type": "float",
"access": "rw"
},
{
"name": "pos_gain",
"id": 4,
"type": "float",
"access": "rw"
},
{
"name": "vel_setpoint",
"id": 5,
"type": "float",
"access": "rw"
}
]
}
]
Firmware/MotorControl/legacy_commands.c
-291
View File
@@ -1,291 +0,0 @@
/* Includes ------------------------------------------------------------------*/
#include "legacy_commands.h"
#include <utils.h>
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
// This automatically updates to the interface that most
// recently recieved a command. In the future we may want to separate
// debug printf and the main serial comms.
SerialPrintf_t serial_printf_select = SERIAL_PRINTF_IS_UART;
/* Private constant data -----------------------------------------------------*/
// variables exposed to usb/serial interface via set/get/monitor
// Note: this will be depricated soon
float* exposed_floats[] = {
&vbus_voltage, // ro
NULL, //&elec_rad_per_enc, // ro
&motors[0].pos_setpoint, // rw
&motors[0].pos_gain, // rw
&motors[0].vel_setpoint, // rw
&motors[0].vel_gain, // rw
&motors[0].vel_integrator_gain, // rw
&motors[0].vel_integrator_current, // rw
&motors[0].vel_limit, // rw
&motors[0].current_setpoint, // rw
&motors[0].calibration_current, // rw
&motors[0].phase_inductance, // ro
&motors[0].phase_resistance, // ro
&motors[0].current_meas.phB, // ro
&motors[0].current_meas.phC, // ro
&motors[0].DC_calib.phB, // rw
&motors[0].DC_calib.phC, // rw
&motors[0].shunt_conductance, // rw
&motors[0].phase_current_rev_gain, // rw
&motors[0].current_control.current_lim, // rw
&motors[0].current_control.p_gain, // rw
&motors[0].current_control.i_gain, // rw
&motors[0].current_control.v_current_control_integral_d, // rw
&motors[0].current_control.v_current_control_integral_q, // rw
&motors[0].current_control.Ibus, // ro
&motors[0].encoder.phase, // ro
&motors[0].encoder.pll_pos, // rw
&motors[0].encoder.pll_vel, // rw
&motors[0].encoder.pll_kp, // rw
&motors[0].encoder.pll_ki, // rw
&motors[1].pos_setpoint, // rw
&motors[1].pos_gain, // rw
&motors[1].vel_setpoint, // rw
&motors[1].vel_gain, // rw
&motors[1].vel_integrator_gain, // rw
&motors[1].vel_integrator_current, // rw
&motors[1].vel_limit, // rw
&motors[1].current_setpoint, // rw
&motors[1].calibration_current, // rw
&motors[1].phase_inductance, // ro
&motors[1].phase_resistance, // ro
&motors[1].current_meas.phB, // ro
&motors[1].current_meas.phC, // ro
&motors[1].DC_calib.phB, // rw
&motors[1].DC_calib.phC, // rw
&motors[1].shunt_conductance, // rw
&motors[1].phase_current_rev_gain, // rw
&motors[1].current_control.current_lim, // rw
&motors[1].current_control.p_gain, // rw
&motors[1].current_control.i_gain, // rw
&motors[1].current_control.v_current_control_integral_d, // rw
&motors[1].current_control.v_current_control_integral_q, // rw
&motors[1].current_control.Ibus, // ro
&motors[1].encoder.phase, // ro
&motors[1].encoder.pll_pos, // rw
&motors[1].encoder.pll_vel, // rw
&motors[1].encoder.pll_kp, // rw
&motors[1].encoder.pll_ki, // rw
};
int* exposed_ints[] = {
(int*)&motors[0].control_mode, // rw
(int*)&motors[0].encoder.encoder_offset, // rw
(int*)&motors[0].encoder.encoder_state, // ro
(int*)&motors[0].error, // rw
(int*)&motors[1].control_mode, // rw
(int*)&motors[1].encoder.encoder_offset, // rw
(int*)&motors[1].encoder.encoder_state, // ro
(int*)&motors[1].error, // rw
};
bool* exposed_bools[] = {
&motors[0].thread_ready, // ro
//For now these are written by Axis::SetupLegacyMappings
NULL, // &motors[0].enable_control, // rw
NULL, // &motors[0].do_calibration, // rw
NULL, // &motors[0].calibration_ok, // ro
&motors[1].thread_ready, // ro
NULL, // &motors[1].enable_control, // rw
NULL, // &motors[1].do_calibration, // rw
NULL, // &motors[1].calibration_ok, // ro
};
uint16_t* exposed_uint16[] = {
&motors[0].control_deadline, // rw
&motors[0].last_cpu_time, // ro
&motors[1].control_deadline, // rw
&motors[1].last_cpu_time, // ro
};
/* Private variables ---------------------------------------------------------*/
monitoring_slot monitoring_slots[20] = {0};
/* Private function prototypes -----------------------------------------------*/
static void print_monitoring(int limit);
/* Function implementations --------------------------------------------------*/
void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_capacity, SerialPrintf_t response_interface) {
// Set response interface
serial_printf_select = response_interface;
// Cast away const and write beyond the array bounds. Because we can.
// (TODO: yeah maybe not, but this should be gone once we disable legacy commands)
((uint8_t *)buffer)[len < buffer_capacity ? len : (buffer_capacity - 1)] = 0;
// check incoming packet type
if (buffer[0] == 'p') {
// position control
unsigned motor_number;
float pos_setpoint, vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, &current_feed_forward);
if (numscan == 4 && motor_number < num_motors) {
set_pos_setpoint(&motors[motor_number], pos_setpoint, vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'v') {
// velocity control
unsigned motor_number;
float vel_feed_forward, current_feed_forward;
int numscan = sscanf((const char*)buffer, "v %u %f %f", &motor_number, &vel_feed_forward, &current_feed_forward);
if (numscan == 3 && motor_number < num_motors) {
set_vel_setpoint(&motors[motor_number], vel_feed_forward, current_feed_forward);
}
} else if (buffer[0] == 'c') {
// current control
unsigned motor_number;
float current_feed_forward;
int numscan = sscanf((const char*)buffer, "c %u %f", &motor_number, &current_feed_forward);
if (numscan == 2 && motor_number < num_motors) {
set_current_setpoint(&motors[motor_number], current_feed_forward);
}
} else if(buffer[0] == 'i'){ // Dump device info
// Retrieves the device signature, revision, flash size, and UUID
printf("Signature: %#x\n", STM_ID_GetSignature());
printf("Revision: %#x\n", STM_ID_GetRevision());
printf("Flash Size: %#x KiB\n", STM_ID_GetFlashSize());
printf("UUID: 0x%lx%lx%lx\n", STM_ID_GetUUID(2), STM_ID_GetUUID(1), STM_ID_GetUUID(0));
} else if (buffer[0] == 'g') { // GET
// g <0:float,1:int,2:bool,3:uint16> index
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "g %u %u", &type, &index);
if (numscan == 2) {
switch(type){
case 0: {
printf("%f\n",*exposed_floats[index]);
break;
};
case 1: {
printf("%d\n",*exposed_ints[index]);
break;
};
case 2: {
printf("%d\n",*exposed_bools[index]);
break;
};
case 3: {
printf("%hu\n",*exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'h'){ // HALT
for(int i = 0; i < num_motors; i++){
set_vel_setpoint(&motors[i], 0.0f, 0.0f);
}
} else if (buffer[0] == 's') { // SET
// s <0:float,1:int,2:bool,3:uint16> index value
int type = 0;
int index = 0;
int numscan = sscanf((const char*)buffer, "s %u %u", &type, &index);
if (numscan == 2) {
switch(type) {
case 0: {
sscanf((const char*)buffer, "s %u %u %f", &type, &index, exposed_floats[index]);
break;
};
case 1: {
sscanf((const char*)buffer, "s %u %u %d", &type, &index, exposed_ints[index]);
break;
};
case 2: {
int btmp = 0;
sscanf((const char*)buffer, "s %u %u %d", &type, &index, &btmp);
*exposed_bools[index] = btmp ? true : false;
break;
};
case 3: {
sscanf((const char*)buffer, "s %u %u %hu", &type, &index, exposed_uint16[index]);
break;
};
}
}
} else if (buffer[0] == 'm') { // Setup Monitor
// m <0:float,1:int,2:bool,3:uint16> index monitoring_slot
int type = 0;
int index = 0;
int slot = 0;
int numscan = sscanf((const char*)buffer, "m %u %u %u", &type, &index, &slot);
if (numscan == 3) {
monitoring_slots[slot].type = type;
monitoring_slots[slot].index = index;
}
} else if (buffer[0] == 'o') { // Output Monitor
int limit = 0;
int numscan = sscanf((const char*)buffer, "o %u", &limit);
if (numscan == 1) {
print_monitoring(limit);
}
} else if (buffer[0] == 't') { // Run Anti-Cogging Calibration
for (int i = 0; i < num_motors; i++) {
// Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating
if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) {
motors[i].anticogging.calib_anticogging = true;
}
}
}
}
void legacy_parse_stream(const uint8_t* buffer, size_t len) {
#define PARSE_BUFFER_SIZE 64
static uint8_t parse_buffer[PARSE_BUFFER_SIZE];
static bool read_active = false;
static uint32_t parse_buffer_idx = 0;
while (len--) {
// Fetch the next char
uint8_t c = *(buffer++);
// Look for start character
if (c == '$') {
read_active = true;
continue; // do not record start char
}
// Record into parse buffer when actively reading
if (read_active) {
parse_buffer[parse_buffer_idx++] = c;
if (c == '\r' || c == '\n' || c == '!') {
// End of command string
legacy_parse_cmd(parse_buffer, parse_buffer_idx, PARSE_BUFFER_SIZE, SERIAL_PRINTF_IS_UART);
// Reset receieve state machine
read_active = false;
parse_buffer_idx = 0;
} else if (parse_buffer_idx == PARSE_BUFFER_SIZE - 1) {
// We are not at end of command, and receiving another character after this
// would go into the last slot, which is reserved for terminating null.
// We have effectively overflowed parse buffer: abort.
read_active = false;
parse_buffer_idx = 0;
}
}
}
}
static void print_monitoring(int limit) {
for (int i=0;i<limit;i++) {
switch (monitoring_slots[i].type) {
case 0:
printf("%f\t",*exposed_floats[monitoring_slots[i].index]);
break;
case 1:
printf("%d\t",*exposed_ints[monitoring_slots[i].index]);
break;
case 2:
printf("%d\t",*exposed_bools[monitoring_slots[i].index]);
break;
case 3:
printf("%hu\t",*exposed_uint16[monitoring_slots[i].index]);
break;
default:
i=100;
}
}
printf("\n");
}
Firmware/MotorControl/low_level.c
-1469
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/low_level.cpp
+445
View File
@@ -0,0 +1,445 @@
/* Includes ------------------------------------------------------------------*/
// Because of broken cmsis_os.h, we need to include arm_math first,
// otherwise chip specific defines are ommited
#include <stm32f405xx.h>
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
#define ARM_MATH_CM4
#include <arm_math.h>
#include <cmsis_os.h>
#include <math.h>
#include <stdint.h>
#include <stdlib.h>
#include <adc.h>
#include <gpio.h>
#include <main.h>
#include <spi.h>
#include <tim.h>
#include <utils.h>
#include "odrive_main.h"
/* Private defines -----------------------------------------------------------*/
// #define DEBUG_PRINT
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
// This value is updated by the DC-bus reading ADC.
// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late
float vbus_voltage = 12.0f;
bool brake_resistor_armed_ = false;
/* Private constant data -----------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* CPU critical section helpers ----------------------------------------------*/
static inline uint8_t cpu_enter_critical() {
uint8_t status_register;
asm (
"MRS R0, PRIMASK\n\t"
"CPSID I\n\t"
"STRB R0, %[output]"
: [output] "=m" (status_register) :: "r0"
);
return status_register;
}
static inline void cpu_exit_critical(uint8_t status_register) {
asm (
"ldrb r0, %[input]\n\t"
"msr PRIMASK,r0;\n\t"
::[input] "m" (status_register) : "r0"
);
}
/* Safety critical functions -------------------------------------------------*/
/*
* This section contains all accesses to safety critical hardware registers.
* Specifically, these registers:
* Motor0 PWMs:
* Timer1.MOE (master output enabled)
* Timer1.CCR1 (counter compare register 1)
* Timer1.CCR2 (counter compare register 2)
* Timer1.CCR3 (counter compare register 3)
* Motor1 PWMs:
* Timer8.MOE (master output enabled)
* Timer8.CCR1 (counter compare register 1)
* Timer8.CCR2 (counter compare register 2)
* Timer8.CCR3 (counter compare register 3)
* Brake resistor PWM:
* Timer2.CCR3 (counter compare register 3)
* Timer2.CCR4 (counter compare register 4)
*
* The following assumptions are made:
* - The hardware operates as described in the datasheet:
* http://www.st.com/content/ccc/resource/technical/document/reference_manual/3d/6d/5a/66/b4/99/40/d4/DM00031020.pdf/files/DM00031020.pdf/jcr:content/translations/en.DM00031020.pdf
* This assumption also requires for instance that there are no radiation
* caused hardware errors.
* - After startup, all variables used in this section are exclusively modified
* by the code in this section (this excludes function parameters)
* This assumption also requires that there is no memory corruption.
* - This code is compiled by a C standard compliant compiler.
*
* Furthermore:
* - Between calls to safety_critical_arm_motor_pwm and
* safety_critical_disarm_motor_pwm the motor's Ibus current is
* set to the correct value and update_brake_resistor is called
* at a high rate.
*/
// @brief Kicks off the arming process of the motor.
// All calls to this function must clearly originate
// from user input.
void safety_critical_arm_motor_pwm(Motor& motor) {
uint8_t sr = cpu_enter_critical();
if (brake_resistor_armed_) {
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_TIMINGS;
}
cpu_exit_critical(sr);
}
// @brief Disarms the motor PWM.
// After calling this function, it is guaranteed that all three
// motor phases are floating and will not be enabled again until
// safety_critical_arm_motor_phases is called.
// @returns true if the motor was in a state other than disarmed before
bool safety_critical_disarm_motor_pwm(Motor& motor) {
uint8_t sr = cpu_enter_critical();
bool was_armed = motor.armed_state_ != Motor::ARMED_STATE_DISARMED;
motor.armed_state_ = Motor::ARMED_STATE_DISARMED;
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motor.hw_config_.timer);
cpu_exit_critical(sr);
return was_armed;
}
// @brief Updates the phase timings unless the motor is disarmed.
//
// If this is called at a rate higher than the motor's timer period,
// the actual PMW timings on the pins can be undefined for up to one
// timer period.
void safety_critical_apply_motor_pwm_timings(Motor& motor, uint16_t timings[3]) {
uint8_t sr = cpu_enter_critical();
if (!brake_resistor_armed_) {
motor.armed_state_ = Motor::ARMED_STATE_ARMED;
}
motor.hw_config_.timer->Instance->CCR1 = timings[0];
motor.hw_config_.timer->Instance->CCR2 = timings[1];
motor.hw_config_.timer->Instance->CCR3 = timings[2];
if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_TIMINGS) {
// timings were just loaded into the timer registers
// the timer register are buffered, so they won't have an effect
// on the output just yet so we need to wait until the next
// interrupt before we actually enable the output
motor.armed_state_ = Motor::ARMED_STATE_WAITING_FOR_UPDATE;
} else if (motor.armed_state_ == Motor::ARMED_STATE_WAITING_FOR_UPDATE) {
// now we waited long enough. Enter armed state and
// enable the actual PWM outputs.
motor.armed_state_ = Motor::ARMED_STATE_ARMED;
__HAL_TIM_MOE_ENABLE(motor.hw_config_.timer); // enable pwm outputs
} else if (motor.armed_state_ == Motor::ARMED_STATE_ARMED) {
// nothing to do, PWM is running, all good
} else {
// unknown state oh no
safety_critical_disarm_motor_pwm(motor);
}
cpu_exit_critical(sr);
}
// @brief Arms the brake resistor
void safety_critical_arm_brake_resistor() {
uint8_t sr = cpu_enter_critical();
brake_resistor_armed_ = true;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
cpu_exit_critical(sr);
}
// @brief Disarms the brake resistor and by extension
// all motor PWM outputs.
// After calling this, the brake resistor can only be armed again
// by calling safety_critical_arm_brake_resistor().
void safety_critical_disarm_brake_resistor() {
uint8_t sr = cpu_enter_critical();
brake_resistor_armed_ = false;
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
}
cpu_exit_critical(sr);
}
// @brief Updates the brake resistor PWM timings unless
// the brake resistor is disarmed.
void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t high_on) {
if (high_on - low_off < TIM_APB1_DEADTIME_CLOCKS)
for(;;);
uint8_t sr = cpu_enter_critical();
if (brake_resistor_armed_) {
// Safe update of low and high side timings
// To avoid race condition, first reset timings to safe state
// ch3 is low side, ch4 is high side
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
htim2.Instance->CCR3 = low_off;
htim2.Instance->CCR4 = high_on;
}
cpu_exit_critical(sr);
}
/* Function implementations --------------------------------------------------*/
void start_adc_pwm() {
// Enable ADC and interrupts
__HAL_ADC_ENABLE(&hadc1);
__HAL_ADC_ENABLE(&hadc2);
__HAL_ADC_ENABLE(&hadc3);
// Warp field stabilize.
osDelay(2);
__HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC);
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC);
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC);
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC);
// Ensure that debug halting of the core doesn't leave the motor PWM running
__HAL_DBGMCU_FREEZE_TIM1();
__HAL_DBGMCU_FREEZE_TIM8();
start_pwm(&htim1);
start_pwm(&htim8);
// TODO: explain why this offset
sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128);
// Motor output starts in the disabled state
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1);
__HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8);
// Start brake resistor PWM in floating output configuration
htim2.Instance->CCR3 = 0;
htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1;
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
// Disarm motors and arm brake resistor
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
}
safety_critical_arm_brake_resistor();
}
void start_pwm(TIM_HandleTypeDef* htim) {
// Init PWM
int half_load = TIM_1_8_PERIOD_CLOCKS / 2;
htim->Instance->CCR1 = half_load;
htim->Instance->CCR2 = half_load;
htim->Instance->CCR3 = half_load;
// This hardware obfustication layer really is getting on my nerves
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2);
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3);
HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3);
htim->Instance->CCR4 = 1;
HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4);
}
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) {
// Store intial timer configs
uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE);
uint16_t CR2_store = htim_a->Instance->CR2;
uint16_t SMCR_store = htim_b->Instance->SMCR;
// Turn off output
htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE);
htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE);
// Disable both timer counters
htim_a->Instance->CR1 &= ~TIM_CR1_CEN;
htim_b->Instance->CR1 &= ~TIM_CR1_CEN;
// Set first timer to send TRGO on counter enable
htim_a->Instance->CR2 &= ~TIM_CR2_MMS;
htim_a->Instance->CR2 |= TIM_TRGO_ENABLE;
// Set Trigger Source of second timer to the TRGO of the first timer
htim_b->Instance->SMCR &= ~TIM_SMCR_TS;
htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx;
// Set 2nd timer to start on trigger
htim_b->Instance->SMCR &= ~TIM_SMCR_SMS;
htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER;
// Dir bit is read only in center aligned mode, so we clear the mode for now
uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS;
uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS;
htim_a->Instance->CR1 &= ~TIM_CR1_CMS;
htim_b->Instance->CR1 &= ~TIM_CR1_CMS;
// Set both timers to up-counting state
htim_a->Instance->CR1 &= ~TIM_CR1_DIR;
htim_b->Instance->CR1 &= ~TIM_CR1_DIR;
// Restore center aligned mode
htim_a->Instance->CR1 |= CMS_store_a;
htim_b->Instance->CR1 |= CMS_store_b;
// set counter offset
htim_a->Instance->CNT = count_offset;
htim_b->Instance->CNT = 0;
// Start Timer a
htim_a->Instance->CR1 |= (TIM_CR1_CEN);
// Restore timer configs
htim_a->Instance->CR2 = CR2_store;
htim_b->Instance->SMCR = SMCR_store;
// restore output
htim_a->Instance->BDTR |= MOE_store_a;
htim_b->Instance->BDTR |= MOE_store_b;
}
// @brief Floats ALL phases immediately and disarms both motors and the brake resistor.
void low_level_fault(Motor::Error_t error) {
// Disable all motors NOW!
for (size_t i = 0; i < AXIS_COUNT; ++i) {
safety_critical_disarm_motor_pwm(axes[i]->motor_);
axes[i]->motor_.error_ |= error;
}
safety_critical_disarm_brake_resistor();
}
//--------------------------------
// IRQ Callbacks
//--------------------------------
void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12);
// Only one conversion in sequence, so only rank1
uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
vbus_voltage = ADCValue * voltage_scale;
if (axes[0] && !axes[0]->error_ && axes[1] && !axes[1]->error_) {
if (oscilloscope_pos >= OSCILLOSCOPE_SIZE)
oscilloscope_pos = 0;
oscilloscope[oscilloscope_pos++] = vbus_voltage;
}
}
// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion.
// TODO: Document how the phasing is done, link to timing diagram
void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) {
#define calib_tau 0.2f //@TOTO make more easily configurable
static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau;
// Ensure ADCs are expected ones to simplify the logic below
if (!(hadc == &hadc2 || hadc == &hadc3)) {
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];
Axis& other_axis = injected ? *axes[1] : *axes[0];
bool counting_down = axis.motor_.hw_config_.timer->Instance->CR1 & TIM_CR1_DIR;
bool current_meas_not_DC_CAL = !counting_down;
bool update_timings = false;
if (hadc == &hadc2) {
if (&axis == axes[1] && counting_down)
update_timings = true; // update timings of M0
else if (&axis == axes[0] && !counting_down)
update_timings = true; // update timings of M1
}
// Load next timings for the motor that we're not currently sampling
if (update_timings) {
if (!other_axis.motor_.next_timings_valid_) {
// 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();
}
// Check the timing of the sequencing
if (current_meas_not_DC_CAL)
axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_I);
else
axis.motor_.log_timing(Motor::TIMING_LOG_ADC_CB_DC);
uint32_t ADCValue;
if (injected) {
ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
} else {
ADCValue = HAL_ADC_GetValue(hadc);
}
float current = axis.motor_.phase_current_from_adcval(ADCValue);
if (current_meas_not_DC_CAL) {
// ADC2 and ADC3 record the phB and phC currents concurrently,
// and their interrupts should arrive on the same clock cycle.
// We dispatch the callbacks in order, so ADC2 will always be processed before ADC3.
// Therefore we store the value from ADC2 and signal the thread that the
// measurement is ready when we receive the ADC3 measurement
// return or continue
if (hadc == &hadc2) {
axis.motor_.current_meas_.phB = current - axis.motor_.DC_calib_.phB;
return;
} else {
axis.motor_.current_meas_.phC = current - axis.motor_.DC_calib_.phC;
}
// Trigger axis thread
axis.signal_current_meas();
} else {
// DC_CAL measurement
if (hadc == &hadc2) {
axis.motor_.DC_calib_.phB += (current - axis.motor_.DC_calib_.phB) * calib_filter_k;
} else {
axis.motor_.DC_calib_.phC += (current - axis.motor_.DC_calib_.phC) * calib_filter_k;
}
}
}
// @brief Sums up the Ibus contribution of each motor and updates the
// brake resistor PWM accordingly.
void update_brake_current() {
float Ibus_sum = 0.0f;
for (size_t i = 0; i < AXIS_COUNT; ++i) {
if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) {
Ibus_sum += axes[i]->motor_.current_control_.Ibus;
}
}
float brake_current = -Ibus_sum;
// Clip negative values to 0.0f
if (brake_current < 0.0f) brake_current = 0.0f;
float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage;
// Duty limit at 90% to allow bootstrap caps to charge
// If brake_duty is NaN, this expression will also evaluate to false
if ((brake_duty >= 0.0f) && (brake_duty <= 0.9f)) {
int high_on = static_cast<int>(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty));
int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS;
if (low_off < 0) low_off = 0;
safety_critical_apply_brake_resistor_timings(low_off, high_on);
} else {
safety_critical_disarm_brake_resistor();
}
}
Firmware/MotorControl/low_level.h
+15 -257
View File
@@ -2,287 +2,45 @@
#ifndef __LOW_LEVEL_H
#define __LOW_LEVEL_H
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include <cmsis_os.h>
#include "drv8301.h"
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
#include <stdbool.h>
#include <adc.h>
/* Exported types ------------------------------------------------------------*/
typedef enum {
M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
} Motor_thread_signals_t;
typedef struct {
int index;
float *cogging_map;
bool use_anticogging;
bool calib_anticogging;
float calib_pos_threshold;
float calib_vel_threshold;
} Anticogging_t;
typedef enum {
ERROR_NO_ERROR,
ERROR_PHASE_RESISTANCE_TIMING,
ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE,
ERROR_PHASE_INDUCTANCE_TIMING,
ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE,
ERROR_ENCODER_RESPONSE,
ERROR_ENCODER_MEASUREMENT_TIMEOUT,
ERROR_ADC_FAILED,
ERROR_CALIBRATION_TIMING,
ERROR_FOC_TIMING,
ERROR_FOC_MEASUREMENT_TIMEOUT,
ERROR_SCAN_MOTOR_TIMING,
ERROR_FOC_VOLTAGE_TIMING,
ERROR_GATEDRIVER_INVALID_GAIN,
ERROR_PWM_SRC_FAIL,
ERROR_UNEXPECTED_STEP_SRC,
ERROR_POS_CTRL_DURING_SENSORLESS,
ERROR_SPIN_UP_TIMEOUT,
ERROR_DRV_FAULT,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE,
ERROR_ENCODER_CPR_OUT_OF_RANGE,
ERROR_DC_BUS_UNDERVOLTAGE,
ERROR_DC_BUS_OVERVOLTAGE,
} Error_t;
// Note: these should be sorted from lowest level of control to
// highest level of control, to allow "<" style comparisons.
typedef enum {
CTRL_MODE_VOLTAGE_CONTROL = 0,
CTRL_MODE_CURRENT_CONTROL = 1,
CTRL_MODE_VELOCITY_CONTROL = 2,
CTRL_MODE_POSITION_CONTROL = 3
} Motor_control_mode_t;
typedef enum {
MOTOR_TYPE_HIGH_CURRENT = 0,
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
MOTOR_TYPE_GIMBAL = 2
} Motor_type_t;
typedef struct {
float phB;
float phC;
} Iph_BC_t;
typedef struct {
float current_lim; // [A]
float p_gain; // [V/A]
float i_gain; // [V/As]
float v_current_control_integral_d; // [V]
float v_current_control_integral_q; // [V]
float Ibus; // DC bus current [A]
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq_setpoint;
float Iq_measured;
float max_allowed_current;
} Current_control_t;
typedef enum {
ROTOR_MODE_ENCODER,
ROTOR_MODE_SENSORLESS,
ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS //Run on encoder, but still run estimator for testing
} Rotor_mode_t;
typedef struct {
float phase;
float pll_pos;
float pll_vel;
float pll_kp;
float pll_ki;
float observer_gain; // [rad/s]
float flux_state[2]; // [Vs]
float V_alpha_beta_memory[2]; // [V]
float pm_flux_linkage; // [V / (rad/s)]
bool estimator_good;
float spin_up_current; // [A]
float spin_up_acceleration; // [rad/s^2]
float spin_up_target_vel; // [rad/s]
} Sensorless_t;
typedef struct {
TIM_HandleTypeDef* encoder_timer;
bool use_index;
bool index_found;
bool manually_calibrated;
float idx_search_speed;
int32_t encoder_cpr;
int32_t encoder_offset;
int32_t encoder_state;
int32_t motor_dir; // 1/-1 for fwd/rev alignment to encoder.
float encoder_calib_range;
float phase;
float pll_pos;
float pll_vel;
float pll_kp;
float pll_ki;
} Encoder_t;
typedef struct {
bool* enable_control;
} Axis_legacy_t;
#define TIMING_LOG_SIZE 16
typedef struct {
Axis_legacy_t axis_legacy;
Motor_control_mode_t control_mode;
bool enable_step_dir;
float counts_per_step;
Error_t error;
int32_t pole_pairs;
float pos_setpoint;
float pos_gain;
float vel_setpoint;
float vel_gain;
float vel_integrator_gain;
float vel_integrator_current;
float vel_limit;
float current_setpoint;
float calibration_current;
float resistance_calib_max_voltage;
float dc_bus_undervoltage_trip_level;
float dc_bus_overvoltage_trip_level;
float phase_inductance;
float phase_resistance;
osThreadId motor_thread;
bool thread_ready;
// bool enable_control; // enable/disable via usb to start motor control. will be set to false again in case of errors.requires calibration_ok=true
// bool do_calibration; // trigger motor calibration. will be reset to false after self test
// bool calibration_ok;
TIM_HandleTypeDef* motor_timer;
uint16_t next_timings[3];
uint16_t control_deadline;
uint16_t last_cpu_time;
Iph_BC_t current_meas;
Iph_BC_t DC_calib;
DRV8301_Obj gate_driver;
DRV_SPI_8301_Vars_t gate_driver_regs; //Local view of DRV registers
Motor_type_t motor_type;
float shunt_conductance;
float phase_current_rev_gain; //Reverse gain for ADC to Amps
Current_control_t current_control;
Rotor_mode_t rotor_mode;
Encoder_t encoder;
Sensorless_t sensorless;
uint32_t loop_counter;
uint16_t timing_log[TIMING_LOG_SIZE];
// Cache for remote procedure calls arguments
struct {
float pos_setpoint;
float vel_feed_forward;
float current_feed_forward;
} set_pos_setpoint_args;
struct {
float vel_setpoint;
float current_feed_forward;
} set_vel_setpoint_args;
struct {
float current_setpoint;
} set_current_setpoint_args;
Anticogging_t anticogging;
DRV8301_FaultType_e drv_fault;
} Motor_t;
typedef enum {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_M0_I,
TIMING_LOG_ADC_CB_M0_DC,
TIMING_LOG_ADC_CB_M1_I,
TIMING_LOG_ADC_CB_M1_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
} TimingLog_t;
typedef struct{
int type;
int index;
} monitoring_slot;
/* Exported constants --------------------------------------------------------*/
extern const size_t num_motors;
extern const float elec_rad_per_enc;
/* Exported variables --------------------------------------------------------*/
extern float vbus_voltage;
extern float brake_resistance;
extern Motor_t motors[];
/* Exported macro ------------------------------------------------------------*/
/* Exported functions --------------------------------------------------------*/
//Note: to control without feed forward, set feed forward terms to 0.0f.
void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward);
void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward);
void set_current_setpoint(Motor_t* motor, float current_setpoint);
void 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);
void step_cb(uint16_t GPIO_Pin);
void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index);
// 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 safe_assert(int arg);
void init_motor_control();
void setEncoderCount(Motor_t* motor, uint32_t count);
bool anti_cogging_calibration(Motor_t* motor);
bool motor_calibration(Motor_t* motor);
//// Old private:
// Utility
uint16_t check_timing(Motor_t* motor, TimingLog_t log_idx);
void global_fault(int error);
float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue);
// Initalisation
void DRV8301_setup(Motor_t* motor);
void start_adc_pwm();
void start_pwm(TIM_HandleTypeDef* htim);
void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b,
uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset);
// IRQ Callbacks (are all public)
// Measurement and calibrationa
bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage);
bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high);
bool calib_enc_offset(Motor_t* motor, float voltage_magnitude);
bool scan_for_enc_idx(Motor_t* motor, float v_d, float v_q);
bool anti_cogging_calibration(Motor_t* motor);
// Test functions
void scan_motor_loop(Motor_t* motor, float omega, float voltage_magnitude);
// Main motor control
bool do_checks(Motor_t* motor);
bool loop_updates(Motor_t* motor);
void update_rotor(Motor_t* motor);
bool using_encoder(Motor_t* motor);
bool using_sensorless(Motor_t* motor);
float get_rotor_phase(Motor_t* motor);
float get_pll_vel(Motor_t* motor);
bool spin_up_sensorless(Motor_t* motor);
void update_brake_current();
void set_brake_current(float brake_current);
void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta);
void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta);
bool FOC_voltage(Motor_t* motor, float v_d, float v_q);
bool FOC_current(Motor_t* motor, float Id_des, float Iq_des);
void control_motor_loop(Motor_t* motor);
//motor thread moved to axis object
//void motor_thread(void const * argument);
#ifdef __cplusplus
}
Firmware/MotorControl/main.cpp
+157
View File
@@ -0,0 +1,157 @@
#define __MAIN_CPP__
#include "odrive_main.h"
#include "nvm_config.hpp"
#include "freertos_vars.h"
#include <communication/interface_usb.h>
#include <communication/interface_uart.h>
BoardConfig_t board_config;
EncoderConfig_t encoder_configs[AXIS_COUNT];
ControllerConfig_t controller_configs[AXIS_COUNT];
MotorConfig_t motor_configs[AXIS_COUNT];
AxisConfig_t axis_configs[AXIS_COUNT];
bool user_config_loaded_;
SystemStats_t system_stats_ = { 0 };
Axis *axes[AXIS_COUNT];
typedef Config<
BoardConfig_t,
EncoderConfig_t[AXIS_COUNT],
ControllerConfig_t[AXIS_COUNT],
MotorConfig_t[AXIS_COUNT],
AxisConfig_t[AXIS_COUNT]> ConfigFormat;
void save_configuration(void) {
if (ConfigFormat::safe_store_config(
&board_config,
&encoder_configs,
&controller_configs,
&motor_configs,
&axis_configs)) {
//printf("saving configuration failed\r\n"); osDelay(5);
}
}
void load_configuration(void) {
// Try to load configs
if (NVM_init() ||
ConfigFormat::safe_load_config(
&board_config,
&encoder_configs,
&controller_configs,
&motor_configs,
&axis_configs)) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
for (size_t i = 0; i < AXIS_COUNT; ++i) {
encoder_configs[i] = EncoderConfig_t();
controller_configs[i] = ControllerConfig_t();
motor_configs[i] = MotorConfig_t();
axis_configs[i] = AxisConfig_t();
}
} else {
user_config_loaded_ = true;
}
}
void erase_configuration(void) {
NVM_erase();
}
void enter_dfu_mode() {
if ((hw_version_major == 3) && (hw_version_minor >= 5)) {
__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
_reboot_cookie = 0xDEADBEEF;
NVIC_SystemReset();
} else {
/*
* DFU mode is only allowed on board version >= 3.5 because it can burn
* the brake resistor FETs on older boards.
* If you really want to use it on an older board, add 3.3k pull-down resistors
* to the AUX_L and AUX_H signals and _only then_ uncomment these lines.
*/
//__asm volatile ("CPSID I\n\t":::"memory"); // disable interrupts
//_reboot_cookie = 0xDEADFE75;
//NVIC_SystemReset();
}
}
extern "C" {
int odrive_main(void);
void vApplicationStackOverflowHook(void) {
for (;;); // TODO: safe action
}
void vApplicationIdleHook(void) {
if (system_stats_.fully_booted) {
system_stats_.uptime = xTaskGetTickCount();
system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize();
system_stats_.min_stack_space_comms = uxTaskGetStackHighWaterMark(comm_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_axis0 = uxTaskGetStackHighWaterMark(axes[0]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_axis1 = uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t);
system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_usb_irq = uxTaskGetStackHighWaterMark(usb_irq_thread) * sizeof(StackType_t);
system_stats_.min_stack_space_startup = uxTaskGetStackHighWaterMark(defaultTaskHandle) * sizeof(StackType_t);
}
}
}
int odrive_main(void) {
// Load persistent configuration (or defaults)
load_configuration();
// Construct all objects.
for (size_t i = 0; i < AXIS_COUNT; ++i) {
Encoder *encoder = new Encoder(hw_configs[i].encoder_config,
encoder_configs[i]);
SensorlessEstimator *sensorless_estimator = new SensorlessEstimator();
Controller *controller = new Controller(controller_configs[i]);
Motor *motor = new Motor(hw_configs[i].motor_config,
hw_configs[i].gate_driver_config,
motor_configs[i]);
axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor);
}
// TODO: make dynamically reconfigurable
#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3
if (board_config.enable_uart) {
axes[0]->config_.enable_step_dir = false;
axes[0]->set_step_dir_enabled(false);
SetGPIO12toUART();
}
#endif
//osDelay(100);
// Init communications (this requires the axis objects to be constructed)
init_communication();
// Setup hardware for all components
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->setup();
}
// Start PWM and enable adc interrupts/callbacks
start_adc_pwm();
// This delay serves two purposes:
// - Let the current sense calibration converge (the current
// sense interrupts are firing in background by now)
// - Allow a user to interrupt the code, e.g. by flashing a new code,
// before it does anything crazy
// TODO make timing a function of calibration filter tau
osDelay(1500);
// Start state machine threads. Each thread will go through various calibration
// procedures and then run the actual controller loops.
// TODO: generalize for AXIS_COUNT != 2
for (size_t i = 0; i < AXIS_COUNT; ++i) {
axes[i]->start_thread();
}
system_stats_.fully_booted = true;
return 0;
}
Firmware/MotorControl/motor.cpp
+365
View File
@@ -0,0 +1,365 @@
#include <algorithm>
#include "drv8301.h"
#include "odrive_main.h"
Motor::Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
MotorConfig_t& config) :
hw_config_(hw_config),
gate_driver_config_(gate_driver_config),
config_(config),
gate_driver_({
.spiHandle = gate_driver_config_.spi,
.EngpioHandle = gate_driver_config_.enable_port,
.EngpioNumber = gate_driver_config_.enable_pin,
.nCSgpioHandle = gate_driver_config_.nCS_port,
.nCSgpioNumber = gate_driver_config_.nCS_pin,
})
{
}
// @brief Arms the PWM outputs that belong to this motor.
//
// Note that this does not yet activate the PWM outputs, it just unlocks them.
//
// While the motor is armed, the control loop must set new modulation timings
// between any two interrupts (that is, enqueue_modulation_timings must be executed).
// If the control loop fails to do so, the next interrupt handler floats the
// phases. Once this happens, missed_control_deadline is set to true and
// the motor can be considered disarmed.
//
// @returns: True on success, false otherwise
bool Motor::arm() {
// Reset controller states, integrators, setpoints, etc.
axis_->controller_.reset();
reset_current_control();
// Wait until the interrupt handler triggers twice. This gives
// the control loop the correct time quota to set up modulation timings.
if (!axis_->wait_for_current_meas())
return axis_->error_ |= Axis::ERROR_CURRENT_MEASUREMENT_TIMEOUT, false;
next_timings_valid_ = false;
safety_critical_arm_motor_pwm(*this);
return true;
}
void Motor::reset_current_control() {
current_control_.v_current_control_integral_d = 0.0f;
current_control_.v_current_control_integral_q = 0.0f;
}
// @brief Tune the current controller based on phase resistance and inductance
// This should be invoked whenever one of these values changes.
// TODO: allow update on user-request or update automatically via hooks
void Motor::update_current_controller_gains() {
// Calculate current control gains
float current_control_bandwidth = 1000.0f; // [rad/s]
current_control_.p_gain = current_control_bandwidth * config_.phase_inductance;
float plant_pole = config_.phase_resistance / config_.phase_inductance;
current_control_.i_gain = plant_pole * current_control_.p_gain;
}
// @brief Set up the gate drivers
void Motor::DRV8301_setup() {
DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
DRV8301_enable(&gate_driver_);
DRV8301_setupSpi(&gate_driver_, local_regs);
// TODO we can use reporting only if we actually wire up the nOCTW pin
local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown;
// Overcurrent set to approximately 150A at 100degC. This may need tweaking.
local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V;
// 20V/V on 500uOhm gives a range of +/- 150A
// 40V/V on 500uOhm gives a range of +/- 75A
// 20V/V on 666uOhm gives a range of +/- 110A
// 40V/V on 666uOhm gives a range of +/- 55A
local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV;
// local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV;
switch (local_regs->Ctrl_Reg_2.GAIN) {
case DRV8301_ShuntAmpGain_10VpV:
phase_current_rev_gain_ = 1.0f / 10.0f;
break;
case DRV8301_ShuntAmpGain_20VpV:
phase_current_rev_gain_ = 1.0f / 20.0f;
break;
case DRV8301_ShuntAmpGain_40VpV:
phase_current_rev_gain_ = 1.0f / 40.0f;
break;
case DRV8301_ShuntAmpGain_80VpV:
phase_current_rev_gain_ = 1.0f / 80.0f;
break;
}
float margin = 0.90f;
float max_input = margin * 0.3f * hw_config_.shunt_conductance;
float max_swing = margin * 1.6f * hw_config_.shunt_conductance * phase_current_rev_gain_;
current_control_.max_allowed_current = std::min(max_input, max_swing);
local_regs->SndCmd = true;
DRV8301_writeData(&gate_driver_, local_regs);
local_regs->RcvCmd = true;
DRV8301_readData(&gate_driver_, local_regs);
}
// @brief Checks if the gate driver is in operational state.
// @returns: true if the gate driver is OK (no fault), false otherwise
bool Motor::check_DRV_fault() {
//TODO: make this pin configurable per motor ch
GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(gate_driver_config_.nFAULT_port, gate_driver_config_.nFAULT_pin);
if (nFAULT_state == GPIO_PIN_RESET) {
// Update DRV Fault Code
drv_fault_ = DRV8301_getFaultType(&gate_driver_);
// Update/Cache all SPI device registers
// DRV_SPI_8301_Vars_t* local_regs = &gate_driver_regs_;
// local_regs->RcvCmd = true;
// DRV8301_readData(&gate_driver_, local_regs);
return false;
};
return true;
}
bool Motor::do_checks() {
if (!check_DRV_fault()) {
error_ |= ERROR_DRV_FAULT;
return false;
}
return true;
}
void Motor::log_timing(TimingLog_t log_idx) {
TIM_HandleTypeDef* htim = hw_config_.timer;
uint16_t timing = htim->Instance->CNT;
bool down = htim->Instance->CR1 & TIM_CR1_DIR;
if (down) {
uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing;
timing = TIM_1_8_PERIOD_CLOCKS + delta;
}
if (log_idx < TIMING_LOG_NUM_SLOTS) {
timing_log_[log_idx] = timing;
}
}
float Motor::phase_current_from_adcval(uint32_t ADCValue) {
int adcval_bal = (int)ADCValue - (1 << 11);
float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal;
float shunt_volt = amp_out_volt * phase_current_rev_gain_;
float current = shunt_volt * hw_config_.shunt_conductance;
return current;
}
//--------------------------------
// Measurement and calibration
//--------------------------------
// TODO check Ibeta balance to verify good motor connection
bool Motor::measure_phase_resistance(float test_current, float max_voltage) {
static const float kI = 10.0f; // [(V/s)/A]
static const int num_test_cycles = static_cast<int>(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s
float test_voltage = 0.0f;
size_t i = 0;
axis_->run_control_loop([&](){
float Ialpha = -(current_meas_.phB + current_meas_.phC);
test_voltage += (kI * current_meas_period) * (test_current - Ialpha);
if (test_voltage > max_voltage || test_voltage < -max_voltage)
return error_ |= ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, false;
// Test voltage along phase A
if (!enqueue_voltage_timings(test_voltage, 0.0f))
return false; // error set inside enqueue_voltage_timings
log_timing(TIMING_LOG_MEAS_R);
return ++i < num_test_cycles;
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
//// De-energize motor
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
// return false; // error set inside enqueue_voltage_timings
float R = test_voltage / test_current;
config_.phase_resistance = R;
return true; // if we ran to completion that means success
}
bool Motor::measure_phase_inductance(float voltage_low, float voltage_high) {
float test_voltages[2] = {voltage_low, voltage_high};
float Ialphas[2] = {0.0f};
static const int num_cycles = 5000;
size_t t = 0;
axis_->run_control_loop([&](){
int i = t & 1;
Ialphas[i] += -current_meas_.phB - current_meas_.phC;
// Test voltage along phase A
if (!enqueue_voltage_timings(test_voltages[i], 0.0f))
return false; // error set inside enqueue_voltage_timings
log_timing(TIMING_LOG_MEAS_L);
return ++t < (num_cycles << 1);
});
if (axis_->error_ != Axis::ERROR_NO_ERROR)
return false;
//// De-energize motor
//if (!enqueue_voltage_timings(motor, 0.0f, 0.0f))
// return false; // error set inside enqueue_voltage_timings
float v_L = 0.5f * (voltage_high - voltage_low);
// Note: A more correct formula would also take into account that there is a finite timestep.
// However, the discretisation in the current control loop inverts the same discrepancy
float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles);
float L = v_L / dI_by_dt;
config_.phase_inductance = L;
// TODO arbitrary values set for now
if (L < 1e-6f || L > 500e-6f)
return error_ |= ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, false;
return true;
}
bool Motor::run_calibration() {
float R_calib_max_voltage = config_.resistance_calib_max_voltage;
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if (!measure_phase_resistance(config_.calibration_current, R_calib_max_voltage))
return false;
if (!measure_phase_inductance(-R_calib_max_voltage, R_calib_max_voltage))
return false;
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
// no calibration needed
} else {
return false;
}
update_current_controller_gains();
is_calibrated_ = true;
return true;
}
bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
float tA, tB, tC;
if (SVM(mod_alpha, mod_beta, &tA, &tB, &tC) != 0)
return error_ |= ERROR_NUMERICAL, false;
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_valid_ = true;
return true;
}
bool Motor::enqueue_voltage_timings(float v_alpha, float v_beta) {
float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage);
float mod_alpha = vfactor * v_alpha;
float mod_beta = vfactor * v_beta;
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false;
log_timing(TIMING_LOG_FOC_VOLTAGE);
return true;
}
// TODO: This doesn't update brake current
// We should probably make FOC Current call FOC Voltage to avoid duplication.
bool Motor::FOC_voltage(float v_d, float v_q, float phase) {
float c = arm_cos_f32(phase);
float s = arm_sin_f32(phase);
float v_alpha = c*v_d - s*v_q;
float v_beta = c*v_q + s*v_d;
return enqueue_voltage_timings(v_alpha, v_beta);
}
bool Motor::FOC_current(float Id_des, float Iq_des, float phase) {
Current_control_t* ictrl = &current_control_;
// For Reporting
ictrl->Iq_setpoint = Iq_des;
// Clarke transform
float Ialpha = -current_meas_.phB - current_meas_.phC;
float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC);
// Park transform
float c = arm_cos_f32(phase);
float s = arm_sin_f32(phase);
float Id = c * Ialpha + s * Ibeta;
float Iq = c * Ibeta - s * Ialpha;
ictrl->Iq_measured = Iq;
// Current error
float Ierr_d = Id_des - Id;
float Ierr_q = Iq_des - Iq;
// TODO look into feed forward terms (esp omega, since PI pole maps to RL tau)
// Apply PI control
float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain;
float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain;
float mod_to_V = (2.0f / 3.0f) * vbus_voltage;
float V_to_mod = 1.0f / mod_to_V;
float mod_d = V_to_mod * Vd;
float mod_q = V_to_mod * Vq;
// Vector modulation saturation, lock integrator if saturated
// TODO make maximum modulation configurable
float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q);
if (mod_scalefactor < 1.0f) {
mod_d *= mod_scalefactor;
mod_q *= mod_scalefactor;
// TODO make decayfactor configurable
ictrl->v_current_control_integral_d *= 0.99f;
ictrl->v_current_control_integral_q *= 0.99f;
} else {
ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period);
ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period);
}
// Compute estimated bus current
ictrl->Ibus = mod_d * Id + mod_q * Iq;
// Inverse park transform
float mod_alpha = c * mod_d - s * mod_q;
float mod_beta = c * mod_q + s * mod_d;
// Report final applied voltage in stationary frame (for sensorles estimator)
ictrl->final_v_alpha = mod_to_V * mod_alpha;
ictrl->final_v_beta = mod_to_V * mod_beta;
// Apply SVM
if (!enqueue_modulation_timings(mod_alpha, mod_beta))
return false; // error set inside enqueue_modulation_timings
log_timing(TIMING_LOG_FOC_CURRENT);
return true;
}
bool Motor::update(float current_setpoint, float phase) {
current_setpoint *= config_.direction;
phase *= config_.direction;
// Execute current command
// TODO: move this into the mot
if (config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) {
if(!FOC_current(0.0f, current_setpoint, phase)){
return false;
}
} else if (config_.motor_type == MOTOR_TYPE_GIMBAL) {
//In gimbal motor mode, current is reinterptreted as voltage.
if(!FOC_voltage(0.0f, current_setpoint, phase))
return false;
} else {
error_ |= ERROR_NOT_IMPLEMENTED_MOTOR_TYPE;
return false;
}
return true;
}
Firmware/MotorControl/motor.hpp
+215
View File
@@ -0,0 +1,215 @@
#ifndef __MOTOR_HPP
#define __MOTOR_HPP
#ifndef __ODRIVE_MAIN_H
#error "This file should not be included directly. Include odrive_main.h instead."
#endif
#include "drv8301.h"
typedef enum {
MOTOR_TYPE_HIGH_CURRENT = 0,
// MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented
MOTOR_TYPE_GIMBAL = 2
} Motor_type_t;
typedef struct {
float phB;
float phC;
} Iph_BC_t;
typedef struct {
float p_gain; // [V/A]
float i_gain; // [V/As]
float v_current_control_integral_d; // [V]
float v_current_control_integral_q; // [V]
float Ibus; // DC bus current [A]
// Voltage applied at end of cycle:
float final_v_alpha; // [V]
float final_v_beta; // [V]
float Iq_setpoint;
float Iq_measured;
float max_allowed_current;
} Current_control_t;
// NOTE: for gimbal motors, all units of A are instead V.
// example: vel_gain is [V/(count/s)] instead of [A/(count/s)]
// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor.
typedef struct {
bool pre_calibrated = false; // can be set to true to indicate that all values here are valid
int32_t pole_pairs = 7; // This value is correct for N5065 motors and Turnigy SK3 series.
float calibration_current = 10.0f; // [A]
float resistance_calib_max_voltage = 1.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
float phase_inductance = 0.0f; // to be set by measure_phase_inductance
float phase_resistance = 0.0f; // to be set by measure_phase_resistance
int32_t direction = 1; // 1 or -1
Motor_type_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
// Read out max_allowed_current to see max supported value for current_lim.
// You can change DRV8301_ShuntAmpGain to get a different range.
// float current_lim = 75.0f; //[A]
float current_lim = 10.0f; //[A]
} MotorConfig_t;
class Motor {
public:
enum Error_t {
ERROR_NO_ERROR = 0,
ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x01,
ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x02,
ERROR_ADC_FAILED = 0x04,
ERROR_DRV_FAULT = 0x08,
ERROR_CONTROL_DEADLINE_MISSED = 0x10,
ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x20,
ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x40,
ERROR_NUMERICAL = 0x80
};
enum TimingLog_t {
TIMING_LOG_GENERAL,
TIMING_LOG_ADC_CB_I,
TIMING_LOG_ADC_CB_DC,
TIMING_LOG_MEAS_R,
TIMING_LOG_MEAS_L,
TIMING_LOG_ENC_CALIB,
TIMING_LOG_IDX_SEARCH,
TIMING_LOG_FOC_VOLTAGE,
TIMING_LOG_FOC_CURRENT,
TIMING_LOG_NUM_SLOTS
};
enum ArmedState_t {
ARMED_STATE_DISARMED,
ARMED_STATE_WAITING_FOR_TIMINGS,
ARMED_STATE_WAITING_FOR_UPDATE,
ARMED_STATE_ARMED,
};
Motor(const MotorHardwareConfig_t& hw_config,
const GateDriverHardwareConfig_t& gate_driver_config,
MotorConfig_t& config);
bool arm();
void disarm();
void setup() {
update_current_controller_gains();
DRV8301_setup();
}
void reset_current_control();
void update_current_controller_gains();
void DRV8301_setup();
bool check_DRV_fault();
bool do_checks();
void log_timing(TimingLog_t log_idx);
float phase_current_from_adcval(uint32_t ADCValue);
bool measure_phase_resistance(float test_current, float max_voltage);
bool measure_phase_inductance(float voltage_low, float voltage_high);
bool run_calibration();
bool enqueue_modulation_timings(float mod_alpha, float mod_beta);
bool enqueue_voltage_timings(float v_alpha, float v_beta);
bool FOC_voltage(float v_d, float v_q, float phase);
bool FOC_current(float Id_des, float Iq_des, float phase);
bool update(float current_setpoint, float phase);
const MotorHardwareConfig_t& hw_config_;
const GateDriverHardwareConfig_t gate_driver_config_;
MotorConfig_t& config_;
Axis* axis_ = nullptr; // set by Axis constructor
//private:
DRV8301_Obj gate_driver_; // initialized in constructor
uint16_t next_timings_[3] = {
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2,
TIM_1_8_PERIOD_CLOCKS / 2
};
bool next_timings_valid_ = false;
uint16_t last_cpu_time_ = 0;
int timing_log_index_ = 0;
uint16_t timing_log_[TIMING_LOG_NUM_SLOTS] = { 0 };
// variables exposed on protocol
Error_t error_ = ERROR_NO_ERROR;
// Do not write to this variable directly!
// It is for exclusive use by the safety_critical_... functions.
ArmedState_t armed_state_ = ARMED_STATE_DISARMED;
bool is_calibrated_ = config_.pre_calibrated;
Iph_BC_t current_meas_ = {0.0f, 0.0f};
Iph_BC_t DC_calib_ = {0.0f, 0.0f};
float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup)
Current_control_t current_control_ = {
.p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement
.i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement
.v_current_control_integral_d = 0.0f,
.v_current_control_integral_q = 0.0f,
.Ibus = 0.0f,
.final_v_alpha = 0.0f,
.final_v_beta = 0.0f,
.Iq_setpoint = 0.0f,
.Iq_measured = 0.0f,
.max_allowed_current = 0.0f,
};
DRV8301_FaultType_e drv_fault_ = DRV8301_FaultType_NoFault;
DRV_SPI_8301_Vars_t gate_driver_regs_; //Local view of DRV registers (initialized by DRV8301_setup)
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_ro_property("armed_state", &armed_state_),
make_protocol_ro_property("is_calibrated", &is_calibrated_),
make_protocol_ro_property("current_meas_phB", &current_meas_.phB),
make_protocol_ro_property("current_meas_phC", &current_meas_.phC),
make_protocol_property("DC_calib_phB", &DC_calib_.phB),
make_protocol_property("DC_calib_phC", &DC_calib_.phC),
make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_),
make_protocol_object("current_control",
make_protocol_property("p_gain", &current_control_.p_gain),
make_protocol_property("i_gain", &current_control_.i_gain),
make_protocol_property("v_current_control_integral_d", &current_control_.v_current_control_integral_d),
make_protocol_property("v_current_control_integral_q", &current_control_.v_current_control_integral_q),
make_protocol_property("Ibus", &current_control_.Ibus),
make_protocol_property("final_v_alpha", &current_control_.final_v_alpha),
make_protocol_property("final_v_beta", &current_control_.final_v_beta),
make_protocol_property("Iq_setpoint", &current_control_.Iq_setpoint),
make_protocol_property("Iq_measured", &current_control_.Iq_measured),
make_protocol_property("max_allowed_current", &current_control_.max_allowed_current)
),
make_protocol_object("gate_driver",
make_protocol_ro_property("drv_fault", &drv_fault_)
// make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value),
// make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value),
// make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value),
// make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value)
),
make_protocol_object("timing_log",
make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]),
make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]),
make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]),
make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]),
make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]),
make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]),
make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]),
make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT])
),
make_protocol_object("config",
make_protocol_property("pre_calibrated", &config_.pre_calibrated),
make_protocol_property("pole_pairs", &config_.pole_pairs),
make_protocol_property("calibration_current", &config_.calibration_current),
make_protocol_property("resistance_calib_max_voltage", &config_.resistance_calib_max_voltage),
make_protocol_property("phase_inductance", &config_.phase_inductance),
make_protocol_property("phase_resistance", &config_.phase_resistance),
make_protocol_property("direction", &config_.direction),
make_protocol_property("motor_type", &config_.motor_type),
make_protocol_property("current_lim", &config_.current_lim)
)
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(Motor::Error_t)
#endif // __MOTOR_HPP
Firmware/MotorControl/config.cpp → Firmware/MotorControl/nvm_config.hpp
+18 -131
View File
@@ -1,57 +1,38 @@
/*
* Convenience functions to load and store multiple objects from and to NVM.
*
* The NVM stores consecutive one-to-one copies of arbitrary objects.
* The types of these objects are passed as template arguments to Config<Ts...>.
*/
/* Includes ------------------------------------------------------------------*/
#include "config.h"
#include <stdint.h>
#include <stdlib.h>
#include <stm32f405xx.h>
#include "nvm.h"
#include "crc.hpp"
#include "low_level.h"
#include "axis.h"
#include <communication/crc.hpp>
// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
// the config structs, make sure to increment this number:
uint16_t config_version = 0x0001;
/* Private defines -----------------------------------------------------------*/
#define CRC16_INIT 0xabcd
#define CONFIG_CRC16_INIT 0xabcd
/* Private macros ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
typedef struct {
Motor_control_mode_t control_mode;
float counts_per_step;
int32_t pole_pairs;
float pos_gain;
float vel_gain;
float vel_integrator_gain;
float vel_limit;
float calibration_current;
float resistance_calib_max_voltage;
float phase_inductance;
float phase_resistance;
Motor_type_t motor_type;
Rotor_mode_t rotor_mode;
float current_control_current_lim;
bool encoder_use_index;
bool encoder_manually_calibrated;
float encoder_idx_search_speed;
int32_t encoder_cpr;
int32_t encoder_offset;
int32_t encoder_motor_dir;
} MotorConfig_t;
/* Global constant data ------------------------------------------------------*/
/* Global variables ----------------------------------------------------------*/
/* Private constant data -----------------------------------------------------*/
// IMPORTANT: if you change, reorder or otherwise modify any of the fields in
// the config structs, make sure to increment this number:
static constexpr uint16_t config_version = 0x0001;
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/* Function implementations --------------------------------------------------*/
// @brief Manages configuration load and store operations from and to NVM
//
// The NVM stores consecutive one-to-one copies of arbitrary objects.
@@ -121,11 +102,11 @@ struct Config<T, Ts...> {
// @brief Loads one or more consecutive objects from the NVM. The loaded data
// is validated using a CRC value that is stored at the beginning of the data.
static int load_config(T* val0, Ts* ... vals) {
static int safe_load_config(T* val0, Ts* ... vals) {
//printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5);
if (Config<T, Ts..., uint16_t>::get_size() > NVM_get_max_read_length())
return -1;
uint16_t crc16 = CRC16_INIT ^ config_version;
uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
if (Config<T, Ts..., uint16_t>::load_config(0, &crc16, val0, vals..., &crc16))
return -1;
if (crc16)
@@ -140,14 +121,14 @@ struct Config<T, Ts...> {
// changes of the config structs during firmware update. Note that if the total
// config data length changes, the CRC validation will fail even if the developer
// forgets to update the config version number.
static int store_config(const T* val0, const Ts* ... vals) {
static int safe_store_config(const T* val0, const Ts* ... vals) {
size_t size = Config<T, Ts...>::get_size() + 2;
//printf("config is %d bytes\r\n", size); osDelay(5);
if (size > NVM_get_max_write_length())
return -1;
if (NVM_start_write(size))
return -1;
uint16_t crc16 = CRC16_INIT ^ config_version;
uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version;
if (Config<T, Ts...>::store_config(0, &crc16, val0, vals...))
return -1;
if (Config<uint8_t, uint8_t>::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16))
@@ -157,97 +138,3 @@ struct Config<T, Ts...> {
return 0;
}
};
// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t)
// TODO: make this go away as part of the C++ refactoring
void set_motor_config(const MotorConfig_t* config, Motor_t* motor) {
motor->control_mode = config->control_mode;
motor->counts_per_step = config->counts_per_step;
motor->pole_pairs = config->pole_pairs;
motor->pos_gain = config->pos_gain;
motor->vel_gain = config->vel_gain;
motor->vel_integrator_gain = config->vel_integrator_gain;
motor->vel_limit = config->vel_limit;
motor->calibration_current = config->calibration_current;
motor->resistance_calib_max_voltage = config->resistance_calib_max_voltage;
motor->phase_inductance = config->phase_inductance;
motor->phase_resistance = config->phase_resistance;
motor->motor_type = config->motor_type;
motor->rotor_mode = config->rotor_mode;
motor->current_control.current_lim = config->current_control_current_lim;
motor->encoder.use_index = config->encoder_use_index;
motor->encoder.manually_calibrated = config->encoder_manually_calibrated;
motor->encoder.idx_search_speed = config->encoder_idx_search_speed;
motor->encoder.encoder_cpr = config->encoder_cpr;
motor->encoder.encoder_offset = config->encoder_offset;
motor->encoder.motor_dir = config->encoder_motor_dir;
}
// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t)
// TODO: make this go away as part of the C++ refactoring
void get_motor_config(const Motor_t* motor, MotorConfig_t* config) {
config->control_mode = motor->control_mode;
config->counts_per_step = motor->counts_per_step;
config->pole_pairs = motor->pole_pairs;
config->pos_gain = motor->pos_gain;
config->vel_gain = motor->vel_gain;
config->vel_integrator_gain = motor->vel_integrator_gain;
config->vel_limit = motor->vel_limit;
config->calibration_current = motor->calibration_current;
config->resistance_calib_max_voltage = motor->resistance_calib_max_voltage;
config->phase_inductance = motor->phase_inductance;
config->phase_resistance = motor->phase_resistance;
config->motor_type = motor->motor_type;
config->rotor_mode = motor->rotor_mode;
config->current_control_current_lim = motor->current_control.current_lim;
config->encoder_use_index = motor->encoder.use_index;
config->encoder_manually_calibrated = motor->encoder.manually_calibrated;
config->encoder_idx_search_speed = motor->encoder.idx_search_speed;
config->encoder_cpr = motor->encoder.encoder_cpr;
config->encoder_offset = motor->encoder.encoder_offset;
config->encoder_motor_dir = motor->encoder.motor_dir;
}
bool user_config_loaded = false;
void init_configuration(void) {
MotorConfig_t motor_config[2];
//TODO: we really shouldn't be hardcoding like this
if (NVM_init() || Config<MotorConfig_t, MotorConfig_t, AxisConfig, AxisConfig, float>::load_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) {
//printf("no config found\r\n"); osDelay(5);
// load default config
// motor_config[0] = MotorConfig_t();
// motor_config[1] = MotorConfig_t();
// TODO: temporary hack, this is gonna change after refactoring
axis_configs[0] = AxisConfig();
axis_configs[1] = AxisConfig();
brake_resistance = 0.47f;
// Default config coming from flashed Motor_t
return;
} else {
user_config_loaded = true;
//printf("load config successful\r\n"); osDelay(5);
set_motor_config(&motor_config[0], &motors[0]);
set_motor_config(&motor_config[1], &motors[1]);
}
}
void save_configuration(void) {
MotorConfig_t motor_config[2];
get_motor_config(&motors[0], &motor_config[0]);
get_motor_config(&motors[1], &motor_config[1]);
//TODO: we really shouldn't be hardcoding like this
if (Config<MotorConfig_t, MotorConfig_t, AxisConfig, AxisConfig, float>::store_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) {
//printf("saving configuration failed\r\n"); osDelay(5);
}
}
void erase_configuration(void) {
NVM_erase();
}
Firmware/MotorControl/odrive_main.h
+109
View File
@@ -0,0 +1,109 @@
#ifndef __ODRIVE_MAIN_H
#define __ODRIVE_MAIN_H
#ifdef __cplusplus
extern "C" {
#endif
// STM specific includes
#include <stm32f4xx_hal.h> // Sets up the correct chip specifc defines required by arm_math
#define ARM_MATH_CM4 // TODO: might change in future board versions
#include <arm_math.h>
// OS includes
#include <cmsis_os.h>
// Hardware configuration
#if HW_VERSION_MAJOR == 3
#include "board_config_v3.h"
#else
#error "unknown board version"
#endif
//default timeout waiting for phase measurement signals
#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms]
static const float current_meas_period = CURRENT_MEAS_PERIOD;
static const int current_meas_hz = CURRENT_MEAS_HZ;
extern float vbus_voltage;
extern bool brake_resistor_armed_;
extern const float elec_rad_per_enc;
extern uint32_t _reboot_cookie;
extern bool user_config_loaded_;
extern uint64_t serial_number;
extern char serial_number_str[13];
typedef struct {
bool fully_booted;
uint32_t uptime; // [ms]
uint32_t min_heap_space; // FreeRTOS heap [Bytes]
uint32_t min_stack_space_axis0; // minimum remaining space since startup [Bytes]
uint32_t min_stack_space_axis1;
uint32_t min_stack_space_comms;
uint32_t min_stack_space_usb;
uint32_t min_stack_space_uart;
uint32_t min_stack_space_usb_irq;
uint32_t min_stack_space_startup;
} SystemStats_t;
extern SystemStats_t system_stats_;
#ifdef __cplusplus
}
// @brief general user configurable board configuration
struct BoardConfig_t {
bool enable_uart = true;
float brake_resistance = 0.47f; // [ohm]
float dc_bus_undervoltage_trip_level = 8.0f; //<! [V] minimum voltage below which the motor stops operating
float dc_bus_overvoltage_trip_level = 1.08f * HW_VERSION_VOLTAGE; //<! [V] maximum voltage above which the motor stops operating.
//<! This protects against cases in which the power supply fails to dissipate
//<! the brake power if the brake resistor is disabled.
//<! The default is 26V for the 24V board version and 52V for the 48V board version.
};
extern BoardConfig_t board_config;
extern bool user_config_loaded_;
class Axis;
class Motor;
constexpr size_t AXIS_COUNT = 2;
extern Axis *axes[AXIS_COUNT];
// if you use the oscilloscope feature you can bump up this value
#define OSCILLOSCOPE_SIZE 128
extern float oscilloscope[OSCILLOSCOPE_SIZE];
extern size_t oscilloscope_pos;
// TODO: move
// this is technically not thread-safe but practically it might be
#define DEFINE_ENUM_FLAG_OPERATORS(ENUMTYPE) \
inline ENUMTYPE operator | (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) | static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) & static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast<ENUMTYPE>(static_cast<std::underlying_type_t<ENUMTYPE>>(a) ^ static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) |= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) &= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast<ENUMTYPE&>(reinterpret_cast<std::underlying_type_t<ENUMTYPE>&>(a) ^= static_cast<std::underlying_type_t<ENUMTYPE>>(b)); } \
inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_cast<std::underlying_type_t<ENUMTYPE>>(a)); }
// ODrive specific includes
#include <communication/protocol.hpp>
#include <utils.h>
#include <low_level.h>
#include <encoder.hpp>
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <axis.hpp>
#include <communication/communication.h>
#endif // __cplusplus
// general system functions defined in main.cpp
void save_configuration(void);
void erase_configuration(void);
void enter_dfu_mode(void);
#endif /* __ODRIVE_MAIN_H */
Firmware/MotorControl/protocol.hpp
-534
View File
File diff suppressed because it is too large Load Diff
Firmware/MotorControl/sensorless_estimator.cpp
+103
View File
@@ -0,0 +1,103 @@
#include "odrive_main.h"
SensorlessEstimator::SensorlessEstimator()
{
// Calculate pll gains
// This calculation is currently identical to the PLL in Encoder
float pll_bandwidth = 1000.0f; // [rad/s]
pll_kp_ = 2.0f * pll_bandwidth;
// Critically damped
pll_ki_ = 0.25f * (pll_kp_ * pll_kp_);
}
bool SensorlessEstimator::update(float* pos_estimate, float* vel_estimate, float* phase_output) {
// Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer
// http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf
// In particular, equation 8 (and by extension eqn 4 and 6).
// The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle
// is the one computed two cycles ago. To get the correct measurement, it was stored twice:
// once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory.
// Check that we don't get problems with discrete time approximation
if (!(current_meas_period * pll_kp_ < 1.0f)) {
error_ |= ERROR_NUMERICAL;
return false;
}
// Clarke transform
float I_alpha_beta[2] = {
-axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC,
one_by_sqrt3 * (axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC)};
// 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) {
// y is the total flux-driving voltage (see paper eqn 4)
float y = -axis_->motor_.config_.phase_resistance * I_alpha_beta[i] + V_alpha_beta_memory_[i];
// flux dynamics (prediction)
float x_dot = y;
// integrate prediction to current timestep
flux_state_[i] += x_dot * current_meas_period;
// eta is the estimated permanent magnet flux (see paper eqn 6)
eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
}
// Non-linear observer (see paper eqn 8):
float pm_flux_sqr = pm_flux_linkage_ * pm_flux_linkage_;
float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1];
float bandwidth_factor = 1.0f / pm_flux_sqr;
float eta_factor = 0.5f * (observer_gain_ * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr);
static float eta_factor_avg_test = 0.0f;
eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test);
// alpha-beta vector operations
for (int i = 0; i <= 1; ++i) {
// add observer action to flux estimate dynamics
float x_dot = eta_factor * eta[i];
// convert action to discrete-time
flux_state_[i] += x_dot * current_meas_period;
// update new eta
eta[i] = flux_state_[i] - axis_->motor_.config_.phase_inductance * I_alpha_beta[i];
}
// Flux state estimation done, store V_alpha_beta for next timestep
V_alpha_beta_memory_[0] = axis_->motor_.current_control_.final_v_alpha;
V_alpha_beta_memory_[1] = axis_->motor_.current_control_.final_v_beta * axis_->motor_.config_.direction;
// PLL
// TODO: the PLL part has some code duplication with the encoder PLL
// predict PLL phase with velocity
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_vel_);
// update PLL phase with observer permanent magnet phase
phase_ = fast_atan2(eta[1], eta[0]);
float delta_phase = wrap_pm_pi(phase_ - pll_pos_);
pll_pos_ = wrap_pm_pi(pll_pos_ + current_meas_period * pll_kp_ * delta_phase);
// update PLL velocity
pll_vel_ += current_meas_period * pll_ki_ * delta_phase;
//TODO TEMP TEST HACK
// static int trigger_ctr = 0;
// if (++trigger_ctr >= 3*current_meas_hz) {
// trigger_ctr = 0;
// //Change to sensorless units
// motor->vel_gain = 15.0f / 200.0f;
// motor->vel_setpoint = 800.0f * motor->encoder.motor_dir;
// //Change mode
// motor->rotor_mode = ROTOR_MODE_SENSORLESS;
// }
if (pos_estimate) *pos_estimate = pll_pos_;
if (vel_estimate) *vel_estimate = pll_vel_;
if (phase_output) *phase_output = phase_;
return true;
};
Firmware/MotorControl/sensorless_estimator.hpp
+45
View File
@@ -0,0 +1,45 @@
#ifndef __SENSORLESS_ESTIMATOR_HPP
#define __SENSORLESS_ESTIMATOR_HPP
class SensorlessEstimator {
public:
enum Error_t {
ERROR_NONE = 0,
ERROR_NUMERICAL = 0x01,
};
SensorlessEstimator();
bool update(float* pos_estimate, float* vel_estimate, float* phase);
Axis* axis_ = nullptr; // set by Axis constructor
// TODO: expose on protocol
Error_t error_ = ERROR_NONE;
float phase_ = 0.0f; // [rad]
float pll_pos_ = 0.0f; // [rad]
float pll_vel_ = 0.0f; // [rad/s]
float pll_kp_ = 0.0f; // [rad/s / rad]
float pll_ki_ = 0.0f; // [(rad/s^2) / rad]
float observer_gain_ = 1000.0f; // [rad/s]
float flux_state_[2] = {0.0f, 0.0f}; // [Vs]
float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V]
float pm_flux_linkage_ = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / (<pole pairs> * <rpm/v>) }
bool estimator_good_ = false;
// Communication protocol definitions
auto make_protocol_definitions() {
return make_protocol_member_list(
make_protocol_property("error", &error_),
make_protocol_property("phase", &phase_),
make_protocol_property("pll_pos", &pll_pos_),
make_protocol_property("pll_vel", &pll_vel_),
make_protocol_property("pll_kp", &pll_kp_),
make_protocol_property("pll_ki", &pll_ki_)
);
}
};
DEFINE_ENUM_FLAG_OPERATORS(SensorlessEstimator::Error_t)
#endif /* __SENSORLESS_ESTIMATOR_HPP */

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