diff --git a/.travis.yml b/.travis.yml index d530f0a1..7260e431 100644 --- a/.travis.yml +++ b/.travis.yml @@ -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: diff --git a/Firmware/.gitignore b/Firmware/.gitignore index d50af3bb..496462db 100644 --- a/Firmware/.gitignore +++ b/Firmware/.gitignore @@ -17,6 +17,9 @@ Odrive.xml .settings/ .project +# VSCode stuff +/.vscode/.cortex-debug.*.state.json + # STM32CubeMX (in case you put it in this folder, or a symlink) STM32CubeMX diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index 92b2b983..eab99cd5 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -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__" diff --git a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch index da4b9707..b9fef648 100644 --- a/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch +++ b/Firmware/Board/v3/0001-expose-correct-serial-number-on-USB.patch @@ -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; } diff --git a/Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch b/Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch new file mode 100644 index 00000000..afe0326e --- /dev/null +++ b/Firmware/Board/v3/0002-FreeRTOS-constness-fixes.patch @@ -0,0 +1,64 @@ +From 510ead2b159e1d8116e5241066c54a7bf8b7bfbe Mon Sep 17 00:00:00 2001 +From: Samuel Sadok +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 + diff --git a/Firmware/Board/v3/Inc/FreeRTOSConfig.h b/Firmware/Board/v3/Inc/FreeRTOSConfig.h index b31807fa..53280d99 100644 --- a/Firmware/Board/v3/Inc/FreeRTOSConfig.h +++ b/Firmware/Board/v3/Inc/FreeRTOSConfig.h @@ -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 diff --git a/Firmware/Board/v3/Inc/freertos_vars.h b/Firmware/Board/v3/Inc/freertos_vars.h index c1e122a0..6982ee28 100644 --- a/Firmware/Board/v3/Inc/freertos_vars.h +++ b/Firmware/Board/v3/Inc/freertos_vars.h @@ -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 */ \ No newline at end of file diff --git a/Firmware/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index 756ea9f3..f8ffe61b 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -59,7 +59,7 @@ #include "main.h" /* USER CODE BEGIN Includes */ - +#include /* USER CODE END Includes */ /* USER CODE BEGIN Private defines */ @@ -71,8 +71,12 @@ void MX_GPIO_Init(void); /* USER CODE BEGIN Prototypes */ void SetGPIO12toUART(); -void SetGPIO12toStepDir(); void SetupENCIndexGPIO(); +bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, + uint32_t pull_up_down, + void (*callback)(void*), void* ctx); +void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); + /* USER CODE END Prototypes */ diff --git a/Firmware/Board/v3/Inc/main.h b/Firmware/Board/v3/Inc/main.h index 4292821d..25d37a98 100644 --- a/Firmware/Board/v3/Inc/main.h +++ b/Firmware/Board/v3/Inc/main.h @@ -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 diff --git a/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h index 8c8eff81..bd3f6305 100644 --- a/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h +++ b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_2.h @@ -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 diff --git a/Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h new file mode 100644 index 00000000..19428406 --- /dev/null +++ b/Firmware/Board/v3/Inc/prev_board_ver/main_V3_4.h @@ -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 diff --git a/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h b/Firmware/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.h index 09cdf273..754be245 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/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index 583cb7f7..70c238a4 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -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 diff --git a/Firmware/Board/v3/Src/adc.c b/Firmware/Board/v3/Src/adc.c index 6d0db380..bb536024 100644 --- a/Firmware/Board/v3/Src/adc.c +++ b/Firmware/Board/v3/Src/adc.c @@ -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 */ diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index f370b539..f1d46642 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -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); diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 02b7d691..85b9028f 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -50,11 +50,14 @@ /* Includes ------------------------------------------------------------------*/ #include "gpio.h" /* USER CODE BEGIN 0 */ -#include "low_level.h" +#include #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 1 \ || HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR == 2 #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); } } diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 7b9d2c6c..98b64d31 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -59,8 +59,8 @@ #include "gpio.h" /* USER CODE BEGIN Includes */ -#include "utils.h" -#include "commands.h" +#include +#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(); diff --git a/Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c b/Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c new file mode 100644 index 00000000..49862c97 --- /dev/null +++ b/Firmware/Board/v3/Src/prev_board_ver/adc_V3_4.c @@ -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 */ + } +} diff --git a/Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c b/Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c new file mode 100644 index 00000000..075be197 --- /dev/null +++ b/Firmware/Board/v3/Src/prev_board_ver/gpio_V3_4.c @@ -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); + +} diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 07d3bb9d..1a650abc 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -38,11 +38,15 @@ /* USER CODE BEGIN 0 */ #include "freertos_vars.h" -#include "low_level.h" +#include 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); } diff --git a/Firmware/Board/v3/Src/syscalls.c b/Firmware/Board/v3/Src/syscalls.c index da7674fc..eff893dc 100644 --- a/Firmware/Board/v3/Src/syscalls.c +++ b/Firmware/Board/v3/Src/syscalls.c @@ -10,7 +10,6 @@ #include #include #include -#include // 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); -} diff --git a/Firmware/Board/v3/Src/usbd_cdc_if.c b/Firmware/Board/v3/Src/usbd_cdc_if.c index a7c0bd8c..1a9c43c4 100644 --- a/Firmware/Board/v3/Src/usbd_cdc_if.c +++ b/Firmware/Board/v3/Src/usbd_cdc_if.c @@ -52,9 +52,7 @@ /* USER CODE BEGIN INCLUDE */ #include "cmsis_os.h" -#include "freertos_vars.h" -#include "utils.h" -#include "commands.h" +#include #include /* 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 */ diff --git a/Firmware/Board/v3/Src/usbd_desc.c b/Firmware/Board/v3/Src/usbd_desc.c index e812a013..856d05e2 100644 --- a/Firmware/Board/v3/Src/usbd_desc.c +++ b/Firmware/Board/v3/Src/usbd_desc.c @@ -51,10 +51,9 @@ #include "usbd_core.h" #include "usbd_desc.h" #include "usbd_conf.h" -#include "commands.h" /* USER CODE BEGIN INCLUDE */ - +#include /* 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; } diff --git a/Firmware/Board/v3/startup_stm32f405xx.s b/Firmware/Board/v3/startup_stm32f405xx.s index ea0e76a9..34c01d1d 100644 --- a/Firmware/Board/v3/startup_stm32f405xx.s +++ b/Firmware/Board/v3/startup_stm32f405xx.s @@ -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.*/ diff --git a/Firmware/CHANGELOG.md b/Firmware/CHANGELOG.md index 4868b804..66a71605 100644 --- a/Firmware/CHANGELOG.md +++ b/Firmware/CHANGELOG.md @@ -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 `.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 diff --git a/Firmware/Makefile b/Firmware/Makefile index 3143961b..794b7695 100644 --- a/Firmware/Makefile +++ b/Firmware/Makefile @@ -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' \ diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 553c7190..7ddce582 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -1,97 +1,296 @@ -#include "axis.h" #include -#include "legacy_commands.h" +#include +#include "gpio.h" -//TODO: goal of refactor is to kick this out completely -extern "C" { -#include "low_level.h" -} +#include "utils.h" +#include "odrive_main.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(ctx)->step_cb(); +} + +// @brief Sets up all components of the axis, +// such as gate driver and encoder hardware. +void Axis::setup() { + encoder_.setup(); + motor_.setup(); +} + +static void run_state_machine_loop_wrapper(void* ctx) { + reinterpret_cast(ctx)->run_state_machine_loop(); +} + +// @brief Starts run_state_machine_loop in a new thread +void Axis::start_thread() { + osThreadDef(thread_def, run_state_machine_loop_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, ¤t_setpoint)) + return error_ |= ERROR_CONTROLLER_FAILED, false; + if (!motor_.update(current_setpoint, phase)) + return error_ |= ERROR_MOTOR_FAILED, false; + return true; + }); + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; +} + +bool Axis::run_closed_loop_control_loop() { + set_step_dir_enabled(config_.enable_step_dir); + run_control_loop([this](){ + float pos_estimate, vel_estimate, phase, current_setpoint; + + // We update the sensorless estimator just in case someone needs the output for testing + sensorless_estimator_.update(nullptr, nullptr, nullptr); + if (!encoder_.update(&pos_estimate, &vel_estimate, &phase)) + return error_ |= ERROR_ENCODER_FAILED, false; + if (!controller_.update(pos_estimate, vel_estimate, ¤t_setpoint)) + return error_ |= ERROR_CONTROLLER_FAILED, false; + if (!motor_.update(current_setpoint, phase)) + return error_ |= ERROR_MOTOR_FAILED, false; + return true; + }); + set_step_dir_enabled(false); + return error_ == ERROR_NO_ERROR; +} + +bool Axis::run_idle_loop() { + // run_control_loop ignores missed modulation timing updates + // if and only if we're in AXIS_STATE_IDLE + 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; -} \ No newline at end of file + + thread_id_valid_ = false; +} diff --git a/Firmware/MotorControl/axis.h b/Firmware/MotorControl/axis.h deleted file mode 100644 index 455515b1..00000000 --- a/Firmware/MotorControl/axis.h +++ /dev/null @@ -1,63 +0,0 @@ -#ifndef __AXIS_HPP -#define __AXIS_HPP - -//TODO: goal of refactor is to kick this out completely -extern "C" { -#include "low_level.h" -} - -//Outside axis: - //command handler - //callback dispatch - -// TODO: decide if we want to consolidate all default configs in one file for ease of use? -struct AxisConfig { - bool enable_control_at_start = true; - bool do_calibration_at_start = true; -}; -extern AxisConfig axis_configs[]; - -class Axis { -public: - //thread/os/system management - //timing log - //thread id - //etc. - //state machine - //control mode - //control_en/calib_ok - //error state - //motor - //current controller - //contains rotor phase logic - //motor level calibration routines - //low_level (implementation specifics) - //DRV driver - //adc callback handling - //pwm queueing - //rotor estimator - //kick out rotor phase logic - //pos/vel controller - //step/dir handler - - // Object operation requires ptr to legacy object for now, TODO: get rid of this dep - Axis(AxisConfig& config, uint8_t axis_number, Motor_t* legacy_motor_ref); - - // Infinite loop that does calibration and enters main control loop as appropriate - void StateMachineLoop(); - - uint8_t axis_number_; - - bool enable_control_; - bool do_calibration_; - - AxisConfig& config_; - - Motor_t* legacy_motor_ref_; - -private: - void SetupLegacyMappings(); - -}; - -#endif /* __AXIS_HPP */ diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp new file mode 100644 index 00000000..8e2245d4 --- /dev/null +++ b/Firmware/MotorControl/axis.hpp @@ -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, // + 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", ¤t_state_), + make_protocol_property("requested_state", &requested_state_), + make_protocol_ro_property("loop_counter", &loop_counter_), + make_protocol_object("config", + make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration), + make_protocol_property("startup_encoder_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 */ diff --git a/Firmware/MotorControl/axis_c_interface.h b/Firmware/MotorControl/axis_c_interface.h deleted file mode 100644 index f891e19b..00000000 --- a/Firmware/MotorControl/axis_c_interface.h +++ /dev/null @@ -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 */ diff --git a/Firmware/MotorControl/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h new file mode 100644 index 00000000..791246de --- /dev/null +++ b/Firmware/MotorControl/board_config_v3.h @@ -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 +#include +#include +#include + +#if HW_VERSION_MAJOR == 3 +#if HW_VERSION_MINOR <= 3 +#define SHUNT_RESISTANCE (675e-6f) +#else +#define SHUNT_RESISTANCE (500e-6f) +#endif +#endif + + +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 diff --git a/Firmware/MotorControl/commands.cpp b/Firmware/MotorControl/commands.cpp deleted file mode 100644 index 2ca2a066..00000000 --- a/Firmware/MotorControl/commands.cpp +++ /dev/null @@ -1,601 +0,0 @@ - -/* Includes ------------------------------------------------------------------*/ - -// TODO: remove this option -// and once the legacy protocol is phased out, remove the seq-no hack in protocol.py -// todo: make clean switches for protocol -#define ENABLE_LEGACY_PROTOCOL - -#include "commands.h" -#include "low_level.h" -#include "axis.h" -#include "protocol.hpp" -#include "freertos_vars.h" -#include "utils.h" -#include "config.h" -#include "../build/version.h" // autogenerated based on Git state - -#ifdef ENABLE_LEGACY_PROTOCOL -#include "legacy_commands.h" -#endif - -#include -#include -#include -#include -#include -#include - -#include - -#define UART_TX_BUFFER_SIZE 64 - -/* Private defines -----------------------------------------------------------*/ -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ - -extern PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern USBD_HandleTypeDef hUsbDeviceFS; -uint64_t serial_number; - -/* Private constant data -----------------------------------------------------*/ -// TODO: make command to switch gpio_mode during run-time - -typedef enum { - GPIO_MODE_NONE, - GPIO_MODE_UART, - GPIO_MODE_STEP_DIR, -} GpioMode_t; - -#if defined(USE_GPIO_MODE_STEP_DIR) -static const GpioMode_t gpio_mode = GPIO_MODE_STEP_DIR; //GPIO 1,2 is M0 Step,Dir -#elif !defined(UART_PROTOCOL_NONE) -static const GpioMode_t gpio_mode = GPIO_MODE_UART; //GPIO 1,2 is UART Tx,Rx -#else -static const GpioMode_t gpio_mode = GPIO_MODE_NONE; //GPIO 1,2 is not configured -#endif - -/* Private variables ---------------------------------------------------------*/ - -static uint8_t* usb_buf; -static uint32_t usb_len; - -// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable -static thread_local uint32_t deadline_ms = 0; - -/* Variables exposed to USB & UART via read/write commands */ -// TODO: include range information in JSON description - - -// TODO: Autogenerate these functions -void motors_0_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[0], - motors[0].set_pos_setpoint_args.pos_setpoint, - motors[0].set_pos_setpoint_args.vel_feed_forward, - motors[0].set_pos_setpoint_args.current_feed_forward); -} -void motors_0_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[0], - motors[0].set_vel_setpoint_args.vel_setpoint, - motors[0].set_vel_setpoint_args.current_feed_forward); -} -void motors_0_set_current_setpoint_func(void) { - set_current_setpoint(&motors[0], - motors[0].set_current_setpoint_args.current_setpoint); -} -void motors_1_set_pos_setpoint_func(void) { - set_pos_setpoint(&motors[1], - motors[1].set_pos_setpoint_args.pos_setpoint, - motors[1].set_pos_setpoint_args.vel_feed_forward, - motors[1].set_pos_setpoint_args.current_feed_forward); -} -void motors_1_set_vel_setpoint_func(void) { - set_vel_setpoint(&motors[1], - motors[1].set_vel_setpoint_args.vel_setpoint, - motors[1].set_vel_setpoint_args.current_feed_forward); -} -void motors_1_set_current_setpoint_func(void) { - set_current_setpoint(&motors[1], - motors[1].set_current_setpoint_args.current_setpoint); -} -void motors_run_anticogging_calibration_func() { - for (uint8_t i = 0; i < num_motors; i++) { - // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating - if (motors[i].anticogging.cogging_map != NULL && motors[i].error == ERROR_NO_ERROR) { - motors[i].anticogging.calib_anticogging = true; - } - } -} - -#if HW_VERSION_MAJOR == 3 -// Determine start address of the OTP struct: -// The OTP is organized into 16-byte blocks. -// If the first block starts with "0xfe" we use the first block. -// If the first block starts with "0x00" and the second block starts with "0xfe", -// we use the second block. This gives the user the chance to screw up once. -// If none of the above is the case, we consider the OTP invalid (otp_ptr will be NULL). -const uint8_t* otp_ptr = - (*(uint8_t*)FLASH_OTP_BASE == 0xfe) ? (uint8_t*)FLASH_OTP_BASE : - (*(uint8_t*)FLASH_OTP_BASE != 0x00) ? NULL : - (*(uint8_t*)(FLASH_OTP_BASE + 0x10) != 0xfe) ? NULL : - (uint8_t*)(FLASH_OTP_BASE + 0x10); - -// Read hardware version from OTP if available, otherwise fall back -// to software defined version. -const uint8_t board_version_major = otp_ptr ? otp_ptr[3] : HW_VERSION_MAJOR; -const uint8_t board_version_minor = otp_ptr ? otp_ptr[4] : HW_VERSION_MINOR; -const uint8_t board_version_variant = otp_ptr ? otp_ptr[5] : HW_VERSION_VOLTAGE; -#else -#error "not implemented" -#endif - -// the corresponding macros are defined in the autogenerated version.h -const uint8_t fw_version_major = FW_VERSION_MAJOR; -const uint8_t fw_version_minor = FW_VERSION_MINOR; -const uint8_t fw_version_revision = FW_VERSION_REVISION; -const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise - -void enter_dfu_mode() { - if ((board_version_major == 3) && (board_version_minor >= 5)) { - *((unsigned long *)0x2001C000) = 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. - */ - //*((unsigned long *)0x2001C000) = 0xDEADFE75; - //NVIC_SystemReset(); - } -} - -// This table specifies which fields and functions are exposed on the USB and UART ports. -// TODO: Autogenerate this table. It will come up again very soon in the Arduino library. -// clang-format off -const Endpoint endpoints[] = { - Endpoint::make_property("vbus_voltage", const_cast(&vbus_voltage)), - Endpoint::make_property("serial_number", const_cast(&serial_number)), - Endpoint::make_property("board_version_major", &board_version_major), - Endpoint::make_property("board_version_minor", &board_version_minor), - Endpoint::make_property("board_version_variant", &board_version_variant), - Endpoint::make_property("fw_version_major", &fw_version_major), - Endpoint::make_property("fw_version_minor", &fw_version_minor), - Endpoint::make_property("fw_version_revision", &fw_version_revision), - Endpoint::make_property("fw_version_unreleased", &fw_version_unreleased), - Endpoint::make_function("run_anticogging_calibration", &motors_run_anticogging_calibration_func), - // No parameters, but still requires a close_tree() - Endpoint::close_tree(), - Endpoint::make_object("config"), - Endpoint::make_property("brake_resistance", &brake_resistance), - Endpoint::close_tree(), - Endpoint::make_property("user_config_loaded", const_cast(&user_config_loaded)), - Endpoint::make_object("axis0"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control_at_start", &axis_configs[0].enable_control_at_start), - Endpoint::make_property("do_calibration_at_start", &axis_configs[0].do_calibration_at_start), - Endpoint::close_tree(), - Endpoint::close_tree(), - Endpoint::make_object("motor0"), - Endpoint::make_object("config"), - Endpoint::make_property("control_mode", reinterpret_cast*>(&motors[0].control_mode)), - Endpoint::make_property("counts_per_step", &motors[0].counts_per_step), - Endpoint::make_property("pole_pairs", &motors[0].pole_pairs), - Endpoint::make_property("pos_gain", &motors[0].pos_gain), - Endpoint::make_property("vel_gain", &motors[0].vel_gain), - Endpoint::make_property("vel_integrator_gain", &motors[0].vel_integrator_gain), - Endpoint::make_property("vel_limit", &motors[0].vel_limit), - Endpoint::make_property("calibration_current", &motors[0].calibration_current), - Endpoint::make_property("resistance_calib_max_voltage", &motors[0].resistance_calib_max_voltage), - Endpoint::make_property("phase_inductance", &motors[0].phase_inductance), - Endpoint::make_property("phase_resistance", &motors[0].phase_resistance), - Endpoint::make_property("motor_type", reinterpret_cast*>(&motors[0].motor_type)), - Endpoint::make_property("rotor_mode", reinterpret_cast*>(&motors[0].rotor_mode)), - Endpoint::close_tree(), - Endpoint::make_property("error", reinterpret_cast*>(&motors[0].error)), - Endpoint::make_property("pos_setpoint", &motors[0].pos_setpoint), - Endpoint::make_property("vel_setpoint", &motors[0].vel_setpoint), - Endpoint::make_property("vel_integrator_current", &motors[0].vel_integrator_current), - Endpoint::make_property("current_setpoint", &motors[0].current_setpoint), - Endpoint::make_property("current_meas_phB", const_cast(&motors[0].current_meas.phB)), - Endpoint::make_property("current_meas_phC", const_cast(&motors[0].current_meas.phC)), - Endpoint::make_property("DC_calib.phB", &motors[0].DC_calib.phB), - Endpoint::make_property("DC_calib.phC", &motors[0].DC_calib.phC), - Endpoint::make_property("shunt_conductance", &motors[0].shunt_conductance), - Endpoint::make_property("phase_current_rev_gain", &motors[0].phase_current_rev_gain), - Endpoint::make_property("thread_ready", &motors[0].thread_ready), - Endpoint::make_property("control_deadline", &motors[0].control_deadline), - Endpoint::make_property("last_cpu_time", &motors[0].last_cpu_time), - Endpoint::make_property("loop_counter", &motors[0].loop_counter), - Endpoint::make_object("current_control"), - Endpoint::make_object("config"), - Endpoint::make_property("current_lim", &motors[0].current_control.current_lim), - Endpoint::close_tree(), - Endpoint::make_property("p_gain", &motors[0].current_control.p_gain), - Endpoint::make_property("i_gain", &motors[0].current_control.i_gain), - Endpoint::make_property("v_current_control_integral_d", &motors[0].current_control.v_current_control_integral_d), - Endpoint::make_property("v_current_control_integral_q", &motors[0].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_setpoint", &motors[0].current_control.Iq_setpoint), - Endpoint::make_property("Iq_measured", &motors[0].current_control.Iq_measured), - Endpoint::make_property("Ibus", const_cast(&motors[0].current_control.Ibus)), - Endpoint::close_tree(), - Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_fault", reinterpret_cast*>(&motors[0].drv_fault)), - Endpoint::make_property("status_reg_1", (&motors[0].gate_driver_regs.Stat_Reg_1_Value)), - Endpoint::make_property("status_reg_2", (&motors[0].gate_driver_regs.Stat_Reg_2_Value)), - Endpoint::make_property("ctrl_reg_1", (&motors[0].gate_driver_regs.Ctrl_Reg_1_Value)), - Endpoint::make_property("ctrl_reg_2", (&motors[0].gate_driver_regs.Ctrl_Reg_2_Value)), - Endpoint::close_tree(), - Endpoint::make_object("encoder"), - Endpoint::make_object("config"), - Endpoint::make_property("use_index", &motors[0].encoder.use_index), - Endpoint::make_property("manually_calibrated", &motors[0].encoder.manually_calibrated), - Endpoint::make_property("idx_search_speed", &motors[0].encoder.idx_search_speed), - Endpoint::make_property("cpr", &motors[0].encoder.encoder_cpr), - Endpoint::make_property("offset", &motors[0].encoder.encoder_offset), - Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_property("phase", const_cast(&motors[0].encoder.phase)), - Endpoint::make_property("pll_pos", &motors[0].encoder.pll_pos), - Endpoint::make_property("pll_vel", &motors[0].encoder.pll_vel), - Endpoint::make_property("pll_kp", &motors[0].encoder.pll_kp), - Endpoint::make_property("pll_ki", &motors[0].encoder.pll_ki), - Endpoint::make_property("encoder_offset", &motors[0].encoder.encoder_offset), - Endpoint::make_property("encoder_state", &motors[0].encoder.encoder_state), - Endpoint::make_property("motor_dir", &motors[0].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_function("set_pos_setpoint", &motors_0_set_pos_setpoint_func), - Endpoint::make_property("pos_setpoint", &motors[0].set_pos_setpoint_args.pos_setpoint), - Endpoint::make_property("vel_feed_forward", &motors[0].set_pos_setpoint_args.vel_feed_forward), - Endpoint::make_property("current_feed_forward", &motors[0].set_pos_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_vel_setpoint", &motors_0_set_vel_setpoint_func), - Endpoint::make_property("vel_setpoint", &motors[0].set_vel_setpoint_args.vel_setpoint), - Endpoint::make_property("current_feed_forward", &motors[0].set_vel_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_current_setpoint", &motors_0_set_current_setpoint_func), - Endpoint::make_property("current_setpoint", &motors[0].set_current_setpoint_args.current_setpoint), - Endpoint::close_tree(), - Endpoint::make_object("timing_log"), - Endpoint::make_property("TIMING_LOG_GENERAL", &motors[0].timing_log[TIMING_LOG_GENERAL]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_I", &motors[0].timing_log[TIMING_LOG_ADC_CB_M0_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_DC", &motors[0].timing_log[TIMING_LOG_ADC_CB_M0_DC]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_I", &motors[0].timing_log[TIMING_LOG_ADC_CB_M1_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_DC", &motors[0].timing_log[TIMING_LOG_ADC_CB_M1_DC]), - Endpoint::make_property("TIMING_LOG_MEAS_R", &motors[0].timing_log[TIMING_LOG_MEAS_R]), - Endpoint::make_property("TIMING_LOG_MEAS_L", &motors[0].timing_log[TIMING_LOG_MEAS_L]), - Endpoint::make_property("TIMING_LOG_ENC_CALIB", &motors[0].timing_log[TIMING_LOG_ENC_CALIB]), - Endpoint::make_property("TIMING_LOG_IDX_SEARCH", &motors[0].timing_log[TIMING_LOG_IDX_SEARCH]), - Endpoint::make_property("TIMING_LOG_FOC_VOLTAGE", &motors[0].timing_log[TIMING_LOG_FOC_VOLTAGE]), - Endpoint::make_property("TIMING_LOG_FOC_CURRENT", &motors[0].timing_log[TIMING_LOG_FOC_CURRENT]), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor0 - Endpoint::make_object("axis1"), - Endpoint::make_object("config"), - Endpoint::make_property("enable_control_at_start", &axis_configs[1].enable_control_at_start), - Endpoint::make_property("do_calibration_at_start", &axis_configs[1].do_calibration_at_start), - Endpoint::close_tree(), - Endpoint::close_tree(), - Endpoint::make_object("motor1"), - Endpoint::make_object("config"), - Endpoint::make_property("control_mode", reinterpret_cast*>(&motors[1].control_mode)), - Endpoint::make_property("counts_per_step", &motors[1].counts_per_step), - Endpoint::make_property("pole_pairs", &motors[1].pole_pairs), - Endpoint::make_property("pos_gain", &motors[1].pos_gain), - Endpoint::make_property("vel_gain", &motors[1].vel_gain), - Endpoint::make_property("vel_integrator_gain", &motors[1].vel_integrator_gain), - Endpoint::make_property("vel_limit", &motors[1].vel_limit), - Endpoint::make_property("calibration_current", &motors[1].calibration_current), - Endpoint::make_property("resistance_calib_max_voltage", &motors[1].resistance_calib_max_voltage), - Endpoint::make_property("phase_inductance", &motors[1].phase_inductance), - Endpoint::make_property("phase_resistance", &motors[1].phase_resistance), - Endpoint::make_property("motor_type", reinterpret_cast*>(&motors[1].motor_type)), - Endpoint::make_property("rotor_mode", reinterpret_cast*>(&motors[1].rotor_mode)), - Endpoint::close_tree(), - Endpoint::make_property("error", reinterpret_cast*>(&motors[1].error)), - Endpoint::make_property("pos_setpoint", &motors[1].pos_setpoint), - Endpoint::make_property("vel_setpoint", &motors[1].vel_setpoint), - Endpoint::make_property("vel_integrator_current", &motors[1].vel_integrator_current), - Endpoint::make_property("current_setpoint", &motors[1].current_setpoint), - Endpoint::make_property("current_meas_phB", const_cast(&motors[1].current_meas.phB)), - Endpoint::make_property("current_meas_phC", const_cast(&motors[1].current_meas.phC)), - Endpoint::make_property("DC_calib.phB", &motors[1].DC_calib.phB), - Endpoint::make_property("DC_calib.phC", &motors[1].DC_calib.phC), - Endpoint::make_property("shunt_conductance", &motors[1].shunt_conductance), - Endpoint::make_property("phase_current_rev_gain", &motors[1].phase_current_rev_gain), - Endpoint::make_property("thread_ready", &motors[1].thread_ready), - Endpoint::make_property("control_deadline", &motors[1].control_deadline), - Endpoint::make_property("last_cpu_time", &motors[1].last_cpu_time), - Endpoint::make_property("loop_counter", &motors[1].loop_counter), - Endpoint::make_object("current_control"), - Endpoint::make_object("config"), - Endpoint::make_property("current_lim", &motors[1].current_control.current_lim), - Endpoint::close_tree(), - Endpoint::make_property("p_gain", &motors[1].current_control.p_gain), - Endpoint::make_property("i_gain", &motors[1].current_control.i_gain), - Endpoint::make_property("v_current_control_integral_d", &motors[1].current_control.v_current_control_integral_d), - Endpoint::make_property("v_current_control_integral_q", &motors[1].current_control.v_current_control_integral_q), - Endpoint::make_property("Iq_setpoint", &motors[1].current_control.Iq_setpoint), - Endpoint::make_property("Iq_measured", &motors[1].current_control.Iq_measured), - Endpoint::make_property("Ibus", const_cast(&motors[1].current_control.Ibus)), - Endpoint::close_tree(), - Endpoint::make_object("gate_driver"), - Endpoint::make_property("drv_fault", reinterpret_cast*>(&motors[1].drv_fault)), - Endpoint::make_property("status_reg_1", (&motors[1].gate_driver_regs.Stat_Reg_1_Value)), - Endpoint::make_property("status_reg_2", (&motors[1].gate_driver_regs.Stat_Reg_2_Value)), - Endpoint::make_property("ctrl_reg_1", (&motors[1].gate_driver_regs.Ctrl_Reg_1_Value)), - Endpoint::make_property("ctrl_reg_2", (&motors[1].gate_driver_regs.Ctrl_Reg_2_Value)), - Endpoint::close_tree(), - Endpoint::make_object("encoder"), - Endpoint::make_object("config"), - Endpoint::make_property("use_index", &motors[1].encoder.use_index), - Endpoint::make_property("manually_calibrated", &motors[1].encoder.manually_calibrated), - Endpoint::make_property("idx_search_speed", &motors[1].encoder.idx_search_speed), - Endpoint::make_property("cpr", &motors[1].encoder.encoder_cpr), - Endpoint::make_property("offset", &motors[1].encoder.encoder_offset), - Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_property("phase", const_cast(&motors[1].encoder.phase)), - Endpoint::make_property("pll_pos", &motors[1].encoder.pll_pos), - Endpoint::make_property("pll_vel", &motors[1].encoder.pll_vel), - Endpoint::make_property("pll_kp", &motors[1].encoder.pll_kp), - Endpoint::make_property("pll_ki", &motors[1].encoder.pll_ki), - Endpoint::make_property("encoder_offset", &motors[1].encoder.encoder_offset), - Endpoint::make_property("encoder_state", &motors[1].encoder.encoder_state), - Endpoint::make_property("motor_dir", &motors[1].encoder.motor_dir), - Endpoint::close_tree(), - Endpoint::make_function("set_pos_setpoint", &motors_1_set_pos_setpoint_func), - Endpoint::make_property("pos_setpoint", &motors[1].set_pos_setpoint_args.pos_setpoint), - Endpoint::make_property("vel_feed_forward", &motors[1].set_pos_setpoint_args.vel_feed_forward), - Endpoint::make_property("current_feed_forward", &motors[1].set_pos_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_vel_setpoint", &motors_1_set_vel_setpoint_func), - Endpoint::make_property("vel_setpoint", &motors[1].set_vel_setpoint_args.vel_setpoint), - Endpoint::make_property("current_feed_forward", &motors[1].set_vel_setpoint_args.current_feed_forward), - Endpoint::close_tree(), - Endpoint::make_function("set_current_setpoint", &motors_1_set_current_setpoint_func), - Endpoint::make_property("current_setpoint", &motors[1].set_current_setpoint_args.current_setpoint), - Endpoint::close_tree(), - Endpoint::make_object("timing_log"), - Endpoint::make_property("TIMING_LOG_GENERAL", &motors[1].timing_log[TIMING_LOG_GENERAL]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_I", &motors[1].timing_log[TIMING_LOG_ADC_CB_M0_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M0_DC", &motors[1].timing_log[TIMING_LOG_ADC_CB_M0_DC]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_I", &motors[1].timing_log[TIMING_LOG_ADC_CB_M1_I]), - Endpoint::make_property("TIMING_LOG_ADC_CB_M1_DC", &motors[1].timing_log[TIMING_LOG_ADC_CB_M1_DC]), - Endpoint::make_property("TIMING_LOG_MEAS_R", &motors[1].timing_log[TIMING_LOG_MEAS_R]), - Endpoint::make_property("TIMING_LOG_MEAS_L", &motors[1].timing_log[TIMING_LOG_MEAS_L]), - Endpoint::make_property("TIMING_LOG_ENC_CALIB", &motors[1].timing_log[TIMING_LOG_ENC_CALIB]), - Endpoint::make_property("TIMING_LOG_IDX_SEARCH", &motors[1].timing_log[TIMING_LOG_IDX_SEARCH]), - Endpoint::make_property("TIMING_LOG_FOC_VOLTAGE", &motors[1].timing_log[TIMING_LOG_FOC_VOLTAGE]), - Endpoint::make_property("TIMING_LOG_FOC_CURRENT", &motors[1].timing_log[TIMING_LOG_FOC_CURRENT]), - Endpoint::close_tree(), - Endpoint::close_tree(), // motor1 - Endpoint::make_function("save_configuration", &save_configuration), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("erase_configuration", &erase_configuration), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("reboot", &NVIC_SystemReset), - // no arguments - Endpoint::close_tree(), - Endpoint::make_function("enter_dfu_mode", &enter_dfu_mode), - // no arguments - Endpoint::close_tree(), -}; -// clang-format on - -constexpr size_t NUM_ENDPOINTS = sizeof(endpoints) / sizeof(endpoints[0]); - - -#if defined(USB_PROTOCOL_NATIVE) - -class USBSender : public PacketSink { -public: - int process_packet(const uint8_t* buffer, size_t length) { - // cannot send partial packets - if (length > USB_TX_DATA_SIZE) - return -1; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit packet - uint8_t status = CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, length); - return (status == USBD_OK) ? 0 : -1; - } -} usb_sender; - -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_sender); - -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - -class USBSender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; - // wait for USB interface to become ready - if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - if (CDC_Transmit_FS( - const_cast(buffer) /* casting this const away is safe because... - well... it's not actually. Stupid STM. */, chunk) != USBD_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -} usb_sender; - -PacketToStreamConverter usb_packet_sender(usb_sender); -BidirectionalPacketBasedChannel usb_channel(endpoints, NUM_ENDPOINTS, usb_packet_sender); -StreamToPacketConverter usb_stream_sink(usb_channel); - -#endif - -#if defined(UART_PROTOCOL_NATIVE) -class UART4Sender : public StreamSink { -public: - int process_bytes(const uint8_t* buffer, size_t length) { - // Loop to ensure all bytes get sent - while (length) { - size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; - // wait for USB interface to become ready - // TODO: implement ring buffer to get a more continuous stream of data - if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) - return -1; - // transmit chunk - memcpy(tx_buf_, buffer, chunk); - if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) - return -1; - buffer += chunk; - length -= chunk; - } - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } -private: - uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; -} uart4_sender; - -PacketToStreamConverter uart4_packet_sender(uart4_sender); -BidirectionalPacketBasedChannel uart4_channel(endpoints, NUM_ENDPOINTS, uart4_packet_sender); -StreamToPacketConverter UART4_stream_sink(uart4_channel); -#endif - -/* Private function prototypes -----------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -void init_communication(void) { - switch (gpio_mode) { - case GPIO_MODE_NONE: - break; //do nothing - case GPIO_MODE_UART: { -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 - SetGPIO12toUART(); -#endif - } break; - case GPIO_MODE_STEP_DIR: { - SetGPIO12toStepDir(); - } break; - default: - //TODO: report error unexpected mode - break; - } -} - -// Thread to handle deffered processing of USB interrupt, and -// read commands out of the UART DMA circular buffer -void communication_task(void const * argument) { - (void) argument; - - -#if !defined(UART_PROTOCOL_NONE) - //DMA open loop continous circular buffer - //1ms delay periodic, chase DMA ptr around - - #define UART_RX_BUFFER_SIZE 64 - static uint8_t dma_circ_buffer[UART_RX_BUFFER_SIZE]; - - // DMA is set up to recieve in a circular buffer forever. - // We dont use interrupts to fetch the data, instead we periodically read - // data out of the circular buffer into a parse buffer, controlled by a state machine - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - uint32_t last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; -#endif - - // Re-run state-machine forever - for (;;) { -#if !defined(UART_PROTOCOL_NONE) - // Check for UART errors and restart recieve DMA transfer if required - if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { - HAL_UART_AbortReceive(&huart4); - HAL_UART_Receive_DMA(&huart4, dma_circ_buffer, sizeof(dma_circ_buffer)); - } - // Fetch the circular buffer "write pointer", where it would write next - uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; - - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(UART_PROTOCOL_NATIVE) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - UART4_stream_sink.process_bytes(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#elif defined(UART_PROTOCOL_LEGACY) - // Process bytes in one or two chunks (two in case there was a wrap) - if (new_rcv_idx < last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - UART_RX_BUFFER_SIZE - last_rcv_idx); - last_rcv_idx = 0; - } - if (new_rcv_idx > last_rcv_idx) { - legacy_parse_stream(dma_circ_buffer + last_rcv_idx, - new_rcv_idx - last_rcv_idx); - last_rcv_idx = new_rcv_idx; - } -#endif -#endif - -#if !defined(USB_PROTOCOL_NONE) - // When we reach here, we are out of immediate characters to fetch out of UART buffer - // Now we check if there is any USB processing to do: we wait for up to 1 ms, - // before going back to checking UART again. - const uint32_t usb_check_timeout = 1; // ms - osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); - if (sem_stat == osOK) { - deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); -#if defined(USB_PROTOCOL_NATIVE) - usb_channel.process_packet(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) - usb_stream_sink.process_bytes(usb_buf, usb_len); -#elif defined(USB_PROTOCOL_LEGACY) - legacy_parse_cmd(usb_buf, usb_len, USB_RX_DATA_SIZE, SERIAL_PRINTF_IS_USB); -#endif - USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet - } -#endif - } - - // If we get here, then this task is done - vTaskDelete(osThreadGetId()); -} - -// Called from CDC_Receive_FS callback function, this allows motor_parse_cmd to access the -// incoming USB data -void set_cmd_buffer(uint8_t *buf, uint32_t len) { - usb_buf = buf; - usb_len = len; -} - -void usb_update_thread() { - for (;;) { - // Wait for signalling from USB interrupt (OTG_FS_IRQHandler) - osStatus semaphore_status = osSemaphoreWait(sem_usb_irq, osWaitForever); - if (semaphore_status == osOK) { - // We have a new incoming USB transmission: handle it - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - // Let the irq (OTG_FS_IRQHandler) fire again. - HAL_NVIC_EnableIRQ(OTG_FS_IRQn); - } - } - - vTaskDelete(osThreadGetId()); -} diff --git a/Firmware/MotorControl/config.h b/Firmware/MotorControl/config.h deleted file mode 100644 index 69191451..00000000 --- a/Firmware/MotorControl/config.h +++ /dev/null @@ -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 */ diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp new file mode 100644 index 00000000..7cc55894 --- /dev/null +++ b/Firmware/MotorControl/controller.cpp @@ -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(pos_estimate), axis_->encoder_.config_.cpr)]; + } + + float v_err = vel_des - vel_estimate; + if (config_.control_mode >= CTRL_MODE_VELOCITY_CONTROL) { + Iq += config_.vel_gain * v_err; + } + + // Velocity integral action before limiting + Iq += vel_integrator_current_; + + // Current limiting + float Ilim = std::min(axis_->motor_.config_.current_lim, axis_->motor_.current_control_.max_allowed_current); + bool limited = false; + if (Iq > Ilim) { + limited = true; + Iq = Ilim; + } + if (Iq < -Ilim) { + limited = true; + Iq = -Ilim; + } + + // Velocity integrator (behaviour dependent on limiting) + if (config_.control_mode < CTRL_MODE_VELOCITY_CONTROL) { + // reset integral if not in use + vel_integrator_current_ = 0.0f; + } else { + if (limited) { + // TODO make decayfactor configurable + vel_integrator_current_ *= 0.99f; + } else { + vel_integrator_current_ += (config_.vel_integrator_gain * current_meas_period) * v_err; + } + } + + if (current_setpoint_output) *current_setpoint_output = Iq; + return true; +} diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp new file mode 100644 index 00000000..c76161ea --- /dev/null +++ b/Firmware/MotorControl/controller.hpp @@ -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)] + 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; + float vel_integrator_current_ = 0.0f; // [A] + float current_setpoint_ = 0.0f; // [A] + + // Communication protocol definitions + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_property("pos_setpoint", &pos_setpoint_), + make_protocol_property("vel_setpoint", &vel_setpoint_), + make_protocol_property("vel_integrator_current", &vel_integrator_current_), + make_protocol_property("current_setpoint", ¤t_setpoint_), + make_protocol_object("config", + make_protocol_property("control_mode", &config_.control_mode), + make_protocol_property("pos_gain", &config_.pos_gain), + make_protocol_property("vel_gain", &config_.vel_gain), + make_protocol_property("vel_integrator_gain", &config_.vel_integrator_gain), + make_protocol_property("vel_limit", &config_.vel_limit) + ), + make_protocol_function("set_pos_setpoint", *this, &Controller::set_pos_setpoint, + "pos_setpoint", + "vel_feed_forward", + "current_feed_forward"), + make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint, + "vel_setpoint", + "current_feed_forward"), + make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint, + "current_setpoint"), + make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) + ); + } +}; + +#endif // __CONTROLLER_HPP diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp new file mode 100644 index 00000000..6267e152 --- /dev/null +++ b/Firmware/MotorControl/encoder.cpp @@ -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(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; +} diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp new file mode 100644 index 00000000..9a59654e --- /dev/null +++ b/Firmware/MotorControl/encoder.hpp @@ -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(&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 diff --git a/Firmware/MotorControl/example.json b/Firmware/MotorControl/example.json new file mode 100644 index 00000000..7d3c4dc6 --- /dev/null +++ b/Firmware/MotorControl/example.json @@ -0,0 +1,37 @@ +[ + { + "name": "", + "id": 0, + "type": "json" + }, + { + "name": "subscriptions", + "id": 1, + "type": "int32[]" + }, + { + "name": "motor0", + "id": 2, + "type": "tree", + "content": [ + { + "name": "pos_setpoint", + "id": 3, + "type": "float", + "access": "rw" + }, + { + "name": "pos_gain", + "id": 4, + "type": "float", + "access": "rw" + }, + { + "name": "vel_setpoint", + "id": 5, + "type": "float", + "access": "rw" + } + ] + } +] \ No newline at end of file diff --git a/Firmware/MotorControl/legacy_commands.c b/Firmware/MotorControl/legacy_commands.c deleted file mode 100644 index 0de2acdf..00000000 --- a/Firmware/MotorControl/legacy_commands.c +++ /dev/null @@ -1,291 +0,0 @@ -/* Includes ------------------------------------------------------------------*/ -#include "legacy_commands.h" -#include - -/* 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, ¤t_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, ¤t_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, ¤t_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 -#include // Sets up the correct chip specifc defines required by arm_math -#define ARM_MATH_CM4 -#include - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -/* Private defines -----------------------------------------------------------*/ - -// #define DEBUG_PRINT - -/* Private macros ------------------------------------------------------------*/ -/* Private typedef -----------------------------------------------------------*/ -/* Global constant data ------------------------------------------------------*/ -/* Global variables ----------------------------------------------------------*/ -// This value is updated by the DC-bus reading ADC. -// Arbitrary non-zero inital value to avoid division by zero if ADC reading is late -float vbus_voltage = 12.0f; - -#if HW_VERSION_MAJOR == 3 -#if HW_VERSION_MINOR <= 3 -#define SHUNT_RESISTANCE (675e-6f) -#else -#define SHUNT_RESISTANCE (500e-6f) -#endif -#endif - -// TODO: Migrate to C++, clearly we are actually doing object oriented code here... -// TODO: For nice encapsulation, consider not having the motor objects public - -// NOTE: for gimbal motors, all units of A are instead V. -// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] -// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. -Motor_t motors[] = { - { - // M0 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pole_pairs = 7, // This value is correct for N5065 motors and Turnigy SK3 series. - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - // .vel_setpoint = 800.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] - // .vel_gain = 15.0f / 200.0f, // [A/(rad/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - // .vel_integrator_gain = 0.0f, // [A/(rad/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - .dc_bus_undervoltage_trip_level = 8.0f, // [V] - .dc_bus_overvoltage_trip_level = VBUS_OVERVOLTAGE_LEVEL, // [V] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim1, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = TIM_1_8_PERIOD_CLOCKS, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M0_nCS_GPIO_Port, - .nCSgpioNumber = M0_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .motor_type = MOTOR_TYPE_HIGH_CURRENT, - // .motor_type = MOTOR_TYPE_GIMBAL, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .max_allowed_current = 0.0f, - }, - // .rotor_mode = ROTOR_MODE_SENSORLESS, - // .rotor_mode = ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim3, - .use_index = false, - .index_found = false, - .manually_calibrated = false, - .idx_search_speed = 10.0f, // [rad/s electrical] - .encoder_cpr = (2048 * 4), // Default resolution of CUI-AMT102 encoder, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 1, // 1 or -1 - .encoder_calib_range = 0.02, - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .loop_counter = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - .drv_fault = DRV8301_FaultType_NoFault, - }, - { // M1 - .control_mode = CTRL_MODE_POSITION_CONTROL, //see: Motor_control_mode_t - .enable_step_dir = false, //auto enabled after calibration - .counts_per_step = 2.0f, - .error = ERROR_NO_ERROR, - .pole_pairs = 7, // This value is correct for N5065 motors and Turnigy SK3 series. - .pos_setpoint = 0.0f, - .pos_gain = 20.0f, // [(counts/s) / counts] - .vel_setpoint = 0.0f, - .vel_gain = 5.0f / 10000.0f, // [A/(counts/s)] - .vel_integrator_gain = 10.0f / 10000.0f, // [A/(counts/s * s)] - .vel_integrator_current = 0.0f, // [A] - .vel_limit = 20000.0f, // [counts/s] - .current_setpoint = 0.0f, // [A] - .calibration_current = 10.0f, // [A] - .resistance_calib_max_voltage = 1.0f, // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor. - .dc_bus_undervoltage_trip_level = 8.0f, // [V] - .dc_bus_overvoltage_trip_level = VBUS_OVERVOLTAGE_LEVEL, // [V] - .phase_inductance = 0.0f, // to be set by measure_phase_inductance - .phase_resistance = 0.0f, // to be set by measure_phase_resistance - .motor_thread = 0, - .thread_ready = false, - // .enable_control = true, - // .do_calibration = true, - // .calibration_ok = false, - .motor_timer = &htim8, - .next_timings = {TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2, TIM_1_8_PERIOD_CLOCKS / 2}, - .control_deadline = (3 * TIM_1_8_PERIOD_CLOCKS) / 2, - .last_cpu_time = 0, - .current_meas = {0.0f, 0.0f}, - .DC_calib = {0.0f, 0.0f}, - .gate_driver = { - .spiHandle = &hspi3, - // Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M1_nCS_GPIO_Port, - .nCSgpioNumber = M1_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false, - }, - // .gate_driver_regs Init by DRV8301_setup - .motor_type = MOTOR_TYPE_HIGH_CURRENT, - .shunt_conductance = 1.0f / SHUNT_RESISTANCE, //[S] - .phase_current_rev_gain = 0.0f, // to be set by DRV8301_setup - .current_control = { - // Read out max_allowed_current to see max supported value for current_lim. - // You can change DRV8301_ShuntAmpGain to get a different range. - // .current_lim = 75.0f, //[A] - .current_lim = 10.0f, //[A] - .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement - .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement - .v_current_control_integral_d = 0.0f, - .v_current_control_integral_q = 0.0f, - .Ibus = 0.0f, - .final_v_alpha = 0.0f, - .final_v_beta = 0.0f, - .Iq_setpoint = 0.0f, - .Iq_measured = 0.0f, - .max_allowed_current = 0.0f, - }, - .rotor_mode = ROTOR_MODE_ENCODER, - .encoder = { - .encoder_timer = &htim4, - .use_index = false, - .index_found = false, - .manually_calibrated = false, - .idx_search_speed = 10.0f, // [rad/s electrical] - .encoder_cpr = (2048 * 4), // Default resolution of CUI-AMT102 encoder, - .encoder_offset = 0, - .encoder_state = 0, - .motor_dir = 1, // 1 or -1 - .encoder_calib_range = 0.02, - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - }, - .sensorless = { - .phase = 0.0f, // [rad] - .pll_pos = 0.0f, // [rad] - .pll_vel = 0.0f, // [rad/s] - .pll_kp = 0.0f, // [rad/s / rad] - .pll_ki = 0.0f, // [(rad/s^2) / rad] - .observer_gain = 1000.0f, // [rad/s] - .flux_state = {0.0f, 0.0f}, // [Vs] - .V_alpha_beta_memory = {0.0f, 0.0f}, // [V] - .pm_flux_linkage = 1.58e-3f, // [V / (rad/s)] { 5.51328895422 / ( * ) } - .estimator_good = false, - .spin_up_current = 10.0f, // [A] - .spin_up_acceleration = 400.0f, // [rad/s^2] - .spin_up_target_vel = 400.0f, // [rad/s] - }, - .loop_counter = 0, - .timing_log = {0}, - .anticogging = { - .index = 0, - .cogging_map = NULL, - .use_anticogging = false, - .calib_anticogging = false, - .calib_pos_threshold = 1.0f, - .calib_vel_threshold = 1.0f, - }, - .drv_fault = DRV8301_FaultType_NoFault, - } -}; -const size_t num_motors = sizeof(motors) / sizeof(motors[0]); - -float brake_resistance = 0.47f; // [ohm] - -/* Private constant data -----------------------------------------------------*/ -static const float one_by_sqrt3 = 0.57735026919f; -static const float sqrt3_by_2 = 0.86602540378f; -static const float current_meas_period = CURRENT_MEAS_PERIOD; -static const int current_meas_hz = CURRENT_MEAS_HZ; - -/* Private variables ---------------------------------------------------------*/ -/* Function implementations --------------------------------------------------*/ - -//-------------------------------- -// Command Handling -//-------------------------------- - -void set_pos_setpoint(Motor_t* motor, float pos_setpoint, float vel_feed_forward, float current_feed_forward) { - motor->pos_setpoint = pos_setpoint; - motor->vel_setpoint = vel_feed_forward; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_POSITION_CONTROL; -#ifdef DEBUG_PRINT - printf("POSITION_CONTROL %6.0f %3.3f %3.3f\n", motor->pos_setpoint, motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_vel_setpoint(Motor_t* motor, float vel_setpoint, float current_feed_forward) { - motor->vel_setpoint = vel_setpoint; - motor->current_setpoint = current_feed_forward; - motor->control_mode = CTRL_MODE_VELOCITY_CONTROL; -#ifdef DEBUG_PRINT - printf("VELOCITY_CONTROL %3.3f %3.3f\n", motor->vel_setpoint, motor->current_setpoint); -#endif -} - -void set_current_setpoint(Motor_t* motor, float current_setpoint) { - motor->current_setpoint = current_setpoint; - motor->control_mode = CTRL_MODE_CURRENT_CONTROL; -#ifdef DEBUG_PRINT - printf("CURRENT_CONTROL %3.3f\n", motor->current_setpoint); -#endif -} - -//-------------------------------- -// Utility -//-------------------------------- - -uint16_t check_timing(Motor_t* motor, TimingLog_t log_idx) { - TIM_HandleTypeDef* htim = motor->motor_timer; - uint16_t timing = htim->Instance->CNT; - bool down = htim->Instance->CR1 & TIM_CR1_DIR; - if (down) { - uint16_t delta = TIM_1_8_PERIOD_CLOCKS - timing; - timing = TIM_1_8_PERIOD_CLOCKS + delta; - } - - if (log_idx < TIMING_LOG_SIZE) { - motor->timing_log[log_idx] = timing; - } - - return timing; -} - -void global_fault(int error) { - // Disable motors NOW! - for (int i = 0; i < num_motors; ++i) { - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(motors[i].motor_timer); - } - // Set fault codes, etc. - for (int i = 0; i < num_motors; ++i) { - motors[i].error = error; - *(motors[i].axis_legacy.enable_control) = false; - } - // disable brake resistor - set_brake_current(0.0f); -} - -float phase_current_from_adcval(Motor_t* motor, uint32_t ADCValue) { - int adcval_bal = (int)ADCValue - (1 << 11); - float amp_out_volt = (3.3f / (float)(1 << 12)) * (float)adcval_bal; - float shunt_volt = amp_out_volt * motor->phase_current_rev_gain; - float current = shunt_volt * motor->shunt_conductance; - return current; -} - -//-------------------------------- -// Initalisation -//-------------------------------- - -// Initalises the low level motor control and then starts the motor control threads -void init_motor_control() { - // Init gate drivers - DRV8301_setup(&motors[0]); - DRV8301_setup(&motors[1]); - - // Start PWM and enable adc interrupts/callbacks - start_adc_pwm(); - - // Start Encoders - HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL); - HAL_TIM_Encoder_Start(&htim4, TIM_CHANNEL_ALL); - //TODO: Enable index on only one channel - if (motors[0].encoder.use_index || motors[1].encoder.use_index) { - SetupENCIndexGPIO(); - } - - // Wait for current sense calibration to converge - // TODO make timing a function of calibration filter tau - osDelay(1500); -} - -// Set up the gate drivers -void DRV8301_setup(Motor_t* motor) { - DRV8301_Obj* gate_driver = &motor->gate_driver; - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - - DRV8301_enable(gate_driver); - DRV8301_setupSpi(gate_driver, local_regs); - - // TODO we can use reporting only if we actually wire up the nOCTW pin - local_regs->Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - // Overcurrent set to approximately 150A at 100degC. This may need tweaking. - local_regs->Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - // 20V/V on 500uOhm gives a range of +/- 150A - // 40V/V on 500uOhm gives a range of +/- 75A - // 20V/V on 666uOhm gives a range of +/- 110A - // 40V/V on 666uOhm gives a range of +/- 55A - local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - // local_regs->Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_20VpV; - - switch (local_regs->Ctrl_Reg_2.GAIN) { - case DRV8301_ShuntAmpGain_10VpV: - motor->phase_current_rev_gain = 1.0f / 10.0f; - break; - case DRV8301_ShuntAmpGain_20VpV: - motor->phase_current_rev_gain = 1.0f / 20.0f; - break; - case DRV8301_ShuntAmpGain_40VpV: - motor->phase_current_rev_gain = 1.0f / 40.0f; - break; - case DRV8301_ShuntAmpGain_80VpV: - motor->phase_current_rev_gain = 1.0f / 80.0f; - break; - } - - float margin = 0.90f; - float max_input = margin * 0.3f * motor->shunt_conductance; - float max_swing = margin * 1.6f * motor->shunt_conductance * motor->phase_current_rev_gain; - motor->current_control.max_allowed_current = MACRO_MIN(max_input, max_swing); - - local_regs->SndCmd = true; - DRV8301_writeData(gate_driver, local_regs); - local_regs->RcvCmd = true; - DRV8301_readData(gate_driver, local_regs); -} - -void start_adc_pwm() { - // Enable ADC and interrupts - __HAL_ADC_ENABLE(&hadc1); - __HAL_ADC_ENABLE(&hadc2); - __HAL_ADC_ENABLE(&hadc3); - // Warp field stabilize. - osDelay(2); - __HAL_ADC_ENABLE_IT(&hadc1, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_EOC); - __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_EOC); - - // Ensure that debug halting of the core doesn't leave the motor PWM running - __HAL_DBGMCU_FREEZE_TIM1(); - __HAL_DBGMCU_FREEZE_TIM8(); - - start_pwm(&htim1); - start_pwm(&htim8); - // TODO: explain why this offset - sync_timers(&htim1, &htim8, TIM_CLOCKSOURCE_ITR0, TIM_1_8_PERIOD_CLOCKS / 2 - 1 * 128); - - // Motor output starts in the disabled state - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim1); - __HAL_TIM_MOE_DISABLE_UNCONDITIONALLY(&htim8); - - // Start brake resistor PWM in floating output configuration - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); - HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); -} - -void start_pwm(TIM_HandleTypeDef* htim) { - // Init PWM - int half_load = TIM_1_8_PERIOD_CLOCKS / 2; - htim->Instance->CCR1 = half_load; - htim->Instance->CCR2 = half_load; - htim->Instance->CCR3 = half_load; - - // This hardware obfustication layer really is getting on my nerves - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_1); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_1); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_2); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_2); - HAL_TIM_PWM_Start(htim, TIM_CHANNEL_3); - HAL_TIMEx_PWMN_Start(htim, TIM_CHANNEL_3); - - htim->Instance->CCR4 = 1; - HAL_TIM_PWM_Start_IT(htim, TIM_CHANNEL_4); -} - -void sync_timers(TIM_HandleTypeDef* htim_a, TIM_HandleTypeDef* htim_b, - uint16_t TIM_CLOCKSOURCE_ITRx, uint16_t count_offset) { - // Store intial timer configs - uint16_t MOE_store_a = htim_a->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t MOE_store_b = htim_b->Instance->BDTR & (TIM_BDTR_MOE); - uint16_t CR2_store = htim_a->Instance->CR2; - uint16_t SMCR_store = htim_b->Instance->SMCR; - // Turn off output - htim_a->Instance->BDTR &= ~(TIM_BDTR_MOE); - htim_b->Instance->BDTR &= ~(TIM_BDTR_MOE); - // Disable both timer counters - htim_a->Instance->CR1 &= ~TIM_CR1_CEN; - htim_b->Instance->CR1 &= ~TIM_CR1_CEN; - // Set first timer to send TRGO on counter enable - htim_a->Instance->CR2 &= ~TIM_CR2_MMS; - htim_a->Instance->CR2 |= TIM_TRGO_ENABLE; - // Set Trigger Source of second timer to the TRGO of the first timer - htim_b->Instance->SMCR &= ~TIM_SMCR_TS; - htim_b->Instance->SMCR |= TIM_CLOCKSOURCE_ITRx; - // Set 2nd timer to start on trigger - htim_b->Instance->SMCR &= ~TIM_SMCR_SMS; - htim_b->Instance->SMCR |= TIM_SLAVEMODE_TRIGGER; - // Dir bit is read only in center aligned mode, so we clear the mode for now - uint16_t CMS_store_a = htim_a->Instance->CR1 & TIM_CR1_CMS; - uint16_t CMS_store_b = htim_b->Instance->CR1 & TIM_CR1_CMS; - htim_a->Instance->CR1 &= ~TIM_CR1_CMS; - htim_b->Instance->CR1 &= ~TIM_CR1_CMS; - // Set both timers to up-counting state - htim_a->Instance->CR1 &= ~TIM_CR1_DIR; - htim_b->Instance->CR1 &= ~TIM_CR1_DIR; - // Restore center aligned mode - htim_a->Instance->CR1 |= CMS_store_a; - htim_b->Instance->CR1 |= CMS_store_b; - // set counter offset - htim_a->Instance->CNT = count_offset; - htim_b->Instance->CNT = 0; - // Start Timer a - htim_a->Instance->CR1 |= (TIM_CR1_CEN); - // Restore timer configs - htim_a->Instance->CR2 = CR2_store; - htim_b->Instance->SMCR = SMCR_store; - // restore output - htim_a->Instance->BDTR |= MOE_store_a; - htim_b->Instance->BDTR |= MOE_store_b; -} - -//-------------------------------- -// IRQ Callbacks -//-------------------------------- - -// step/direction interface -void step_cb(uint16_t GPIO_Pin) { - GPIO_PinState dir_pin; - float dir; - switch (GPIO_Pin) { - case GPIO_1_Pin: - //M0 stepped - if (motors[0].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_2_GPIO_Port, GPIO_2_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[0].pos_setpoint += dir * motors[0].counts_per_step; - } - break; - case GPIO_3_Pin: - //M1 stepped - if (motors[1].enable_step_dir) { - dir_pin = HAL_GPIO_ReadPin(GPIO_4_GPIO_Port, GPIO_4_Pin); - dir = (dir_pin == GPIO_PIN_SET) ? 1.0f : -1.0f; - motors[1].pos_setpoint += dir * motors[1].counts_per_step; - } - break; - default: - global_fault(ERROR_UNEXPECTED_STEP_SRC); - break; - } -} - -// Triggered when an encoder passes over the "Index" pin -// TODO: only arm index edge interrupt when we know encoder has powered up -void enc_index_cb(uint16_t GPIO_Pin, uint8_t motor_index) { - Motor_t* motor = &motors[motor_index]; - if (!motor->encoder.index_found) { - setEncoderCount(motor, 0); - motor->encoder.index_found = true; - } - //TODO: Hardcoded EXTI line not portable. Get mapping out of Cubemx by setting EXTI default - if(GPIO_Pin == M0_ENC_Z_Pin){ - HAL_NVIC_DisableIRQ(EXTI15_10_IRQn); - } else { - HAL_NVIC_DisableIRQ(EXTI3_IRQn); - } -} - -void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); - // Only one conversion in sequence, so only rank1 - uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - vbus_voltage = ADCValue * voltage_scale; -} - -// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. -// TODO: Document how the phasing is done, link to timing diagram -void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { -#define calib_tau 0.2f //@TOTO make more easily configurable - static const float calib_filter_k = CURRENT_MEAS_PERIOD / calib_tau; - - // Ensure ADCs are expected ones to simplify the logic below - if (!(hadc == &hadc2 || hadc == &hadc3)) { - global_fault(ERROR_ADC_FAILED); - return; - }; - - // Motor 0 is on Timer 1, which triggers ADC 2 and 3 on an injected conversion - // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion - // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current - // If we are counting down, we just sampled in SVM vector 7, with zero current - Motor_t* motor = injected ? &motors[0] : &motors[1]; - bool counting_down = motor->motor_timer->Instance->CR1 & TIM_CR1_DIR; - - bool current_meas_not_DC_CAL; - if (motor == &motors[1] && counting_down) { - // We are measuring M1 DC_CAL here - current_meas_not_DC_CAL = false; - // Load next timings for M0 (only once is sufficient) - if (hadc == &hadc2) { - motors[0].motor_timer->Instance->CCR1 = motors[0].next_timings[0]; - motors[0].motor_timer->Instance->CCR2 = motors[0].next_timings[1]; - motors[0].motor_timer->Instance->CCR3 = motors[0].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor, TIMING_LOG_ADC_CB_M1_DC); - - } else if (motor == &motors[0] && !counting_down) { - // We are measuring M0 current here - current_meas_not_DC_CAL = true; - // Load next timings for M1 (only once is sufficient) - if (hadc == &hadc2) { - motors[1].motor_timer->Instance->CCR1 = motors[1].next_timings[0]; - motors[1].motor_timer->Instance->CCR2 = motors[1].next_timings[1]; - motors[1].motor_timer->Instance->CCR3 = motors[1].next_timings[2]; - } - // Check the timing of the sequencing - check_timing(motor, TIMING_LOG_ADC_CB_M0_I); - - } else if (motor == &motors[1] && !counting_down) { - // We are measuring M1 current here - current_meas_not_DC_CAL = true; - // Check the timing of the sequencing - check_timing(motor, TIMING_LOG_ADC_CB_M1_I); - - } else if (motor == &motors[0] && counting_down) { - // We are measuring M0 DC_CAL here - current_meas_not_DC_CAL = false; - // Check the timing of the sequencing - check_timing(motor, TIMING_LOG_ADC_CB_M0_DC); - - } else { - global_fault(ERROR_PWM_SRC_FAIL); - return; - } - - uint32_t ADCValue; - if (injected) { - ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); - } else { - ADCValue = HAL_ADC_GetValue(hadc); - } - float current = phase_current_from_adcval(motor, ADCValue); - - if (current_meas_not_DC_CAL) { - // ADC2 and ADC3 record the phB and phC currents concurrently, - // and their interrupts should arrive on the same clock cycle. - // We dispatch the callbacks in order, so ADC2 will always be processed before ADC3. - // Therefore we store the value from ADC2 and signal the thread that the - // measurement is ready when we receive the ADC3 measurement - - // return or continue - if (hadc == &hadc2) { - motor->current_meas.phB = current - motor->DC_calib.phB; - return; - } else { - motor->current_meas.phC = current - motor->DC_calib.phC; - } - // Trigger motor thread - if (motor->thread_ready) - osSignalSet(motor->motor_thread, M_SIGNAL_PH_CURRENT_MEAS); - } else { - // DC_CAL measurement - if (hadc == &hadc2) { - motor->DC_calib.phB += (current - motor->DC_calib.phB) * calib_filter_k; - } else { - motor->DC_calib.phC += (current - motor->DC_calib.phC) * calib_filter_k; - } - } -} - -//-------------------------------- -// Measurement and calibration -//-------------------------------- - -// TODO check Ibeta balance to verify good motor connection -bool measure_phase_resistance(Motor_t* motor, float test_current, float max_voltage) { - static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = 3.0f / CURRENT_MEAS_PERIOD; // Test runs for 3s - float test_voltage = 0.0f; - for (int i = 0; i < num_test_cycles; ++i) { - osEvent evt = osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT); - if (evt.status != osEventSignal) { - motor->error = ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - - float Ialpha = -(motor->current_meas.phB + motor->current_meas.phC); - test_voltage += (kI * current_meas_period) * (test_current - Ialpha); - if (test_voltage > max_voltage) test_voltage = max_voltage; - if (test_voltage < -max_voltage) test_voltage = -max_voltage; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltage, 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor, TIMING_LOG_MEAS_R); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_RESISTANCE_TIMING; - return false; - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float R = test_voltage / test_current; - motor->phase_resistance = R; - if (fabs(test_voltage) == fabs(max_voltage) || R < 0.01f || R > 1.0f) { - motor->error = ERROR_PHASE_RESISTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -bool measure_phase_inductance(Motor_t* motor, float voltage_low, float voltage_high) { - float test_voltages[2] = {voltage_low, voltage_high}; - float Ialphas[2] = {0.0f}; - static const int num_cycles = 5000; - - for (int t = 0; t < num_cycles; ++t) { - for (int i = 0; i < 2; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - - Ialphas[i] += -motor->current_meas.phB - motor->current_meas.phC; - - // Test voltage along phase A - queue_voltage_timings(motor, test_voltages[i], 0.0f); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor, TIMING_LOG_MEAS_L); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_PHASE_INDUCTANCE_TIMING; - return false; - } - } - } - - // De-energize motor - queue_voltage_timings(motor, 0.0f, 0.0f); - - float v_L = 0.5f * (voltage_high - voltage_low); - // Note: A more correct formula would also take into account that there is a finite timestep. - // However, the discretisation in the current control loop inverts the same discrepancy - float dI_by_dt = (Ialphas[1] - Ialphas[0]) / (current_meas_period * (float)num_cycles); - float L = v_L / dI_by_dt; - - motor->phase_inductance = L; - // TODO arbitrary values set for now - if (L < 1e-6f || L > 500e-6f) { - motor->error = ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE; - return false; - } - return true; -} - -// TODO: Do the scan with current, not voltage! -// TODO: add check_timing -bool calib_enc_offset(Motor_t* motor, float voltage_magnitude) { - static const float start_lock_duration = 1.0f; - static const int num_steps = 1024*2; - static const float dt_step = 1.0f / 500.0f; - static const float scan_range = 16.0f * M_PI; - const float step_size = scan_range / (float)num_steps; // TODO handle const expressions better (maybe switch to C++ ?) - - // go to motor zero phase for start_lock_duration to get ready to scan - for (int i = 0; i < start_lock_duration * current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - queue_voltage_timings(motor, voltage_magnitude, 0.0f); - } - - int32_t init_enc_val = (int16_t)motor->encoder.encoder_timer->Instance->CNT; - int32_t encvaluesum = 0; - - // scan forwards - for (float ph = -scan_range / 2.0f; ph < scan_range / 2.0f; ph += step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = motor->pole_pairs * 2 * M_PI * (1.0f / (float)(motor->encoder.encoder_cpr)); - float expected_encoder_delta = scan_range / elec_rad_per_enc; - float actual_encoder_delta_abs = fabsf((int16_t)motor->encoder.encoder_timer->Instance->CNT-init_enc_val); - if(fabsf(actual_encoder_delta_abs - expected_encoder_delta)/expected_encoder_delta > motor->encoder.encoder_calib_range) - { - motor->error = ERROR_ENCODER_CPR_OUT_OF_RANGE; - return false; - } - // check direction - if ((int16_t)motor->encoder.encoder_timer->Instance->CNT > init_enc_val + 8) { - // motor same dir as encoder - motor->encoder.motor_dir = 1; - } else if ((int16_t)motor->encoder.encoder_timer->Instance->CNT < init_enc_val - 8) { - // motor opposite dir as encoder - motor->encoder.motor_dir = -1; - } else { - // Encoder response error - motor->error = ERROR_ENCODER_RESPONSE; - return false; - } - // scan backwards - for (float ph = scan_range / 2.0f; ph > -scan_range / 2.0f; ph -= step_size) { - for (int i = 0; i < dt_step * (float)current_meas_hz; ++i) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_ENCODER_MEASUREMENT_TIMEOUT; - return false; - } - if (!do_checks(motor)) - return false; - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - } - encvaluesum += (int16_t)motor->encoder.encoder_timer->Instance->CNT; - } - - int offset = encvaluesum / (num_steps * 2); - motor->encoder.encoder_offset = offset; - return true; -} - -bool motor_calibration(Motor_t* motor) { - motor->error = ERROR_NO_ERROR; - - float R_calib_max_voltage = motor->resistance_calib_max_voltage; - float enc_calibration_voltage = 0.0f; - if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if (!measure_phase_resistance(motor, motor->calibration_current, R_calib_max_voltage)) - return false; - enc_calibration_voltage = motor->calibration_current * motor->phase_resistance; - - if (!measure_phase_inductance(motor, -R_calib_max_voltage, R_calib_max_voltage)) - return false; - } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - enc_calibration_voltage = motor->calibration_current; - } else { - return false; - } - - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - if (motor->encoder.use_index && !motor->encoder.index_found) - if (!scan_for_enc_idx(motor, - (float)(motor->encoder.motor_dir) * motor->encoder.idx_search_speed, - enc_calibration_voltage)) - return false; - if (!motor->encoder.manually_calibrated) - if (!calib_enc_offset(motor, enc_calibration_voltage)) - return false; - } - - // Calculate current control gains - float current_control_bandwidth = 1000.0f; // [rad/s] - motor->current_control.p_gain = current_control_bandwidth * motor->phase_inductance; - float plant_pole = motor->phase_resistance / motor->phase_inductance; - motor->current_control.i_gain = plant_pole * motor->current_control.p_gain; - - // Calculate encoder pll gains - float encoder_pll_bandwidth = 1000.0f; // [rad/s] - motor->encoder.pll_kp = 2.0f * encoder_pll_bandwidth; - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * motor->encoder.pll_kp < 1.0f)) { - motor->error = ERROR_CALIBRATION_TIMING; - return false; - } - // Critically damped - motor->encoder.pll_ki = 0.25f * (motor->encoder.pll_kp * motor->encoder.pll_kp); - - // sensorless pll same as encoder (for now) - motor->sensorless.pll_kp = motor->encoder.pll_kp; - motor->sensorless.pll_ki = motor->encoder.pll_ki; - - return true; -} - -/* - * This anti-cogging implementation iterates through each encoder position, - * waits for zero velocity & position error, - * then samples the current required to maintain that position. - * - * This holding current is added as a feedforward term in the control loop. - */ -bool anti_cogging_calibration(Motor_t* motor) { - if (motor->anticogging.calib_anticogging && motor->anticogging.cogging_map != NULL) { - float pos_err = motor->anticogging.index - motor->encoder.pll_pos; - if (fabsf(pos_err) <= motor->anticogging.calib_pos_threshold && - fabsf(motor->encoder.pll_vel) < motor->anticogging.calib_vel_threshold) { - motor->anticogging.cogging_map[motor->anticogging.index++] = motor->vel_integrator_current; - } - if (motor->anticogging.index < motor->encoder.encoder_cpr) { - set_pos_setpoint(motor, motor->anticogging.index, 0.0f, 0.0f); - return false; - } else { - motor->anticogging.index = 0; - set_pos_setpoint(motor, 0.0f, 0.0f, 0.0f); // Send the motor home - motor->anticogging.use_anticogging = true; // We're good to go, enable anti-cogging - motor->anticogging.calib_anticogging = false; - return true; - } - } - return false; -} - -//-------------------------------- -// Test functions -//-------------------------------- - -bool scan_for_enc_idx(Motor_t* motor, float omega, float voltage_magnitude) { - for (;;) { - for (float ph = 0.0f; ph < 2.0f * M_PI; ph += omega * current_meas_period) { - osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, osWaitForever); - if (!do_checks(motor)) - return false; - - if (motor->encoder.index_found) - return true; - - float v_alpha = voltage_magnitude * arm_cos_f32(ph); - float v_beta = voltage_magnitude * arm_sin_f32(ph); - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor, TIMING_LOG_IDX_SEARCH); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_SCAN_MOTOR_TIMING; - return false; - } - } - } -} - -//-------------------------------- -// Main motor control -//-------------------------------- - -void update_rotor(Motor_t* motor) { - switch (motor->rotor_mode) { - case ROTOR_MODE_ENCODER: - case ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS: { - //for convenience - Encoder_t* encoder = &motor->encoder; - - // update internal encoder state - int16_t delta_enc = (int16_t)encoder->encoder_timer->Instance->CNT - (int16_t)encoder->encoder_state; - encoder->encoder_state += (int32_t)delta_enc; - - // compute electrical phase - int corrected_enc = encoder->encoder_state % motor->encoder.encoder_cpr; - corrected_enc -= encoder->encoder_offset; - corrected_enc *= encoder->motor_dir; - //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = motor->pole_pairs * 2 * M_PI * (1.0f / (float)(motor->encoder.encoder_cpr)); - float ph = elec_rad_per_enc * (float)corrected_enc; - // ph = fmodf(ph, 2*M_PI); - encoder->phase = wrap_pm_pi(ph); - - // run pll (for now pll is in units of encoder counts) - // TODO pll_pos runs out of precision very quickly here! Perhaps decompose into integer and fractional part? - // Predict current pos - encoder->pll_pos += current_meas_period * encoder->pll_vel; - // discrete phase detector - float delta_pos = (float)(encoder->encoder_state - (int32_t)floorf(encoder->pll_pos)); - // pll feedback - encoder->pll_pos += current_meas_period * encoder->pll_kp * delta_pos; - encoder->pll_vel += current_meas_period * encoder->pll_ki * delta_pos; - } - // Drop through to sensorless if also testing - if (motor->rotor_mode != ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - break; - case ROTOR_MODE_SENSORLESS: { - // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer - // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf - // In particular, equation 8 (and by extension eqn 4 and 6). - - // The V_alpha_beta applied immedietly prior to the current measurement associated with this cycle - // is the one computed two cycles ago. To get the correct measurement, it was stored twice: - // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. - - //for convenience - Sensorless_t* sensorless = &motor->sensorless; - - // Clarke transform - float I_alpha_beta[2] = { - -motor->current_meas.phB - motor->current_meas.phC, - one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC)}; - - // alpha-beta vector operations - float eta[2]; - for (int i = 0; i <= 1; ++i) { - // y is the total flux-driving voltage (see paper eqn 4) - float y = -motor->phase_resistance * I_alpha_beta[i] + sensorless->V_alpha_beta_memory[i]; - // flux dynamics (prediction) - float x_dot = y; - // integrate prediction to current timestep - sensorless->flux_state[i] += x_dot * current_meas_period; - - // eta is the estimated permanent magnet flux (see paper eqn 6) - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = sensorless->pm_flux_linkage * sensorless->pm_flux_linkage; - float est_pm_flux_sqr = eta[0] * eta[0] + eta[1] * eta[1]; - float bandwidth_factor = 1.0f / (sensorless->pm_flux_linkage * sensorless->pm_flux_linkage); - float eta_factor = 0.5f * (sensorless->observer_gain * bandwidth_factor) * (pm_flux_sqr - est_pm_flux_sqr); - - static float eta_factor_avg_test = 0.0f; - eta_factor_avg_test += 0.001f * (eta_factor - eta_factor_avg_test); - - // alpha-beta vector operations - for (int i = 0; i <= 1; ++i) { - // add observer action to flux estimate dynamics - float x_dot = eta_factor * eta[i]; - // convert action to discrete-time - sensorless->flux_state[i] += x_dot * current_meas_period; - // update new eta - eta[i] = sensorless->flux_state[i] - motor->phase_inductance * I_alpha_beta[i]; - } - - // Flux state estimation done, store V_alpha_beta for next timestep - sensorless->V_alpha_beta_memory[0] = motor->current_control.final_v_alpha; - sensorless->V_alpha_beta_memory[1] = motor->current_control.final_v_beta; - - // PLL - // predict PLL phase with velocity - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_vel); - // update PLL phase with observer permanent magnet phase - sensorless->phase = fast_atan2(eta[1], eta[0]); - float delta_phase = wrap_pm_pi(sensorless->phase - sensorless->pll_pos); - sensorless->pll_pos = wrap_pm_pi(sensorless->pll_pos + current_meas_period * sensorless->pll_kp * delta_phase); - // update PLL velocity - sensorless->pll_vel += current_meas_period * sensorless->pll_ki * delta_phase; - - //TODO TEMP TEST HACK - // static int trigger_ctr = 0; - // if (++trigger_ctr >= 3*current_meas_hz) { - // trigger_ctr = 0; - - // //Change to sensorless units - // motor->vel_gain = 15.0f / 200.0f; - // motor->vel_setpoint = 800.0f * motor->encoder.motor_dir; - - // //Change mode - // motor->rotor_mode = ROTOR_MODE_SENSORLESS; - // } - - } break; - default: - //TODO error handling - break; - } -} - -bool using_encoder(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) - return true; - else - return false; -} - -bool using_sensorless(Motor_t* motor) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) - return true; - else - return false; -} - -float get_rotor_phase(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.phase; - else if (using_sensorless(motor)) - return motor->sensorless.phase; - else - //TODO error handling - return 0.0f; -} - -float get_pll_vel(Motor_t* motor) { - if (using_encoder(motor)) - return motor->encoder.pll_vel; - else if (using_sensorless(motor)) - return motor->sensorless.pll_vel; - else - //TODO error handling - return 0.0f; -} - -// Function that sets the current encoder count to a desired 32-bit value. -void setEncoderCount(Motor_t* motor, uint32_t count) { - // Disable interrupts to make a critical section to avoid race condition - uint32_t prim = __get_PRIMASK(); - __disable_irq(); - motor->encoder.encoder_state = count; - motor->encoder.encoder_timer->Instance->CNT = count; - motor->encoder.pll_pos = (float)count; - __set_PRIMASK(prim); -} - -bool spin_up_timestep(Motor_t* motor, float phase, float I_mag) { - // wait for new timestep - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_SPIN_UP_TIMEOUT; - return false; - } - - if (!do_checks(motor)) - return false; - // run estimator - if (!loop_updates(motor)) - return false; - - // override the phase during spinup - motor->sensorless.phase = phase; - // run current control (with the phase override) - FOC_current(motor, I_mag, 0.0f); - - return true; -} - -bool spin_up_sensorless(Motor_t* motor) { - static const float ramp_up_time = 0.4f; - static const float ramp_up_distance = 4 * M_PI; - float ramp_step = current_meas_period / ramp_up_time; - - float phase = 0.0f; - float vel = ramp_up_distance / ramp_up_time; - float I_mag = 0.0f; - - // spiral up current - for (float x = 0.0f; x < 1.0f; x += ramp_step) { - phase = wrap_pm_pi(ramp_up_distance * x); - I_mag = motor->sensorless.spin_up_current * x; - if (!spin_up_timestep(motor, phase, I_mag)) - return false; - } - - // accelerate - while (vel < motor->sensorless.spin_up_target_vel) { - vel += motor->sensorless.spin_up_acceleration * current_meas_period; - phase = wrap_pm_pi(phase + vel * current_meas_period); - if (!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - return false; - } - - // // test keep spinning - // while (true) { - // phase = wrap_pm_pi(phase + vel * current_meas_period); - // if(!spin_up_timestep(motor, phase, motor->sensorless.spin_up_current)) - // return false; - // } - - return true; - - // TODO: check pll vel (abs ratio, 0.8) -} - -void update_brake_current() { - float Ibus_sum = 0.0f; - for (int i = 0; i < num_motors; ++i) { - Ibus_sum += motors[i].current_control.Ibus; - } - // Note: set_brake_current will clip negative values to 0.0f - set_brake_current(-Ibus_sum); -} - -void set_brake_current(float brake_current) { - if (brake_current < 0.0f) brake_current = 0.0f; - float brake_duty = brake_current * brake_resistance / vbus_voltage; - - // Duty limit at 90% to allow bootstrap caps to charge - if (brake_duty > 0.9f) brake_duty = 0.9f; - int high_on = TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty); - int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; - if (low_off < 0) low_off = 0; - - // Safe update of low and high side timings - // To avoid race condition, first reset timings to safe state - // ch3 is low side, ch4 is high side - htim2.Instance->CCR3 = 0; - htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; - htim2.Instance->CCR3 = low_off; - htim2.Instance->CCR4 = high_on; -} - -void queue_modulation_timings(Motor_t* motor, float mod_alpha, float mod_beta) { - float tA, tB, tC; - SVM(mod_alpha, mod_beta, &tA, &tB, &tC); - motor->next_timings[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS); - motor->next_timings[2] = (uint16_t)(tC * (float)TIM_1_8_PERIOD_CLOCKS); -} - -void queue_voltage_timings(Motor_t* motor, float v_alpha, float v_beta) { - float vfactor = 1.0f / ((2.0f / 3.0f) * vbus_voltage); - float mod_alpha = vfactor * v_alpha; - float mod_beta = vfactor * v_beta; - queue_modulation_timings(motor, mod_alpha, mod_beta); -} - -// TODO: This doesn't update brake current -// We should probably make FOC Current call FOC Voltage to avoid duplication. -bool FOC_voltage(Motor_t* motor, float v_d, float v_q) { - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float v_alpha = c*v_d - s*v_q; - float v_beta = c*v_q + s*v_d; - queue_voltage_timings(motor, v_alpha, v_beta); - - // Check we meet deadlines after queueing - if (!(check_timing(motor, TIMING_LOG_FOC_VOLTAGE) < motor->control_deadline)) { - motor->error = ERROR_FOC_VOLTAGE_TIMING; - return false; - } - return true; -} - -bool FOC_current(Motor_t* motor, float Id_des, float Iq_des) { - Current_control_t* ictrl = &motor->current_control; - - // For Reporting - ictrl->Iq_setpoint = Iq_des; - - // Clarke transform - float Ialpha = -motor->current_meas.phB - motor->current_meas.phC; - float Ibeta = one_by_sqrt3 * (motor->current_meas.phB - motor->current_meas.phC); - - // Park transform - float phase = get_rotor_phase(motor); - float c = arm_cos_f32(phase); - float s = arm_sin_f32(phase); - float Id = c * Ialpha + s * Ibeta; - float Iq = c * Ibeta - s * Ialpha; - ictrl->Iq_measured = Iq; - - // Current error - float Ierr_d = Id_des - Id; - float Ierr_q = Iq_des - Iq; - - // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) - // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; - - float mod_to_V = (2.0f / 3.0f) * vbus_voltage; - float V_to_mod = 1.0f / mod_to_V; - float mod_d = V_to_mod * Vd; - float mod_q = V_to_mod * Vq; - - // Vector modulation saturation, lock integrator if saturated - // TODO make maximum modulation configurable - float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); - if (mod_scalefactor < 1.0f) { - mod_d *= mod_scalefactor; - mod_q *= mod_scalefactor; - // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; - } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); - } - - // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; - - // Inverse park transform - float mod_alpha = c * mod_d - s * mod_q; - float mod_beta = c * mod_q + s * mod_d; - - // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; - - // Apply SVM - queue_modulation_timings(motor, mod_alpha, mod_beta); - - // Check we meet deadlines after queueing - motor->last_cpu_time = check_timing(motor, TIMING_LOG_FOC_CURRENT); - if (!(motor->last_cpu_time < motor->control_deadline)) { - motor->error = ERROR_FOC_TIMING; - return false; - } - - update_brake_current(); - return true; -} - -//Returns true if everything is OK (no fault) -bool check_DRV_fault(Motor_t* motor) { - //TODO: make this pin configurable per motor ch - GPIO_PinState nFAULT_state = HAL_GPIO_ReadPin(nFAULT_GPIO_Port, nFAULT_Pin); - return (nFAULT_state == GPIO_PIN_RESET) ? false : true; -} - -//Returns true if everything is OK (no fault) -bool check_vbus_undervoltage(Motor_t* motor) { - if(vbus_voltage < motor->dc_bus_undervoltage_trip_level) - return false; - return true; -} - -//Returns true if everything is OK (no fault) -// TODO This will be less repetitive with the refactoring -bool check_vbus_overvoltage(Motor_t* motor) { - if(vbus_voltage > motor->dc_bus_overvoltage_trip_level) - return false; - return true; -} - -// Returns true if everything is ok. Sets motor->error and returns false otherwise. -bool do_checks(Motor_t* motor) { - if (!check_DRV_fault(motor)) { - motor->error = ERROR_DRV_FAULT; - // Update DRV Fault Code - motor->drv_fault = DRV8301_getFaultType(&motor->gate_driver); - // Update/Cache all SPI device registers - DRV_SPI_8301_Vars_t* local_regs = &motor->gate_driver_regs; - local_regs->RcvCmd = true; - DRV8301_readData(&motor->gate_driver, local_regs); - return false; - } - if (!check_vbus_undervoltage(motor)) { - motor->error = ERROR_DC_BUS_UNDERVOLTAGE; - return false; - } - if (!check_vbus_overvoltage(motor)) { - motor->error = ERROR_DC_BUS_OVERVOLTAGE; - return false; - } - return true; -} - -bool loop_updates(Motor_t* motor) { - update_rotor(motor); - return true; -} - -void control_motor_loop(Motor_t* motor) { - while (*(motor->axis_legacy.enable_control)) { - if (osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status != osEventSignal) { - motor->error = ERROR_FOC_MEASUREMENT_TIMEOUT; - break; - } - - if (!do_checks(motor)) - break; - if (!loop_updates(motor)) - break; - - // Only runs if anticogging.calib_anticogging is true; non-blocking - anti_cogging_calibration(motor); - - // Position control - // TODO Decide if we want to use encoder or pll position here - float vel_des = motor->vel_setpoint; - if (motor->control_mode >= CTRL_MODE_POSITION_CONTROL) { - if (motor->rotor_mode == ROTOR_MODE_SENSORLESS) { - motor->error = ERROR_POS_CTRL_DURING_SENSORLESS; - break; - } - float pos_err = motor->pos_setpoint - motor->encoder.pll_pos; - vel_des += motor->pos_gain * pos_err; - } - - // Velocity limiting - float vel_lim = motor->vel_limit; - if (vel_des > vel_lim) vel_des = vel_lim; - if (vel_des < -vel_lim) vel_des = -vel_lim; - - // Velocity control - float Iq = motor->current_setpoint; - - // Anti-cogging is enabled after calibration - // We get the current position and apply a current feed-forward - // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) - if (motor->anticogging.use_anticogging) { - Iq += motor->anticogging.cogging_map[mod(motor->encoder.pll_pos, motor->encoder.encoder_cpr)]; - } - - float v_err = vel_des - get_pll_vel(motor); - if (motor->control_mode >= CTRL_MODE_VELOCITY_CONTROL) { - Iq += motor->vel_gain * v_err; - } - - // Velocity integral action before limiting - Iq += motor->vel_integrator_current; - - // Apply motor direction correction - if (motor->rotor_mode == ROTOR_MODE_ENCODER || - motor->rotor_mode == ROTOR_MODE_RUN_ENCODER_TEST_SENSORLESS) { - Iq *= motor->encoder.motor_dir; - } - - // Current limiting - float Ilim = MACRO_MIN(motor->current_control.current_lim, motor->current_control.max_allowed_current); - bool limited = false; - if (Iq > Ilim) { - limited = true; - Iq = Ilim; - } - if (Iq < -Ilim) { - limited = true; - Iq = -Ilim; - } - - // Velocity integrator (behaviour dependent on limiting) - if (motor->control_mode < CTRL_MODE_VELOCITY_CONTROL) { - // reset integral if not in use - motor->vel_integrator_current = 0.0f; - } else { - if (limited) { - // TODO make decayfactor configurable - motor->vel_integrator_current *= 0.99f; - } else { - motor->vel_integrator_current += (motor->vel_integrator_gain * current_meas_period) * v_err; - } - } - - // Execute current command - if (motor->motor_type == MOTOR_TYPE_HIGH_CURRENT) { - if(!FOC_current(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_current - } - } else if (motor->motor_type == MOTOR_TYPE_GIMBAL) { - //In gimbal motor mode, current is reinterptreted as voltage. - if(!FOC_voltage(motor, 0.0f, Iq)){ - break; // in case of error exit loop, motor->error has been set by FOC_voltage - } - } else { - motor->error = ERROR_NOT_IMPLEMENTED_MOTOR_TYPE; - break; - } - - ++(motor->loop_counter); - } - - //We are exiting control, reset Ibus, and update brake current - motor->current_control.Ibus = 0.0f; - update_brake_current(); -} diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp new file mode 100644 index 00000000..a984b789 --- /dev/null +++ b/Firmware/MotorControl/low_level.cpp @@ -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 +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "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(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(); + } +} diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 1ed956db..e3784788 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -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 -#include "drv8301.h" - -//default timeout waiting for phase measurement signals -#define PH_CURRENT_MEAS_TIMEOUT 2 // [ms] +#include +#include /* Exported types ------------------------------------------------------------*/ -typedef enum { - M_SIGNAL_PH_CURRENT_MEAS = 1u << 0 -} Motor_thread_signals_t; - -typedef struct { - int index; - float *cogging_map; - bool use_anticogging; - bool calib_anticogging; - float calib_pos_threshold; - float calib_vel_threshold; -} Anticogging_t; - -typedef enum { - ERROR_NO_ERROR, - ERROR_PHASE_RESISTANCE_TIMING, - ERROR_PHASE_RESISTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_RESISTANCE_OUT_OF_RANGE, - ERROR_PHASE_INDUCTANCE_TIMING, - ERROR_PHASE_INDUCTANCE_MEASUREMENT_TIMEOUT, - ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE, - ERROR_ENCODER_RESPONSE, - ERROR_ENCODER_MEASUREMENT_TIMEOUT, - ERROR_ADC_FAILED, - ERROR_CALIBRATION_TIMING, - ERROR_FOC_TIMING, - ERROR_FOC_MEASUREMENT_TIMEOUT, - ERROR_SCAN_MOTOR_TIMING, - ERROR_FOC_VOLTAGE_TIMING, - ERROR_GATEDRIVER_INVALID_GAIN, - ERROR_PWM_SRC_FAIL, - ERROR_UNEXPECTED_STEP_SRC, - ERROR_POS_CTRL_DURING_SENSORLESS, - ERROR_SPIN_UP_TIMEOUT, - ERROR_DRV_FAULT, - ERROR_NOT_IMPLEMENTED_MOTOR_TYPE, - ERROR_ENCODER_CPR_OUT_OF_RANGE, - ERROR_DC_BUS_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 } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp new file mode 100644 index 00000000..ea3c155c --- /dev/null +++ b/Firmware/MotorControl/main.cpp @@ -0,0 +1,157 @@ + +#define __MAIN_CPP__ +#include "odrive_main.h" +#include "nvm_config.hpp" + +#include "freertos_vars.h" +#include +#include + +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; +} diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp new file mode 100644 index 00000000..3272c746 --- /dev/null +++ b/Firmware/MotorControl/motor.cpp @@ -0,0 +1,365 @@ + +#include + +#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(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 = ¤t_control_; + + // For Reporting + ictrl->Iq_setpoint = Iq_des; + + // Clarke transform + float Ialpha = -current_meas_.phB - current_meas_.phC; + float Ibeta = one_by_sqrt3 * (current_meas_.phB - current_meas_.phC); + + // Park transform + float c = arm_cos_f32(phase); + float s = arm_sin_f32(phase); + float Id = c * Ialpha + s * Ibeta; + float Iq = c * Ibeta - s * Ialpha; + ictrl->Iq_measured = Iq; + + // Current error + float Ierr_d = Id_des - Id; + float Ierr_q = Iq_des - Iq; + + // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) + // Apply PI control + float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; + float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + + float mod_to_V = (2.0f / 3.0f) * vbus_voltage; + float V_to_mod = 1.0f / mod_to_V; + float mod_d = V_to_mod * Vd; + float mod_q = V_to_mod * Vq; + + // Vector modulation saturation, lock integrator if saturated + // TODO make maximum modulation configurable + float mod_scalefactor = 0.80f * sqrt3_by_2 * 1.0f / sqrtf(mod_d * mod_d + mod_q * mod_q); + if (mod_scalefactor < 1.0f) { + mod_d *= mod_scalefactor; + mod_q *= mod_scalefactor; + // TODO make decayfactor configurable + ictrl->v_current_control_integral_d *= 0.99f; + ictrl->v_current_control_integral_q *= 0.99f; + } else { + ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); + ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + } + + // Compute estimated bus current + ictrl->Ibus = mod_d * Id + mod_q * Iq; + + // Inverse park transform + float mod_alpha = c * mod_d - s * mod_q; + float mod_beta = c * mod_q + s * mod_d; + + // Report final applied voltage in stationary frame (for sensorles estimator) + ictrl->final_v_alpha = mod_to_V * mod_alpha; + ictrl->final_v_beta = mod_to_V * mod_beta; + + // Apply SVM + 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; +} diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp new file mode 100644 index 00000000..950141bb --- /dev/null +++ b/Firmware/MotorControl/motor.hpp @@ -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", ¤t_meas_.phB), + make_protocol_ro_property("current_meas_phC", ¤t_meas_.phC), + make_protocol_property("DC_calib_phB", &DC_calib_.phB), + make_protocol_property("DC_calib_phC", &DC_calib_.phC), + make_protocol_property("phase_current_rev_gain", &phase_current_rev_gain_), + make_protocol_object("current_control", + make_protocol_property("p_gain", ¤t_control_.p_gain), + make_protocol_property("i_gain", ¤t_control_.i_gain), + make_protocol_property("v_current_control_integral_d", ¤t_control_.v_current_control_integral_d), + make_protocol_property("v_current_control_integral_q", ¤t_control_.v_current_control_integral_q), + make_protocol_property("Ibus", ¤t_control_.Ibus), + make_protocol_property("final_v_alpha", ¤t_control_.final_v_alpha), + make_protocol_property("final_v_beta", ¤t_control_.final_v_beta), + make_protocol_property("Iq_setpoint", ¤t_control_.Iq_setpoint), + make_protocol_property("Iq_measured", ¤t_control_.Iq_measured), + make_protocol_property("max_allowed_current", ¤t_control_.max_allowed_current) + ), + make_protocol_object("gate_driver", + make_protocol_ro_property("drv_fault", &drv_fault_) + // make_protocol_ro_property("status_reg_1", &gate_driver_regs_.Stat_Reg_1_Value), + // make_protocol_ro_property("status_reg_2", &gate_driver_regs_.Stat_Reg_2_Value), + // make_protocol_ro_property("ctrl_reg_1", &gate_driver_regs_.Ctrl_Reg_1_Value), + // make_protocol_ro_property("ctrl_reg_2", &gate_driver_regs_.Ctrl_Reg_2_Value) + ), + make_protocol_object("timing_log", + make_protocol_ro_property("TIMING_LOG_GENERAL", &timing_log_[TIMING_LOG_GENERAL]), + make_protocol_ro_property("TIMING_LOG_ADC_CB_I", &timing_log_[TIMING_LOG_ADC_CB_I]), + make_protocol_ro_property("TIMING_LOG_ADC_CB_DC", &timing_log_[TIMING_LOG_ADC_CB_DC]), + make_protocol_ro_property("TIMING_LOG_MEAS_R", &timing_log_[TIMING_LOG_MEAS_R]), + make_protocol_ro_property("TIMING_LOG_MEAS_L", &timing_log_[TIMING_LOG_MEAS_L]), + make_protocol_ro_property("TIMING_LOG_ENC_CALIB", &timing_log_[TIMING_LOG_ENC_CALIB]), + make_protocol_ro_property("TIMING_LOG_IDX_SEARCH", &timing_log_[TIMING_LOG_IDX_SEARCH]), + make_protocol_ro_property("TIMING_LOG_FOC_VOLTAGE", &timing_log_[TIMING_LOG_FOC_VOLTAGE]), + make_protocol_ro_property("TIMING_LOG_FOC_CURRENT", &timing_log_[TIMING_LOG_FOC_CURRENT]) + ), + make_protocol_object("config", + make_protocol_property("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 diff --git a/Firmware/MotorControl/config.cpp b/Firmware/MotorControl/nvm_config.hpp similarity index 50% rename from Firmware/MotorControl/config.cpp rename to Firmware/MotorControl/nvm_config.hpp index abbb915d..bf0f9134 100644 --- a/Firmware/MotorControl/config.cpp +++ b/Firmware/MotorControl/nvm_config.hpp @@ -1,57 +1,38 @@ +/* +* Convenience functions to load and store multiple objects from and to NVM. +* +* The NVM stores consecutive one-to-one copies of arbitrary objects. +* The types of these objects are passed as template arguments to Config. +*/ /* Includes ------------------------------------------------------------------*/ -#include "config.h" - #include #include #include #include "nvm.h" -#include "crc.hpp" -#include "low_level.h" -#include "axis.h" +#include -// IMPORTANT: if you change, reorder or otherwise modify any of the fields in -// the config structs, make sure to increment this number: -uint16_t config_version = 0x0001; /* Private defines -----------------------------------------------------------*/ -#define CRC16_INIT 0xabcd +#define CONFIG_CRC16_INIT 0xabcd /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ - -typedef struct { - Motor_control_mode_t control_mode; - float counts_per_step; - int32_t pole_pairs; - float pos_gain; - float vel_gain; - float vel_integrator_gain; - float vel_limit; - float calibration_current; - float resistance_calib_max_voltage; - float phase_inductance; - float phase_resistance; - Motor_type_t motor_type; - Rotor_mode_t rotor_mode; - float current_control_current_lim; - bool encoder_use_index; - bool encoder_manually_calibrated; - float encoder_idx_search_speed; - int32_t encoder_cpr; - int32_t encoder_offset; - int32_t encoder_motor_dir; -} MotorConfig_t; - /* Global constant data ------------------------------------------------------*/ /* Global variables ----------------------------------------------------------*/ /* Private constant data -----------------------------------------------------*/ + +// IMPORTANT: if you change, reorder or otherwise modify any of the fields in +// the config structs, make sure to increment this number: +static constexpr uint16_t config_version = 0x0001; + /* Private variables ---------------------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/ /* Function implementations --------------------------------------------------*/ + // @brief Manages configuration load and store operations from and to NVM // // The NVM stores consecutive one-to-one copies of arbitrary objects. @@ -121,11 +102,11 @@ struct Config { // @brief Loads one or more consecutive objects from the NVM. The loaded data // is validated using a CRC value that is stored at the beginning of the data. - static int load_config(T* val0, Ts* ... vals) { + static int safe_load_config(T* val0, Ts* ... vals) { //printf("have %d bytes\r\n", NVM_get_max_read_length()); osDelay(5); if (Config::get_size() > NVM_get_max_read_length()) return -1; - uint16_t crc16 = CRC16_INIT ^ config_version; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; if (Config::load_config(0, &crc16, val0, vals..., &crc16)) return -1; if (crc16) @@ -140,14 +121,14 @@ struct Config { // changes of the config structs during firmware update. Note that if the total // config data length changes, the CRC validation will fail even if the developer // forgets to update the config version number. - static int store_config(const T* val0, const Ts* ... vals) { + static int safe_store_config(const T* val0, const Ts* ... vals) { size_t size = Config::get_size() + 2; //printf("config is %d bytes\r\n", size); osDelay(5); if (size > NVM_get_max_write_length()) return -1; if (NVM_start_write(size)) return -1; - uint16_t crc16 = CRC16_INIT ^ config_version; + uint16_t crc16 = CONFIG_CRC16_INIT ^ config_version; if (Config::store_config(0, &crc16, val0, vals...)) return -1; if (Config::store_config(size - 2, nullptr, (uint8_t *)&crc16 + 1, (uint8_t *)&crc16)) @@ -157,97 +138,3 @@ struct Config { return 0; } }; - -// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t) -// TODO: make this go away as part of the C++ refactoring -void set_motor_config(const MotorConfig_t* config, Motor_t* motor) { - motor->control_mode = config->control_mode; - motor->counts_per_step = config->counts_per_step; - motor->pole_pairs = config->pole_pairs; - motor->pos_gain = config->pos_gain; - motor->vel_gain = config->vel_gain; - motor->vel_integrator_gain = config->vel_integrator_gain; - motor->vel_limit = config->vel_limit; - motor->calibration_current = config->calibration_current; - motor->resistance_calib_max_voltage = config->resistance_calib_max_voltage; - motor->phase_inductance = config->phase_inductance; - motor->phase_resistance = config->phase_resistance; - motor->motor_type = config->motor_type; - motor->rotor_mode = config->rotor_mode; - - motor->current_control.current_lim = config->current_control_current_lim; - - motor->encoder.use_index = config->encoder_use_index; - motor->encoder.manually_calibrated = config->encoder_manually_calibrated; - motor->encoder.idx_search_speed = config->encoder_idx_search_speed; - motor->encoder.encoder_cpr = config->encoder_cpr; - motor->encoder.encoder_offset = config->encoder_offset; - motor->encoder.motor_dir = config->encoder_motor_dir; -} - -// This function is obviously stupid and should go away (make MotorConfig_t a member of Motor_t) -// TODO: make this go away as part of the C++ refactoring -void get_motor_config(const Motor_t* motor, MotorConfig_t* config) { - config->control_mode = motor->control_mode; - config->counts_per_step = motor->counts_per_step; - config->pole_pairs = motor->pole_pairs; - config->pos_gain = motor->pos_gain; - config->vel_gain = motor->vel_gain; - config->vel_integrator_gain = motor->vel_integrator_gain; - config->vel_limit = motor->vel_limit; - config->calibration_current = motor->calibration_current; - config->resistance_calib_max_voltage = motor->resistance_calib_max_voltage; - config->phase_inductance = motor->phase_inductance; - config->phase_resistance = motor->phase_resistance; - config->motor_type = motor->motor_type; - config->rotor_mode = motor->rotor_mode; - - config->current_control_current_lim = motor->current_control.current_lim; - - config->encoder_use_index = motor->encoder.use_index; - config->encoder_manually_calibrated = motor->encoder.manually_calibrated; - config->encoder_idx_search_speed = motor->encoder.idx_search_speed; - config->encoder_cpr = motor->encoder.encoder_cpr; - config->encoder_offset = motor->encoder.encoder_offset; - config->encoder_motor_dir = motor->encoder.motor_dir; -} - -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::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::store_config(&motor_config[0], &motor_config[1], &axis_configs[0], &axis_configs[1], &brake_resistance)) { - //printf("saving configuration failed\r\n"); osDelay(5); - } -} - -void erase_configuration(void) { - NVM_erase(); -} diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h new file mode 100644 index 00000000..2b29b6ac --- /dev/null +++ b/Firmware/MotorControl/odrive_main.h @@ -0,0 +1,109 @@ +#ifndef __ODRIVE_MAIN_H +#define __ODRIVE_MAIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +// STM specific includes +#include // Sets up the correct chip specifc defines required by arm_math +#define ARM_MATH_CM4 // TODO: might change in future board versions +#include + +// OS includes +#include + +// 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; //(static_cast>(a) | static_cast>(b)); } \ +inline ENUMTYPE operator & (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) & static_cast>(b)); } \ +inline ENUMTYPE operator ^ (ENUMTYPE a, ENUMTYPE b) { return static_cast(static_cast>(a) ^ static_cast>(b)); } \ +inline ENUMTYPE &operator |= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) |= static_cast>(b)); } \ +inline ENUMTYPE &operator &= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) &= static_cast>(b)); } \ +inline ENUMTYPE &operator ^= (ENUMTYPE &a, ENUMTYPE b) { return reinterpret_cast(reinterpret_cast&>(a) ^= static_cast>(b)); } \ +inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_cast>(a)); } + + +// ODrive specific includes +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 */ diff --git a/Firmware/MotorControl/protocol.hpp b/Firmware/MotorControl/protocol.hpp deleted file mode 100644 index 46a59f06..00000000 --- a/Firmware/MotorControl/protocol.hpp +++ /dev/null @@ -1,534 +0,0 @@ -/* -see protocol.md for the protocol specification -*/ - -#ifndef __PROTOCOL_HPP -#define __PROTOCOL_HPP - -// TODO: resolve assert -#define assert(expr) - -#include -#include -#include -#include "crc.hpp" - - -constexpr uint8_t SYNC_BYTE = 0xAA; -constexpr uint8_t CRC8_INIT = 0x42; -constexpr uint16_t CRC16_INIT = 0x1337; -constexpr uint16_t PROTOCOL_VERSION = 1; - -// This value must not be larger than USB_TX_DATA_SIZE defined in usbd_cdc_if.h -//Oskar: What's the error? What values work? Does 63 work? Ideally we figure out how to get 64 to work, but if not let's find something better than 32. -constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason -constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently no effect because of protocol limitations - -// Maximum time we allocate for processing and responding to a request -constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10; - -template -inline size_t write_le(T value, uint8_t* buffer); - -template -inline size_t read_le(T* value, const uint8_t* buffer); - -template<> -inline size_t write_le(bool value, uint8_t* buffer) { - buffer[0] = value ? 1 : 0; - return 1; -} - -template<> -inline size_t write_le(uint8_t value, uint8_t* buffer) { - buffer[0] = value; - return 1; -} - -template<> -inline size_t write_le(uint16_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - return 2; -} - -template<> -inline size_t write_le(uint32_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - buffer[2] = (value >> 16) & 0xff; - buffer[3] = (value >> 24) & 0xff; - return 4; -} - -template<> -inline size_t write_le(int32_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - buffer[2] = (value >> 16) & 0xff; - buffer[3] = (value >> 24) & 0xff; - return 4; -} - -template<> -inline size_t write_le(uint64_t value, uint8_t* buffer) { - buffer[0] = (value >> 0) & 0xff; - buffer[1] = (value >> 8) & 0xff; - buffer[2] = (value >> 16) & 0xff; - buffer[3] = (value >> 24) & 0xff; - buffer[4] = (value >> 32) & 0xff; - buffer[5] = (value >> 40) & 0xff; - buffer[6] = (value >> 48) & 0xff; - buffer[7] = (value >> 56) & 0xff; - return 8; -} - -template<> -inline size_t write_le(float value, uint8_t* buffer) { - static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); - static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); - return write_le(*reinterpret_cast(&value), buffer); -} - -template<> -inline size_t read_le(bool* value, const uint8_t* buffer) { - *value = buffer[0]; - return 1; -} - -template<> -inline size_t read_le(uint8_t* value, const uint8_t* buffer) { - *value = buffer[0]; - return 1; -} - -template<> -inline size_t read_le(uint16_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8); - return 2; -} - -template<> -inline size_t read_le(int32_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8) | - (static_cast(buffer[2]) << 16) | - (static_cast(buffer[3]) << 24); - return 4; -} - -template<> -inline size_t read_le(uint32_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8) | - (static_cast(buffer[2]) << 16) | - (static_cast(buffer[3]) << 24); - return 4; -} - -template<> -inline size_t read_le(uint64_t* value, const uint8_t* buffer) { - *value = (static_cast(buffer[0]) << 0) | - (static_cast(buffer[1]) << 8) | - (static_cast(buffer[2]) << 16) | - (static_cast(buffer[3]) << 24) | - (static_cast(buffer[4]) << 32) | - (static_cast(buffer[5]) << 40) | - (static_cast(buffer[6]) << 48) | - (static_cast(buffer[7]) << 56); - return 8; -} - -template<> -inline size_t read_le(float* value, const uint8_t* buffer) { - static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); - static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); - return read_le(reinterpret_cast(value), buffer); -} - -// @brief Reads a value of type T from the buffer. -// @param buffer Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read. -// @param length The number of available bytes in buffer. This value is updated to subtract the bytes that were read. -template -static inline T read_le(const uint8_t** buffer, size_t* length) { - T result; - size_t cnt = read_le(&result, *buffer); - *buffer += cnt; - *length -= cnt; - return result; -} - -class PacketSink { -public: - // @brief Processes a packet. - // The blocking behavior shall depend on the thread-local deadline_ms variable. - // @return: 0 on success, otherwise a non-zero error code - // TODO: define what happens when the packet is larger than what the implementation can handle. - virtual int process_packet(const uint8_t* buffer, size_t length) = 0; -}; - -class StreamSink { -public: - // @brief Processes a chunk of bytes that is part of a continuous stream. - // The blocking behavior shall depend on the thread-local deadline_ms variable. - // @return: 0 on success, otherwise a non-zero error code - virtual int process_bytes(const uint8_t* buffer, size_t length) = 0; - - // @brief Returns the number of bytes that can still be written to the stream. - // Shall return SIZE_MAX if the stream has unlimited lenght. - virtual size_t get_free_space() = 0; -}; - - -class StreamToPacketConverter : public StreamSink { -public: - StreamToPacketConverter(PacketSink& output) : - output_(output) - { - }; - - int process_bytes(const uint8_t *buffer, size_t length); - - size_t get_free_space() { return SIZE_MAX; } - -private: - uint8_t header_buffer_[3]; - size_t header_index_ = 0; - uint8_t packet_buffer_[RX_BUF_SIZE]; - size_t packet_index_ = 0; - size_t packet_length_ = 0; - PacketSink& output_; -}; - - -class PacketToStreamConverter : public PacketSink { -public: - PacketToStreamConverter(StreamSink& output) : - output_(output) - { - }; - - int process_packet(const uint8_t *buffer, size_t length); - -private: - StreamSink& output_; -}; - - -// Implements the StreamSink interface by writing into a fixed size -// memory buffer. -class MemoryStreamSink : public StreamSink { -public: - MemoryStreamSink(uint8_t *buffer, size_t length) : - buffer_(buffer), - buffer_length_(length) {} - - // Returns 0 on success and -1 if the buffer could not accept everything because it became full - int process_bytes(const uint8_t* buffer, size_t length) { - int status = 0; - if (length > buffer_length_) { - length = buffer_length_; - status = -1; - } - memcpy(buffer_, buffer, length); - buffer_ += length; - buffer_length_ -= length; - return status; - } - - size_t get_free_space() { return buffer_length_; } - -private: - uint8_t * buffer_; - size_t buffer_length_; -}; - -// Implements the StreamSink interface by discarding the first couple of bytes -// and then forwarding the rest to another stream. -class NullStreamSink : public StreamSink { -public: - NullStreamSink(size_t skip, StreamSink& follow_up_stream) : - skip_(skip), - follow_up_stream_(follow_up_stream) {} - - // Returns 0 on success and -1 if the buffer could not accept everything because it became full - int process_bytes(const uint8_t* buffer, size_t length) { - if (skip_ < length) { - buffer += skip_; - length -= skip_; - skip_ = 0; - return follow_up_stream_.process_bytes(buffer, length); - } else { - skip_ -= length; - return 0; - } - } - - size_t get_free_space() { return skip_ + follow_up_stream_.get_free_space(); } - -private: - size_t skip_; - StreamSink& follow_up_stream_; -}; - - - -// Implements the StreamSink interface by calculating the CRC16 checksum -// on the data that is sent to it. -class CRC16Calculator : public StreamSink { -public: - CRC16Calculator(uint16_t crc16_init) : - crc16_(crc16_init) {} - - int process_bytes(const uint8_t* buffer, size_t length) { - crc16_ = calc_crc16(crc16_, buffer, length); - return 0; - } - - size_t get_free_space() { return SIZE_MAX; } - - uint16_t get_crc16() { return crc16_; } -private: - uint16_t crc16_; -}; - - - -typedef enum { - PROPERTY, - BEGIN_OBJECT, - BEGIN_FUNCTION, - CLOSE_TREE -} EndpointType_t; - - -// @brief Endpoint request handler -// -// When passed a valid endpoint context, implementing functions shall handle an -// endpoint read/write request by reading the provided input data and filling in -// output data. The exact semantics of this function depends on the corresponding -// endpoint's specification. -// -// @param input: pointer to the input data -// @param input_length: number of available input bytes -// @param output: The stream where to write the output to. Can be null. -// The handler shall abort as soon as the stream returns -// a non-zero error code on write. -typedef std::function EndpointHandler; - - -template -void default_read_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - const T* value = reinterpret_cast(ctx); - // If the old value was requested, call the corresponding little endian serialization function - if (output) { - // TODO: make buffer size dependent on the type - uint8_t buffer[sizeof(T)]; - size_t cnt = write_le(*value, buffer); - if (cnt <= output->get_free_space()) - output->process_bytes(buffer, cnt); - } -} - -template -void default_readwrite_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - T* value = reinterpret_cast(ctx); - - // Read the endpoint value into output - default_read_endpoint_handler(ctx, input, input_length, output); - - // If a new value was passed, call the corresponding little endian deserialization function - uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type - if (input_length >= sizeof(buffer)) - read_le(value, input); -} - -static void trigger_endpoint_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - (void) input; - (void) input_length; - (void) output; - std::function function = reinterpret_cast(ctx); - function(); -} - - -template -static inline const char* get_default_json_modifier(); - -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"float\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"float\",\"access\":\"rw\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint64\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint64\",\"access\":\"rw\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"int32\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"int32\",\"access\":\"rw\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint32\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint32\",\"access\":\"rw\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint16\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint16\",\"access\":\"rw\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint8\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"uint8\",\"access\":\"rw\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"bool\",\"access\":\"r\""; -} -template<> -inline const char* get_default_json_modifier() { - return "\"type\":\"bool\",\"access\":\"rw\""; -} - -class Endpoint { -public: - const char* const name_; - - Endpoint(const char* name, EndpointType_t type, EndpointHandler handler, const char* json_modifier, void *ctx) : - name_(name), - type_(type), - handler_(handler), - json_modifier_(json_modifier), - ctx_(ctx) - { - } - - template - static Endpoint make_property(const char* name, const T* ctx) { - return Endpoint(name, PROPERTY, - default_read_endpoint_handler, - get_default_json_modifier(), - const_cast(ctx) /* it's safe to cast the const away here because we - know that the default_read_endpoint_handler immediately adds it back */); - } - - template - static Endpoint make_property(const char* name, T* ctx) { - return Endpoint(name, PROPERTY, - default_readwrite_endpoint_handler, - get_default_json_modifier(), ctx); - } - - static Endpoint make_object(const char* name) { - return Endpoint(name, BEGIN_OBJECT, nullptr, - "\"type\":\"object\"", nullptr); - } - - static Endpoint make_function(const char* name, void(*function)(void)) { - return Endpoint(name, BEGIN_FUNCTION, trigger_endpoint_handler, - "\"type\":\"function\"", reinterpret_cast(function)); - } - - static Endpoint close_tree() { - return Endpoint(nullptr, CLOSE_TREE, nullptr, nullptr, nullptr); - } - - void write_json(size_t id, bool* need_comma, StreamSink* output) const; - - void handle(const uint8_t* input, size_t input_length, StreamSink* output) const { - if (handler_) - return handler_(ctx_, input, input_length, output); - } - -private: - const EndpointType_t type_; - const EndpointHandler handler_; - const char* json_modifier_; - void* const ctx_; -}; - - -/* @brief Handles the communication protocol on one channel. -* -* When instantiated with a list of endpoints and an output packet sink, -* objects of this class will handle packets passed into process_packet, -* pass the relevant data to the corresponding endpoints and dispatch response -* packets on the output. -*/ -class BidirectionalPacketBasedChannel : public PacketSink { -public: - BidirectionalPacketBasedChannel(const Endpoint* endpoints, size_t n_endpoints, PacketSink& output) : - global_endpoints_(endpoints), - n_endpoints_(NUM_CHANNEL_SPECIFIC_ENDPOINTS + n_endpoints), - output_(output), - json_crc_(calculate_json_crc16()) - { - } - - int process_packet(const uint8_t* buffer, size_t length); - -private: - - uint16_t calculate_json_crc16(void); - void interface_query(const uint8_t* input, size_t input_length, StreamSink* output); - - static void interface_query_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - reinterpret_cast(ctx)->interface_query(input, input_length, output); - } - - static void subscription_handler(void* ctx, const uint8_t* input, size_t input_length, StreamSink* output) { - reinterpret_cast(ctx)->subscription(input, input_length, output); - } - - const Endpoint channel_specific_endpoints_[1] = { - Endpoint("", PROPERTY, BidirectionalPacketBasedChannel::interface_query_handler, "\"type\":\"json\",\"access\":\"rw\"", this), - //Endpoint("subscriptions", PROPERTY, BidirectionalPacketBasedChannel::subscription_handler, nullptr, this) - }; - static constexpr size_t NUM_CHANNEL_SPECIFIC_ENDPOINTS = sizeof(channel_specific_endpoints_) / sizeof(channel_specific_endpoints_[0]); - - const Endpoint* get_endpoint(size_t index) { - if (index < NUM_CHANNEL_SPECIFIC_ENDPOINTS){ - return &channel_specific_endpoints_[index]; - } else if (index < n_endpoints_) { - return &global_endpoints_[index - NUM_CHANNEL_SPECIFIC_ENDPOINTS]; - } else { - return nullptr; - } - } - - void subscription(const uint8_t* input, size_t input_length, StreamSink* output) { - // TODO: handle - return; - } - - const Endpoint * const global_endpoints_; - size_t n_endpoints_; - PacketSink& output_; - uint8_t tx_buf_[TX_BUF_SIZE]; - const uint16_t json_crc_; -}; - -#endif diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp new file mode 100644 index 00000000..1098b38c --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -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; +}; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp new file mode 100644 index 00000000..910bc05a --- /dev/null +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -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 / ( * ) } + 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 */ diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index a6a02a5a..4fa8c378 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -4,8 +4,6 @@ #include #include -static const float one_by_sqrt3 = 0.57735026919f; -static const float two_by_sqrt3 = 1.15470053838f; int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { int Sextant; @@ -122,23 +120,12 @@ int SVM(float alpha, float beta, float* tA, float* tB, float* tC) { } } - int retval = 0; - if ( - *tA < 0.0f - || *tA > 1.0f - || *tB < 0.0f - || *tB > 1.0f - || *tC < 0.0f - || *tC > 1.0f - ) retval = -1; - return retval; -} - -//beware of inserting large angles! -float wrap_pm_pi(float theta) { - while (theta >= M_PI) theta -= (2.0f * M_PI); - while (theta < -M_PI) theta += (2.0f * M_PI); - return theta; + // if any of the results becomes NaN, result_valid will evaluate to false + int result_valid = + *tA >= 0.0f && *tA <= 1.0f + && *tB >= 0.0f && *tB <= 1.0f + && *tC >= 0.0f && *tC <= 1.0f; + return result_valid ? 0 : -1; } // based on https://math.stackexchange.com/a/1105038/81278 diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index bf329605..bc95cd27 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -7,11 +7,7 @@ extern "C" { #endif #include - -/** - * @brief Unique ID register address location - */ -#define ID_UNIQUE_ADDRESS (0x1FFF7A10) +#include /** * @brief Flash size register address @@ -59,17 +55,6 @@ extern "C" { */ #define STM_ID_GetFlashSize() (*(uint16_t *)(ID_FLASH_ADDRESS)) -/** - * "Returns" the given 32-bit value of the UUID. - * - * Parameters: - * - uint8_t x: - * Value between 0 and 2, corresponding to 4-bytes you want to read from 96bits (12bytes) - * - * Returned data is 32-bit - */ -#define STM_ID_GetUUID(x) ((x >= 0 && x < 3) ? (*(uint32_t *)(ID_UNIQUE_ADDRESS + 4 * (x))) : 0) - #ifdef M_PI #undef M_PI #endif @@ -78,14 +63,36 @@ extern "C" { #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +static const float one_by_sqrt3 = 0.57735026919f; +static const float two_by_sqrt3 = 1.15470053838f; +static const float sqrt3_by_2 = 0.86602540378f; + +//beware of inserting large values! +static inline float wrap_pm(float x, float pm_range) { + while (x >= pm_range) x -= (2.0f * pm_range); + while (x < -pm_range) x += (2.0f * pm_range); + return x; +} + +//beware of inserting large angles! +static inline float wrap_pm_pi(float theta) { + return wrap_pm(theta, M_PI); +} + +// like fmodf, but always positive +static inline float fmodf_pos(float x, float y) { + float out = fmodf(x, y); + if (out < 0.0f) + out += y; + return out; +} + // Compute rising edge timings (0.0 - 1.0) as a function of alpha-beta // as per the magnitude invariant clarke transform // The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2 // Returns 0 on success, and -1 if the input was out of range int SVM(float alpha, float beta, float* tA, float* tB, float* tC); -//beware of inserting large angles! -float wrap_pm_pi(float theta); float fast_atan2(float y, float x); int mod(int dividend, int divisor); diff --git a/Firmware/README.md b/Firmware/README.md index c5a6059e..ad315ac0 100644 --- a/Firmware/README.md +++ b/Firmware/README.md @@ -30,7 +30,7 @@ In this section we will set the compile-time parameters, later we will also set To customize the compile time parameters, copy or rename the file `Firmware/tup.config.default` to `Firmware/tup.config` and edit the parameters in that file: -__CONFIG_BOARD_VERSION__: The board version you're using. Can be `v3.1`, `v3.2`, `v3.3`, `v3.4-24V` or `v3.4-48V`. Check for a label on the upper side of the ODrive to find out which version you have. +__CONFIG_BOARD_VERSION__: The board version you're using. Can be `v3.1`, `v3.2`, `v3.3`, `v3.4-24V`, `v3.4-48V`, `v3.5-24V` or `v3.5-48V`. Check for a label on the upper side of the ODrive to find out which version you have. __CONFIG_USB_PROTOCOL__: Defines which protocol the ODrive should use on the USB interface. * `native`: The native ODrive protocol. Use this if you want to use the python tools in this repo. @@ -43,8 +43,6 @@ __CONFIG_UART_PROTOCOL__: Defines which protocol the ODrive should use on the UA * `ascii`: The ASCII protocol. Use this option if you control the ODrive with an Arduino. The ODrive Arduino library is not yet updated to the native protocol. * `none`: Disable UART. -__CONFIG_STEP_DIR__: Set to `y` to use the GPIO1 and GPIO2 for step/direction input. Set to `n` otherwise. To use this, `CONFIG_UART_PROTOCOL` must be `none` because UART uses the same pins. -

## Downloading and Installing Tools @@ -176,7 +174,7 @@ pip install pyusb pyserial [See ODrive Arduino Library](https://github.com/madcowswe/ODriveArduino) ### Other platforms -See the [protocol specification](protocol.md) or the [legacy protocol specification](legacy-protocol.md). +See the [protocol specification](protocol.md) or the [ASCII protocol specification](ascii-protocol.md).

## Configuring parameters diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index 0b5b263b..ff95f0d1 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -23,6 +23,14 @@ elseif boardversion == "v3.4-48V" then boarddir = 'Board/v3' FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=4" FLAGS += "-DHW_VERSION_VOLTAGE=48" +elseif boardversion == "v3.5-24V" then + boarddir = 'Board/v3' + FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=5" + FLAGS += "-DHW_VERSION_VOLTAGE=24" +elseif boardversion == "v3.5-48V" then + boarddir = 'Board/v3' + FLAGS += "-DHW_VERSION_MAJOR=3 -DHW_VERSION_MINOR=5" + FLAGS += "-DHW_VERSION_VOLTAGE=48" elseif boardversion == "" then error("board version not specified - take a look at tup.config.default") else @@ -36,7 +44,9 @@ if tup.getconfig("USB_PROTOCOL") == "native" or tup.getconfig("USB_PROTOCOL") == elseif tup.getconfig("USB_PROTOCOL") == "native-stream" then FLAGS += "-DUSB_PROTOCOL_NATIVE_STREAM_BASED" elseif tup.getconfig("USB_PROTOCOL") == "ascii" then - FLAGS += "-DUSB_PROTOCOL_LEGACY" + FLAGS += "-DUSB_PROTOCOL_ASCII" +elseif tup.getconfig("USB_PROTOCOL") == "stdout" then + FLAGS += "-DUSB_PROTOCOL_STDOUT" elseif tup.getconfig("USB_PROTOCOL") == "none" then FLAGS += "-DUSB_PROTOCOL_NONE" else @@ -47,7 +57,9 @@ end if tup.getconfig("UART_PROTOCOL") == "native" then FLAGS += "-DUART_PROTOCOL_NATIVE" elseif tup.getconfig("UART_PROTOCOL") == "ascii" or tup.getconfig("UART_PROTOCOL") == "" then - FLAGS += "-DUART_PROTOCOL_LEGACY" + FLAGS += "-DUART_PROTOCOL_ASCII" +elseif tup.getconfig("UART_PROTOCOL") == "stdout" then + FLAGS += "-DUART_PROTOCOL_STDOUT" elseif tup.getconfig("UART_PROTOCOL") == "none" then FLAGS += "-DUART_PROTOCOL_NONE" else @@ -63,6 +75,11 @@ if tup.getconfig("STEP_DIR") == "y" then end end +-- Compiler settings +if tup.getconfig("STRICT") == "true" then + FLAGS += '-Werror' +end + -- C-specific flags FLAGS += '-D__weak="__attribute__((weak))"' @@ -74,7 +91,7 @@ FLAGS += '-mthumb' FLAGS += '-mcpu=cortex-m4' FLAGS += '-mfpu=fpv4-sp-d16' FLAGS += '-mfloat-abi=hard' -FLAGS += { '-Wall', '-fdata-sections', '-ffunction-sections'} +FLAGS += { '-Wall', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} -- debug build FLAGS += '-g -gdwarf-2' @@ -90,7 +107,7 @@ LDFLAGS += '-Wl,--undefined=uxTopUsedPriority' -- common flags for ASM, C and C++ OPT += '-Og' -OPT += '-ffast-math' +OPT += '-ffast-math -fno-finite-math-only' tup.append_table(FLAGS, OPT) tup.append_table(LDFLAGS, OPT) @@ -108,7 +125,7 @@ for src in string.gmatch(vars['C_INCLUDES'] or '', "%S+") do end -- TODO: cleaner separation of the platform code and the rest -stm_includes += 'MotorControl' +stm_includes += '.' stm_includes += 'Drivers/DRV8301' stm_sources += boarddir..'/Src/syscalls.c' build{ @@ -121,7 +138,7 @@ build{ } tup.frule{ - command='bash dump_version.sh %o', + command='python ../tools/odrive/version.py --output %o', outputs={'build/version.h'} } @@ -133,17 +150,24 @@ build{ sources={ 'Drivers/DRV8301/drv8301.c', 'MotorControl/utils.c', - 'MotorControl/legacy_commands.c', - 'MotorControl/low_level.c', + 'MotorControl/low_level.cpp', 'MotorControl/nvm.c', 'MotorControl/axis.cpp', - 'MotorControl/commands.cpp', - 'MotorControl/protocol.cpp', - 'MotorControl/config.cpp', + 'MotorControl/motor.cpp', + 'MotorControl/encoder.cpp', + 'MotorControl/controller.cpp', + 'MotorControl/sensorless_estimator.cpp', + 'MotorControl/main.cpp', + 'communication/communication.cpp', + 'communication/ascii_protocol.cpp', + 'communication/protocol.cpp', + 'communication/interface_uart.cpp', + 'communication/interface_usb.cpp', 'FreeRTOS-openocd.c' }, includes={ 'Drivers/DRV8301', - 'MotorControl' + 'MotorControl', + '.' } } diff --git a/Firmware/ascii-protocol.md b/Firmware/ascii-protocol.md new file mode 100644 index 00000000..f7d89235 --- /dev/null +++ b/Firmware/ascii-protocol.md @@ -0,0 +1,52 @@ + +## How to send commands + + * **Via USB:** + * **Windows:** Use the Zadig utility to set the ODrive's driver to "usbser". Windows will then make the device available as COM port. You can use [PuTTY](https://www.chiark.greenend.org.uk/~sgtatham/putty/) to manually send commands or open the COM port using your favorite programming language + * **Linux/macOS:** Run `/dev/tty*` to list all serial ports. The ODrive will show up as `/dev/ttyACM0` on Linux and `/dev/tty.usbmodem[...]` on macOS. Once you know the name, you can use `screen /dev/ttyACM0` (with the correct name) to send commands manually or open the device using your favorite programming language. Serial ports on Unix can be opened, written to and read from like a normal file. + * **Via UART:** Connect the ODrive's TX (GPIO1) to your host's RX. Connect your ODrive's RX (GPIO2) to your host's TX. The logic level of the ODrive is 3.3V. + * **Arduino:** You can use the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) to talk to the ODrive. + * **Windows/Linux/macOS:** You can use an FTDI USB-UART cable to connect to the ODrive. + + +## Command Reference + +#### Motor Position command +``` +p motor position velocity_ff current_ff +``` +* `p` for position +* `motor` is the motor number, `0` or `1`. +* `position` is the desired position, in encoder counts. +* `velocity_ff` is the velocity feed-forward term, in counts/s (optional). +* `current_ff` is the current feed-forward term, in A (optional). + +Example: `p 0 -20000 0 0` + +Note that if you don't know what feed-forward is or what it's used for, simply omit it. + + +#### Motor Velocity command +``` +v motor velocity current_ff +``` +* `v` for velocity +* `motor` is the motor number, `0` or `1`. +* `velocity` is the desired velocity in counts/s. +* `current_ff` is the current feed-forward term, in A (optional). + +Example: `v 0 1000 0` + +Note that if you don't know what feed-forward is or what it's used for, simply omit it. + +#### Motor Current command +``` +c motor current +``` +* `c` for current +* `motor` is the motor number, `0` or `1`. +* `current` is the desired current in A. + +#### Parameter reading/writing + +This is currently not supported. Use the native protocol. diff --git a/Firmware/build.lua b/Firmware/build.lua index 898b357f..8ef67627 100644 --- a/Firmware/build.lua +++ b/Firmware/build.lua @@ -67,7 +67,7 @@ function GCCToolchain(prefix, builddir, compiler_flags, linker_flags) else extra_outputs = {} end - if src == 'MotorControl/commands.cpp' then extra_inputs = 'build/version.h' end -- TODO: fix hack + if src == 'communication/communication.cpp' then extra_inputs = 'build/version.h' end -- TODO: fix hack tup.frule{ inputs= { src, extra_inputs=extra_inputs }, command=compiler..' -c %f '.. diff --git a/Firmware/build.sh b/Firmware/build.sh index 811a4ab7..1622fa69 100755 --- a/Firmware/build.sh +++ b/Firmware/build.sh @@ -8,6 +8,9 @@ set -euo pipefail THIS_DIR="$(dirname "$0")" cd "$THIS_DIR" +# Treat warnings as errors +export CONFIG_STRICT=true + # Write all environment variables that start with "CONFIG_" to tup.config rm -rdf build mkdir -p build diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp new file mode 100644 index 00000000..98f1cd02 --- /dev/null +++ b/Firmware/communication/ascii_protocol.cpp @@ -0,0 +1,206 @@ +/* +* The ASCII protocol is a simpler, human readable alternative to the main native +* protocol. +* In the future this protocol might be extended to support selected GCode commands. +* For a list of supported commands see doc/ascii-protocol.md +*/ + +/* Includes ------------------------------------------------------------------*/ + +#include "odrive_main.h" +#include "communication.h" +#include "ascii_protocol.h" +#include + +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ +/* Private constant data -----------------------------------------------------*/ + +#define MAX_LINE_LENGTH 256 +#define TO_STR_INNER(s) #s +#define TO_STR(s) TO_STR_INNER(s) + +/* Private variables ---------------------------------------------------------*/ +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +// @brief Sends a line on the specified output. +template +void respond(StreamSink& output, bool include_checksum, const char * fmt, TArgs&& ... args) { + char response[64]; + size_t len = snprintf(response, sizeof(response), fmt, std::forward(args)...); + output.process_bytes((uint8_t*)response, len); + if (include_checksum) { + uint8_t checksum = 0; + for (size_t i = 0; i < len; ++i) + checksum ^= response[i]; + len = snprintf(response, sizeof(response), "*%u", checksum); + output.process_bytes((uint8_t*)response, len); + } + output.process_bytes((const uint8_t*)"\r\n", 2); +} + + +// @brief Executes an ASCII protocol command +// @param buffer buffer of ASCII encoded characters +// @param len size of the buffer +void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& response_channel) { + static_assert(sizeof(char) == sizeof(uint8_t)); + + // scan line to find beginning of checksum and prune comment + uint8_t checksum = 0; + size_t checksum_start = SIZE_MAX; + for (size_t i = 0; i < len; ++i) { + if (buffer[i] == ';') { // ';' is the comment start char + len = i; + break; + } + if (checksum_start > i) { + if (buffer[i] == '*') { + checksum_start = i + 1; + } else { + checksum ^= buffer[i]; + } + } + } + + // copy everything into a local buffer so we can insert null-termination + char cmd[MAX_LINE_LENGTH + 1]; + if (len > MAX_LINE_LENGTH) len = MAX_LINE_LENGTH; + memcpy(cmd, buffer, len); + + // optional checksum validation + bool use_checksum = (checksum_start < len); + if (use_checksum) { + unsigned int received_checksum; + sscanf((const char *)cmd + checksum_start, "%u", &received_checksum); + if (received_checksum != checksum) + return; + len = checksum_start - 1; // prune checksum and asterisk + } + + cmd[len] = 0; // null-terminate + + // check incoming packet type + if (cmd[0] == 'p') { // position control + unsigned motor_number; + float pos_setpoint, vel_feed_forward, current_feed_forward; + int numscan = sscanf(cmd, "p %u %f %f %f", &motor_number, &pos_setpoint, &vel_feed_forward, ¤t_feed_forward); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + if (numscan < 3) + vel_feed_forward = 0.0f; + if (numscan < 4) + current_feed_forward = 0.0f; + axes[motor_number]->controller_.set_pos_setpoint(pos_setpoint, vel_feed_forward, current_feed_forward); + } + + } else if (cmd[0] == 'v') { // velocity control + unsigned motor_number; + float vel_setpoint, current_feed_forward; + int numscan = sscanf(cmd, "v %u %f %f", &motor_number, &vel_setpoint, ¤t_feed_forward); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + if (numscan < 3) + current_feed_forward = 0.0f; + axes[motor_number]->controller_.set_vel_setpoint(vel_setpoint, current_feed_forward); + } + + } else if (cmd[0] == 'c') { // current control + unsigned motor_number; + float current_setpoint; + int numscan = sscanf(cmd, "c %u %f", &motor_number, ¤t_setpoint); + if (numscan < 2) { + respond(response_channel, use_checksum, "invalid command format"); + } else if (motor_number >= AXIS_COUNT) { + respond(response_channel, use_checksum, "invalid motor %u", motor_number); + } else { + axes[motor_number]->controller_.set_current_setpoint(current_setpoint); + respond(response_channel, use_checksum, "ok", motor_number); + } + + } else if (cmd[0] == 'i'){ // Dump device info + respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature()); + respond(response_channel, use_checksum, "Revision: %#x", STM_ID_GetRevision()); + respond(response_channel, use_checksum, "Flash Size: %#x KiB", STM_ID_GetFlashSize()); + respond(response_channel, use_checksum, "Serial number: %s", serial_number_str); + + } else if (cmd[0] == 'r') { // read property + char name[MAX_LINE_LENGTH]; + int numscan = sscanf(cmd, "r %" TO_STR(MAX_LINE_LENGTH) "s", name); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + char response[10]; + bool success = endpoint->get_string(response, sizeof(response)); + if (!success) + respond(response_channel, use_checksum, "not implemented"); + else + respond(response_channel, use_checksum, response); + } + } + } else if (cmd[0] == 'w') { // write property + char name[MAX_LINE_LENGTH]; + char value[MAX_LINE_LENGTH]; + int numscan = sscanf(cmd, "w %" TO_STR(MAX_LINE_LENGTH) "s %" TO_STR(MAX_LINE_LENGTH) "s", name, value); + if (numscan < 1) { + respond(response_channel, use_checksum, "invalid command format"); + } else { + Endpoint* endpoint = application_endpoints->get_by_name(name, sizeof(name)); + if (!endpoint) { + respond(response_channel, use_checksum, "invalid property"); + } else { + bool success = endpoint->set_string(value, sizeof(value)); + if (!success) + respond(response_channel, use_checksum, "not implemented"); + } + } + + } else if (cmd[0] == 'h') { // HALT + for(size_t i = 0; i < AXIS_COUNT; i++){ + axes[i]->controller_.set_vel_setpoint(0.0f, 0.0f); + } + } else if (cmd[0] != 0) { + respond(response_channel, use_checksum, "unknown command"); + } +} + +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel) { + static uint8_t parse_buffer[MAX_LINE_LENGTH]; + static bool read_active = true; + static uint32_t parse_buffer_idx = 0; + + while (len--) { + // if the line becomes too long, reset buffer and wait for the next line + if (parse_buffer_idx >= MAX_LINE_LENGTH) { + read_active = false; + parse_buffer_idx = 0; + } + + // Fetch the next char + uint8_t c = *(buffer++); + bool is_end_of_line = (c == '\r' || c == '\n' || c == '!'); + if (is_end_of_line) { + if (read_active) + ASCII_protocol_process_line(parse_buffer, parse_buffer_idx, response_channel); + parse_buffer_idx = 0; + read_active = true; + } else { + if (read_active) { + parse_buffer[parse_buffer_idx++] = c; + } + } + } +} diff --git a/Firmware/MotorControl/legacy_commands.h b/Firmware/communication/ascii_protocol.h similarity index 51% rename from Firmware/MotorControl/legacy_commands.h rename to Firmware/communication/ascii_protocol.h index bb945dab..82830e10 100644 --- a/Firmware/MotorControl/legacy_commands.h +++ b/Firmware/communication/ascii_protocol.h @@ -1,37 +1,29 @@ -#ifndef LEGACY_COMMANDS_H -#define LEGACY_COMMANDS_H +#ifndef __ASCII_PROTOCOL_H +#define __ASCII_PROTOCOL_H #ifdef __cplusplus extern "C" { #endif /* Includes ------------------------------------------------------------------*/ -#include "low_level.h" + +#include "protocol.hpp" + +#include +#include +#include + /* Exported types ------------------------------------------------------------*/ - -typedef enum { - SERIAL_PRINTF_IS_NONE, - SERIAL_PRINTF_IS_USB, - SERIAL_PRINTF_IS_UART, -} SerialPrintf_t; - /* Exported constants --------------------------------------------------------*/ /* Exported variables --------------------------------------------------------*/ -extern SerialPrintf_t serial_printf_select; -// Exposed comms table during refactor transition -extern float* exposed_floats[]; -extern int* exposed_ints[]; -extern bool* exposed_bools[]; -extern uint16_t* exposed_uint16[]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ -void legacy_parse_cmd(const uint8_t* buffer, size_t len, size_t buffer_length, SerialPrintf_t response_interface); -void legacy_parse_stream(const uint8_t* buffer, size_t len); +void ASCII_protocol_parse_stream(const uint8_t* buffer, size_t len, StreamSink& response_channel); #ifdef __cplusplus } #endif -#endif /* LEGACY_COMMANDS_H */ +#endif /* __ASCII_PROTOCOL_H */ diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp new file mode 100644 index 00000000..1c4accfd --- /dev/null +++ b/Firmware/communication/communication.cpp @@ -0,0 +1,191 @@ + +/* Includes ------------------------------------------------------------------*/ + +#include "communication.h" + +#include "interface_usb.h" +#include "interface_uart.h" + +#include "odrive_main.h" +#include "protocol.hpp" +#include "freertos_vars.h" +#include "utils.h" + +#include "../build/version.h" // autogenerated based on Git state + +#include +#include +//#include +//#include +//#include +//#include + +#include + +/* Private defines -----------------------------------------------------------*/ +/* Private macros ------------------------------------------------------------*/ +/* Private typedef -----------------------------------------------------------*/ +/* Global constant data ------------------------------------------------------*/ +/* Global variables ----------------------------------------------------------*/ + +uint64_t serial_number; +char serial_number_str[13]; // 12 digits + null termination + +/* Private constant data -----------------------------------------------------*/ +/* Private variables ---------------------------------------------------------*/ + +#if HW_VERSION_MAJOR == 3 +// Determine start address of the OTP struct: +// The OTP is organized into 16-byte blocks. +// If the first block starts with "0xfe" we use the first block. +// If the first block starts with "0x00" and the second block starts with "0xfe", +// we use the second block. This gives the user the chance to screw up once. +// If none of the above is the case, we consider the OTP invalid (otp_ptr will be NULL). +const uint8_t* otp_ptr = + (*(uint8_t*)FLASH_OTP_BASE == 0xfe) ? (uint8_t*)FLASH_OTP_BASE : + (*(uint8_t*)FLASH_OTP_BASE != 0x00) ? NULL : + (*(uint8_t*)(FLASH_OTP_BASE + 0x10) != 0xfe) ? NULL : + (uint8_t*)(FLASH_OTP_BASE + 0x10); + +// Read hardware version from OTP if available, otherwise fall back +// to software defined version. +const uint8_t hw_version_major = otp_ptr ? otp_ptr[3] : HW_VERSION_MAJOR; +const uint8_t hw_version_minor = otp_ptr ? otp_ptr[4] : HW_VERSION_MINOR; +const uint8_t hw_version_variant = otp_ptr ? otp_ptr[5] : HW_VERSION_VOLTAGE; +#else +#error "not implemented" +#endif + +// the corresponding macros are defined in the autogenerated version.h +const uint8_t fw_version_major = FW_VERSION_MAJOR; +const uint8_t fw_version_minor = FW_VERSION_MINOR; +const uint8_t fw_version_revision = FW_VERSION_REVISION; +const uint8_t fw_version_unreleased = FW_VERSION_UNRELEASED; // 0 for official releases, 1 otherwise + +osThreadId comm_thread; + +static uint32_t test_property = 0; + +/* Private function prototypes -----------------------------------------------*/ +/* Function implementations --------------------------------------------------*/ + +void init_communication(void) { + printf("hi!\r\n"); + + // Start command handling thread + osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 5000 /* in 32-bit words */); // TODO: fix stack issues + comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL); +} + + +float oscilloscope[OSCILLOSCOPE_SIZE] = {0}; +size_t oscilloscope_pos = 0; + + +// Helper class because the protocol library doesn't yet +// support non-member functions +// TODO: make this go away +class StaticFunctions { +public: + void save_configuration_helper() { save_configuration(); } + void erase_configuration_helper() { erase_configuration(); } + void NVIC_SystemReset_helper() { NVIC_SystemReset(); } + void enter_dfu_mode_helper() { enter_dfu_mode(); } + float get_oscilloscope_val(uint32_t index) { return oscilloscope[index]; } + int32_t test_function(int32_t delta) { static int cnt = 0; return cnt += delta; } +} static_functions; + +// When adding new functions/variables to the protocol, be careful not to +// blow the communication stack. You can check comm_stack_info to see +// how much headroom you have. +static inline auto make_obj_tree() { + return make_protocol_member_list( + make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("serial_number", &serial_number), + make_protocol_ro_property("hw_version_major", &hw_version_major), + make_protocol_ro_property("hw_version_minor", &hw_version_minor), + make_protocol_ro_property("hw_version_variant", &hw_version_variant), + make_protocol_ro_property("fw_version_major", &fw_version_major), + make_protocol_ro_property("fw_version_minor", &fw_version_minor), + make_protocol_ro_property("fw_version_revision", &fw_version_revision), + make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased), + make_protocol_ro_property("user_config_loaded", const_cast(&user_config_loaded_)), + make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed_), + make_protocol_object("system_stats", + make_protocol_ro_property("uptime", &system_stats_.uptime), + make_protocol_ro_property("min_heap_space", &system_stats_.min_heap_space), + make_protocol_ro_property("min_stack_space_axis0", &system_stats_.min_stack_space_axis0), + make_protocol_ro_property("min_stack_space_axis1", &system_stats_.min_stack_space_axis1), + make_protocol_ro_property("min_stack_space_comms", &system_stats_.min_stack_space_comms), + make_protocol_ro_property("min_stack_space_usb", &system_stats_.min_stack_space_usb), + make_protocol_ro_property("min_stack_space_uart", &system_stats_.min_stack_space_uart), + make_protocol_ro_property("min_stack_space_usb_irq", &system_stats_.min_stack_space_usb_irq), + make_protocol_ro_property("min_stack_space_startup", &system_stats_.min_stack_space_startup), + make_protocol_object("usb", + make_protocol_ro_property("rx_cnt", &usb_stats_.rx_cnt), + make_protocol_ro_property("tx_cnt", &usb_stats_.tx_cnt), + make_protocol_ro_property("tx_overrun_cnt", &usb_stats_.tx_overrun_cnt) + ) + ), + make_protocol_object("config", + make_protocol_property("brake_resistance", &board_config.brake_resistance), + // TODO: changing this currently requires a reboot - fix this + make_protocol_property("enable_uart", &board_config.enable_uart), + make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level), + make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level) + ), + make_protocol_object("axis0", axes[0]->make_protocol_definitions()), + make_protocol_object("axis1", axes[1]->make_protocol_definitions()), + make_protocol_property("test_property", &test_property), + make_protocol_function("test_function", static_functions, &StaticFunctions::test_function, "delta"), + make_protocol_function("get_oscilloscope_val", static_functions, &StaticFunctions::get_oscilloscope_val, "index"), + make_protocol_function("save_configuration", static_functions, &StaticFunctions::save_configuration_helper), + make_protocol_function("erase_configuration", static_functions, &StaticFunctions::erase_configuration_helper), + make_protocol_function("reboot", static_functions, &StaticFunctions::NVIC_SystemReset_helper), + make_protocol_function("enter_dfu_mode", static_functions, &StaticFunctions::enter_dfu_mode_helper) + ); +} + +using tree_type = decltype(make_obj_tree()); +uint8_t tree_buffer[sizeof(tree_type)]; + +// the protocol has one additional built-in endpoint +constexpr size_t MAX_ENDPOINTS = decltype(make_obj_tree())::endpoint_count + 1; +Endpoint* endpoints_[MAX_ENDPOINTS] = { 0 }; +const size_t max_endpoints_ = MAX_ENDPOINTS; +size_t n_endpoints_ = 0; + +// Thread to handle deffered processing of USB interrupt, and +// read commands out of the UART DMA circular buffer +void communication_task(void * ctx) { + (void) ctx; // unused parameter + + // TODO: this is supposed to use the move constructor, but currently + // the compiler uses the copy-constructor instead. Thus the make_obj_tree + // ends up with a stupid stack size of around 8000 bytes. Fix this. + auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); + auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); + set_application_endpoints(&endpoint_provider); + + serve_on_uart(); + serve_on_usb(); + + for (;;) { + osDelay(1000); // nothing to do + } +} + +extern "C" { +int _write(int file, const char* data, int len); +} + +// @brief This is what printf calls internally +int _write(int file, const char* data, int len) { +#ifdef USB_PROTOCOL_STDOUT + usb_stream_output.process_bytes((const uint8_t *)data, len); +#endif +#ifdef UART_PROTOCOL_STDOUT + uart4_stream_output.process_bytes((const uint8_t *)data, len); +#endif + return len; +} diff --git a/Firmware/MotorControl/commands.h b/Firmware/communication/communication.h similarity index 55% rename from Firmware/MotorControl/commands.h rename to Firmware/communication/communication.h index f7a7fe19..03da784b 100644 --- a/Firmware/MotorControl/commands.h +++ b/Firmware/communication/communication.h @@ -13,13 +13,16 @@ extern "C" { #endif -void init_communication(void); -void communication_task(void const * argument); -void set_cmd_buffer(uint8_t *buf, uint32_t len); -void usb_update_thread(); -void USB_receive_packet(const uint8_t *buffer, size_t length); +#include -extern uint64_t serial_number; +extern osThreadId comm_thread; + +extern const uint8_t hw_version_major; +extern const uint8_t hw_version_minor; +extern const uint8_t hw_version_variant; + +void init_communication(void); +void communication_task(void * ctx); #ifdef __cplusplus } diff --git a/Firmware/MotorControl/crc.hpp b/Firmware/communication/crc.hpp similarity index 100% rename from Firmware/MotorControl/crc.hpp rename to Firmware/communication/crc.hpp diff --git a/Firmware/communication/interface_uart.cpp b/Firmware/communication/interface_uart.cpp new file mode 100644 index 00000000..90d976aa --- /dev/null +++ b/Firmware/communication/interface_uart.cpp @@ -0,0 +1,103 @@ + +#include "interface_uart.h" +#include "protocol.hpp" + +#include "ascii_protocol.h" + +#include + +#include +#include +#include + +#define UART_TX_BUFFER_SIZE 64 +#define UART_RX_BUFFER_SIZE 64 + +// DMA open loop continous circular buffer +// 1ms delay periodic, chase DMA ptr around +static uint8_t dma_rx_buffer[UART_RX_BUFFER_SIZE]; +static uint32_t dma_last_rcv_idx; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + +osThreadId uart_thread; + + +class UART4Sender : public StreamSink { +public: + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < UART_TX_BUFFER_SIZE ? length : UART_TX_BUFFER_SIZE; + // wait for USB interface to become ready + // TODO: implement ring buffer to get a more continuous stream of data + if (osSemaphoreWait(sem_uart_dma, deadline_to_timeout(deadline_ms)) != osOK) + return -1; + // transmit chunk + memcpy(tx_buf_, buffer, chunk); + if (HAL_UART_Transmit_DMA(&huart4, tx_buf_, chunk) != HAL_OK) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } +private: + uint8_t tx_buf_[UART_TX_BUFFER_SIZE]; +} uart4_stream_output; + +PacketToStreamConverter uart4_packet_output(uart4_stream_output); +BidirectionalPacketBasedChannel uart4_channel(uart4_packet_output); +StreamToPacketConverter uart4_stream_input(uart4_channel); + +static void uart_server_thread(void * ctx) { + (void) ctx; + + for (;;) { + // Check for UART errors and restart recieve DMA transfer if required + if (huart4.ErrorCode != HAL_UART_ERROR_NONE) { + HAL_UART_AbortReceive(&huart4); + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); + } + // Fetch the circular buffer "write pointer", where it would write next + uint32_t new_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); + // Process bytes in one or two chunks (two in case there was a wrap) + if (new_rcv_idx < dma_last_rcv_idx) { + uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, + UART_RX_BUFFER_SIZE - dma_last_rcv_idx); + ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx, + UART_RX_BUFFER_SIZE - dma_last_rcv_idx, uart4_stream_output); + dma_last_rcv_idx = 0; + } + if (new_rcv_idx > dma_last_rcv_idx) { + uart4_stream_input.process_bytes(dma_rx_buffer + dma_last_rcv_idx, + new_rcv_idx - dma_last_rcv_idx); + ASCII_protocol_parse_stream(dma_rx_buffer + dma_last_rcv_idx, + new_rcv_idx - dma_last_rcv_idx, uart4_stream_output); + dma_last_rcv_idx = new_rcv_idx; + } + + osDelay(1); + }; +} + +void serve_on_uart() { + // DMA is set up to recieve in a circular buffer forever. + // We dont use interrupts to fetch the data, instead we periodically read + // data out of the circular buffer into a parse buffer, controlled by a state machine + HAL_UART_Receive_DMA(&huart4, dma_rx_buffer, sizeof(dma_rx_buffer)); + dma_last_rcv_idx = UART_RX_BUFFER_SIZE - huart4.hdmarx->Instance->NDTR; + + // Start UART communication thread + osThreadDef(uart_server_thread_def, uart_server_thread, osPriorityNormal, 0, 512); + uart_thread = osThreadCreate(osThread(uart_server_thread_def), NULL); +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef* huart) { + osSemaphoreRelease(sem_uart_dma); +} diff --git a/Firmware/communication/interface_uart.h b/Firmware/communication/interface_uart.h new file mode 100644 index 00000000..b5f1ed72 --- /dev/null +++ b/Firmware/communication/interface_uart.h @@ -0,0 +1,18 @@ +#ifndef __INTERFACE_UART_HPP +#define __INTERFACE_UART_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +extern osThreadId uart_thread; + +void serve_on_uart(void); + +#ifdef __cplusplus +} +#endif + +#endif // __INTERFACE_UART_HPP diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp new file mode 100644 index 00000000..cee94a99 --- /dev/null +++ b/Firmware/communication/interface_usb.cpp @@ -0,0 +1,116 @@ + +#include "interface_usb.h" +#include "protocol.hpp" + +#include + +#include +#include +#include +#include +#include + +static uint8_t* usb_buf; +static uint32_t usb_len; + +// FIXME: the stdlib doesn't know about CMSIS threads, so this is just a global variable +static thread_local uint32_t deadline_ms = 0; + +osThreadId usb_thread; + +USBStats_t usb_stats_ = {0}; + +class USBSender : public PacketSink { +public: + int process_packet(const uint8_t* buffer, size_t length) { + // cannot send partial packets + if (length > USB_TX_DATA_SIZE) + return -1; + // wait for USB interface to become ready + if (osSemaphoreWait(sem_usb_tx, deadline_to_timeout(deadline_ms)) != osOK) { + // If the host resets the device it might be that the TX-complete handler is never called + // and the sem_usb_tx semaphore is never released. To handle this we just override the + // TX buffer if this wait times out. The implication is that the channel is no longer lossless. + // TODO: handle endpoint reset properly + usb_stats_.tx_overrun_cnt++; + } + // transmit packet + uint8_t status = CDC_Transmit_FS( + const_cast(buffer) /* casting this const away is safe because... + well... it's not actually. Stupid STM. */, length); + if (status != USBD_OK) { + osSemaphoreRelease(sem_usb_tx); + return -1; + } + usb_stats_.tx_cnt = 0; + return 0; + } +} usb_packet_output; + +#if !defined(USB_PROTOCOL_NATIVE) +class TreatPacketSinkAsStreamSink : public StreamSink { +public: + TreatPacketSinkAsStreamSink(PacketSink& output) : output_(output) {} + int process_bytes(const uint8_t* buffer, size_t length) { + // Loop to ensure all bytes get sent + while (length) { + size_t chunk = length < USB_TX_DATA_SIZE ? length : USB_TX_DATA_SIZE; + if (output_.process_packet(buffer, length) != 0) + return -1; + buffer += chunk; + length -= chunk; + } + return 0; + } + size_t get_free_space() { return SIZE_MAX; } +private: + PacketSink& output_; +} usb_stream_output(usb_packet_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE) +BidirectionalPacketBasedChannel usb_channel(usb_packet_output); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +PacketToStreamConverter usb_packetized_output(usb_stream_output); +BidirectionalPacketBasedChannel usb_channel(usb_packetized_output); +#endif + +#if defined(USB_PROTOCOL_NATIVE_STREAM_BASED) +StreamToPacketConverter usb_native_stream_input(usb_channel); +#endif + + +static void usb_server_thread(void * ctx) { + (void) ctx; + + for (;;) { + const uint32_t usb_check_timeout = 1; // ms + osStatus sem_stat = osSemaphoreWait(sem_usb_rx, usb_check_timeout); + if (sem_stat == osOK) { + usb_stats_.rx_cnt++; + deadline_ms = timeout_to_deadline(PROTOCOL_SERVER_TIMEOUT_MS); +#if defined(USB_PROTOCOL_NATIVE) + usb_channel.process_packet(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_NATIVE_STREAM_BASED) + usb_native_stream_input.process_bytes(usb_buf, usb_len); +#elif defined(USB_PROTOCOL_ASCII) + ASCII_protocol_parse_stream(usb_buf, usb_len, usb_stream_output); +#endif + USBD_CDC_ReceivePacket(&hUsbDeviceFS); // Allow next packet + } + } +} + +// Called from CDC_Receive_FS callback function, this allows the communication +// thread to handle the incoming data +void usb_process_packet(uint8_t *buf, uint32_t len) { + usb_buf = buf; + usb_len = len; + osSemaphoreRelease(sem_usb_rx); +} + +void serve_on_usb() { + // Start USB communication thread + osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512); + usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL); +} diff --git a/Firmware/communication/interface_usb.h b/Firmware/communication/interface_usb.h new file mode 100644 index 00000000..a56bca36 --- /dev/null +++ b/Firmware/communication/interface_usb.h @@ -0,0 +1,28 @@ +#ifndef __INTERFACE_USB_HPP +#define __INTERFACE_USB_HPP + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +extern osThreadId usb_thread; + +typedef struct { + uint32_t rx_cnt; + uint32_t tx_cnt; + uint32_t tx_overrun_cnt; +} USBStats_t; + +extern USBStats_t usb_stats_; + +void usb_process_packet(uint8_t *buf, uint32_t len); +void serve_on_usb(void); + +#ifdef __cplusplus +} +#endif + +#endif // __INTERFACE_USB_HPP diff --git a/Firmware/MotorControl/protocol.cpp b/Firmware/communication/protocol.cpp similarity index 72% rename from Firmware/MotorControl/protocol.cpp rename to Firmware/communication/protocol.cpp index 56f9afb5..50e38fab 100644 --- a/Firmware/MotorControl/protocol.cpp +++ b/Firmware/communication/protocol.cpp @@ -1,23 +1,14 @@ /* Includes ------------------------------------------------------------------*/ -#include "low_level.h" +//#include "low_level.h" #include "protocol.hpp" #include #include /* Private defines -----------------------------------------------------------*/ -// Note that this option cannot be used to debug UART because it prints on UART -//#define DEGUG_PROTOCOL /* Private macros ------------------------------------------------------------*/ - -#ifdef DEGUG_PROTOCOL -#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0) -#else -#define LOG_PROTO(...) ((void) 0) -#endif - /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ /* Global variables ----------------------------------------------------------*/ @@ -46,48 +37,6 @@ void hexdump(const uint8_t* buf, size_t len) { } #endif -static inline int write_string(const char* str, StreamSink* output) { - return output->process_bytes(reinterpret_cast(str), strlen(str)); -} - -void Endpoint::write_json(size_t id, bool* need_comma, StreamSink* output) const { - if (type_ == CLOSE_TREE) { - write_string("]}", output); - *need_comma = true; - } else { - if (*need_comma) - write_string(",", output); - - // write name - write_string("{\"name\":\"", output); - if (name_) - write_string(name_, output); - - // write endpoint ID - write_string("\",\"id\":", output); - char id_buf[10]; - snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf - write_string(id_buf, output); - - // write additional JSON data - if (json_modifier_ && json_modifier_[0]) { - write_string(",", output); - write_string(json_modifier_, output); - } - - if (type_ == BEGIN_OBJECT) { - write_string(",\"members\":[", output); - *need_comma = false; - } else if (type_ == BEGIN_FUNCTION) { - write_string(",\"arguments\":[", output); - *need_comma = false; - } else if (type_ == PROPERTY) { - write_string("}", output); - *need_comma = true; - } - } -} - int StreamToPacketConverter::process_bytes(const uint8_t *buffer, size_t length) { @@ -157,36 +106,75 @@ int PacketToStreamConverter::process_packet(const uint8_t *buffer, size_t length } -// Calculates the CRC16 of the JSON interface descriptor. -// The init value is the protocol version. -uint16_t BidirectionalPacketBasedChannel::calculate_json_crc16(void) { - CRC16Calculator crc16_calculator(PROTOCOL_VERSION); +class JSONDescriptorEndpoint : Endpoint { +public: + static constexpr size_t endpoint_count = 1; + void write_json(size_t id, StreamSink* output); + void register_endpoints(Endpoint** list, size_t id, size_t length); + void handle(const uint8_t* input, size_t input_length, StreamSink* output); +}; - uint8_t offset[4] = { 0 }; - interface_query(offset, sizeof(offset), &crc16_calculator); +JSONDescriptorEndpoint json_file_endpoint = JSONDescriptorEndpoint(); +EndpointProvider* application_endpoints; +uint16_t json_crc_; - return crc16_calculator.get_crc16(); +void JSONDescriptorEndpoint::write_json(size_t id, StreamSink* output) { + write_string("{\"name\":\"\",", output); + + // write endpoint ID + write_string("\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + write_string(",\"type\":\"json\",\"access\":\"r\"}", output); } +void JSONDescriptorEndpoint::register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + +}; + // Returns part of the JSON interface definition. -void BidirectionalPacketBasedChannel::interface_query(const uint8_t* input, size_t input_length, StreamSink* output) { +void JSONDescriptorEndpoint::handle(const uint8_t* input, size_t input_length, StreamSink* output) { // The request must contain a 32 bit integer to specify an offset if (input_length < 4) return; uint32_t offset = 0; read_le(&offset, input); NullStreamSink output_with_offset = NullStreamSink(offset, *output); - - bool need_comma = false; + + size_t id = 0; write_string("[", &output_with_offset); - for (size_t i = 0; i < n_endpoints_; ++i) { - get_endpoint(i)->write_json(i, &need_comma, &output_with_offset); - if (!output->get_free_space()) - return; // return early if the output cannot take more bytes - } + json_file_endpoint.write_json(id, &output_with_offset); + id += decltype(json_file_endpoint)::endpoint_count; + write_string(",", &output_with_offset); + application_endpoints->write_json(id, &output_with_offset); write_string("]", &output_with_offset); } +void set_application_endpoints(EndpointProvider* endpoints) { + application_endpoints = endpoints; + + n_endpoints_ = 0; + json_file_endpoint.register_endpoints(endpoints_, 0, max_endpoints_); + n_endpoints_ += decltype(json_file_endpoint)::endpoint_count; + application_endpoints->register_endpoints(endpoints_, n_endpoints_, max_endpoints_); + n_endpoints_ += application_endpoints->get_endpoint_count(); + + // Calculates the CRC16 of the JSON file. + // The init value is the protocol version. + CRC16Calculator crc16_calculator(PROTOCOL_VERSION); + uint8_t offset[4] = { 0 }; + json_file_endpoint.handle(offset, sizeof(offset), &crc16_calculator); + json_crc_ = crc16_calculator.get_crc16(); + + CRC16Calculator crc16_calculator2(PROTOCOL_VERSION); + endpoints_[0]->handle(offset, sizeof(offset), &crc16_calculator2); + json_crc_ = crc16_calculator2.get_crc16(); +} + int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_t length) { LOG_PROTO("got packet of length %d: \r\n", length); hexdump(buffer, length); @@ -205,10 +193,15 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_ bool expect_response = endpoint_id & 0x8000; endpoint_id &= 0x7fff; - const Endpoint* endpoint = get_endpoint(endpoint_id); - if (!endpoint) + if (endpoint_id >= n_endpoints_) return -1; + Endpoint* endpoint = endpoints_[endpoint_id]; + if (!endpoint) { + LOG_PROTO("critical: no endpoint at %d", endpoint_id); + return -1; + } + // Verify packet trailer. The expected trailer value depends on the selected endpoint. // For endpoint 0 this is just the protocol version, for all other endpoints it's a // CRC over the entire JSON descriptor tree (this may change in future versions). @@ -218,7 +211,7 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_ LOG_PROTO("trailer mismatch for endpoint %d: expected %04x, got %04x\r\n", endpoint_id, expected_trailer, actual_trailer); return -1; } - LOG_PROTO("trailer ok\r\n"); + LOG_PROTO("trailer ok for endpoint %d\r\n", endpoint_id); // TODO: if more bytes than the MTU were requested, should we abort or just return as much as possible? diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp new file mode 100644 index 00000000..87ef82cc --- /dev/null +++ b/Firmware/communication/protocol.hpp @@ -0,0 +1,927 @@ +/* +see protocol.md for the protocol specification +*/ + +#ifndef __PROTOCOL_HPP +#define __PROTOCOL_HPP + +// TODO: resolve assert +#define assert(expr) + +#include +#include +#include +#include "crc.hpp" + +// Note that this option cannot be used to debug UART because it prints on UART +//#define DEBUG_PROTOCOL +#ifdef DEBUG_PROTOCOL +#define LOG_PROTO(...) do { printf(__VA_ARGS__); osDelay(10); } while (0) +#else +#define LOG_PROTO(...) ((void) 0) +#endif + + +constexpr uint8_t SYNC_BYTE = 0xAA; +constexpr uint8_t CRC8_INIT = 0x42; +constexpr uint16_t CRC16_INIT = 0x1337; +constexpr uint16_t PROTOCOL_VERSION = 1; + +// This value must not be larger than USB_TX_DATA_SIZE defined in usbd_cdc_if.h +//Oskar: What's the error? What values work? Does 63 work? Ideally we figure out how to get 64 to work, but if not let's find something better than 32. +constexpr uint16_t TX_BUF_SIZE = 32; // does not work with 64 for some reason +constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently no effect because of protocol limitations + +// Maximum time we allocate for processing and responding to a request +constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10; + +template +inline size_t write_le(T value, uint8_t* buffer); + +template +inline size_t read_le(T* value, const uint8_t* buffer); + +template<> +inline size_t write_le(bool value, uint8_t* buffer) { + buffer[0] = value ? 1 : 0; + return 1; +} + +template<> +inline size_t write_le(uint8_t value, uint8_t* buffer) { + buffer[0] = value; + return 1; +} + +template<> +inline size_t write_le(uint16_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + return 2; +} + +template<> +inline size_t write_le(uint32_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + buffer[2] = (value >> 16) & 0xff; + buffer[3] = (value >> 24) & 0xff; + return 4; +} + +template<> +inline size_t write_le(int32_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + buffer[2] = (value >> 16) & 0xff; + buffer[3] = (value >> 24) & 0xff; + return 4; +} + +template<> +inline size_t write_le(uint64_t value, uint8_t* buffer) { + buffer[0] = (value >> 0) & 0xff; + buffer[1] = (value >> 8) & 0xff; + buffer[2] = (value >> 16) & 0xff; + buffer[3] = (value >> 24) & 0xff; + buffer[4] = (value >> 32) & 0xff; + buffer[5] = (value >> 40) & 0xff; + buffer[6] = (value >> 48) & 0xff; + buffer[7] = (value >> 56) & 0xff; + return 8; +} + +template<> +inline size_t write_le(float value, uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + const uint32_t * value_as_uint32 = reinterpret_cast(&value); + return write_le(*value_as_uint32, buffer); +} + +template<> +inline size_t read_le(bool* value, const uint8_t* buffer) { + *value = buffer[0]; + return 1; +} + +template<> +inline size_t read_le(uint8_t* value, const uint8_t* buffer) { + *value = buffer[0]; + return 1; +} + +template<> +inline size_t read_le(uint16_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8); + return 2; +} + +template<> +inline size_t read_le(int32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24); + return 4; +} + +template<> +inline size_t read_le(uint32_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24); + return 4; +} + +template<> +inline size_t read_le(uint64_t* value, const uint8_t* buffer) { + *value = (static_cast(buffer[0]) << 0) | + (static_cast(buffer[1]) << 8) | + (static_cast(buffer[2]) << 16) | + (static_cast(buffer[3]) << 24) | + (static_cast(buffer[4]) << 32) | + (static_cast(buffer[5]) << 40) | + (static_cast(buffer[6]) << 48) | + (static_cast(buffer[7]) << 56); + return 8; +} + +template<> +inline size_t read_le(float* value, const uint8_t* buffer) { + static_assert(CHAR_BIT * sizeof(float) == 32, "32 bit floating point expected"); + static_assert(std::numeric_limits::is_iec559, "IEEE 754 floating point expected"); + return read_le(reinterpret_cast(value), buffer); +} + +// @brief Reads a value of type T from the buffer. +// @param buffer Pointer to the buffer to be read. The pointer is updated by the number of bytes that were read. +// @param length The number of available bytes in buffer. This value is updated to subtract the bytes that were read. +template +static inline T read_le(const uint8_t** buffer, size_t* length) { + T result; + size_t cnt = read_le(&result, *buffer); + *buffer += cnt; + *length -= cnt; + return result; +} + +class PacketSink { +public: + // @brief Processes a packet. + // The blocking behavior shall depend on the thread-local deadline_ms variable. + // @return: 0 on success, otherwise a non-zero error code + // TODO: define what happens when the packet is larger than what the implementation can handle. + virtual int process_packet(const uint8_t* buffer, size_t length) = 0; +}; + +class StreamSink { +public: + // @brief Processes a chunk of bytes that is part of a continuous stream. + // The blocking behavior shall depend on the thread-local deadline_ms variable. + // @return: 0 on success, otherwise a non-zero error code + virtual int process_bytes(const uint8_t* buffer, size_t length) = 0; + + // @brief Returns the number of bytes that can still be written to the stream. + // Shall return SIZE_MAX if the stream has unlimited lenght. + virtual size_t get_free_space() = 0; +}; + + +class StreamToPacketConverter : public StreamSink { +public: + StreamToPacketConverter(PacketSink& output) : + output_(output) + { + }; + + int process_bytes(const uint8_t *buffer, size_t length); + + size_t get_free_space() { return SIZE_MAX; } + +private: + uint8_t header_buffer_[3]; + size_t header_index_ = 0; + uint8_t packet_buffer_[RX_BUF_SIZE]; + size_t packet_index_ = 0; + size_t packet_length_ = 0; + PacketSink& output_; +}; + + +class PacketToStreamConverter : public PacketSink { +public: + PacketToStreamConverter(StreamSink& output) : + output_(output) + { + }; + + int process_packet(const uint8_t *buffer, size_t length); + +private: + StreamSink& output_; +}; + + +// Implements the StreamSink interface by writing into a fixed size +// memory buffer. +class MemoryStreamSink : public StreamSink { +public: + MemoryStreamSink(uint8_t *buffer, size_t length) : + buffer_(buffer), + buffer_length_(length) {} + + // Returns 0 on success and -1 if the buffer could not accept everything because it became full + int process_bytes(const uint8_t* buffer, size_t length) { + int status = 0; + if (length > buffer_length_) { + length = buffer_length_; + status = -1; + } + memcpy(buffer_, buffer, length); + buffer_ += length; + buffer_length_ -= length; + return status; + } + + size_t get_free_space() { return buffer_length_; } + +private: + uint8_t * buffer_; + size_t buffer_length_; +}; + +// Implements the StreamSink interface by discarding the first couple of bytes +// and then forwarding the rest to another stream. +class NullStreamSink : public StreamSink { +public: + NullStreamSink(size_t skip, StreamSink& follow_up_stream) : + skip_(skip), + follow_up_stream_(follow_up_stream) {} + + // Returns 0 on success and -1 if the buffer could not accept everything because it became full + int process_bytes(const uint8_t* buffer, size_t length) { + if (skip_ < length) { + buffer += skip_; + length -= skip_; + skip_ = 0; + return follow_up_stream_.process_bytes(buffer, length); + } else { + skip_ -= length; + return 0; + } + } + + size_t get_free_space() { return skip_ + follow_up_stream_.get_free_space(); } + +private: + size_t skip_; + StreamSink& follow_up_stream_; +}; + + + +// Implements the StreamSink interface by calculating the CRC16 checksum +// on the data that is sent to it. +class CRC16Calculator : public StreamSink { +public: + CRC16Calculator(uint16_t crc16_init) : + crc16_(crc16_init) {} + + int process_bytes(const uint8_t* buffer, size_t length) { + crc16_ = calc_crc16(crc16_, buffer, length); + return 0; + } + + size_t get_free_space() { return SIZE_MAX; } + + uint16_t get_crc16() { return crc16_; } +private: + uint16_t crc16_; +}; + + +// @brief Endpoint request handler +// +// When passed a valid endpoint context, implementing functions shall handle an +// endpoint read/write request by reading the provided input data and filling in +// output data. The exact semantics of this function depends on the corresponding +// endpoint's specification. +// +// @param input: pointer to the input data +// @param input_length: number of available input bytes +// @param output: The stream where to write the output to. Can be null. +// The handler shall abort as soon as the stream returns +// a non-zero error code on write. +typedef std::function EndpointHandler; + + +template +void default_readwrite_endpoint_handler(const T* value, const uint8_t* input, size_t input_length, StreamSink* output) { + // If the old value was requested, call the corresponding little endian serialization function + if (output) { + // TODO: make buffer size dependent on the type + uint8_t buffer[sizeof(T)]; + size_t cnt = write_le(*value, buffer); + if (cnt <= output->get_free_space()) + output->process_bytes(buffer, cnt); + } +} + +template +void default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { + // Read the endpoint value into output + default_readwrite_endpoint_handler(const_cast(value), input, input_length, output); + + // If a new value was passed, call the corresponding little endian deserialization function + uint8_t buffer[sizeof(T)] = { 0 }; // TODO: make buffer size dependent on the type + if (input_length >= sizeof(buffer)) + read_le(value, input); +} + + + +template +static inline const char* get_default_json_modifier(); + +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"float\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"float\",\"access\":\"rw\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint64\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint64\",\"access\":\"rw\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"int32\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"int32\",\"access\":\"rw\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint32\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint32\",\"access\":\"rw\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint16\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint16\",\"access\":\"rw\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint8\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"uint8\",\"access\":\"rw\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"bool\",\"access\":\"r\""; +} +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"bool\",\"access\":\"rw\""; +} + +class Endpoint { +public: + //const char* const name_; + virtual void handle(const uint8_t* input, size_t input_length, StreamSink* output) = 0; + virtual bool get_string(char * output, size_t length) { return false; }; + virtual bool set_string(char * buffer, size_t length) { return false; } +}; + +class EndpointProvider { +public: + virtual size_t get_endpoint_count() = 0; + virtual void write_json(size_t id, StreamSink* output) = 0; + virtual Endpoint* get_by_name(char * name, size_t length) = 0; + virtual void register_endpoints(Endpoint** list, size_t id, size_t length) = 0; +}; + + +static inline int write_string(const char* str, StreamSink* output) { + return output->process_bytes(reinterpret_cast(str), strlen(str)); +} + + +/* @brief Handles the communication protocol on one channel. +* +* When instantiated with a list of endpoints and an output packet sink, +* objects of this class will handle packets passed into process_packet, +* pass the relevant data to the corresponding endpoints and dispatch response +* packets on the output. +*/ +class BidirectionalPacketBasedChannel : public PacketSink { +public: + BidirectionalPacketBasedChannel(PacketSink& output) : + output_(output) + { } + + int process_packet(const uint8_t* buffer, size_t length); +private: + PacketSink& output_; + uint8_t tx_buf_[TX_BUF_SIZE]; +}; + + +template +struct MemberList; + +template<> +struct MemberList<> { +public: + static constexpr size_t endpoint_count = 0; + static constexpr bool is_empty = true; + void write_json(size_t id, StreamSink* output) { + // no action + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { + // no action + } + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; + } + std::tuple<> get_names_as_tuple() const { return std::tuple<>(); } +}; + +template +struct MemberList { +public: + static constexpr size_t endpoint_count = TMember::endpoint_count + MemberList::endpoint_count; + static constexpr bool is_empty = false; + + MemberList(TMember&& this_member, TMembers&&... subsequent_members) : + this_member_(std::forward(this_member)), + subsequent_members_(std::forward(subsequent_members)...) {} + + MemberList(TMember&& this_member, MemberList&& subsequent_members) : + this_member_(std::forward(this_member)), + subsequent_members_(std::forward>(subsequent_members)) {} + + // @brief Move constructor +/* MemberList(MemberList&& other) : + this_member_(std::move(other.this_member_)), + subsequent_members_(std::move(other.subsequent_members_)) {}*/ + + void write_json(size_t id, StreamSink* output) /*final*/ { + this_member_.write_json(id, output); + if (!MemberList::is_empty) + write_string(",", output); + subsequent_members_.write_json(id + TMember::endpoint_count, output); + } + + Endpoint* get_by_name(const char * name, size_t length) { + Endpoint* result = this_member_.get_by_name(name, length); + if (result) return result; + else return subsequent_members_.get_by_name(name, length); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) /*final*/ { + this_member_.register_endpoints(list, id, length); + subsequent_members_.register_endpoints(list, id + TMember::endpoint_count, length); + } + + TMember this_member_; + MemberList subsequent_members_; +}; + +template +MemberList make_protocol_member_list(TMembers&&... member_list) { + return MemberList(std::forward(member_list)...); +} + +template +class ProtocolObject { +public: + ProtocolObject(const char * name, TMembers&&... member_list) : + name_(name), + member_list_(std::forward(member_list)...) {} + + static constexpr size_t endpoint_count = MemberList::endpoint_count; + + void write_json(size_t id, StreamSink* output) { + write_string("{\"name\":\"", output); + write_string(name_, output); + write_string("\",\"type\":\"object\",\"members\":[", output); + member_list_.write_json(id, output), + write_string("]}", output); + } + + Endpoint* get_by_name(const char * name, size_t length) { + size_t segment_length = strlen(name); + if (!strncmp(name, name_, length)) + return member_list_.get_by_name(name + segment_length + 1, length - segment_length - 1); + else + return nullptr; + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + member_list_.register_endpoints(list, id, length); + } + + const char * name_; + MemberList member_list_; +}; + +template +ProtocolObject make_protocol_object(const char * name, TMembers&&... member_list) { + return ProtocolObject(name, std::forward(member_list)...); +} + + +// TODO: move to cpp_utils +#define ENABLE_IF_SAME(a, b, type) \ + template typename std::enable_if_t::value, bool> + +template +class ProtocolProperty : public Endpoint { +public: + static constexpr const char * json_modifier = get_default_json_modifier(); + static constexpr size_t endpoint_count = 1; + + ProtocolProperty(const char * name, TProperty* property) + : name_(name), property_(property) + {} + +/* TODO: find out why the move constructor is not used when it could be + ProtocolProperty(const ProtocolProperty&) = delete; + // @brief Move constructor + ProtocolProperty(ProtocolProperty&& other) : + Endpoint(std::move(other)), + name_(std::move(other.name_)), + property_(other.property_) + {} + constexpr ProtocolProperty& operator=(const ProtocolProperty& other) = delete; + constexpr ProtocolProperty& operator=(const ProtocolProperty& other) { + //Endpoint(std::move(other)), + //name_(std::move(other.name_)), + //property_(other.property_) + name_ = other.name_; + property_ = other.property_; + return *this; + } + ProtocolProperty& operator=(ProtocolProperty&& other) + : name_(other.name_), property_(other.property_) + {} + ProtocolProperty& operator=(const ProtocolProperty& other) + : name_(other.name_), property_(other.property_) + {}*/ + + void write_json(size_t id, StreamSink* output) { + // write name + write_string("{\"name\":\"", output); + LOG_PROTO("json: this at %x, name at %x is s\r\n", (uintptr_t)this, (uintptr_t)name_); + //LOG_PROTO("json\r\n"); + write_string(name_, output); + + // write endpoint ID + write_string("\",\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + // write additional JSON data + if (json_modifier && json_modifier[0]) { + write_string(",", output); + write_string(json_modifier, output); + } + + write_string("}", output); + } + + Endpoint* get_by_name(const char * name, size_t length) { + if (!strncmp(name, name_, length)) + return this; + else + return nullptr; + } + + + // *** ASCII protocol handlers *** + + ENABLE_IF_SAME(std::decay_t, float, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%f", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, int32_t, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%ld", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, uint32_t, bool) + get_string_ex(char * buffer, size_t length, int) { + snprintf(buffer, length, "%lu", *property_); + return true; + } + ENABLE_IF_SAME(std::decay_t, bool, bool) + get_string_ex(char * buffer, size_t length, int) { + buffer[0] = (*property_) ? '1' : '0'; + buffer[1] = 0; + return true; + } + bool get_string_ex(char * buffer, size_t length, ...) { + return false; + } + bool get_string(char * buffer, size_t length) final { + return get_string_ex(buffer, length, 0); + } + ENABLE_IF_SAME(TProperty, float, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%f", property_) == 1; + } + ENABLE_IF_SAME(TProperty, int32_t, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%ld", property_) == 1; + } + ENABLE_IF_SAME(TProperty, uint32_t, bool) + set_string_ex(char * buffer, size_t length, int) { + return sscanf(buffer, "%lu", property_) == 1; + } + ENABLE_IF_SAME(TProperty, bool, bool) + set_string_ex(char * buffer, size_t length, int) { + int val; + if (sscanf(buffer, "%d", &val) != 1) + return false; + *property_ = val; + return true; + } + bool set_string_ex(char * buffer, size_t length, ...) { + return false; + } + bool set_string(char * buffer, size_t length) final { + //__asm ("bkpt"); + return set_string_ex(buffer, length, 0); + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + } + void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + default_readwrite_endpoint_handler(property_, input, input_length, output); + } + /*void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + handle(input, input_length, output); + }*/ + + const char * name_; + TProperty* property_; +}; + +// Non-const non-enum types +template::value>> +ProtocolProperty make_protocol_property(const char * name, TProperty* property) { + return ProtocolProperty(name, property); +}; + +// Const non-enum types +template::value>> +ProtocolProperty make_protocol_ro_property(const char * name, const TProperty* property) { + return ProtocolProperty(name, property); +}; + +// Non-const enum types +template::value>> +ProtocolProperty> make_protocol_property(const char * name, TProperty* property) { + return ProtocolProperty>(name, reinterpret_cast*>(property)); +}; + +// Const enum types +template::value>> +ProtocolProperty> make_protocol_ro_property(const char * name, const TProperty* property) { + return ProtocolProperty>(name, reinterpret_cast*>(property)); +}; + + + +template +class FunctionTraits { +public: + template> + static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args, TUnpackedArgs ... args) { + return invoke(obj, func_ptr, packed_args, args..., std::get(packed_args)); + } + + template + static TRet invoke(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args, TArgs ... args) { + return (obj.*func_ptr)(args...); + } +}; + +/* @brief Invoke a class member function with a variable number of arguments that are supplied as a tuple + +Example usage: + +class MyClass { +public: + int MyFunction(int a, int b) { + return 0; + } +}; + +MyClass my_object; +std::tuple my_args(3, 4); // arguments are supplied as a tuple +int result = invoke_function_with_tuple(my_object, &MyClass::MyFunction, my_args); +*/ +template +TRet invoke_function_with_tuple(TObj& obj, TRet(TObj::*func_ptr)(TArgs...), std::tuple packed_args) { + return FunctionTraits::template invoke<0>(obj, func_ptr, packed_args); +} + + +template +struct PropertyListFactory; + +template<> +struct PropertyListFactory<> { + template + static MemberList<> make_property_list(std::array names, std::tuple& values) { + return MemberList<>(); + } +}; + +template +struct PropertyListFactory { + template + static MemberList, ProtocolProperty...> + make_property_list(std::array names, std::tuple& values) { + return MemberList, ProtocolProperty...>( + make_protocol_property(std::get(names), &std::get(values)), + PropertyListFactory::template make_property_list(names, values) + ); + } +}; + + +template +struct return_type; + +template<> +struct return_type<> { typedef void type; }; +template +struct return_type { typedef T type; }; +template +struct return_type { typedef std::tuple type; }; + + + +template +class ProtocolFunction; + +template + //template typename asd, + //template typename ssss> +class ProtocolFunction, std::tuple> : Endpoint { +public: + + // @brief The return type of the function as written by a C++ programmer + using TRet = typename return_type::type; + + static constexpr size_t endpoint_count = 1 + MemberList...>::endpoint_count + MemberList...>::endpoint_count; + + ProtocolFunction(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TInputs...), + std::array input_names, + std::array output_names) : + name_(name), obj_(&obj), func_ptr_(func_ptr), + input_names_{input_names}, output_names_{output_names}, + input_properties_(PropertyListFactory::template make_property_list<0>(input_names_, in_args_)), + output_properties_(PropertyListFactory::template make_property_list<0>(output_names_, out_args_)) + { + LOG_PROTO("my tuple is at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + } + + void write_json(size_t id, StreamSink* output) { + // write name + write_string("{\"name\":\"", output); + write_string(name_, output); + + // write endpoint ID + write_string("\",\"id\":", output); + char id_buf[10]; + snprintf(id_buf, sizeof(id_buf), "%u", id); // TODO: get rid of printf + write_string(id_buf, output); + + // write arguments + write_string(",\"type\":\"function\",\"inputs\":[", output); + input_properties_.write_json(id + 1, output), + write_string("],\"outputs\":[", output); + output_properties_.write_json(id + 1 + decltype(input_properties_)::endpoint_count, output), + write_string("]}", output); + } + + Endpoint* get_by_name(const char * name, size_t length) { + return nullptr; // can't address functions by name + } + + void register_endpoints(Endpoint** list, size_t id, size_t length) { + if (id < length) + list[id] = this; + input_properties_.register_endpoints(list, id + 1, length); + output_properties_.register_endpoints(list, id + 1 + decltype(input_properties_)::endpoint_count, length); + } + + template std::enable_if_t + handle_ex() { + invoke_function_with_tuple(*obj_, func_ptr_, in_args_); + } + + template std::enable_if_t + handle_ex() { + std::get<0>(out_args_) = invoke_function_with_tuple(*obj_, func_ptr_, in_args_); + } + + template std::enable_if_t= 2> + handle_ex() { + out_args_ = invoke_function_with_tuple(*obj_, func_ptr_, in_args_); + } + + void handle(const uint8_t* input, size_t input_length, StreamSink* output) { + (void) input; + (void) input_length; + (void) output; + LOG_PROTO("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_)); + LOG_PROTO("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_)); + handle_ex(); + } + + const char * name_; + TObj* obj_; + TRet(TObj::*func_ptr_)(TInputs...); + std::array input_names_; // TODO: remove + std::array output_names_; // TODO: remove + std::tuple in_args_; + std::tuple out_args_; + MemberList...> input_properties_; + MemberList...> output_properties_; +}; + +template> +ProtocolFunction, std::tuple<>> make_protocol_function(const char * name, TObj& obj, void(TObj::*func_ptr)(TArgs...), TNames ... names) { + return ProtocolFunction, std::tuple<>>(name, obj, func_ptr, {names...}, {}); +} + +template::value>> +ProtocolFunction, std::tuple> make_protocol_function(const char * name, TObj& obj, TRet(TObj::*func_ptr)(TArgs...), TNames ... names) { + return ProtocolFunction, std::tuple>(name, obj, func_ptr, {names...}, {"result"}); +} + + + +template +class EndpointProvider_from_MemberList : public EndpointProvider { +public: + EndpointProvider_from_MemberList(T& member_list) : member_list_(member_list) {} + size_t get_endpoint_count() final { + return T::endpoint_count; + } + void write_json(size_t id, StreamSink* output) final { + return member_list_.write_json(id, output); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) final { + return member_list_.register_endpoints(list, id, length); + } + Endpoint* get_by_name(char * name, size_t length) final { + for (size_t i = 0; i < length; i++) { + if (name[i] == '.') + name[i] = 0; + } + name[length-1] = 0; + return member_list_.get_by_name(name, length); + } + T& member_list_; +}; + +void set_application_endpoints(EndpointProvider* endpoints); + + +// defined in communication.cpp +extern Endpoint* endpoints_[]; +extern size_t n_endpoints_; +extern const size_t max_endpoints_; +extern EndpointProvider* application_endpoints; + +#endif diff --git a/Firmware/dump_version.sh b/Firmware/dump_version.sh deleted file mode 100755 index cdaaa372..00000000 --- a/Firmware/dump_version.sh +++ /dev/null @@ -1,47 +0,0 @@ -#!/bin/bash -set -euo pipefail - -if [ $# -eq 1 ]; then - OUTPUT="$1" -else - OUTPUT="/dev/stdout" -fi - -# The git root lies outside of the tup root -export GIT_DISCOVERY_ACROSS_FILESYSTEM=1 - -# Get a description of the current Git state -# Examples of what this string may become: -# fw-v0.3.6 The current commit is exactly at tag "fw-v0.3.6" -# There may or may not be untracked files in the -# working directory. -# fw-v0.3.6* The current commit is at tag "fw-v0.3.6" and there -# are uncommitted changes in the working directory. -# fw-v0.3.6-4-g3703ae5 The working directory at a commit with hash 3703ae5, -# 4 commits ahead of tag fw-v0.3.6 and clean. -FW_VERSION="$(git describe --always --tags --dirty=* || echo "[unknown commit]")" - -# Extract version numbers -FW_VERSION_MAJOR="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\1/p' <<< "$FW_VERSION")" -FW_VERSION_MINOR="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\2/p' <<< "$FW_VERSION")" -FW_VERSION_REVISION="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\3/p' <<< "$FW_VERSION")" -FW_VERSION_SUFFIX="$(sed -n 's/.*v\([0-9a-zA-Z]\).\([0-9a-zA-Z]\).\([0-9a-zA-Z]\)\(.*\)/\4/p' <<< "$FW_VERSION")" - -# Fall back to 0 if the verions does not match the expected pattern -[ "$FW_VERSION_MAJOR" == "" ] && FW_VERSION_MAJOR=0 -[ "$FW_VERSION_MINOR" == "" ] && FW_VERSION_MINOR=0 -[ "$FW_VERSION_REVISION" == "" ] && FW_VERSION_REVISION=0 - -if [ "$FW_VERSION_SUFFIX" == "" ]; then - FW_VERSION_UNRELEASED=0 -else - FW_VERSION_UNRELEASED=1 -fi - -cat > "$OUTPUT" <&1 | \ + xxd -p | \ + tr -d '\n' | \ + sed -n 's/^.*6e756d6265722027\([0-9a-f]*\)2720646f65736e27.*$/\1/p' | sed -e 's/.\{2\}/\\x&/g'; echo diff --git a/Firmware/legacy-protocol.md b/Firmware/legacy-protocol.md deleted file mode 100644 index 2898432f..00000000 --- a/Firmware/legacy-protocol.md +++ /dev/null @@ -1,68 +0,0 @@ - -Warning: this protocol has [been replaced](https://github.com/madcowswe/ODrive/blob/devel/Firmware/protocol.md). -It's still operational but for new applications it's recommended to use the new protocol. - -### Command set -The most accurate way to understand the commands is to read [the code](MotorControl/commands.c) that parses the commands. Also you can have a look at the [ODrive Arduino library](https://github.com/madcowswe/ODriveArduino) that makes it easy to use the UART interface on Arduino. You can also look at it as an implementation example of how to talk to the ODrive over UART. - -#### UART framing -USB communicates with packets, so it is easy to frame a command as one command per packet. However, UART doesn't have any packeting, so we need a way to frame the commands. The start-of-packet symbol is `$` and the end-of-packet symbol is `!`, that is, something like this: `$command!`. An example of a valid UART position command: -``` -$p 0 10000 0 0! -``` - -#### Motor Position command -``` -p motor position velocity_ff current_ff -``` -* `p` for position -* `motor` is the motor number, `0` or `1`. -* `position` is the desired position, in encoder counts. -* `velocity_ff` is the velocity feed-forward term, in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Velocity command -``` -v motor velocity current_ff -``` -* `v` for velocity -* `motor` is the motor number, `0` or `1`. -* `velocity` is the desired velocity in counts/s. -* `current_ff` is the current feed-forward term, in A. - -Note that if you don't know what feed-forward is or what it's used for, simply set it to 0. - -#### Motor Current command -``` -c motor current -``` -* `c` for current -* `motor` is the motor number, `0` or `1`. -* `current` is the desired current in A. - -#### Variable getting and setting -``` -g type index -s type index value -``` -* `g` for get, `s` for set -* `type` is the data type as follows: -** `0` is float -** `1` is int -** `2` is bool -* `index` is the index in the corresponding [exposed variable table](MotorControl/legacy_commands.c). - -For example -* `g 0 12` will return the phase resistance of M0 -* `s 0 8 10000.0` will set the velocity limit on M0 to 10000 counts/s -* `g 1 3` will return the error status of M0 -* `g 1 7` will return the error status of M1 - -The error status corresponds to the [Error_t enum in low_level.h](MotorControl/low_level.h). - -Note that the links in this section are to a specific commits to make sure that the line numbers are accurate. That is, they don't link to the newest master, but to an old version. Please check the corresponding lines in the code you are using. This is especially important to get the correct indicies in the exposed variable tables, and the error enum values. - -#### Continous monitoring of variables -You can set up variables in monitoring slots, and then have them (or a subset of them) repeatedly printed upon request. Please see the code for this. diff --git a/Firmware/tup.config.default b/Firmware/tup.config.default index be0515fc..8ebb4402 100644 --- a/Firmware/tup.config.default +++ b/Firmware/tup.config.default @@ -1,6 +1,5 @@ # Copy this file to tup.config and adapt it to your needs # make sure this fits your board -#CONFIG_BOARD_VERSION=v3.4-24V +#CONFIG_BOARD_VERSION=v3.5-24V CONFIG_USB_PROTOCOL=native CONFIG_UART_PROTOCOL=ascii -CONFIG_STEP_DIR=n diff --git a/docs/testing.md b/docs/testing.md new file mode 100644 index 00000000..347db2b1 --- /dev/null +++ b/docs/testing.md @@ -0,0 +1,23 @@ +# Automated Testing + +This section describes how to use the automated testing facilities. +You don't have to do this as an end user. + +They test the following aspects: + - System functions (communication interfaces, configuration storage) + - Functionality of the motor controller and state machine + - High speed and high load conditions + +The testing facility consists of the following components: + * **Test rig:** In the simplest case this can be a single ODrive with a single motor and encoder pair. Can also be multiple ODrives with multiple axes, some of which may be mechanically coupled. + * **Test host:** The PC on which the test script runs. All ODrives must be connected to the test host via USB. + * **test-rig.yaml:** Describes your test rig. Make sure all values are correct. Incorrect values may physically break or fry your test setup. + * **run_tests.py:** This is the main script that runs all the tests. + +## How to run + +Example: + +``` +./run_tests.py --skip-boring-tests --ignore top-odrive.yellow bottom-odrive.yellow +``` diff --git a/tools/odrive/__init__.py b/tools/odrive/__init__.py index 6eca987b..7be9b9af 100644 --- a/tools/odrive/__init__.py +++ b/tools/odrive/__init__.py @@ -1,5 +1,5 @@ # Standard convention is to add a __version__ attribute to the package -from .version import get_version -__version__ = get_version() -del get_version +from .version import get_version_str +__version__ = get_version_str() +del get_version_str diff --git a/tools/odrive/dfuse/DfuDevice.py b/tools/odrive/dfuse/DfuDevice.py index dc5ac152..b9ca449c 100644 --- a/tools/odrive/dfuse/DfuDevice.py +++ b/tools/odrive/dfuse/DfuDevice.py @@ -83,7 +83,11 @@ class DfuDevice: else: states = state - status = self.get_status() + try: + status = self.get_status() + except: + time.sleep(0.100) + status = self.get_status() while (status[1] in states): claimed_timeout = status[2] diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index a58404b1..52b1e610 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -1,6 +1,28 @@ -# some enums described in the README # TODO: This is dangerous. Transmit as part of the JSON + +AXIS_STATE_UNDEFINED = 0 +AXIS_STATE_IDLE = 1 +AXIS_STATE_STARTUP_SEQUENCE = 2 +AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3 +AXIS_STATE_MOTOR_CALIBRATION = 4 +AXIS_STATE_SENSORLESS_CONTROL = 5 +AXIS_STATE_ENCODER_INDEX_SEARCH = 6 +AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7 +AXIS_STATE_CLOSED_LOOP_CONTROL = 8 + +AXIS_ERROR_NO_ERROR = 0 +AXIS_ERROR_INVALID_STATE = 1 +#AXIS_ERROR_DC_BUS_UNDER_VOLTAGE = 2 +#AXIS_ERROR_DC_BUS_OVER_VOLTAGE = 3 +#AXIS_ERROR_CURRENT_MEASUREMENT_TIMEOUT = 4 +#AXIS_ERROR_CONTROL_LOOP_TIMEOUT = 5 +#AXIS_ERROR_MOTOR_FAILED = 6 +#AXIS_ERROR_SENSORLESS_ESTIMATOR_FAILED = 7 +#AXIS_ERROR_ENCODER_FAILED = 8 +#AXIS_ERROR_CONTROLLER_FAILED = 9 +#AXIS_ERROR_POS_CTRL_DURING_SENSORLESS = 10 + MOTOR_TYPE_HIGH_CURRENT = 0 #MOTOR_TYPE_LOW_CURRENT = 1 MOTOR_TYPE_GIMBAL = 2 diff --git a/tools/odrive/protocol.py b/tools/odrive/protocol.py index 42d96e7f..14ef07d2 100644 --- a/tools/odrive/protocol.py +++ b/tools/odrive/protocol.py @@ -232,8 +232,10 @@ class Channel(PacketSink): The thread quits as soon as the channel enters a broken state. """ def receiver_thread(): + error_ctr = 0 try: - while (not cancellation_token.is_set()) and (not self._channel_broken.is_set()): + while (not cancellation_token.is_set() and not self._channel_broken.is_set() + and error_ctr < 10): # Set an arbitrary deadline because the get_packet function # currently doesn't support a cancellation_token deadline = time.monotonic() + 1.0 @@ -242,7 +244,10 @@ class Channel(PacketSink): except odrive.utils.TimeoutException: continue # try again except ChannelDamagedException: + error_ctr += 1 continue # try again + if (error_ctr > 0): + error_ctr -= 1 # Process response # This should not throw an exception, otherwise the channel breaks self.process_packet(response) @@ -268,7 +273,7 @@ class Channel(PacketSink): seq_no = self._outbound_seq_no finally: self._my_lock.release() - seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the legacy protocol + seq_no |= 0x80 # FIXME: we hardwire one bit of the seq-no to 1 to avoid conflicts with the ascii protocol packet = struct.pack(' 0: + return self._outputs[0].get_value() def dump(self): return "{}({})".format(self._name, ", ".join("{}: {}".format(x._name, x._property_type.__name__) for x in self._inputs)) diff --git a/tools/odrive/shell.py b/tools/odrive/shell.py index 8c441097..ef072010 100644 --- a/tools/odrive/shell.py +++ b/tools/odrive/shell.py @@ -22,7 +22,7 @@ def print_help(args): print('Type "odrv0." and press ') print('This will present you with all the properties that you can reference') print('') - print('For example: "odrv0.motor0.encoder.pll_pos"') + print('For example: "odrv0.motor0.encoder.pos_estimate"') print('will print the current encoder position on motor 0') print('and "odrv0.motor0.pos_setpoint = 10000"') print('will send motor0 to 10000') diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py new file mode 100644 index 00000000..43ef182b --- /dev/null +++ b/tools/odrive/tests.py @@ -0,0 +1,671 @@ + +import subprocess +import shlex +import math +import time +import sys +import threading +import odrive.discovery +from odrive.enums import * +import odrive.utils +import numpy as np + +import functools +print = functools.partial(print, flush=True) + +import abc +ABC = abc.ABC + +class TestFailed(Exception): + def __init__(self, message): + Exception.__init__(self, message) + +class PreconditionsNotMet(Exception): + pass + +class ODriveTestContext(): + def __init__(self, name: str, yaml: dict): + self.handle = None + self.yaml = yaml + self.name = name + self.axes = [] + for axis_idx, axis_yaml in enumerate(yaml['axes']): + axis_name = (name + "." + axis_yaml['name']) if 'name' in axis_yaml else '{}.axis{}'.format(name, axis_idx) + self.axes.append(AxisTestContext(axis_name, axis_yaml, self)) + + def rediscover(self): + """ + Reconnects to the ODrive + """ + self.handle = odrive.discovery.find_any( + path="usb", serial_number=self.yaml['serial-number'], timeout=15)#, printer=print) + for axis_idx, axis_ctx in enumerate(self.axes): + axis_ctx.handle = self.handle.__dict__['axis{}'.format(axis_idx)] + +class AxisTestContext(): + def __init__(self, name: str, yaml: dict, odrv_ctx: ODriveTestContext): + self.handle = None + self.yaml = yaml + self.name = name + self.lock = threading.Lock() + self.odrv_ctx = odrv_ctx + +def test_assert_eq(observed, expected, range=None, accuracy=None): + if range is None and accuracy is None and observed != expected: + raise TestFailed("value mismatch: expected {} but observed {}".format(expected, observed)) + if not range is None and ((observed < expected - range) or (observed > expected + range)): + raise TestFailed("value out of range: expected {}+-{} but observed {}".format(expected, range, observed)) + elif not accuracy is None and ((observed < expected * (1 - accuracy)) or (observed > expected * (1 + accuracy))): + raise TestFailed("value out of range: expected {}+-{}% but observed {}".format(expected, accuracy*100.0, observed)) + +def get_errors(axis_ctx: AxisTestContext): + errors = [] + if axis_ctx.handle.motor.error != 0: + errors.append("motor failed with error 0x{:04X}".format(axis_ctx.handle.motor.error)) + if axis_ctx.handle.encoder.error != 0: + errors.append("encoder failed with error 0x{:04X}".format(axis_ctx.handle.encoder.error)) + if axis_ctx.handle.sensorless_estimator.error != 0: + errors.append("sensorless_estimator failed with error 0x{:04X}".format(axis_ctx.handle.sensorless_estimator.error)) + if axis_ctx.handle.error != 0: + errors.append("axis failed with error 0x{:04X}".format(axis_ctx.handle.error)) + elif len(errors) > 0: + errors.append("and by the way: axis reports no error even though there is one") + return errors + +def dump_errors(axis_ctx: AxisTestContext, logger): + errors = get_errors(axis_ctx) + if len(errors): + logger.error("errors on " + axis_ctx.name) + for error in errors: + logger.error(error) + +def clear_errors(axis_ctx: AxisTestContext): + axis_ctx.handle.error = 0 + axis_ctx.handle.encoder.error = 0 + axis_ctx.handle.motor.error = 0 + axis_ctx.handle.sensorless_estimator.error = 0 + +def test_assert_no_error(axis_ctx: AxisTestContext): + errors = get_errors(axis_ctx) + if len(errors) > 0: + raise TestFailed("\n".join(errors)) + +def run(command_line, logger, timeout=None): + """ + Runs a shell command in the current directory + """ + logger.debug("invoke: " + command_line) + cmd = shlex.split(command_line) + result = subprocess.run(cmd, timeout=timeout, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT) + if result.returncode != 0: + logger.error(result.stdout.decode(sys.stdout.encoding)) + raise TestFailed("command {} failed".format(command_line)) + +def request_state(axis_ctx: AxisTestContext, state, expect_success=True): + axis_ctx.handle.requested_state = state + time.sleep(0.001) + if expect_success: + test_assert_eq(axis_ctx.handle.current_state, state) + else: + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis_ctx.handle.error, AXIS_ERROR_INVALID_STATE) + axis_ctx.handle.error = AXIS_ERROR_NO_ERROR # reset error + +def set_limits(axis_ctx: AxisTestContext, logger, vel_limit=20000, current_limit=10): + """ + Sets the velocity and current limits for the axis, subject to the following constraints: + - the arguments given to this function are not exceeded + - max motor current is not exceeded + - max brake resistor power divided by two is not exceeded (here velocity takes precedence over current) + """ + max_rpm = vel_limit / axis_ctx.yaml['encoder-cpr'] * 60 + max_emf_voltage = max_rpm / axis_ctx.yaml['motor-kv'] + max_brake_power = axis_ctx.odrv_ctx.yaml['max-brake-power'] / 2 * 0.8 # 20% safety margin + max_motor_current = max_brake_power / max_emf_voltage + logger.debug("velocity limit = {} => V_emf = {:.3}V, I_lim = {:.3}A".format(vel_limit, max_emf_voltage, max_motor_current)) + + # Bound current limit based on the motor's current limit and the brake resistor current limit + current_limit = min(current_limit, axis_ctx.yaml['motor-max-current'], max_motor_current) + # TODO: set as an atomic operation + axis_ctx.handle.motor.config.current_lim = current_limit + axis_ctx.handle.controller.config.vel_limit = vel_limit + +def get_max_rpm(axis_ctx: AxisTestContext): + + # Calculate theoretical max velocity in rpm based on the nominal + # V_bus and motor KV rating. + # The KV-rating assumes square-waves on the motor phases (hexagonal space vector trajectory) + # whereas the ODrive modulates the space vector around a circular trajectory. + # See Fig 4.28 here: http://krex.k-state.edu/dspace/bitstream/handle/2097/1507/JamesMevey2009.pdf + effective_bus_voltage = axis_ctx.odrv_ctx.yaml['vbus-voltage'] + effective_bus_voltage *= (2/math.sqrt(3)) / (4/math.pi) # roughtly 90% + # The ODrive only goes to 80% modulation depth in order to save some time for the ADC measurements. + # See FOC_current in motor.cpp. + effective_bus_voltage *= 0.8 + + # If we are using a higher bus voltage than rated: use rated voltage, + # since that is an effective speed rating of the motor + voltage_for_speed = min(effective_bus_voltage, axis_ctx.yaml['motor-max-voltage']) + base_speed_rpm = voltage_for_speed * axis_ctx.yaml['motor-kv'] + + #but don't go over encoder max rpm + rated_rpm = min(base_speed_rpm, axis_ctx.yaml['encoder-max-rpm']) + return rated_rpm + +class ODriveTest(ABC): + """ + Tests inheriting from this class get full ownership of the ODrive + being tested. However no guarantees are made for the mechanical + state of the axes. + The test can demand exclusive run time which means that the host will + not run any other test at the same time. This can be used if the test + invokes a command that's so lame that it can't run twice concurrently. + """ + def __init__(self, exclusive=False): + self._exclusive = exclusive + def check_preconditions(self, odrv_ctx: ODriveTestContext, logger): + pass + @abc.abstractmethod + def run_test(self, odrv_ctx: ODriveTestContext, logger): + pass + +class AxisTest(ABC): + """ + Tests inheriting from this class get ownership of one axis of + an ODrive. If the axis is mechanically coupled to another + axis, the other axis is guaranteed to be disabled (high impedance) + during this test. + """ + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + if (abs(axis_ctx.handle.encoder.pll_vel) > 100): + logger.warn("axis still in motion, delaying 2 sec...") + time.sleep(2) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=500) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001) + test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) + #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 0.96, accuracy=0.001) + #test_assert_eq(axis_ctx.odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, axis_ctx.odrv_ctx.yaml['vbus-voltage'] * 1.04, accuracy=0.001) + + @abc.abstractmethod + def run_test(self, axis_ctx: AxisTestContext, logger): + pass + +class DualAxisTest(ABC): + """ + Tests using this scope get ownership of two axes that are mechanically + coupled. + """ + def check_preconditions(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + test_assert_no_error(axis0_ctx) + test_assert_no_error(axis1_ctx) + test_assert_eq(axis0_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_eq(axis1_ctx.handle.current_state, AXIS_STATE_IDLE) + if (abs(axis0_ctx.handle.encoder.pll_vel) > 100) or (abs(axis1_ctx.handle.encoder.pll_vel) > 100): + logger.warn("some axis still in motion, delaying 2 sec...") + time.sleep(2) + test_assert_eq(axis0_ctx.handle.encoder.pll_vel, 0, range=500) + test_assert_eq(axis1_ctx.handle.encoder.pll_vel, 0, range=500) + + @abc.abstractmethod + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + pass + +class TestDiscoverAndGotoIdle(ODriveTest): + def run_test(self, odrv_ctx: ODriveTestContext, logger): + odrv_ctx.rediscover() + clear_errors(odrv_ctx.axes[0]) + clear_errors(odrv_ctx.axes[1]) + request_state(odrv_ctx.axes[0], AXIS_STATE_IDLE) + request_state(odrv_ctx.axes[1], AXIS_STATE_IDLE) + +class TestFlashAndErase(ODriveTest): + def __init__(self): + ODriveTest.__init__(self, exclusive=True) + def run_test(self, odrv_ctx: ODriveTestContext, logger): + # Set board-version and compile + with open("tup.config", mode="w") as tup_config: + tup_config.write("CONFIG_STRICT=true\n") + tup_config.write("CONFIG_BOARD_VERSION={}\n".format(odrv_ctx.yaml['board-version'])) + #exit(1) + run("make", logger, timeout=10) + run("make flash PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=20) + # FIXME: device does not reboot correctly after erasing config this way + #run("make erase_config PROGRAMMER='" + test_rig.programmer + "'", timeout=10) + + logger.debug("waiting for ODrive...") + odrv_ctx.rediscover() + # ensure the correct odrive is returned + test_assert_eq(format(odrv_ctx.handle.serial_number, 'x').upper(), odrv_ctx.yaml['serial-number']) + + # erase configuration and reboot + logger.debug("erasing old configuration...") + odrv_ctx.handle.erase_configuration() + #time.sleep(0.1) + try: + # FIXME: sometimes the device does not reappear after this ("no response - probably incompatible") + # this is a firmware issue since it persists when unplugging/replugging + # but goes away when power cycling the device + odrv_ctx.handle.reboot() + except odrive.protocol.ChannelBrokenException: + pass # this is expected + time.sleep(0.5) + +class TestSetup(ODriveTest): + """ + Preconditions: ODrive is unconfigured and just rebooted + """ + def run_test(self, odrv_ctx: ODriveTestContext, logger): + odrv_ctx.rediscover() + + # initial protocol tests and setup + logger.debug("setting up ODrive...") + odrv_ctx.handle.config.enable_uart = True + test_assert_eq(odrv_ctx.handle.config.enable_uart, True) + odrv_ctx.handle.config.enable_uart = False + test_assert_eq(odrv_ctx.handle.config.enable_uart, False) + odrv_ctx.handle.config.brake_resistance = 1.0 + test_assert_eq(odrv_ctx.handle.config.brake_resistance, 1.0) + odrv_ctx.handle.config.brake_resistance = odrv_ctx.yaml['brake-resistance'] + test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) + odrv_ctx.handle.config.dc_bus_undervoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 0.85 + odrv_ctx.handle.config.dc_bus_overvoltage_trip_level = odrv_ctx.yaml['vbus-voltage'] * 1.08 + test_assert_eq(odrv_ctx.handle.config.dc_bus_undervoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 0.85, accuracy=0.001) + test_assert_eq(odrv_ctx.handle.config.dc_bus_overvoltage_trip_level, odrv_ctx.yaml['vbus-voltage'] * 1.08, accuracy=0.001) + + # firmware has 1500ms startup delay + time.sleep(2) + + logger.debug("ensure we're in idle state") + test_assert_eq(odrv_ctx.handle.axis0.current_state, AXIS_STATE_IDLE) + test_assert_eq(odrv_ctx.handle.axis1.current_state, AXIS_STATE_IDLE) + +class TestMotorCalibration(AxisTest): + """ + Tests motor calibration. + The calibration results are compared against well known test rig values. + Preconditions: The motor must be uncalibrated. + Postconditions: The motor will be calibrated after this test. + """ + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestMotorCalibration, self).check_preconditions(axis_ctx, logger) + test_assert_eq(axis_ctx.handle.motor.is_calibrated, False) + + def run_test(self, axis_ctx: AxisTestContext, logger): + logger.debug("try to enter closed loop control (should be rejected)") + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) + + logger.debug("try to start encoder index search (should be rejected)") + request_state(axis_ctx, AXIS_STATE_ENCODER_INDEX_SEARCH, expect_success=False) + + logger.debug("try to start encoder offset calibration (should be rejected)") + request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION, expect_success=False) + + logger.debug("motor calibration (takes about 4.5 seconds)") + axis_ctx.handle.motor.config.pole_pairs = axis_ctx.yaml['motor-pole-pairs'] + request_state(axis_ctx, AXIS_STATE_MOTOR_CALIBRATION) + time.sleep(6) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2) + test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) + axis_ctx.handle.motor.config.pre_calibrated = True + +class TestEncoderOffsetCalibration(AxisTest): + """ + Tests encoder offset calibration. + Preconditions: The encoder must be non-ready. + Postconditions: The encoder will be ready after this test. + """ + def __init__(self, pass_if_ready=False): + AxisTest.__init__(self) + self._pass_if_ready = pass_if_ready + + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestEncoderOffsetCalibration, self).check_preconditions(axis_ctx, logger) + if not self._pass_if_ready: + test_assert_eq(axis_ctx.handle.encoder.is_ready, False) + + def run_test(self, axis_ctx: AxisTestContext, logger): + if (self._pass_if_ready and axis_ctx.handle.encoder.is_ready): + logger.debug("encoder already ready, skipping this test") + return + + logger.debug("try to enter closed loop control (should be rejected)") + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL, expect_success=False) + + logger.debug("encoder offset calibration (takes about 9.5 seconds)") + axis_ctx.handle.encoder.config.cpr = axis_ctx.yaml['encoder-cpr'] # TODO: test setting a wrong CPR + request_state(axis_ctx, AXIS_STATE_ENCODER_OFFSET_CALIBRATION) + # TODO: ensure the encoder calibration doesn't do crap + time.sleep(11) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.motor.config.direction, axis_ctx.yaml['motor-direction']) + axis_ctx.handle.encoder.config.pre_calibrated = True + +class TestClosedLoopControl(AxisTest): + """ + Tests closed loop position control and velocity control + and verifies that the sensorless estimator works + Precondition: The axis is calibrated and ready for closed loop control + """ + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + super(TestClosedLoopControl, self).check_preconditions(axis_ctx, logger) + test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) + test_assert_eq(axis_ctx.handle.encoder.is_ready, True) + + def run_test(self, axis_ctx: AxisTestContext, logger): + logger.debug("closed loop control: test tiny position changes") + axis_ctx.handle.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL + time.sleep(0.001) + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + time.sleep(0.1) # give the PLL some time to settle + init_pos = axis_ctx.handle.encoder.pos_estimate + axis_ctx.handle.controller.set_pos_setpoint(init_pos+1000, 0, 0) + time.sleep(0.5) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos+1000, range=200) + axis_ctx.handle.controller.set_pos_setpoint(init_pos-1000, 0, 0) + time.sleep(0.5) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, init_pos-1000, range=400) + + logger.debug("closed loop control: test vel_limit") + axis_ctx.handle.controller.set_pos_setpoint(50000, 0, 0) + axis_ctx.handle.controller.config.vel_limit = 40000 + time.sleep(0.3) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 40000, range=4000) + expected_sensorless_estimation = 40000 * 2 * math.pi / axis_ctx.yaml['encoder-cpr'] * axis_ctx.yaml['motor-pole-pairs'] + test_assert_eq(axis_ctx.handle.sensorless_estimator.pll_vel, expected_sensorless_estimation, range=50) + time.sleep(3) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=1000) + time.sleep(0.5) + request_state(axis_ctx, AXIS_STATE_IDLE) + +class TestStoreAndReboot(ODriveTest): + """ + Stores the current configuration to NVM and reboots. + """ + def run_test(self, odrv_ctx: ODriveTestContext, logger): + logger.debug("storing configuration and rebooting...") + odrv_ctx.handle.save_configuration() + try: + odrv_ctx.handle.reboot() + except odrive.protocol.ChannelBrokenException: + pass # this is expected + time.sleep(2) + + odrv_ctx.rediscover() + + logger.debug("verifying configuration after reboot...") + test_assert_eq(odrv_ctx.handle.config.brake_resistance, odrv_ctx.yaml['brake-resistance'], accuracy=0.01) + for axis_ctx in odrv_ctx.axes: + test_assert_eq(axis_ctx.handle.encoder.config.cpr, axis_ctx.yaml['encoder-cpr']) + test_assert_eq(axis_ctx.handle.motor.config.phase_resistance, axis_ctx.yaml['motor-phase-resistance'], accuracy=0.2) + test_assert_eq(axis_ctx.handle.motor.config.phase_inductance, axis_ctx.yaml['motor-phase-inductance'], accuracy=0.5) + + +class TestHighVelocity(AxisTest): + """ + Spins the motor up to it's max speed during a period of 10s. + The commanded max speed is based on the motor's KV rating and nominal V_bus, + however due to several factors the theoretical limit is about 72% of that. + The test passes if the motor follows the commanded ramp closely up to 90% of + the theoretical limit (and if no errors occur along the way). + """ + def __init__(self, override_current_limit=None, load_current=0, brake=True): + """ + param override_current_limit: If None, the test selects a current limit that is guaranteed + not to fry the brake resistor. If you override the limit, you're + on your own. + """ + self._override_current_limit = override_current_limit + self._load_current = load_current + self._brake = brake + + def check_preconditions(self, axis_ctx: AxisTestContext, logger): + # time.sleep(2.5) #delay in case load needs time to stop moving + super(TestHighVelocity, self).check_preconditions(axis_ctx, logger) + test_assert_eq(axis_ctx.handle.motor.is_calibrated, True) + test_assert_eq(axis_ctx.handle.encoder.is_ready, True) + + def run_test(self, axis_ctx: AxisTestContext, logger): + rated_limit = get_max_rpm(axis_ctx) / 60 * axis_ctx.yaml['encoder-cpr'] + expected_limit = rated_limit + + # TODO: remove the following two lines, but for now we want to stay away from the modulation depth limit + expected_limit *= 0.6 + rated_limit = expected_limit + + # Add a 10% margin to account for + expected_limit *= 0.9 + + logger.debug("rated max speed: {}, expected max speed: >= {}".format(rated_limit, expected_limit)) + #theoretical_limit = 100000 + + # Set the current limit accordingly so we don't burn the brake resistor while slowing down + if self._override_current_limit is None: + set_limits(axis_ctx, logger, vel_limit=rated_limit, current_limit=50) + else: + axis_ctx.handle.motor.config.current_lim = self._override_current_limit + axis_ctx.handle.controller.config.vel_limit = rated_limit + axis_ctx.handle.controller.set_vel_setpoint(0, 0) + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + logger.debug("Drive current {}A, Load current {}A".format(axis_ctx.handle.motor.config.current_lim, self._load_current)) + + ramp_up_time = 15.0 + max_measured_vel = 0.0 + logger.debug("ramping to {} over {} s".format(rated_limit, ramp_up_time)) + t_0 = time.monotonic() + last_print = t_0 + while True: + ratio = (time.monotonic() - t_0) / ramp_up_time + if ratio >= 1: + break + + #TODO based on integrator gain and torque ramp rate + expected_ramp_lag = 1.0 * (rated_limit / ramp_up_time) + expected_lag = 0 + + # While ramping up we want to remain within +-5% of the setpoint. + # However we accept if we can only approach 80% of the theoretical limit. + vel_setpoint = ratio * rated_limit + expected_velocity = max(vel_setpoint - expected_lag, 0) + vel_range = max(0.05*expected_velocity, max(expected_lag+expected_ramp_lag, 2000)) + if expected_velocity - vel_range > expected_limit: + vel_range = expected_velocity - expected_limit + + # set and measure velocity + axis_ctx.handle.controller.set_vel_setpoint(vel_setpoint, 0) + measured_vel = axis_ctx.handle.encoder.pll_vel + max_measured_vel = max(measured_vel, max_measured_vel) + test_assert_eq(measured_vel, expected_velocity, range=vel_range) + test_assert_no_error(axis_ctx) + + # log progress + if time.monotonic() - last_print > 1: + last_print = time.monotonic() + logger.debug("ramping up: commanded {}, expected {}, measured {} ".format(vel_setpoint, expected_velocity, measured_vel)) + + time.sleep(0.001) + + logger.debug("reached top speed of {} counts/sec".format(max_measured_vel)) + + if self._brake: + axis_ctx.handle.controller.set_vel_setpoint(0, 0) + time.sleep(0.5) + # If the velocity integrator at work, it may now work against slowing down. + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=rated_limit*0.3) + # TODO: this is not a good bound, but the encoder float resolution results in a bad velocity estimate after this many turns + time.sleep(0.5) + test_assert_eq(axis_ctx.handle.encoder.pll_vel, 0, range=2000) + request_state(axis_ctx, AXIS_STATE_IDLE) + test_assert_no_error(axis_ctx) + + +class TestHighVelocityInViscousFluid(DualAxisTest): + """ + Runs TestHighVelocity on one motor while using the other motor as a load. + The load is created by running velocity control with setpoint 0. + """ + def __init__(self, load_current=10, driver_current=20): + self._load_current = load_current + self._driver_current = driver_current + + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + load_ctx = axis0_ctx + driver_ctx = axis1_ctx + if driver_ctx.name == 'top-odrive.black': + # odrive.utils.start_liveplotter(lambda: [driver_ctx.odrv_ctx.handle.vbus_voltage]) + odrive.utils.start_liveplotter(lambda: [driver_ctx.handle.motor.current_control.Iq_measured, + driver_ctx.handle.motor.current_control.Iq_setpoint]) + + # Set up viscous fluid load + logger.debug("activating load on {}...".format(load_ctx.name)) + load_ctx.handle.controller.config.vel_integrator_gain = 0 + load_ctx.handle.motor.config.current_lim = self._load_current + load_ctx.odrv_ctx.handle.config.brake_resistance = 0 # disable brake resistance, the power will go into the bus + load_ctx.handle.controller.set_vel_setpoint(0, 0) + + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + driver_test = TestHighVelocity( + override_current_limit=self._driver_current, + load_current=self._load_current, brake=False) + driver_test.check_preconditions(driver_ctx, logger) + driver_test.run_test(driver_ctx, logger) + + # put load to idle as quickly as possible, otherwise, because the brake resistor is disabled, + # it will try to put the braking power into the power rail where it has nowhere to go. + request_state(load_ctx, AXIS_STATE_IDLE) + request_state(driver_ctx, AXIS_STATE_IDLE) + +class TestSelfLoadedPosVelDistribution(DualAxisTest): + """ + Uses an ODrive mechanically connected to itself to test a distribution of + speeds and currents. Since it's connected to itself, we can be a lot less + strict about the brake resistor power use. + """ + def __init__(self, rpm_range=1000, load_current_range=10, driver_current_lim=20): + self._rpm_range = rpm_range + self._load_current_range = load_current_range + self._driver_current_lim = driver_current_lim + + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + load_ctx = axis0_ctx + driver_ctx = axis1_ctx + + logger.debug("Iload range: {} A, Idriver: {} A".format(self._load_current_range, self._driver_current_lim)) + + # max speed for rig in counts/s for each encoder (may be different CPR) + max_rpm = min(self._rpm_range, get_max_rpm(driver_ctx), get_max_rpm(load_ctx)) + driver_max_speed = max_rpm / 60 * driver_ctx.yaml['encoder-cpr'] + load_max_speed = max_rpm / 60 * load_ctx.yaml['encoder-cpr'] + logger.debug("RPM range: {} = driver {} = load {}".format(max_rpm, driver_max_speed, load_max_speed)) + + # Set up velocity controlled load + logger.debug("activating load on {}".format(load_ctx.name)) + load_ctx.handle.controller.config.vel_integrator_gain = 0 + load_ctx.handle.controller.config.vel_limit = load_max_speed + load_ctx.handle.motor.config.current_lim = 0 #load current to be set during runtime + load_ctx.handle.controller.set_vel_setpoint(0, 0) # vel sign also set during runtime + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Set up velocity controlled driver + logger.debug("activating driver on {}".format(driver_ctx.name)) + driver_ctx.handle.motor.config.current_lim = self._driver_current_lim + driver_ctx.handle.controller.config.vel_limit = driver_max_speed + driver_ctx.handle.controller.set_vel_setpoint(0, 0) + request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Spiral parameters + command_rate = 500.0 #Hz (nominal, achived rate is less due to time.sleep approx) + test_duration = 20.0 #s + num_cycles = 3.0 # number of spiral "rotations" + + t_0 = time.monotonic() + t_ratio = 0 + last_print = t_0 + while t_ratio < 1: + t_ratio = (time.monotonic() - t_0) / test_duration + phase = 2 * math.pi * num_cycles * t_ratio + driver_speed = t_ratio * driver_max_speed * math.sin(phase) + # print(driver_speed) + driver_ctx.handle.controller.set_vel_setpoint(driver_speed, 0) + load_current = t_ratio * self._load_current_range * math.cos(phase) + Iload_mag = abs(load_current) + Iload_sign = np.sign(load_current) + # print("I: {}, vel {}".format(Iload_mag, Iload_sign * load_max_speed)) + load_ctx.handle.motor.config.current_lim = Iload_mag + load_ctx.handle.controller.set_vel_setpoint(Iload_sign * load_max_speed, 0) + + test_assert_no_error(driver_ctx) + test_assert_no_error(load_ctx) + + # log progress + if time.monotonic() - last_print > 1: + last_print = time.monotonic() + logger.debug("Envelope -- vel: {:.2f}, I: {:.2f}".format(t_ratio * driver_max_speed, t_ratio * self._load_current_range)) + + time.sleep(1/command_rate) + + request_state(load_ctx, AXIS_STATE_IDLE) + request_state(driver_ctx, AXIS_STATE_IDLE) + test_assert_no_error(driver_ctx) + test_assert_no_error(load_ctx) + +class TestVelCtrlVsPosCtrl(DualAxisTest): + """ + Uses one ODrive as a load operating in velocity control mode. + The other ODrive tries to "fight" against the load in position mode. + """ + def run_test(self, axis0_ctx: AxisTestContext, axis1_ctx: AxisTestContext, logger): + load_ctx = axis0_ctx + driver_ctx = axis1_ctx + + # Set up viscous fluid load + logger.debug("activating load on {}...".format(load_ctx.name)) + load_ctx.handle.controller.config.vel_integrator_gain = 0 + load_ctx.handle.controller.vel_integrator_current = 0 + set_limits(load_ctx, logger, vel_limit=100000, current_limit=50) + load_ctx.handle.controller.set_vel_setpoint(0, 0) + request_state(load_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Turn to some position + logger.debug("using {} as driver against load, vel=100000...".format(driver_ctx.name)) + set_limits(driver_ctx, logger, vel_limit=100000, current_limit=50) + init_pos = driver_ctx.handle.encoder.pos_estimate + driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) + request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + for _ in range(int(4000/5)): + logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint)) + time.sleep(0.005) + + test_assert_no_error(load_ctx) + test_assert_no_error(driver_ctx) + + logger.debug("using {} as driver against load, vel=20000...".format(driver_ctx.name)) + set_limits(driver_ctx, logger, vel_limit=20000, current_limit=50) + init_pos = driver_ctx.handle.encoder.pos_estimate + driver_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) + request_state(driver_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + #for _ in range(int(5*4000/5)): + # logger.debug(str(driver_ctx.handle.motor.current_control.Iq_setpoint)) + # time.sleep(0.005) + time.sleep(7) + + odrive.utils.print_drv_regs("load motor ({})".format(load_ctx.name), load_ctx.handle.motor) + odrive.utils.print_drv_regs("driver motor ({})".format(driver_ctx.name), driver_ctx.handle.motor) + + test_assert_no_error(load_ctx) + test_assert_no_error(driver_ctx) + + ## Turn to another position + #logger.debug("controlling against load, vel=40000...") + #set_limits(axis1_ctx, logger, vel_limit=40000, current_limit=20) + #init_pos = axis1_ctx.handle.encoder.pos_estimate + #axis1_ctx.handle.controller.set_pos_setpoint(init_pos + 100000, 0, 0) + #request_state(axis1_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) diff --git a/tools/odrive/usbbulk_transport.py b/tools/odrive/usbbulk_transport.py index 5a3bd772..eefc33e8 100644 --- a/tools/odrive/usbbulk_transport.py +++ b/tools/odrive/usbbulk_transport.py @@ -118,7 +118,7 @@ class USBBulkTransport(odrive.protocol.PacketSource, odrive.protocol.PacketSink) except usb.core.USBError as ex: if ex.errno == 19: # "no such device" raise odrive.protocol.ChannelBrokenException() - elif ex.errno == 110: # timeout + elif ex.errno is None or ex.errno == 110: # timeout raise odrive.utils.TimeoutException() else: self._printer("halt condition: {}".format(ex.errno)) @@ -167,7 +167,7 @@ def discover_channels(path, serial_number, callback, cancellation_token, channel return True while not cancellation_token.is_set(): - printer("USB discover loop") + # printer("USB discover loop") devices = usb.core.find(find_all=True, custom_match=device_matcher) for usb_device in devices: try: diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 6d8c8cc1..050d4110 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -16,7 +16,9 @@ try: import colorama colorama.init() except ModuleNotFoundError: - print("Could not init terminal colors") + print("Could not init terminal features.") + print("Refer to install instructions at http://docs.odriverobotics.com/#downloading-and-installing-tools") + sys.stdout.flush() pass data_rate = 100 @@ -92,6 +94,16 @@ def print_drv_regs(name, motor): print("Control Reg 1: " + str(ctrl_reg_1) + " (" + format(ctrl_reg_1, '#013b') + ")") print("Control Reg 2: " + str(ctrl_reg_2) + " (" + format(ctrl_reg_2, '#09b') + ")") +def show_oscilloscope(odrv): + size = 18000 + values = [] + for i in range(size): + values.append(odrv.get_oscilloscope_val(i)) + + import matplotlib.pyplot as plt + plt.plot(values) + plt.show() + def rate_test(device): """ Tests how many integers per second can be transmitted @@ -113,6 +125,27 @@ def rate_test(device): FramePerSec = loopsPerSec/loopsPerFrame print("Frames per second: " + str(FramePerSec)) +def usb_burn_in_test(get_var_callback, cancellation_token): + """ + Starts background threads that read a values form the USB device in a spin-loop + """ + + def fetch_data(): + global vals + i = 0 + while not cancellation_token.is_set(): + try: + get_var_callback() + i += 1 + except Exception as ex: + print(str(ex)) + time.sleep(1) + i = 0 + continue + if i % 1000 == 0: + print("read {} values".format(i)) + threading.Thread(target=fetch_data).start() + def setup_udev_rules(logger): if platform.system() != 'Linux': logger.error("This command only makes sense on Linux") @@ -221,6 +254,7 @@ def wait_any(timeout=None, *events): return i raise TimeoutException() + class Logger(): """ Logs messages to stdout @@ -252,6 +286,7 @@ class Logger(): self._prefix = '' self._skip_bottom_line = False # If true, messages are printed one line above the cursor self._verbose = verbose + self._print_lock = threading.Lock() if platform.system() == 'Windows': self._stdout_buf = win32console.GetStdHandle(win32console.STD_OUTPUT_HANDLE) @@ -299,18 +334,22 @@ class Logger(): # (print text) # ESC 8: restore old cursor position + self._print_lock.acquire() sys.stdout.write('\x1b7\x1b[1A\x1b[1S\x1b[1L') sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT]) sys.stdout.write('\x1b8') sys.stdout.flush() + self._print_lock.release() def print_colored(self, text, color): if self._skip_bottom_line: self.print_on_second_last_line(text, color) else: # On Windows, colorama does the job of interpreting the VT100 escape sequences + self._print_lock.acquire() sys.stdout.write(Logger._VT100Colors[color] + text + Logger._VT100Colors[Logger.COLOR_DEFAULT] + '\n') sys.stdout.flush() + self._print_lock.release() def debug(self, text): if self._verbose: diff --git a/tools/odrive/version.py b/tools/odrive/version.py index 327c915c..68bad7d0 100644 --- a/tools/odrive/version.py +++ b/tools/odrive/version.py @@ -4,7 +4,29 @@ import subprocess import os import sys -def get_version(git_only=False): +def get_version_from_git(): + script_dir = os.path.dirname(os.path.realpath(__file__)) + try: + # Determine the current git commit version + git_tag = subprocess.check_output(["git", "describe", "--always", "--tags", "--dirty=*"], + cwd=script_dir) + git_tag = git_tag.decode(sys.stdout.encoding).rstrip('\n') + + regex=r'.*v([0-9a-zA-Z]).([0-9a-zA-Z]).([0-9a-zA-Z])(.*)' + package_version_major = int(re.sub(regex, r"\1", git_tag)) + package_version_minor = int(re.sub(regex, r"\2", git_tag)) + package_version_revision = int(re.sub(regex, r"\3", git_tag)) + package_version_unreleased = (re.sub(regex, r"\4", git_tag) != "") + + if package_version_unreleased: + package_version_revision += 1 + + except Exception as ex: + print(ex) + return "[unknown version]", 0, 0, 0, 1 + return git_tag, package_version_major, package_version_minor, package_version_revision, package_version_unreleased + +def get_version_str(git_only=False): """ Returns the versions of the tools If git_only is true, the version.txt file is ignored even @@ -18,29 +40,24 @@ def get_version(git_only=False): if os.path.exists(version_file_path) and git_only == False: with open(version_file_path) as version_file: return version_file.readline().rstrip('\n') - - try: - # Determine the current git commit version - git_result = subprocess.run(["git", "describe", "--always", "--tags", "--dirty=*"], - cwd=script_dir, - stdout=subprocess.PIPE, timeout=10) - git_tag = git_result.stdout.decode(sys.stdout.encoding) - - regex=r'.*v([0-9a-zA-Z]).([0-9a-zA-Z]).([0-9a-zA-Z])(.*)' - package_version_major = int(re.sub(regex, r"\1", git_tag)) - package_version_minor = int(re.sub(regex, r"\2", git_tag)) - package_version_revision = int(re.sub(regex, r"\3", git_tag)) - package_version_unreleased = (re.sub(regex, r"\4", git_tag) != "") - - if package_version_unreleased: - package_version_revision += 1 - - # TODO: fetch from Git describe - version = '{}.{}.{}'.format(package_version_major, package_version_minor, package_version_revision) - - if package_version_unreleased: - version += ".dev" - except Exception as ex: - print(ex) - version = "whatever version in " + script_dir + + _, major, minor, revision, unreleased = get_version_from_git() + version = '{}.{}.{}'.format(major, minor, revision) + if unreleased: + version += ".dev" return version + +if __name__ == '__main__': + import argparse + parser = argparse.ArgumentParser(description='Version Dump\n') + parser.add_argument("--output", type=argparse.FileType('w'), default='-', + help="C header output file") + + args = parser.parse_args() + + git_name, major, minor, revision, unreleased = get_version_from_git() + args.output.write('#define FW_VERSION "{}"\n'.format(git_name)) + args.output.write('#define FW_VERSION_MAJOR {}\n'.format(major)) + args.output.write('#define FW_VERSION_MINOR {}\n'.format(minor)) + args.output.write('#define FW_VERSION_REVISION {}\n'.format(revision)) + args.output.write('#define FW_VERSION_UNRELEASED {}\n'.format(1 if unreleased else 0)) diff --git a/tools/odrivetool b/tools/odrivetool index a455df08..901ebde3 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -17,7 +17,6 @@ def print(*args, **kwargs): kwargs.pop('flush', False) old_print(*args, **kwargs) file = kwargs.get('file', sys.stdout) - # Why might file=None? IDK, but it works for print(i, file=None) file.flush() if file is not None else sys.stdout.flush() @@ -87,7 +86,8 @@ else: logger = Logger(verbose=args.verbose) def print_version(): - print("ODrive control utility v" + odrive.__version__) + sys.stderr.write("ODrive control utility v" + odrive.__version__ + "\n") + sys.stderr.flush() app_shutdown_token = Event() @@ -119,8 +119,8 @@ try: # If you want to plot different values, change them here. # You can plot any number of values concurrently. - start_liveplotter(lambda: [my_odrive.motor0.encoder.pll_pos, - my_odrive.motor1.encoder.pll_pos]) + start_liveplotter(lambda: [my_odrive.motor0.encoder.pos_estimate, + my_odrive.motor1.encoder.pos_estimate]) elif args.command == 'drv-status': from odrive.utils import print_drv_regs diff --git a/tools/run_tests.py b/tools/run_tests.py new file mode 100755 index 00000000..1cb1bcd8 --- /dev/null +++ b/tools/run_tests.py @@ -0,0 +1,246 @@ +#!/bin/env python3 +# +# This script tests various functions of the ODrive firmware and +# the ODrive Python library. +# +# Usage: +# 1. adapt test-rig.yaml for your test rig. +# 2. ./run_tests.py + +import yaml +import os +import sys +import threading +import traceback +import argparse +from odrive.tests import * +from odrive.utils import Logger, Event + + +def for_all_parallel(objects, get_name, callback): + """ + Executes the specified callback for every object in the objects + list concurrently. This function waits for all callbacks to + finish and throws an exception if any of the callbacks throw + an exception. + """ + tracebacks = [] + + def run_callback(element): + try: + callback(element) + except Exception as ex: + tracebacks.append((get_name(element), ex)) + + # Start a thread for each element in the list + all_threads = [] + for element in objects: + thread = threading.Thread(target=run_callback, args=(element,)) + thread.start() + all_threads.append(thread) + + # Wait for all threads to complete + for thread in all_threads: + thread.join() + + if len(tracebacks) == 1: + msg = "task {} failed.".format(tracebacks[0][0]) + raise Exception(msg) from tracebacks[0][1] + elif len(tracebacks) > 1: + msg = "task {} and {} failed.".format( + tracebacks[0][0], + "one other" if len(tracebacks) == 2 else str(len(tracebacks)-1) + " others" + ) + raise Exception(msg) from tracebacks[0][1] + + +script_path=os.path.dirname(os.path.realpath(__file__)) + +parser = argparse.ArgumentParser(description='ODrive automated test tool\n') +parser.add_argument("--skip-boring-tests", action="store_true", + help="Skip the boring tests and go right to the high power tests") +parser.add_argument("--ignore", metavar='DEVICE', action='store', nargs='+', + help="Ignore one or more ODrives or axes") +parser.add_argument("--test-rig-yaml", type=argparse.FileType('r'), + help="test rig YAML file") +# parser.set_defaults(test_rig_yaml=script_path + '/test-rig-parallel.yaml') +parser.set_defaults(ignore=[]) +args = parser.parse_args() +test_rig_yaml = yaml.load(args.test_rig_yaml) + +# TODO: add --only option + + +all_tests = [] +if not args.skip_boring_tests: + all_tests.append(TestFlashAndErase()) + all_tests.append(TestSetup()) + all_tests.append(TestMotorCalibration()) + # # TODO: test encoder index search + all_tests.append(TestEncoderOffsetCalibration()) + # # TODO: hold down one motor while the other one does an index search (should fail) + all_tests.append(TestClosedLoopControl()) + all_tests.append(TestStoreAndReboot()) + all_tests.append(TestEncoderOffsetCalibration()) # need to find offset _or_ index after reboot + all_tests.append(TestClosedLoopControl()) +else: + all_tests.append(TestDiscoverAndGotoIdle()) + all_tests.append(TestEncoderOffsetCalibration(pass_if_ready=True)) + +if test_rig_yaml['type'] == 'parallel': + #all_tests.append(TestHighVelocity()) + all_tests.append(TestHighVelocityInViscousFluid(load_current=35, driver_current=45)) + # all_tests.append(TestVelCtrlVsPosCtrl()) + # TODO: test step/dir + # TODO: test sensorless + # TODO: test ASCII protocol + # TODO: test protocol over UART +elif test_rig_yaml['type'] == 'loopback': + all_tests.append(TestSelfLoadedPosVelDistribution( + rpm_range=3000, load_current_range=60, driver_current_lim=70)) + + +print(str(args.ignore)) +logger = Logger() + +os.chdir(script_path + '/../Firmware') + +# Build a dictionary of odrive test contexts by name +odrives_by_name = {} +for odrv_idx, odrv_yaml in enumerate(test_rig_yaml['odrives']): + name = odrv_yaml['name'] if 'name' in odrv_yaml else 'odrive{}'.format(odrv_idx) + if not name in args.ignore: + odrives_by_name[name] = ODriveTestContext(name, odrv_yaml) + +# Build a dictionary of axis test contexts by name (e.g. odrive0.axis0) +axes_by_name = {} +for odrv_ctx in odrives_by_name.values(): + for axis_idx, axis_ctx in enumerate(odrv_ctx.axes): + if not axis_ctx.name in args.ignore: + axes_by_name[axis_ctx.name] = axis_ctx + +# Ensure mechanical couplings are valid +couplings = [] +if test_rig_yaml['couplings'] is None: + test_rig_yaml['couplings'] = {} +else: + for coupling in test_rig_yaml['couplings']: + c = [axes_by_name[axis_name] for axis_name in coupling if (axis_name in axes_by_name)] + if len(c) > 1: + couplings.append(c) + +app_shutdown_token = Event() + +try: + for test in all_tests: + if isinstance(test, ODriveTest): + def odrv_test_thread(odrv_name): + odrv_ctx = odrives_by_name[odrv_name] + logger.info('* running {} on {}...'.format(type(test).__name__, odrv_name)) + try: + test.check_preconditions(odrv_ctx, + logger.indent(' {}: '.format(odrv_name))) + except: + raise PreconditionsNotMet() + test.run_test(odrv_ctx, + logger.indent(' {}: '.format(odrv_name))) + + if test._exclusive: + for odrv in odrives_by_name: + odrv_test_thread(odrv) + else: + for_all_parallel(odrives_by_name, lambda x: type(test).__name__ + " on " + x, odrv_test_thread) + + elif isinstance(test, AxisTest): + def axis_test_thread(axis_name): + # Get all axes that are mechanically coupled with the axis specified by axis_name + conflicting_axes = sum([c for c in couplings if (axis_name in [a.name for a in c])], []) + # Remove duplicates + conflicting_axes = list(set(conflicting_axes)) + # Acquire lock for all conflicting axes + conflicting_axes.sort(key=lambda x: x.name) # prevent deadlocks + axis_ctx = axes_by_name[axis_name] + for conflicting_axis in conflicting_axes: + conflicting_axis.lock.acquire() + try: + if not app_shutdown_token.is_set(): + # Run test on this axis + logger.info('* running {} on {}...'.format(type(test).__name__, axis_name)) + try: + test.check_preconditions(axis_ctx, + logger.indent(' {}: '.format(axis_name))) + except: + raise PreconditionsNotMet() + test.run_test(axis_ctx, + logger.indent(' {}: '.format(axis_name))) + else: + logger.warn('- skipping {} on {}'.format(type(test).__name__, axis_name)) + except: + app_shutdown_token.set() + raise + finally: + # Release all conflicting axes + for conflicting_axis in conflicting_axes: + conflicting_axis.lock.release() + + for_all_parallel(axes_by_name, lambda x: type(test).__name__ + " on " + x, axis_test_thread) + + elif isinstance(test, DualAxisTest): + def dual_axis_test_thread(coupling): + coupling_name = "...".join([a.name for a in coupling]) + # Remove duplicates + coupled_axes = list(set(coupling)) + # Acquire lock for all conflicting axes + coupled_axes.sort(key=lambda x: x.name) # prevent deadlocks + for axis_ctx in coupled_axes: + axis_ctx.lock.acquire() + try: + if not app_shutdown_token.is_set(): + # Run test on this axis + logger.info('* running {} on {}...'.format(type(test).__name__, coupling_name)) + try: + test.check_preconditions(coupled_axes[0], coupled_axes[1], + logger.indent(' {}: '.format(coupling_name))) + except: + raise PreconditionsNotMet() + test.run_test(coupled_axes[0], coupled_axes[1], + logger.indent(' {}: '.format(coupling_name))) + else: + logger.warn('- skipping {} on {}...'.format(type(test).__name__, coupling_name)) + except: + app_shutdown_token.set() + raise + finally: + # Release all conflicting axes + for axis_ctx in coupled_axes: + axis_ctx.lock.release() + + for_all_parallel(couplings, lambda x: type(test).__name__ + " on " + "..".join([a.name for a in x]), dual_axis_test_thread) + + else: + logger.warn("ignoring unknown test type {}".format(type(test))) + +except: + logger.error(traceback.format_exc()) + logger.debug('=> Test failed. Please wait while I secure the test rig...') + try: + dont_secure_after_failure = False # TODO: disable + if not dont_secure_after_failure: + def odrv_reset_thread(odrv_name): + odrv_ctx = odrives_by_name[odrv_name] + #run("make erase PROGRAMMER='" + odrv_ctx.yaml['programmer'] + "'", logger, timeout=30) + odrv_ctx.handle.axis0.requested_state = AXIS_STATE_IDLE + odrv_ctx.handle.axis1.requested_state = AXIS_STATE_IDLE + dump_errors(odrv_ctx.axes[0], logger) + dump_errors(odrv_ctx.axes[1], logger) + + for_all_parallel(odrives_by_name, lambda x: x['name'], odrv_reset_thread) + except: + logger.error('///////////////////////////////////////////') + logger.error('/// CRITICAL: COULD NOT SECURE TEST RIG ///') + logger.error('/// CUT THE POWER IMMEDIATELY! ///') + logger.error('///////////////////////////////////////////') + else: + logger.error('some test failed!') +else: + logger.success('All tests succeeded!') diff --git a/tools/setup.py b/tools/setup.py index 946e6900..3cdb67d6 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -40,7 +40,7 @@ creating_package = "sdist" in sys.argv # Load version from Git tag import odrive.version -version = odrive.version.get_version(git_only=creating_package) +version = odrive.version.get_version_str(git_only=creating_package) # Change this if you already uploaded the current # version but need to release a hotfix diff --git a/tools/test-rig-loopback.yaml b/tools/test-rig-loopback.yaml new file mode 100644 index 00000000..12b87235 --- /dev/null +++ b/tools/test-rig-loopback.yaml @@ -0,0 +1,69 @@ + +type: loopback + +odrives: + - name: odrv-blackside + board-version: v3.4-24V + serial-number: "3061395B3235" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '\x49\x3f\x6f\x06\x49\x3f\x56\x54\x09\x29\x11\x3f' + vbus-voltage: 24 # [V] + max-brake-power: 150 # [W] + axes: + - name: 'M0' + motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 270 + motor-max-current: 70 + motor-max-voltage: 32 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: 'M1' + motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 270 + motor-max-current: 70 + motor-max-voltage: 32 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: odrv-yellowside + board-version: v3.5-48V + serial-number: "3660335E3037" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '\x53\x3f\x75\x06\x49\x3f\x49\x51\x44\x54\x19\x3f' + vbus-voltage: 48 # [V] + max-brake-power: 150 # [W] + axes: + - name: 'M0' + motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 190 + motor-max-current: 70 + motor-max-voltage: 40 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: 'M1' + motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 190 + motor-max-current: 70 + motor-max-voltage: 40 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + +# Mechanical couplings +couplings: + - [ odrv-blackside.M0, odrv-blackside.M1 ] + - [ odrv-yellowside.M0, odrv-yellowside.M1 ] \ No newline at end of file diff --git a/tools/test-rig-parallel.yaml b/tools/test-rig-parallel.yaml new file mode 100644 index 00000000..47173166 --- /dev/null +++ b/tools/test-rig-parallel.yaml @@ -0,0 +1,70 @@ + +type: parallel + +# ODrives +odrives: + - name: top-odrive + board-version: v3.5-48V + serial-number: "3660335E3037" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '\x53\x3f\x75\x06\x49\x3f\x49\x51\x44\x54\x19\x3f' + vbus-voltage: 24 # [V] + max-brake-power: 150 # [W] + axes: + - name: 'yellow' + motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 190 + motor-max-current: 70 + motor-max-voltage: 40 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: 'black' + motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 270 + motor-max-current: 70 + motor-max-voltage: 32 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: bottom-odrive + board-version: v3.5-24V + serial-number: "3661335E3037" + brake-resistance: 0.47 + uart: /dev/serial/by-id/[not-yet-used] + usb: auto + programmer: '\x49\x3f\x6f\x06\x49\x3f\x56\x54\x09\x29\x11\x3f' + vbus-voltage: 24 # [V] + max-brake-power: 150 # [W] + axes: + - name: 'black' + motor-phase-resistance: 0.028 + motor-phase-inductance: 1.6e-05 + motor-pole-pairs: 7 + motor-direction: 1 + motor-kv: 270 + motor-max-current: 70 + motor-max-voltage: 32 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + - name: 'yellow' + motor-phase-resistance: 0.0245 + motor-phase-inductance: 2.03e-05 + motor-pole-pairs: 7 + motor-direction: -1 + motor-kv: 190 + motor-max-current: 70 + motor-max-voltage: 40 + encoder-cpr: 8192 + encoder-max-rpm: 7000 + +# Mechanical couplings +couplings: + - [ top-odrive.yellow, bottom-odrive.yellow ] + - [ top-odrive.black, bottom-odrive.black ]