From ebfe0755bd2f5c2747d386e2760ee8da91b1495b Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 14 May 2018 01:54:02 -0700 Subject: [PATCH 01/35] set bandwidth to 100 --- Firmware/MotorControl/encoder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 289f97f18..3eec1edcd 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -9,7 +9,7 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, { // Calculate encoder pll gains // This calculation is currently identical to the PLL in SensorlessEstimator - float pll_bandwidth = 1000.0f; // [rad/s] + float pll_bandwidth = 100.0f; // [rad/s] pll_kp_ = 2.0f * pll_bandwidth; // Critically damped From e68181b1252d74a2ada25877ebedb9e09e23f771 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 14 May 2018 19:05:26 -0700 Subject: [PATCH 02/35] Cube set to enable TIM5 input capture on GPIO 3 and 4 --- Firmware/Board/v3/Inc/tim.h | 2 + Firmware/Board/v3/Odrive.ioc | 32 +++++++---- Firmware/Board/v3/Src/freertos.c | 16 ++++++ Firmware/Board/v3/Src/gpio.c | 12 ++--- Firmware/Board/v3/Src/main.c | 5 +- Firmware/Board/v3/Src/tim.c | 91 ++++++++++++++++++++++++++++++++ 6 files changed, 140 insertions(+), 18 deletions(-) diff --git a/Firmware/Board/v3/Inc/tim.h b/Firmware/Board/v3/Inc/tim.h index 4ef670767..7006b95a2 100644 --- a/Firmware/Board/v3/Inc/tim.h +++ b/Firmware/Board/v3/Inc/tim.h @@ -65,6 +65,7 @@ extern TIM_HandleTypeDef htim1; extern TIM_HandleTypeDef htim2; extern TIM_HandleTypeDef htim3; extern TIM_HandleTypeDef htim4; +extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim8; /* USER CODE BEGIN Private defines */ @@ -77,6 +78,7 @@ void MX_TIM1_Init(void); void MX_TIM2_Init(void); void MX_TIM3_Init(void); void MX_TIM4_Init(void); +void MX_TIM5_Init(void); void MX_TIM8_Init(void); void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index 70c238a47..2dbc6b727 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -104,10 +104,11 @@ Dma.UART4_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataA FREERTOS.FootprintOK=true FREERTOS.INCLUDE_uxTaskGetStackHighWaterMark=1 FREERTOS.INCLUDE_vTaskDelayUntil=1 -FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,INCLUDE_uxTaskGetStackHighWaterMark +FREERTOS.IPParameters=Tasks01,INCLUDE_vTaskDelayUntil,configTOTAL_HEAP_SIZE,FootprintOK,configCHECK_FOR_STACK_OVERFLOW,INCLUDE_uxTaskGetStackHighWaterMark,configUSE_IDLE_HOOK FREERTOS.Tasks01=defaultTask,0,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL FREERTOS.configCHECK_FOR_STACK_OVERFLOW=1 FREERTOS.configTOTAL_HEAP_SIZE=65536 +FREERTOS.configUSE_IDLE_HOOK=1 File.Version=6 KeepUserPlacement=true Mcu.Family=STM32F4 @@ -117,10 +118,11 @@ Mcu.IP10=TIM1 Mcu.IP11=TIM2 Mcu.IP12=TIM3 Mcu.IP13=TIM4 -Mcu.IP14=TIM8 -Mcu.IP15=UART4 -Mcu.IP16=USB_DEVICE -Mcu.IP17=USB_OTG_FS +Mcu.IP14=TIM5 +Mcu.IP15=TIM8 +Mcu.IP16=UART4 +Mcu.IP17=USB_DEVICE +Mcu.IP18=USB_OTG_FS Mcu.IP2=ADC3 Mcu.IP3=CAN1 Mcu.IP4=DMA @@ -129,7 +131,7 @@ Mcu.IP6=NVIC Mcu.IP7=RCC Mcu.IP8=SPI3 Mcu.IP9=SYS -Mcu.IPNb=18 +Mcu.IPNb=19 Mcu.Name=STM32F405RGTx Mcu.Package=LQFP64 Mcu.Pin0=PC13-ANTI_TAMP @@ -243,12 +245,11 @@ PA15.Signal=GPIO_Input PA2.GPIOParameters=GPIO_Label PA2.GPIO_Label=GPIO_3 PA2.Locked=true -PA2.Signal=GPIO_Input -PA3.GPIOParameters=GPIO_PuPd,GPIO_Label +PA2.Signal=S_TIM5_CH3 +PA3.GPIOParameters=GPIO_Label PA3.GPIO_Label=GPIO_4 -PA3.GPIO_PuPd=GPIO_NOPULL PA3.Locked=true -PA3.Signal=GPIO_Input +PA3.Signal=S_TIM5_CH4 PA4.GPIOParameters=GPIO_Label PA4.GPIO_Label=M1_TEMP PA4.Locked=true @@ -432,7 +433,7 @@ ProjectManager.StackSize=0x800 ProjectManager.TargetToolchain=Makefile ProjectManager.ToolChainLocation= ProjectManager.UnderRoot=false -ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-MX_ADC1_Init-ADC1-false-HAL-true,4-MX_ADC2_Init-ADC2-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_TIM8_Init-TIM8-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_TIM4_Init-TIM4-false-HAL-true,10-MX_SPI3_Init-SPI3-false-HAL-true,11-MX_ADC3_Init-ADC3-false-HAL-true,12-SystemClock_Config-RCC-false-HAL-true,13-MX_TIM2_Init-TIM2-false-HAL-true,14-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true +ProjectManager.functionlistsort=1-MX_GPIO_Init-GPIO-false-HAL-true,2-MX_DMA_Init-DMA-false-HAL-true,3-MX_ADC1_Init-ADC1-false-HAL-true,4-MX_ADC2_Init-ADC2-false-HAL-true,5-MX_CAN1_Init-CAN1-false-HAL-true,6-MX_TIM1_Init-TIM1-false-HAL-true,7-MX_TIM8_Init-TIM8-false-HAL-true,8-MX_TIM3_Init-TIM3-false-HAL-true,9-MX_TIM4_Init-TIM4-false-HAL-true,10-MX_SPI3_Init-SPI3-false-HAL-true,11-MX_ADC3_Init-ADC3-false-HAL-true,12-SystemClock_Config-RCC-false-HAL-true,13-MX_TIM2_Init-TIM2-false-HAL-true,14-MX_USB_DEVICE_Init-USB_DEVICE-false-HAL-true,15-MX_UART4_Init-UART4-false-HAL-true,16-MX_TIM5_Init-TIM5-false-HAL-true RCC.48MHZClocksFreq_Value=48000000 RCC.AHBFreq_Value=168000000 RCC.APB1CLKDivider=RCC_HCLK_DIV4 @@ -513,6 +514,10 @@ SH.S_TIM4_CH1.0=TIM4_CH1,Encoder_Interface SH.S_TIM4_CH1.ConfNb=1 SH.S_TIM4_CH2.0=TIM4_CH2,Encoder_Interface SH.S_TIM4_CH2.ConfNb=1 +SH.S_TIM5_CH3.0=TIM5_CH3,Input_Capture3_from_TI3 +SH.S_TIM5_CH3.ConfNb=1 +SH.S_TIM5_CH4.0=TIM5_CH4,Input_Capture4_from_TI4 +SH.S_TIM5_CH4.ConfNb=1 SH.S_TIM8_CH1.0=TIM8_CH1,PWM Generation1 CH1 CH1N SH.S_TIM8_CH1.ConfNb=1 SH.S_TIM8_CH2.0=TIM8_CH2,PWM Generation2 CH2 CH2N @@ -574,6 +579,11 @@ TIM4.IC2Filter=4 TIM4.IC2Polarity=TIM_ICPOLARITY_RISING TIM4.IPParameters=EncoderMode,IC1Polarity,IC2Polarity,IC1Filter,IC2Filter,Period TIM4.Period=0xffff +TIM5.Channel-Input_Capture3_from_TI3=TIM_CHANNEL_3 +TIM5.Channel-Input_Capture4_from_TI4=TIM_CHANNEL_4 +TIM5.ICFilter_CH3=15 +TIM5.ICFilter_CH4=15 +TIM5.IPParameters=Channel-Input_Capture3_from_TI3,Channel-Input_Capture4_from_TI4,ICFilter_CH3,ICFilter_CH4 TIM8.Channel-Output\ Compare4\ No\ Output=TIM_CHANNEL_4 TIM8.Channel-PWM\ Generation1\ CH1\ CH1N=TIM_CHANNEL_1 TIM8.Channel-PWM\ Generation2\ CH2\ CH2N=TIM_CHANNEL_2 diff --git a/Firmware/Board/v3/Src/freertos.c b/Firmware/Board/v3/Src/freertos.c index f1d466428..524cd6cbc 100644 --- a/Firmware/Board/v3/Src/freertos.c +++ b/Firmware/Board/v3/Src/freertos.c @@ -86,8 +86,24 @@ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ /* USER CODE END FunctionPrototypes */ /* Hook prototypes */ +void vApplicationIdleHook(void); void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName); +/* USER CODE BEGIN 2 */ +__weak void vApplicationIdleHook( void ) +{ + /* vApplicationIdleHook() will only be called if configUSE_IDLE_HOOK is set + to 1 in FreeRTOSConfig.h. It will be called on each iteration of the idle + task. It is essential that code added to this hook function never attempts + to block in any way (for example, call xQueueReceive() with a block time + specified, or call vTaskDelay()). If the application makes use of the + vTaskDelete() API function (as this demo application does) then it is also + important that vApplicationIdleHook() is permitted to return to its calling + function, because it is the responsibility of the idle task to clean up + memory allocated by the kernel to any task that has since been deleted. */ +} +/* USER CODE END 2 */ + /* USER CODE BEGIN 4 */ __weak void vApplicationStackOverflowHook(xTaskHandle xTask, signed char *pcTaskName) { diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 85b9028f0..25967a388 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -106,12 +106,6 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Pull = GPIO_NOPULL; HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); - /*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_6_Pin|GPIO_8_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; @@ -125,6 +119,12 @@ void MX_GPIO_Init(void) GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; HAL_GPIO_Init(EN_GATE_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : PtPin */ + GPIO_InitStruct.Pin = GPIO_7_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_INPUT; + GPIO_InitStruct.Pull = GPIO_NOPULL; + HAL_GPIO_Init(GPIO_7_GPIO_Port, &GPIO_InitStruct); + /*Configure GPIO pin : PtPin */ GPIO_InitStruct.Pin = nFAULT_Pin; GPIO_InitStruct.Mode = GPIO_MODE_INPUT; diff --git a/Firmware/Board/v3/Src/main.c b/Firmware/Board/v3/Src/main.c index 98b64d319..d515ba05d 100644 --- a/Firmware/Board/v3/Src/main.c +++ b/Firmware/Board/v3/Src/main.c @@ -178,6 +178,7 @@ int main(void) MX_DMA_Init(); MX_ADC1_Init(); MX_ADC2_Init(); + MX_CAN1_Init(); MX_TIM1_Init(); MX_TIM8_Init(); MX_TIM3_Init(); @@ -186,6 +187,7 @@ int main(void) MX_ADC3_Init(); MX_TIM2_Init(); MX_UART4_Init(); + MX_TIM5_Init(); /* USER CODE BEGIN 2 */ //Required to use OC4 for ADC triggering. @@ -233,8 +235,9 @@ void SystemClock_Config(void) /**Initializes the CPU, AHB and APB busses clocks */ - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_LSI|RCC_OSCILLATORTYPE_HSE; RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.LSIState = RCC_LSI_ON; RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; RCC_OscInitStruct.PLL.PLLM = 4; diff --git a/Firmware/Board/v3/Src/tim.c b/Firmware/Board/v3/Src/tim.c index 5a63b6dfb..e2e4e80e7 100644 --- a/Firmware/Board/v3/Src/tim.c +++ b/Firmware/Board/v3/Src/tim.c @@ -78,6 +78,7 @@ TIM_HandleTypeDef htim1; TIM_HandleTypeDef htim2; TIM_HandleTypeDef htim3; TIM_HandleTypeDef htim4; +TIM_HandleTypeDef htim5; TIM_HandleTypeDef htim8; /* TIM1 init function */ @@ -272,6 +273,44 @@ void MX_TIM4_Init(void) _Error_Handler(__FILE__, __LINE__); } +} +/* TIM5 init function */ +void MX_TIM5_Init(void) +{ + TIM_MasterConfigTypeDef sMasterConfig; + TIM_IC_InitTypeDef sConfigIC; + + htim5.Instance = TIM5; + htim5.Init.Prescaler = 0; + htim5.Init.CounterMode = TIM_COUNTERMODE_UP; + htim5.Init.Period = 0; + htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + if (HAL_TIM_IC_Init(&htim5) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING; + sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; + sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; + sConfigIC.ICFilter = 15; + if (HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, TIM_CHANNEL_3) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + + if (HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, TIM_CHANNEL_4) != HAL_OK) + { + _Error_Handler(__FILE__, __LINE__); + } + } /* TIM8 init function */ void MX_TIM8_Init(void) @@ -434,6 +473,35 @@ void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle) /* USER CODE END TIM4_MspInit 1 */ } } + +void HAL_TIM_IC_MspInit(TIM_HandleTypeDef* tim_icHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct; + if(tim_icHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspInit 0 */ + + /* USER CODE END TIM5_MspInit 0 */ + /* TIM5 clock enable */ + __HAL_RCC_TIM5_CLK_ENABLE(); + + /**TIM5 GPIO Configuration + PA2 ------> TIM5_CH3 + PA3 ------> TIM5_CH4 + */ + GPIO_InitStruct.Pin = GPIO_3_Pin|GPIO_4_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; + HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + + /* USER CODE BEGIN TIM5_MspInit 1 */ + + /* USER CODE END TIM5_MspInit 1 */ + } +} void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle) { @@ -617,6 +685,29 @@ void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle) /* USER CODE END TIM4_MspDeInit 1 */ } +} + +void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef* tim_icHandle) +{ + + if(tim_icHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspDeInit 0 */ + + /* USER CODE END TIM5_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM5_CLK_DISABLE(); + + /**TIM5 GPIO Configuration + PA2 ------> TIM5_CH3 + PA3 ------> TIM5_CH4 + */ + HAL_GPIO_DeInit(GPIOA, GPIO_3_Pin|GPIO_4_Pin); + + /* USER CODE BEGIN TIM5_MspDeInit 1 */ + + /* USER CODE END TIM5_MspDeInit 1 */ + } } /* USER CODE BEGIN 1 */ From d74c1b4b10688643db3c052c08f239a3ef73998c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 14 May 2018 19:33:55 -0700 Subject: [PATCH 03/35] add TIM5 irq, and custom decoder and callback dispatcher --- Firmware/Board/v3/Inc/stm32f4xx_it.h | 1 + Firmware/Board/v3/Odrive.ioc | 5 +++- Firmware/Board/v3/Src/stm32f4xx_it.c | 39 ++++++++++++++++++++++++++++ Firmware/Board/v3/Src/tim.c | 7 ++++- 4 files changed, 50 insertions(+), 2 deletions(-) diff --git a/Firmware/Board/v3/Inc/stm32f4xx_it.h b/Firmware/Board/v3/Inc/stm32f4xx_it.h index 1970c2a8b..d5c53ffa4 100644 --- a/Firmware/Board/v3/Inc/stm32f4xx_it.h +++ b/Firmware/Board/v3/Inc/stm32f4xx_it.h @@ -58,6 +58,7 @@ void DMA1_Stream2_IRQHandler(void); void DMA1_Stream4_IRQHandler(void); void ADC_IRQHandler(void); void TIM8_TRG_COM_TIM14_IRQHandler(void); +void TIM5_IRQHandler(void); void UART4_IRQHandler(void); void OTG_FS_IRQHandler(void); diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index 2dbc6b727..d87ee816b 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -209,6 +209,7 @@ NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:true NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:true NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:true +NVIC.TIM5_IRQn=true\:5\:0\:false\:false\:true\:true\:true NVIC.TIM8_TRG_COM_TIM14_IRQn=true\:0\:0\:false\:false\:true\:false\:false NVIC.TimeBase=TIM8_TRG_COM_TIM14_IRQn NVIC.TimeBaseIP=TIM14 @@ -583,7 +584,9 @@ TIM5.Channel-Input_Capture3_from_TI3=TIM_CHANNEL_3 TIM5.Channel-Input_Capture4_from_TI4=TIM_CHANNEL_4 TIM5.ICFilter_CH3=15 TIM5.ICFilter_CH4=15 -TIM5.IPParameters=Channel-Input_Capture3_from_TI3,Channel-Input_Capture4_from_TI4,ICFilter_CH3,ICFilter_CH4 +TIM5.ICPolarity_CH3=TIM_INPUTCHANNELPOLARITY_BOTHEDGE +TIM5.ICPolarity_CH4=TIM_INPUTCHANNELPOLARITY_BOTHEDGE +TIM5.IPParameters=Channel-Input_Capture3_from_TI3,Channel-Input_Capture4_from_TI4,ICFilter_CH3,ICFilter_CH4,ICPolarity_CH3,ICPolarity_CH4 TIM8.Channel-Output\ Compare4\ No\ Output=TIM_CHANNEL_4 TIM8.Channel-PWM\ Generation1\ CH1\ CH1N=TIM_CHANNEL_1 TIM8.Channel-PWM\ Generation2\ CH2\ CH2N=TIM_CHANNEL_2 diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 1a650abca..3026634d5 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -43,6 +43,9 @@ typedef void (*ADC_handler_t)(ADC_HandleTypeDef* hadc, bool injected); void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback); +typedef void (*TIM_capture_callback_t)(int channel, uint32_t timestamp); +void decode_tim_capture(TIM_HandleTypeDef *htim, TIM_capture_callback_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); @@ -54,6 +57,7 @@ extern PCD_HandleTypeDef hpcd_USB_OTG_FS; extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; extern ADC_HandleTypeDef hadc3; +extern TIM_HandleTypeDef htim5; extern TIM_HandleTypeDef htim8; extern DMA_HandleTypeDef hdma_uart4_rx; extern DMA_HandleTypeDef hdma_uart4_tx; @@ -253,6 +257,23 @@ void TIM8_TRG_COM_TIM14_IRQHandler(void) /* USER CODE END TIM8_TRG_COM_TIM14_IRQn 1 */ } +/** +* @brief This function handles TIM5 global interrupt. +*/ +void TIM5_IRQHandler(void) +{ + /* USER CODE BEGIN TIM5_IRQn 0 */ + + // We know we only use capture mode here, so bypass HAL + decode_tim_capture(&htim5, &pwm_in_cb); + + /* USER CODE END TIM5_IRQn 0 */ + HAL_TIM_IRQHandler(&htim5); + /* USER CODE BEGIN TIM5_IRQn 1 */ + + /* USER CODE END TIM5_IRQn 1 */ +} + /** * @brief This function handles UART4 global interrupt. */ @@ -308,6 +329,24 @@ void ADC_IRQ_Dispatch(ADC_HandleTypeDef* hadc, ADC_handler_t callback) { } } +void decode_tim_capture(TIM_HandleTypeDef *htim, TIM_capture_callback_t callback) { + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC1)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC1); + callback(1, htim->Instance->CCR1); + } + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC2)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC2); + callback(2, htim->Instance->CCR2); + } + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC3)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC3); + callback(3, htim->Instance->CCR3); + } + if(__HAL_TIM_GET_FLAG(htim, TIM_FLAG_CC4)) { + __HAL_TIM_CLEAR_IT(htim, TIM_IT_CC4); + callback(4, htim->Instance->CCR4); + } +} /** * @brief This function handles EXTI line0 interrupt. diff --git a/Firmware/Board/v3/Src/tim.c b/Firmware/Board/v3/Src/tim.c index e2e4e80e7..467c50308 100644 --- a/Firmware/Board/v3/Src/tim.c +++ b/Firmware/Board/v3/Src/tim.c @@ -297,7 +297,7 @@ void MX_TIM5_Init(void) _Error_Handler(__FILE__, __LINE__); } - sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING; + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; sConfigIC.ICFilter = 15; @@ -497,6 +497,9 @@ void HAL_TIM_IC_MspInit(TIM_HandleTypeDef* tim_icHandle) GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; HAL_GPIO_Init(GPIOA, &GPIO_InitStruct); + /* TIM5 interrupt Init */ + HAL_NVIC_SetPriority(TIM5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(TIM5_IRQn); /* USER CODE BEGIN TIM5_MspInit 1 */ /* USER CODE END TIM5_MspInit 1 */ @@ -704,6 +707,8 @@ void HAL_TIM_IC_MspDeInit(TIM_HandleTypeDef* tim_icHandle) */ HAL_GPIO_DeInit(GPIOA, GPIO_3_Pin|GPIO_4_Pin); + /* TIM5 interrupt Deinit */ + HAL_NVIC_DisableIRQ(TIM5_IRQn); /* USER CODE BEGIN TIM5_MspDeInit 1 */ /* USER CODE END TIM5_MspDeInit 1 */ From 8625ec063572f64164a37c347faf4576c1203458 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 14 May 2018 22:41:04 -0700 Subject: [PATCH 04/35] set tim5 period to max 32bit --- Firmware/Board/v3/Odrive.ioc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Firmware/Board/v3/Odrive.ioc b/Firmware/Board/v3/Odrive.ioc index d87ee816b..6f8e9baeb 100644 --- a/Firmware/Board/v3/Odrive.ioc +++ b/Firmware/Board/v3/Odrive.ioc @@ -586,7 +586,8 @@ TIM5.ICFilter_CH3=15 TIM5.ICFilter_CH4=15 TIM5.ICPolarity_CH3=TIM_INPUTCHANNELPOLARITY_BOTHEDGE TIM5.ICPolarity_CH4=TIM_INPUTCHANNELPOLARITY_BOTHEDGE -TIM5.IPParameters=Channel-Input_Capture3_from_TI3,Channel-Input_Capture4_from_TI4,ICFilter_CH3,ICFilter_CH4,ICPolarity_CH3,ICPolarity_CH4 +TIM5.IPParameters=Channel-Input_Capture3_from_TI3,Channel-Input_Capture4_from_TI4,ICFilter_CH3,ICFilter_CH4,ICPolarity_CH3,ICPolarity_CH4,Period +TIM5.Period=0xFFFFFFFF TIM8.Channel-Output\ Compare4\ No\ Output=TIM_CHANNEL_4 TIM8.Channel-PWM\ Generation1\ CH1\ CH1N=TIM_CHANNEL_1 TIM8.Channel-PWM\ Generation2\ CH2\ CH2N=TIM_CHANNEL_2 From 51972dec7214441162ca86fd180e9d7a8b113829 Mon Sep 17 00:00:00 2001 From: Paul Guenette Date: Thu, 26 Apr 2018 00:04:21 -0400 Subject: [PATCH 05/35] Add GPIO translation functions --- Firmware/Board/v3/Inc/gpio.h | 2 ++ Firmware/Board/v3/Src/gpio.c | 20 ++++++++++++++++++++ 2 files changed, 22 insertions(+) diff --git a/Firmware/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index f8ffe61b1..7a5afb345 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -77,6 +77,8 @@ bool GPIO_subscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin, void (*callback)(void*), void* ctx); void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); +uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin); +GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin); /* USER CODE END Prototypes */ diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 25967a388..d6b551671 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -266,6 +266,26 @@ void HAL_GPIO_EXTI_Callback(uint16_t GPIO_pin) { } } +GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){ + switch(GPIO_pin){ + case 1: return GPIO_1_GPIO_Port; break; + case 2: return GPIO_2_GPIO_Port; break; + case 3: return GPIO_3_GPIO_Port; break; + case 4: return GPIO_4_GPIO_Port; break; + case 5: return GPIO_5_GPIO_Port; break; + } +} + +uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){ + switch(GPIO_pin){ + case 1: return GPIO_1_Pin; break; + case 2: return GPIO_2_Pin; break; + case 3: return GPIO_3_Pin; break; + case 4: return GPIO_4_Pin; break; + case 5: return GPIO_5_Pin; break; + } +} + /* USER CODE END 2 */ /** From 359eab32f6f0f60b5e21bd72bc1087ff7d54dbf4 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 15 May 2018 10:10:00 -0700 Subject: [PATCH 06/35] implement PWM input --- Firmware/Board/v3/Inc/gpio.h | 6 ++ Firmware/Board/v3/Src/gpio.c | 2 + Firmware/Board/v3/Src/stm32f4xx_it.c | 1 + Firmware/Board/v3/Src/tim.c | 2 +- Firmware/MotorControl/axis.hpp | 2 +- Firmware/MotorControl/low_level.cpp | 101 +++++++++++++++++++++++ Firmware/MotorControl/low_level.h | 2 + Firmware/MotorControl/main.cpp | 2 + Firmware/MotorControl/odrive_main.h | 7 ++ Firmware/communication/communication.cpp | 15 +++- Firmware/communication/protocol.hpp | 42 +++++++++- 11 files changed, 178 insertions(+), 4 deletions(-) diff --git a/Firmware/Board/v3/Inc/gpio.h b/Firmware/Board/v3/Inc/gpio.h index 7a5afb345..f19b45e5a 100644 --- a/Firmware/Board/v3/Inc/gpio.h +++ b/Firmware/Board/v3/Inc/gpio.h @@ -80,6 +80,12 @@ void GPIO_unsubscribe(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin); uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin); GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin); +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR <= 4 +#define GPIO_COUNT 5 +#else +#define GPIO_COUNT 8 +#endif + /* USER CODE END Prototypes */ #ifdef __cplusplus diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index d6b551671..57016df70 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -273,6 +273,7 @@ GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){ case 3: return GPIO_3_GPIO_Port; break; case 4: return GPIO_4_GPIO_Port; break; case 5: return GPIO_5_GPIO_Port; break; + default: return GPIO_1_GPIO_Port; } } @@ -283,6 +284,7 @@ uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){ case 3: return GPIO_3_Pin; break; case 4: return GPIO_4_Pin; break; case 5: return GPIO_5_Pin; break; + default: return GPIO_1_Pin; } } diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index 3026634d5..0cca1d191 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/Firmware/Board/v3/Src/stm32f4xx_it.c @@ -49,6 +49,7 @@ void decode_tim_capture(TIM_HandleTypeDef *htim, TIM_capture_callback_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); +void pwm_in_cb(int channel, uint32_t timestamp); /* USER CODE END 0 */ diff --git a/Firmware/Board/v3/Src/tim.c b/Firmware/Board/v3/Src/tim.c index 467c50308..c0714d23f 100644 --- a/Firmware/Board/v3/Src/tim.c +++ b/Firmware/Board/v3/Src/tim.c @@ -283,7 +283,7 @@ void MX_TIM5_Init(void) htim5.Instance = TIM5; htim5.Init.Prescaler = 0; htim5.Init.CounterMode = TIM_COUNTERMODE_UP; - htim5.Init.Period = 0; + htim5.Init.Period = 0xFFFFFFFF; htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; if (HAL_TIM_IC_Init(&htim5) != HAL_OK) { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 8e2245d4c..9ad4804a0 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -26,7 +26,7 @@ struct AxisConfig_t { bool startup_encoder_offset_calibration = false; //= 3 + if (channel >= 1 && channel <= 4) { + // the channel numbers just happen to coincide with + // the GPIO numbers + return channel; + } else { + return -1; + } +#else +#error "Not implemented" +#endif +} +// @brief Returns the TIM2 or TIM5 channel number +// for a given GPIO number. +uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) { +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + switch (gpio_num) { + case 1: return TIM_CHANNEL_1; + case 2: return TIM_CHANNEL_2; + case 3: return TIM_CHANNEL_3; + case 4: return TIM_CHANNEL_4; + default: return 0; + } +#else +#error "Not implemented" +#endif +} + +void pwm_in_init() { + GPIO_InitTypeDef GPIO_InitStruct; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; + + for (int i = 1; i <= 4; ++i) { + if (board_config.pwm_mappings[i].endpoint) { + GPIO_InitStruct.Pin = get_gpio_pin_by_pin(i); + HAL_GPIO_Init(get_gpio_port_by_pin(i), &GPIO_InitStruct); + HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(i)); + } + } +} + +#define TIM_2_5_CLOCK_HZ TIM_APB1_CLOCK_HZ +#define PWM_MIN_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 1000UL) // 1ms high is considered full reverse +#define PWM_MAX_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2000UL) // 2ms high is considered full forward +#define PWM_MIN_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 500UL) // ignore high periods shorter than 0.5ms +#define PWM_MAX_LEGAL_HIGH_TIME ((TIM_2_5_CLOCK_HZ / 1000000UL) * 2500UL) // ignore high periods longer than 2.5ms +#define PWM_INVERT_INPUT false + +void handle_pulse(int gpio_num, uint32_t high_time) { + if (high_time < PWM_MIN_LEGAL_HIGH_TIME || high_time > PWM_MAX_LEGAL_HIGH_TIME) + return; + + if (high_time < PWM_MIN_HIGH_TIME) + high_time = PWM_MIN_HIGH_TIME; + if (high_time > PWM_MAX_HIGH_TIME) + high_time = PWM_MAX_HIGH_TIME; + float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME); + float value = board_config.pwm_mappings[gpio_num].min + + (fraction * (board_config.pwm_mappings[gpio_num].max - board_config.pwm_mappings[gpio_num].min)); + + uint32_t endpoint_id = board_config.pwm_mappings[gpio_num].endpoint; + if (endpoint_id >= n_endpoints_) + return; + + Endpoint* endpoint = endpoints_[endpoint_id]; + if (!endpoint) + return; + + endpoint->set_from_float(value); +} + +void pwm_in_cb(int channel, uint32_t timestamp) { + static uint32_t last_timestamp[GPIO_COUNT] = { 0 }; + static bool last_pin_state[GPIO_COUNT] = { false }; + static bool last_sample_valid[GPIO_COUNT] = { false }; + + int gpio_num = tim_2_5_channel_num_to_gpio_num(channel); + if (gpio_num < 0 || gpio_num >= GPIO_COUNT) + return; + bool current_pin_state = HAL_GPIO_ReadPin(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)) != GPIO_PIN_RESET; + + if (last_sample_valid[gpio_num] + && (last_pin_state[gpio_num] != PWM_INVERT_INPUT) + && (current_pin_state == PWM_INVERT_INPUT)) { + handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num]); + } + + last_timestamp[gpio_num] = timestamp; + last_pin_state[gpio_num] = current_pin_state; + last_sample_valid[gpio_num] = true; +} \ No newline at end of file diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index e3784788b..1c40d8576 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -32,6 +32,7 @@ void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t hig extern "C" { void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected); void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected); +void pwm_in_cb(int channel, uint32_t timestamp); } // Initalisation @@ -39,6 +40,7 @@ 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); +void pwm_in_init(); void update_brake_current(); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index ea3c155ca..e26cadfdf 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -117,6 +117,8 @@ int odrive_main(void) { *encoder, *sensorless_estimator, *controller, *motor); } + pwm_in_init(); + // TODO: make dynamically reconfigurable #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 if (board_config.enable_uart) { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 2b29b6acb..b53249b15 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -51,6 +51,12 @@ extern SystemStats_t system_stats_; #ifdef __cplusplus } +struct PWMMapping_t { + uint32_t endpoint = 0; + float min = 0; + float max = 0; +}; + // @brief general user configurable board configuration struct BoardConfig_t { bool enable_uart = true; @@ -60,6 +66,7 @@ struct BoardConfig_t { //make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp index f51fc2dee..cad17d35f 100644 --- a/Firmware/communication/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -11,6 +11,7 @@ see protocol.md for the protocol specification #include #include #include +#include #include "crc.hpp" // Note that this option cannot be used to debug UART because it prints on UART @@ -407,8 +408,9 @@ 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 get_string(char * output, size_t length) { return false; } virtual bool set_string(char * buffer, size_t length) { return false; } + virtual bool set_from_float(float value) { return false; } }; class EndpointProvider { @@ -549,6 +551,40 @@ ProtocolObject make_protocol_object(const char * name, TMembers&&.. return ProtocolObject(name, std::forward(member_list)...); } +//template +//bool set_from_float_ex(float value, T* property) { +// return false; +//} + +namespace conversion { +//template +template +bool set_from_float_ex(float value, float* property, int) { + return *property = value, true; +} +template +bool set_from_float_ex(float value, bool* property, int) { + return *property = (value >= 0.0f), true; +} +template::value && !std::is_const::value>> +bool set_from_float_ex(float value, T* property, int) { + return *property = static_cast(std::round(value)), true; +} +template +bool set_from_float_ex(float value, T* property, ...) { + return false; +} +template +bool set_from_float(float value, T* property) { + return set_from_float_ex(value, property, 0); +} +} + +//template +//bool set_from_float_ex<>(float value, T* property) { +// return false; +//} + // TODO: move to cpp_utils #define ENABLE_IF_SAME(a, b, type) \ @@ -675,6 +711,10 @@ class ProtocolProperty : public Endpoint { return set_string_ex(buffer, length, 0); } + bool set_from_float(float value) final { + return conversion::set_from_float(value, property_); + } + void register_endpoints(Endpoint** list, size_t id, size_t length) { if (id < length) list[id] = this; From f1081afc7bb2ec782fb7f6540d9e128de2bfb12c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Tue, 15 May 2018 10:31:09 -0700 Subject: [PATCH 07/35] encoder bandwidth configurable --- Firmware/MotorControl/encoder.cpp | 22 +++++++++------------- Firmware/MotorControl/encoder.hpp | 10 ++++++---- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 167deeb1a..d19d533ba 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -7,14 +7,6 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, hw_config_(hw_config), config_(config) { - // Calculate encoder pll gains - // This calculation is currently identical to the PLL in SensorlessEstimator - float pll_bandwidth = 100.0f; // [rad/s] - pll_kp_ = 2.0f * pll_bandwidth; - - // Critically damped - pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); - if (config.pre_calibrated && (config.mode == Encoder::MODE_HALL)) { offset_ = config.offset; is_ready_ = true; @@ -237,8 +229,12 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { } bool Encoder::update() { + // Calculate encoder pll gains + float pll_kp = 2.0f * bandwidth_; // basic conversion to discrete time + float pll_ki = 0.25f * (pll_kp * pll_kp); // Critically damped + // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp_ < 1.0f)) { + if (!(current_meas_period * pll_kp < 1.0f)) { set_error(ERROR_UNSTABLE_GAIN); return false; } @@ -285,12 +281,12 @@ bool Encoder::update() { 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_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; + pll_vel_ += current_meas_period * pll_ki * delta_pos_cpr; bool snap_to_zero_vel = false; - if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki_) { + if (fabsf(pll_vel_) < 0.5f * current_meas_period * pll_ki) { pll_vel_ = 0.0f; //align delta-sigma on zero to prevent jitter snap_to_zero_vel = true; } diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 60299dca6..5e63e13bb 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -70,8 +70,9 @@ class Encoder { 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] + // float pll_kp_ = 0.0f; // [rad/s / rad] + // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] + float bandwidth_ = 1000.0f; // [/s] // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC @@ -91,8 +92,8 @@ class Encoder { make_protocol_property("pos_cpr", &pos_cpr_), make_protocol_property("hall_state", &hall_state_), make_protocol_property("pll_vel", &pll_vel_), - make_protocol_property("pll_kp", &pll_kp_), - make_protocol_property("pll_ki", &pll_ki_), + // make_protocol_property("pll_kp", &pll_kp_), + // make_protocol_property("pll_ki", &pll_ki_), make_protocol_object("config", make_protocol_property("mode", &config_.mode), make_protocol_property("use_index", &config_.use_index), @@ -101,6 +102,7 @@ class Encoder { make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), make_protocol_property("offset_float", &config_.offset_float), + make_protocol_property("bandwidth", &bandwidth_), make_protocol_property("calib_range", &config_.calib_range) ) ); From d3105cfcaf22f63d03aa19b5633a3bd31c4aeb65 Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 15 May 2018 14:53:43 -0700 Subject: [PATCH 08/35] expose PWM mappings as endpoint_ref --- Firmware/MotorControl/low_level.cpp | 8 +--- Firmware/MotorControl/main.cpp | 5 ++- Firmware/MotorControl/odrive_main.h | 4 +- Firmware/communication/communication.cpp | 5 +++ Firmware/communication/protocol.cpp | 12 +++++ Firmware/communication/protocol.hpp | 56 +++++++++++++++++++++--- 6 files changed, 75 insertions(+), 15 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 2a870a01c..8fe52c577 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -486,7 +486,7 @@ void pwm_in_init() { GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; for (int i = 1; i <= 4; ++i) { - if (board_config.pwm_mappings[i].endpoint) { + if (is_endpoint_ref_valid(board_config.pwm_mappings[i].endpoint)) { GPIO_InitStruct.Pin = get_gpio_pin_by_pin(i); HAL_GPIO_Init(get_gpio_port_by_pin(i), &GPIO_InitStruct); HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(i)); @@ -513,11 +513,7 @@ void handle_pulse(int gpio_num, uint32_t high_time) { float value = board_config.pwm_mappings[gpio_num].min + (fraction * (board_config.pwm_mappings[gpio_num].max - board_config.pwm_mappings[gpio_num].min)); - uint32_t endpoint_id = board_config.pwm_mappings[gpio_num].endpoint; - if (endpoint_id >= n_endpoints_) - return; - - Endpoint* endpoint = endpoints_[endpoint_id]; + Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num].endpoint); if (!endpoint) return; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index e26cadfdf..28ab8ce45 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -116,8 +116,6 @@ int odrive_main(void) { axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i], *encoder, *sensorless_estimator, *controller, *motor); } - - pwm_in_init(); // TODO: make dynamically reconfigurable #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 @@ -131,6 +129,9 @@ int odrive_main(void) { // Init communications (this requires the axis objects to be constructed) init_communication(); + // must happen after communication is initialized + pwm_in_init(); + // Setup hardware for all components for (size_t i = 0; i < AXIS_COUNT; ++i) { axes[i]->setup(); diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index b53249b15..3f2109839 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -2,6 +2,7 @@ #define __ODRIVE_MAIN_H #ifdef __cplusplus +#include extern "C" { #endif @@ -52,7 +53,7 @@ extern SystemStats_t system_stats_; } struct PWMMapping_t { - uint32_t endpoint = 0; + endpoint_ref_t endpoint = { 0 }; float min = 0; float max = 0; }; @@ -95,7 +96,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c // ODrive specific includes -#include #include #include #include diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 3b74a3e49..811db9433 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -63,6 +63,7 @@ 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; +volatile bool endpoint_list_valid = false; static uint32_t test_property = 0; @@ -84,6 +85,9 @@ void init_communication(void) { // 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); + + while (!endpoint_list_valid) + osDelay(1); } @@ -179,6 +183,7 @@ void communication_task(void * ctx) { auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); auto endpoint_provider = EndpointProvider_from_MemberList(*tree_ptr); set_application_endpoints(&endpoint_provider); + endpoint_list_valid = true; serve_on_uart(); serve_on_usb(); diff --git a/Firmware/communication/protocol.cpp b/Firmware/communication/protocol.cpp index 50e38fab9..078f42902 100644 --- a/Firmware/communication/protocol.cpp +++ b/Firmware/communication/protocol.cpp @@ -237,3 +237,15 @@ int BidirectionalPacketBasedChannel::process_packet(const uint8_t* buffer, size_ return 0; } + +bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref) { + return (endpoint_ref.json_crc == json_crc_) + && (endpoint_ref.endpoint_id < n_endpoints_); +} + +Endpoint* get_endpoint(endpoint_ref_t endpoint_ref) { + if (is_endpoint_ref_valid(endpoint_ref)) + return endpoints_[endpoint_ref.endpoint_id]; + else + return nullptr; +} diff --git a/Firmware/communication/protocol.hpp b/Firmware/communication/protocol.hpp index cad17d35f..01d5af136 100644 --- a/Firmware/communication/protocol.hpp +++ b/Firmware/communication/protocol.hpp @@ -36,7 +36,15 @@ constexpr uint16_t RX_BUF_SIZE = 128; // larger values than 128 have currently n // Maximum time we allocate for processing and responding to a request constexpr uint32_t PROTOCOL_SERVER_TIMEOUT_MS = 10; -template + +typedef struct { + uint16_t json_crc; + uint16_t node_id; + uint16_t endpoint_id; +} endpoint_ref_t; + + +template::value>> inline size_t write_le(T value, uint8_t* buffer); template @@ -100,6 +108,12 @@ inline size_t write_le(float value, uint8_t* buffer) { return write_le(*value_as_uint32, buffer); } +template +typename std::enable_if_t::value, size_t> +write_le(T value, uint8_t* buffer) { + return write_le>(value, buffer); +} + template<> inline size_t read_le(bool* value, const uint8_t* buffer) { *value = buffer[0]; @@ -320,7 +334,29 @@ typedef std::function -void default_readwrite_endpoint_handler(const T* value, const uint8_t* input, size_t input_length, StreamSink* output) { +void default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* input, size_t input_length, StreamSink* output) { + constexpr size_t size = sizeof(value->endpoint_id) + sizeof(value->json_crc); + if (output) { + // TODO: make buffer size dependent on the type + uint8_t buffer[size]; + size_t cnt = write_leendpoint_id)>(value->endpoint_id, buffer); + cnt += write_lejson_crc)>(value->json_crc, buffer + cnt); + if (cnt <= output->get_free_space()) + output->process_bytes(buffer, cnt); + } + + // If a new value was passed, call the corresponding little endian deserialization function + if (input_length >= size) { + read_leendpoint_id)>(&value->endpoint_id, input); + read_lejson_crc)>(&value->json_crc, input + 2); + } +} + + +// @brief Default endpoint handler for const types +template +std::enable_if_t::value && std::is_const::value> +default_readwrite_endpoint_handler(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 @@ -331,10 +367,12 @@ void default_readwrite_endpoint_handler(const T* value, const uint8_t* input, si } } +// @brief Default endpoint handler for non-const types template -void default_readwrite_endpoint_handler(T* value, const uint8_t* input, size_t input_length, StreamSink* output) { +std::enable_if_t::value && !std::is_const::value> +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); + 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 @@ -403,6 +441,10 @@ template<> inline constexpr const char* get_default_json_modifier() { return "\"type\":\"bool\",\"access\":\"rw\""; } +template<> +inline constexpr const char* get_default_json_modifier() { + return "\"type\":\"endpoint_ref\",\"access\":\"rw\""; +} class Endpoint { public: @@ -720,7 +762,7 @@ class ProtocolProperty : public Endpoint { list[id] = this; } void handle(const uint8_t* input, size_t input_length, StreamSink* output) { - default_readwrite_endpoint_handler(property_, input, input_length, 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); @@ -975,5 +1017,9 @@ extern Endpoint* endpoints_[]; extern size_t n_endpoints_; extern const size_t max_endpoints_; extern EndpointProvider* application_endpoints; +extern uint16_t json_crc_; + +bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref); +Endpoint* get_endpoint(endpoint_ref_t endpoint_ref); #endif From 5159aca1bd719efe69b295deba3079dbe00da91a Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 15 May 2018 16:15:11 -0700 Subject: [PATCH 09/35] python support for sending/receiving endpoint refs --- tools/odrive/remote_object.py | 116 ++++++++++++++++++++++------------ 1 file changed, 75 insertions(+), 41 deletions(-) diff --git a/tools/odrive/remote_object.py b/tools/odrive/remote_object.py index 918696429..0d87dd635 100644 --- a/tools/odrive/remote_object.py +++ b/tools/odrive/remote_object.py @@ -8,14 +8,29 @@ import threading import odrive.protocol -#class ObjectDisappearedError(Exception): -# def __init__(self, channel): -# self._obj = obj -# pass class ObjectDefinitionError(Exception): pass +codecs = {} + +class StructCodec(): + """ + Generic serializer/deserializer based on struct pack + """ + def __init__(self, struct_format, target_type): + self._struct_format = struct_format + self._target_type = target_type + def get_length(self): + return struct.calcsize(self._struct_format) + def serialize(self, value): + value = self._target_type(value) + return struct.pack(self._struct_format, value) + def deserialize(self, buffer): + value = struct.unpack(self._struct_format, buffer) + value = value[0] if len(value) == 1 else value + return self._target_type(value) + class RemoteProperty(): """ Used internally by dynamically created objects to translate @@ -24,6 +39,7 @@ class RemoteProperty(): """ def __init__(self, json_data, parent): self._parent = parent + self.__channel__ = parent.__channel__ id_str = json_data.get("id", None) if id_str is None: raise ObjectDefinitionError("unspecified endpoint ID") @@ -37,51 +53,28 @@ def __init__(self, json_data, parent): if type_str is None: raise ObjectDefinitionError("unspecified type") - if type_str == "float": - self._property_type = float - self._struct_format = " Date: Tue, 15 May 2018 17:27:12 -0700 Subject: [PATCH 10/35] move bandwith to config --- Firmware/MotorControl/encoder.cpp | 2 +- Firmware/MotorControl/encoder.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index d19d533ba..cf24a9a09 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -230,7 +230,7 @@ static bool decode_hall(uint8_t hall_state, int32_t* hall_cnt) { bool Encoder::update() { // Calculate encoder pll gains - float pll_kp = 2.0f * bandwidth_; // basic conversion to discrete time + float pll_kp = 2.0f * config_.bandwidth; // basic conversion to discrete time float pll_ki = 0.25f * (pll_kp * pll_kp); // Critically damped // Check that we don't get problems with discrete time approximation diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 5e63e13bb..86bc9b73d 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -35,6 +35,7 @@ class Encoder { // index search succeeds float offset_float = 0.0f; // Sub-count phase alignment offset float calib_range = 0.02f; + float bandwidth = 1000.0f; }; Encoder(const EncoderHardwareConfig_t& hw_config, @@ -72,7 +73,6 @@ class Encoder { 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 bandwidth_ = 1000.0f; // [/s] // Updated by low_level pwm_adc_cb uint8_t hall_state_ = 0x0; // bit[0] = HallA, .., bit[2] = HallC @@ -102,7 +102,7 @@ class Encoder { make_protocol_property("cpr", &config_.cpr), make_protocol_property("offset", &config_.offset), make_protocol_property("offset_float", &config_.offset_float), - make_protocol_property("bandwidth", &bandwidth_), + make_protocol_property("bandwidth", &config_.bandwidth), make_protocol_property("calib_range", &config_.calib_range) ) ); From 689fe1512c1adb204eae058da01fc4587b5cdffb Mon Sep 17 00:00:00 2001 From: Samuel Sadok Date: Tue, 15 May 2018 23:58:55 -0700 Subject: [PATCH 11/35] add PWM channel 1 and 2 initialization, add pulldown, fix off-by-one --- Firmware/MotorControl/low_level.cpp | 40 ++++++++++++++---------- Firmware/communication/communication.cpp | 8 ++--- 2 files changed, 28 insertions(+), 20 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 45a0e1c49..37d1351f5 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -641,15 +641,23 @@ uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) { void pwm_in_init() { GPIO_InitTypeDef GPIO_InitStruct; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; - GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Pull = GPIO_PULLDOWN; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; - for (int i = 1; i <= 4; ++i) { - if (is_endpoint_ref_valid(board_config.pwm_mappings[i].endpoint)) { - GPIO_InitStruct.Pin = get_gpio_pin_by_pin(i); - HAL_GPIO_Init(get_gpio_port_by_pin(i), &GPIO_InitStruct); - HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(i)); + TIM_IC_InitTypeDef sConfigIC; + sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_BOTHEDGE; + sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI; + sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; + sConfigIC.ICFilter = 15; + + for (int gpio_num = 1; gpio_num <= 4; ++gpio_num) { + if (is_endpoint_ref_valid(board_config.pwm_mappings[gpio_num - 1].endpoint)) { + GPIO_InitStruct.Pin = get_gpio_pin_by_pin(gpio_num); + HAL_GPIO_DeInit(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)); + HAL_GPIO_Init(get_gpio_port_by_pin(gpio_num), &GPIO_InitStruct); + HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, gpio_num_to_tim_2_5_channel(gpio_num)); + HAL_TIM_IC_Start_IT(&htim5, gpio_num_to_tim_2_5_channel(gpio_num)); } } } @@ -670,10 +678,10 @@ void handle_pulse(int gpio_num, uint32_t high_time) { if (high_time > PWM_MAX_HIGH_TIME) high_time = PWM_MAX_HIGH_TIME; float fraction = (float)(high_time - PWM_MIN_HIGH_TIME) / (float)(PWM_MAX_HIGH_TIME - PWM_MIN_HIGH_TIME); - float value = board_config.pwm_mappings[gpio_num].min + - (fraction * (board_config.pwm_mappings[gpio_num].max - board_config.pwm_mappings[gpio_num].min)); + float value = board_config.pwm_mappings[gpio_num - 1].min + + (fraction * (board_config.pwm_mappings[gpio_num - 1].max - board_config.pwm_mappings[gpio_num - 1].min)); - Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num].endpoint); + Endpoint* endpoint = get_endpoint(board_config.pwm_mappings[gpio_num - 1].endpoint); if (!endpoint) return; @@ -686,17 +694,17 @@ void pwm_in_cb(int channel, uint32_t timestamp) { static bool last_sample_valid[GPIO_COUNT] = { false }; int gpio_num = tim_2_5_channel_num_to_gpio_num(channel); - if (gpio_num < 0 || gpio_num >= GPIO_COUNT) + if (gpio_num < 1 || gpio_num > GPIO_COUNT) return; bool current_pin_state = HAL_GPIO_ReadPin(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)) != GPIO_PIN_RESET; - if (last_sample_valid[gpio_num] - && (last_pin_state[gpio_num] != PWM_INVERT_INPUT) + if (last_sample_valid[gpio_num - 1] + && (last_pin_state[gpio_num - 1] != PWM_INVERT_INPUT) && (current_pin_state == PWM_INVERT_INPUT)) { - handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num]); + handle_pulse(gpio_num, timestamp - last_timestamp[gpio_num - 1]); } - last_timestamp[gpio_num] = timestamp; - last_pin_state[gpio_num] = current_pin_state; - last_sample_valid[gpio_num] = true; + last_timestamp[gpio_num - 1] = timestamp; + last_pin_state[gpio_num - 1] = current_pin_state; + last_sample_valid[gpio_num - 1] = true; } \ No newline at end of file diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 2f885569e..ce3a562e3 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -154,10 +154,10 @@ static inline auto make_obj_tree() { make_protocol_property("enable_i2c_instead_of_can" , &board_config.enable_i2c_instead_of_can), // requires a reboot 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("gpio1_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])), - make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[2])), - make_protocol_object("gpio3_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3])), - make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[4])) + make_protocol_object("gpio1_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[0])), + make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])), + make_protocol_object("gpio3_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[2])), + make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3])) ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), make_protocol_object("axis1", axes[1]->make_protocol_definitions()), From c6cf7b2132cead5f18f40bf09a2153f30b900ee3 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 17 May 2018 17:07:34 -0700 Subject: [PATCH 12/35] current control bandwidth lowered for high inductance wheels. TODO Make configurable --- Firmware/MotorControl/motor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 0c8707eb6..b1b6c7f87 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -57,7 +57,7 @@ void Motor::reset_current_control() { // 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] + float current_control_bandwidth = 100.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; From 56c082932141203b84a15e9a8e3d6bf627ec117a Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 21 Jun 2018 21:47:21 -0700 Subject: [PATCH 13/35] move includes around --- Firmware/MotorControl/odrive_main.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 7768571d5..bf35821f8 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -2,7 +2,7 @@ #define __ODRIVE_MAIN_H #ifdef __cplusplus -#include +#include extern "C" { #endif @@ -106,7 +106,6 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c // ODrive specific includes -#include #include #include #include From cbee6df642c846e1fe0db1d52ca0704301bed665 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 21 Jun 2018 21:59:16 -0700 Subject: [PATCH 14/35] fix compile errors after merge --- Firmware/fibre/cpp/include/fibre/protocol.hpp | 10 +++++----- Firmware/fibre/cpp/protocol.cpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp index 8ea638e96..ed174dd95 100644 --- a/Firmware/fibre/cpp/include/fibre/protocol.hpp +++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp @@ -440,7 +440,7 @@ void default_readwrite_endpoint_handler(endpoint_ref_t* value, const uint8_t* in size_t cnt = write_leendpoint_id)>(value->endpoint_id, buffer); cnt += write_lejson_crc)>(value->json_crc, buffer + cnt); if (cnt <= output->get_free_space()) - output->process_bytes(buffer, cnt); + output->process_bytes(buffer, cnt, nullptr); } // If a new value was passed, call the corresponding little endian deserialization function @@ -880,11 +880,8 @@ class ProtocolProperty : public Endpoint { 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) final { - default_readwrite_endpoint_handler(property_, input, input_length, 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); @@ -1137,6 +1134,9 @@ extern uint16_t json_crc_; extern JSONDescriptorEndpoint json_file_endpoint_; extern EndpointProvider* application_endpoints_; +bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref); +Endpoint* get_endpoint(endpoint_ref_t endpoint_ref); + // @brief Registers the specified application object list using the provided endpoint table. // This function should only be called once during the lifetime of the application. TODO: fix this. // @param application_objects The application objects to be registred. diff --git a/Firmware/fibre/cpp/protocol.cpp b/Firmware/fibre/cpp/protocol.cpp index c20348857..23a2e0a58 100644 --- a/Firmware/fibre/cpp/protocol.cpp +++ b/Firmware/fibre/cpp/protocol.cpp @@ -221,7 +221,7 @@ bool is_endpoint_ref_valid(endpoint_ref_t endpoint_ref) { Endpoint* get_endpoint(endpoint_ref_t endpoint_ref) { if (is_endpoint_ref_valid(endpoint_ref)) - return endpoints_[endpoint_ref.endpoint_id]; + return endpoint_list_[endpoint_ref.endpoint_id]; else return nullptr; } From 6e9dd068debfcf4241d181ee2062ac710e7b23c4 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 21 Jun 2018 22:41:58 -0700 Subject: [PATCH 15/35] add current control bandwith config setting --- CHANGELOG.md | 31 ++++++++++++++++--------------- Firmware/MotorControl/motor.cpp | 8 ++++++-- Firmware/MotorControl/motor.hpp | 9 +++++++-- 3 files changed, 29 insertions(+), 19 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5c9774873..359319aed 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,18 +1,25 @@ # Unreleased Features Please add a note of your changes below this heading if you make a Pull Request. -# Releases +## [0.4.1] - UNRELEASED +### Added +* Hall sensor feedback +* Configurable RC PWM input +* Config settings for: + * `motor.config.requested_current_range` + * `motor.config.current_control_bandwidth` and `motor.set_current_control_bandwidth`. Latter required to invoke gain recalculation. + * `encoder.config.bandwidth` +# Releases ## [0.4.0] - 2018-06-10 - ### Added - * Encoder can now go forever in velocity/torque mode due to using circular encoder space. - * Protocol supports function return values - * bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_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 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. - * infrastructure to publish the python tools to PyPi. See `tools/setup.py` for details. - * Automated test script `run_tests.py` - * System stats (e.g. stack usage) are exposed under `.system_stats` +* Encoder can now go forever in velocity/torque mode due to using circular encoder space. +* Protocol supports function return values +* bake Git-derived firmware version into firmware binary. The firmware version is exposed through the `fw_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 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. +* infrastructure to publish the python tools to PyPi. See `tools/setup.py` for details. +* Automated test script `run_tests.py` +* System stats (e.g. stack usage) are exposed under `.system_stats` ### Changed * DFU script updates @@ -39,7 +46,6 @@ Please add a note of your changes below this heading if you make a Pull Request. * USB issue where the device would stop responding when the host script would quit abruptly or reset the device during operation ## [0.3.6] - 2018-03-26 - ### Added * **Storing of configuration parameters to Non Volatile Memory** * **USB Bootloader** @@ -62,7 +68,6 @@ Please add a note of your changes below this heading if you make a Pull Request. * malloc now fails if we run out of memory (before it would always succeed even if we are out of ram...) ## [0.3.5] - 2018-03-04 - ### Added * Reporting error if your encoder CPR is incorrect * Ability to start anticogging calibration over USB protocol @@ -76,12 +81,10 @@ Please add a note of your changes below this heading if you make a Pull Request. * Build system is now tup instead of make. Please check the [Readme](README.md#installing-prerequisites) for installation instructions. ## [0.3.4] - 2018-02-13 - ### Fixed * Broken way to check for python 2. Python 2 not supported yet. ## [0.3.3] - 2018-02-12 - ### Added * Liveplotter script * Automatic recovery of USB halt/stall condition @@ -93,14 +96,12 @@ Please add a note of your changes below this heading if you make a Pull Request. * USB CSC (USB serial) now reports a sensible baud rate ## [0.3.2] - 2018-02-02 - ### Added * Gimbal motor mode * Encoder index pulse support * `resistance_calib_max_voltage` parameter ## [0.3.1] - 2018-01-18 - ### Added * UUID Endpoint * Reporting of correct ODrive version on USB descriptor diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index b1b6c7f87..d8e4764b0 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -57,12 +57,16 @@ void Motor::reset_current_control() { // 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 = 100.0f; // [rad/s] - current_control_.p_gain = current_control_bandwidth * config_.phase_inductance; + current_control_.p_gain = config_.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; } +void Motor::set_current_control_bandwidth(float current_control_bandwidth) { + config_.current_control_bandwidth = current_control_bandwidth; + update_current_controller_gains(); +} + // @brief Set up the gate drivers void Motor::DRV8301_setup() { // for reference: diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 852a64f41..ae2cd8700 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -50,6 +50,7 @@ typedef struct { float current_lim = 10.0f; //[A] // Value used to compute shunt amplifier gains float requested_current_range = 70.0f; // [A] + float current_control_bandwidth = 1000.0f; // [rad/s] } MotorConfig_t; class Motor { @@ -101,6 +102,7 @@ class Motor { void reset_current_control(); void update_current_controller_gains(); + void set_current_control_bandwidth(float current_control_bandwidth); void DRV8301_setup(); bool check_DRV_fault(); void set_error(Error_t error); @@ -209,8 +211,11 @@ class Motor { make_protocol_property("direction", &config_.direction), make_protocol_property("motor_type", &config_.motor_type), make_protocol_property("current_lim", &config_.current_lim), - make_protocol_property("requested_current_range", &config_.requested_current_range) - ) + make_protocol_property("requested_current_range", &config_.requested_current_range), + make_protocol_ro_property("current_control_bandwidth", &config_.current_control_bandwidth) + ), + make_protocol_function("set_current_control_bandwidth", *this, &Motor::set_current_control_bandwidth, + "current_control_bandwidth") ); } }; From cd74a98b2535e1b86f8284f04d8716a04300f076 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 21 Jun 2018 23:22:06 -0700 Subject: [PATCH 16/35] fix removed endpoint construction check --- Firmware/communication/communication.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 3287283fa..2ee436d83 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -191,6 +191,9 @@ void communication_task(void * ctx) { // ends up with a stupid stack size of around 8000 bytes. Fix this. auto tree_ptr = new (tree_buffer) tree_type(make_obj_tree()); fibre_publish(*tree_ptr); + + // Allow main init to continue + endpoint_list_valid = true; start_uart_server(); start_usb_server(); From a116c94075af97331ad3b0910403ff3f7d39f2dc Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 22 Jun 2018 00:44:03 -0700 Subject: [PATCH 17/35] sync shadow_count to count_in_cpr when calibrating --- Firmware/MotorControl/encoder.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index cf24a9a09..f5c76a350 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -129,6 +129,10 @@ bool Encoder::run_offset_calibration() { bool old_use_index = config_.use_index; config_.use_index = false; + // We use shadow_count_ to do the calibration, but the offset is used by count_in_cpr_ + // Therefore we have to sync them for calibration + shadow_count_ = count_in_cpr_; + 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; From fac25d9ece181cf86cc0b42490fb9242cde38bae Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Fri, 22 Jun 2018 18:35:02 -0700 Subject: [PATCH 18/35] update pin mapping docs --- docs/hoverboard.md | 49 ++++++++++++++++++++++++++++++++++++++++++++++ docs/interfaces.md | 29 +++++++++++++++------------ 2 files changed, 65 insertions(+), 13 deletions(-) create mode 100644 docs/hoverboard.md diff --git a/docs/hoverboard.md b/docs/hoverboard.md new file mode 100644 index 000000000..4b0efa834 --- /dev/null +++ b/docs/hoverboard.md @@ -0,0 +1,49 @@ +roflcopter + +```txt +odrv0.axis0.motor.config.pole_pairs = 15 +odrv0.axis0.motor.config.resistance_calib_max_voltage = 4 +odrv0.axis0.motor.config.requested_current_range = 25 +odrv0.axis0.motor.set_current_control_bandwidth(100) +odrv0.axis0.encoder.config.cpr = 90 +odrv0.axis0.encoder.config.mode = 1 +odrv0.axis0.encoder.config.bandwidth = 100 +``` + +```txt +odrv0.axis0.controller.config.pos_gain = 1 +odrv0.axis0.controller.config.vel_gain = 0.02 +``` + +```txt +odrv0.axis0.requested_state = AXIS_STATE_MOTOR_CALIBRATION +odrv0.axis0.motor + error = 0x0000 (int) + phase_inductance = 0.00033594953129068017 (float) + phase_resistance = 0.1793474406003952 (float) +odrv0.axis0.motor.config.pre_calibrated = True +``` + +```txt +odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION + error = 0x0000 (int) + offset_float = 0.5126956701278687 (float) + something close to 0.5 is good +odrv0.axis0.encoder.config.pre_calibrated = True +``` + + +```txt +odrv0.config.enable_uart = False +odrv0.config.gpio1_pwm_mapping.min = -100 +odrv0.config.gpio1_pwm_mapping.max = 100 +odrv0.config.gpio1_pwm_mapping.endpoint = odrv0.axis0.controller._remote_attributes['vel_setpoint'] +odrv0.save_configuration() +odrv0.reboot() +odrv0.axis0.controller.vel_setpoint +``` + +```txt +odrv0.axis0.config.startup_closed_loop_control = True +odrv0.save_configuration() +``` \ No newline at end of file diff --git a/docs/interfaces.md b/docs/interfaces.md index e0c2ce7dc..2e8c74c41 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -37,6 +37,17 @@ The ODrive can be controlled over various ports and protocols. If you're comfort ODrive v3.3 and onward have 5V tolerant GPIO pins. +### Pin function priorities +1. PWM in, if enabled. Disabled by default. +1. UART, **Enabled by default**. +1. Step/Dir, if enabled. Disabled by default. +1. Analog, default behaviour if not overriden (only on supported pins). +1. Digital in, default behaviour on pins not capable of analog input. + +For predictable results, try to have only one feature enabled for any one pin. When changing pin assignments you must: +* `odrv0.save_configuration()` +* `odrv0.reboot()` + ## Native Protocol This protocol is what the ODrive Tool uses to talk to the ODrive. If you have a choice, this is the recommended protocol for all applications. The native protocol runs on USB and can also be configured to run on UART. @@ -76,45 +87,37 @@ Pinout: * GND: you must connect the grounds of the devices together. Use any GND pin on J3 of the ODrive. To enable step/dir mode for the GPIO, set `.config.enable_step_dir` to true for each axis that you wish to use this on. -Axis 0 step/dir pins conflicts with UART, and the UART takes priority. So to be able to use step/dir on Axis 0, you must also set `odrv0.config.enable_uart = False`. -To apply these settings you must reboot, and to keep them on reboot you must save configuration: -* `odrv0.save_configuration()` -* `odrv0.reboot()` +Axis 0 step/dir pins conflicts with UART, and the UART takes priority. So to be able to use step/dir on Axis 0, you must also set `odrv0.config.enable_uart = False`. See the [pin function priorities](#pin-function-priorities) for more detail. Don't forget to save configuration and reboot. There is also a config variable called `.config.counts_per_step`, which specifies how many encoder counts a "step" corresponds to. It can be any floating point value. The maximum step rate is pending tests, but it should handle at least 50kHz. If you want to test it, please be aware that the failure mode on too high step rates is expected to be that the motors shuts down and coasts. Please be aware that there is no enable line right now, and the step/direction interface is enabled by default, and remains active as long as the ODrive is in position control mode. To get the ODrive to go into position control mode at bootup, see how to configure the [startup procedure](commands.md#startup-procedure). - +5. With the ODrive powered off, connect the RC receiver ground to the ODrive's GND and one of the RC receiver signals to GPIO4. You may try to power the receiver from the ODrive's 5V supply if it doesn't draw too much power. Power up the the RC transmitter. You should now be able to control axis 0 from one of the RC sticks. ## Ports Note: when you use an existing library you don't have to deal with the specifics described in this section. From f6926d4d69df34eaff6997204c55cc69b03baebf Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 23 Jun 2018 17:18:52 -0700 Subject: [PATCH 19/35] finish hoverboard writeup --- docs/hoverboard.md | 126 ++++++++++++++++++++++++++++++++++++++---- docs/interfaces.md | 9 ++- tools/odrive/enums.py | 3 + 3 files changed, 127 insertions(+), 11 deletions(-) diff --git a/docs/hoverboard.md b/docs/hoverboard.md index 4b0efa834..2ad0098fa 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -1,49 +1,155 @@ -roflcopter +# Hoverboard motor and remote control setup guide +By popular request, here follows a step-by-step guide on how to set up the ODrive to drive hoverboard motors using RC PWM input. +Each step is acompanied by some explanation, so hopefully you can carry over some of the steps to other setups and configuration. + +[![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/ponx_U4xhoM/0.jpg)](https://www.youtube.com/watch?v=ponx_U4xhoM)
Click above to play video. + +### Hoverboard motor configuration +Standard 6.5 inch hoverboard hub motors have 30 permanent magnet poles, and thus 15 pole pairs. If you have a different motor, you need to count the magnets or have a reliable datasheet for this information. ```txt odrv0.axis0.motor.config.pole_pairs = 15 +``` + +Hoverboard hub motors are quite high resistance compared to the hobby aircraft motors, so we want to use a bit higher voltage for the motor calibration, and set up the current sense gain to be more sensitive. +The motors are also fairly high inductance, so we need to reduce the bandwidth of the current controller from the default to keep it stable. +```txt odrv0.axis0.motor.config.resistance_calib_max_voltage = 4 -odrv0.axis0.motor.config.requested_current_range = 25 +odrv0.axis0.motor.config.requested_current_range = 25 #Requires config save and reboot odrv0.axis0.motor.set_current_control_bandwidth(100) +``` + +Set the encoder to hall mode (instead of incremental). See the [pinout](interfaces.md#hall-feedback-pinout) for instructions on how to plug in the hall feedback. +The hall feedback has 6 states for every pole pair in the motor. Since we have 15 pole pairs, we set the cpr to 15*6 = 90. +```txt +odrv0.axis0.encoder.config.mode = ENCODER_MODE_HALL odrv0.axis0.encoder.config.cpr = 90 -odrv0.axis0.encoder.config.mode = 1 -odrv0.axis0.encoder.config.bandwidth = 100 ``` +Since the hall feedback only has 90 counts per revolution, we want to reduce the velocity tracking bandwidth to get smoother velocity estimates. +We can also set these fairly modest gains that will be a bit sloppy but shouldn't shake your rig apart if it's built poorly. Make sure to tune the gains up when you have everything else working, to the stiffness that is applicable to your application. +Lets also start in velocity control mode, since that is probably what you want for a wheeled robot. Note that in velocity mode `pos_gain` isn't used, but I give you a recommended value anyway in case you wanted to run position control mode. ```txt +odrv0.axis0.encoder.config.bandwidth = 100 odrv0.axis0.controller.config.pos_gain = 1 odrv0.axis0.controller.config.vel_gain = 0.02 +odrv0.axis0.controller.config.vel_lim = 1000 +odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL +``` + +In the next step we are going to be start powering the motor, so we want to make sure that some of the above settings that requrie a reboot are applied first. +```txt +odrv0.save_configuration() +odrv0.reboot() ``` +Make sure the motor is free to move, then activate the motor calibration. ```txt odrv0.axis0.requested_state = AXIS_STATE_MOTOR_CALIBRATION +``` + +You can read out all the data pertaining to the motor: +```txt odrv0.axis0.motor +``` + +Check to see that there is no error, and that the phase resistance and inductance are reasonable. Here are the results I got: +```txt error = 0x0000 (int) phase_inductance = 0.00033594953129068017 (float) phase_resistance = 0.1793474406003952 (float) +``` + +If all looks good you can tell the ODrive that saving this calibration to presistent memory is OK: +```txt odrv0.axis0.motor.config.pre_calibrated = True ``` +Next step is to check the alignment between the motor and the hall sensor. +Because of this step you are allowed to plug the motor phases in random order, and also the hall signals can be random. Just don't change it after calibration. +Make sure the motor is free to move and run: ```txt odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION +``` + +Check the status of the encoder object: +```txt +odrv0.axis0.encoder +``` + +Check that there was no error. If your hall sensors has standard timing angle, then `offset_float` should be close to 0.5. +```txt error = 0x0000 (int) offset_float = 0.5126956701278687 (float) - something close to 0.5 is good +``` + +If all looks good you can tell the ODrive that saving this calibration to presistent memory is OK: +```txt odrv0.axis0.encoder.config.pre_calibrated = True ``` +OK we are done with the motor configuration! Time to save, reboot, and then test it. +The ODrive starts in idle (we will look at changing this later), so we enable closed loop control. +```txt +odrv0.save_configuration() +odrv0.reboot() +odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL +odrv0.axis0.controller.vel_setpoint = 120 +# Your motor should spin here +odrv0.axis0.controller.vel_setpoint = 0 +odrv0.axis0.requested_state = AXIS_STATE_IDLE +``` + +Hopefully you got your motor to spin! Feel free to repeat all of the above for the other axis if appropriate. +### PWM input +If you want to drive your hoverboard wheels around with an RC remote contro, you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link. +Lets use GPIO 3/4 for the velocity inputs, because then we don't have to disable UART. +Then let's map the full stick range of these inputs to some suitable velocity setpoint range. +We also have to reboot to activate the PWM input. ```txt -odrv0.config.enable_uart = False -odrv0.config.gpio1_pwm_mapping.min = -100 -odrv0.config.gpio1_pwm_mapping.max = 100 -odrv0.config.gpio1_pwm_mapping.endpoint = odrv0.axis0.controller._remote_attributes['vel_setpoint'] +odrv0.config.gpio3_pwm_mapping.min = -200 +odrv0.config.gpio3_pwm_mapping.max = 200 +odrv0.config.gpio3_pwm_mapping.endpoint = odrv0.axis0.controller._remote_attributes['vel_setpoint'] + +odrv0.config.gpio4_pwm_mapping.min = -200 +odrv0.config.gpio4_pwm_mapping.max = 200 +odrv0.config.gpio4_pwm_mapping.endpoint = odrv0.axis1.controller._remote_attributes['vel_setpoint'] + odrv0.save_configuration() odrv0.reboot() -odrv0.axis0.controller.vel_setpoint ``` +Now we can check that the sticks are writing to the velocity setpoint. Move the stick, print `vel_setpoint`, move to a different position, check again. +```txt +In [1]: odrv0.axis1.controller.vel_setpoint +Out[1]: 0.1904754638671875 + +In [2]: odrv0.axis1.controller.vel_setpoint +Out[2]: 0.1904754638671875 + +In [3]: odrv0.axis1.controller.vel_setpoint +Out[3]: 28.152389526367188 + +In [4]: odrv0.axis1.controller.vel_setpoint +Out[4]: 61.21905517578125 + +In [5]: odrv0.axis1.controller.vel_setpoint +Out[5]: -52.990474700927734 +``` + +Ok, now we should be able to turn on the drive and control the wheels! +```txt +odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL +odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL +``` + +### Automatic startup +Try to reboot and then activate AXIS_STATE_CLOSED_LOOP_CONTROL on both axis. Check that everything directly is operational and works as expected. +If so, you can make the ODrive turn on the motor power automatically after booting. This is useful if you are going to be running the ODrive without a PC or other logic board. ```txt odrv0.axis0.config.startup_closed_loop_control = True +odrv0.axis1.config.startup_closed_loop_control = True odrv0.save_configuration() +odrv0.reboot() ``` \ No newline at end of file diff --git a/docs/interfaces.md b/docs/interfaces.md index 2e8c74c41..1d9cd21cf 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -48,6 +48,14 @@ For predictable results, try to have only one feature enabled for any one pin. W * `odrv0.save_configuration()` * `odrv0.reboot()` +### Hall feedback pinout +When the encoder mode is set to hall feedback, the pinout on the encoder port is as follows: +| Label on ODrive | Hall feedback | +|-----------------|---------------| +| A | Hall A | +| B | Hall B | +| Z | Hall C | + ## Native Protocol This protocol is what the ODrive Tool uses to talk to the ODrive. If you have a choice, this is the recommended protocol for all applications. The native protocol runs on USB and can also be configured to run on UART. @@ -95,7 +103,6 @@ The maximum step rate is pending tests, but it should handle at least 50kHz. If Please be aware that there is no enable line right now, and the step/direction interface is enabled by default, and remains active as long as the ODrive is in position control mode. To get the ODrive to go into position control mode at bootup, see how to configure the [startup procedure](commands.md#startup-procedure). ## RC PWM input - You can control the ODrive directly from an hobby RC receiver. Some GPIO pins can be used for PWM input, if they are not allocated to other functions. For example, you must disable the UART to use GPIO 1,2. See the [pin function priorities](#pin-function-priorities) for more detail. diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index f53690070..19572c909 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -31,3 +31,6 @@ CTRL_MODE_CURRENT_CONTROL = 1 CTRL_MODE_VELOCITY_CONTROL = 2 CTRL_MODE_POSITION_CONTROL = 3 + +ENCODER_MODE_INCREMENTAL = 0 +ENCODER_MODE_HALL = 1 From f32a7e3a218ddf176330950ffe07b1360c3be990 Mon Sep 17 00:00:00 2001 From: Capo01 <503426+Capo01@users.noreply.github.com> Date: Sun, 24 Jun 2018 10:41:49 +1000 Subject: [PATCH 20/35] Update hoverboard.md Reads fine as is. The suggested edits are just a matter of taste. --- docs/hoverboard.md | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/docs/hoverboard.md b/docs/hoverboard.md index 2ad0098fa..f5c998ff3 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -1,12 +1,12 @@ # Hoverboard motor and remote control setup guide -By popular request, here follows a step-by-step guide on how to set up the ODrive to drive hoverboard motors using RC PWM input. -Each step is acompanied by some explanation, so hopefully you can carry over some of the steps to other setups and configuration. +By popular request here follows a step-by-step guide on how to setup the ODrive to drive hoverboard motors using RC PWM input. +Each step is acompanied by some explanation so hopefully you can carry over some of the steps to other setups and configurations. [![IMAGE ALT TEXT HERE](https://img.youtube.com/vi/ponx_U4xhoM/0.jpg)](https://www.youtube.com/watch?v=ponx_U4xhoM)
Click above to play video. ### Hoverboard motor configuration -Standard 6.5 inch hoverboard hub motors have 30 permanent magnet poles, and thus 15 pole pairs. If you have a different motor, you need to count the magnets or have a reliable datasheet for this information. +Standard 6.5 inch hoverboard hub motors have 30 permanent magnet poles, and thus 15 pole pairs. If you have a different motor you need to count the magnets or have a reliable datasheet for this information. ```txt odrv0.axis0.motor.config.pole_pairs = 15 ``` @@ -27,8 +27,8 @@ odrv0.axis0.encoder.config.cpr = 90 ``` Since the hall feedback only has 90 counts per revolution, we want to reduce the velocity tracking bandwidth to get smoother velocity estimates. -We can also set these fairly modest gains that will be a bit sloppy but shouldn't shake your rig apart if it's built poorly. Make sure to tune the gains up when you have everything else working, to the stiffness that is applicable to your application. -Lets also start in velocity control mode, since that is probably what you want for a wheeled robot. Note that in velocity mode `pos_gain` isn't used, but I give you a recommended value anyway in case you wanted to run position control mode. +We can also set these fairly modest gains that will be a bit sloppy but shouldn't shake your rig apart if it's built poorly. Make sure to tune the gains up when you have everything else working to a stiffness that is applicable to your application. +Lets also start in velocity control mode since that is probably what you want for a wheeled robot. Note that in velocity mode `pos_gain` isn't used but I have give you a recommended value anyway in case you wanted to run position control mode. ```txt odrv0.axis0.encoder.config.bandwidth = 100 odrv0.axis0.controller.config.pos_gain = 1 @@ -37,7 +37,7 @@ odrv0.axis0.controller.config.vel_lim = 1000 odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL ``` -In the next step we are going to be start powering the motor, so we want to make sure that some of the above settings that requrie a reboot are applied first. +In the next step we are going to start powering the motor and so we want to make sure that some of the above settings that requrie a reboot are applied first. ```txt odrv0.save_configuration() odrv0.reboot() @@ -53,20 +53,20 @@ You can read out all the data pertaining to the motor: odrv0.axis0.motor ``` -Check to see that there is no error, and that the phase resistance and inductance are reasonable. Here are the results I got: +Check to see that there is no error and that the phase resistance and inductance are reasonable. Here are the results I got: ```txt error = 0x0000 (int) phase_inductance = 0.00033594953129068017 (float) phase_resistance = 0.1793474406003952 (float) ``` -If all looks good you can tell the ODrive that saving this calibration to presistent memory is OK: +If all looks good then you can tell the ODrive that saving this calibration to presistent memory is OK: ```txt odrv0.axis0.motor.config.pre_calibrated = True ``` Next step is to check the alignment between the motor and the hall sensor. -Because of this step you are allowed to plug the motor phases in random order, and also the hall signals can be random. Just don't change it after calibration. +Because of this step you are allowed to plug the motor phases in random order and also the hall signals can be random. Just don't change it after calibration. Make sure the motor is free to move and run: ```txt odrv0.axis0.requested_state = AXIS_STATE_ENCODER_OFFSET_CALIBRATION @@ -77,19 +77,19 @@ Check the status of the encoder object: odrv0.axis0.encoder ``` -Check that there was no error. If your hall sensors has standard timing angle, then `offset_float` should be close to 0.5. +Check that there are no errors. If your hall sensors has a standard timing angle then `offset_float` should be close to 0.5. ```txt error = 0x0000 (int) offset_float = 0.5126956701278687 (float) ``` -If all looks good you can tell the ODrive that saving this calibration to presistent memory is OK: +If all looks good then you can tell the ODrive that saving this calibration to presistent memory is OK: ```txt odrv0.axis0.encoder.config.pre_calibrated = True ``` -OK we are done with the motor configuration! Time to save, reboot, and then test it. -The ODrive starts in idle (we will look at changing this later), so we enable closed loop control. +OK, we are now done with the motor configuration! Time to save, reboot, and then test it. +The ODrive starts in idle (we will look at changing this later) so we can enable closed loop control. ```txt odrv0.save_configuration() odrv0.reboot() @@ -103,8 +103,8 @@ odrv0.axis0.requested_state = AXIS_STATE_IDLE Hopefully you got your motor to spin! Feel free to repeat all of the above for the other axis if appropriate. ### PWM input -If you want to drive your hoverboard wheels around with an RC remote contro, you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link. -Lets use GPIO 3/4 for the velocity inputs, because then we don't have to disable UART. +If you want to drive your hoverboard wheels around with an RC remote contro you can use the [RC PWM input](interfaces.md#rc-pwm-input). There is more information in that link. +Lets use GPIO 3/4 for the velocity inputs so that we don't have to disable UART. Then let's map the full stick range of these inputs to some suitable velocity setpoint range. We also have to reboot to activate the PWM input. ```txt @@ -145,11 +145,11 @@ odrv0.axis1.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL ``` ### Automatic startup -Try to reboot and then activate AXIS_STATE_CLOSED_LOOP_CONTROL on both axis. Check that everything directly is operational and works as expected. -If so, you can make the ODrive turn on the motor power automatically after booting. This is useful if you are going to be running the ODrive without a PC or other logic board. +Try to reboot and then activate AXIS_STATE_CLOSED_LOOP_CONTROL on both axis. Check that everything is operational and works as expected. +If so, you can now make the ODrive turn on the motor power automatically after booting. This is useful if you are going to be running the ODrive without a PC or other logic board. ```txt odrv0.axis0.config.startup_closed_loop_control = True odrv0.axis1.config.startup_closed_loop_control = True odrv0.save_configuration() odrv0.reboot() -``` \ No newline at end of file +``` From f0dea91b0387d5dd04ece13c62502690617d9721 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 23 Jun 2018 17:48:34 -0700 Subject: [PATCH 21/35] Update hoverboard.md --- docs/hoverboard.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/hoverboard.md b/docs/hoverboard.md index f5c998ff3..881a85c2f 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -28,7 +28,7 @@ odrv0.axis0.encoder.config.cpr = 90 Since the hall feedback only has 90 counts per revolution, we want to reduce the velocity tracking bandwidth to get smoother velocity estimates. We can also set these fairly modest gains that will be a bit sloppy but shouldn't shake your rig apart if it's built poorly. Make sure to tune the gains up when you have everything else working to a stiffness that is applicable to your application. -Lets also start in velocity control mode since that is probably what you want for a wheeled robot. Note that in velocity mode `pos_gain` isn't used but I have give you a recommended value anyway in case you wanted to run position control mode. +Lets also start in velocity control mode since that is probably what you want for a wheeled robot. Note that in velocity mode `pos_gain` isn't used but I have given you a recommended value anyway in case you wanted to run position control mode. ```txt odrv0.axis0.encoder.config.bandwidth = 100 odrv0.axis0.controller.config.pos_gain = 1 From cb84e11348dfbbe4123635a5f7453a0d9e865e3f Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 23 Jun 2018 20:05:57 -0700 Subject: [PATCH 22/35] pwm in: only ch4 is available on odrive v3.2 --- Firmware/MotorControl/low_level.cpp | 18 ++++++++++++++++-- Firmware/communication/communication.cpp | 2 ++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 37d1351f5..e39d8a968 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -619,7 +619,12 @@ int tim_2_5_channel_num_to_gpio_num(int channel) { return -1; } #else -#error "Not implemented" + // Only ch4 is available on v3.2 + if (channel == 4) { + return 4; + } else { + return -1; + } #endif } // @brief Returns the TIM2 or TIM5 channel number @@ -634,7 +639,12 @@ uint32_t gpio_num_to_tim_2_5_channel(int gpio_num) { default: return 0; } #else -#error "Not implemented" + // Only ch4 is available on v3.2 + if (gpio_num == 4) { + return TIM_CHANNEL_4; + } else { + return 0; + } #endif } @@ -651,7 +661,11 @@ void pwm_in_init() { sConfigIC.ICPrescaler = TIM_ICPSC_DIV1; sConfigIC.ICFilter = 15; +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 for (int gpio_num = 1; gpio_num <= 4; ++gpio_num) { +#else + int gpio_num = 4; { +#endif if (is_endpoint_ref_valid(board_config.pwm_mappings[gpio_num - 1].endpoint)) { GPIO_InitStruct.Pin = get_gpio_pin_by_pin(gpio_num); HAL_GPIO_DeInit(get_gpio_port_by_pin(gpio_num), get_gpio_pin_by_pin(gpio_num)); diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 2ee436d83..622bdefb3 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -158,9 +158,11 @@ static inline auto make_obj_tree() { make_protocol_property("enable_ascii_protocol_on_usb", &board_config.enable_ascii_protocol_on_usb), 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), +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 make_protocol_object("gpio1_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[0])), make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])), make_protocol_object("gpio3_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[2])), +#endif make_protocol_object("gpio4_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[3])) ), make_protocol_object("axis0", axes[0]->make_protocol_definitions()), From 2e07e0cdd378f2f7c52803edfa7da7e45349c734 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 23 Jun 2018 21:37:47 -0700 Subject: [PATCH 23/35] make a sensorless config --- Firmware/MotorControl/main.cpp | 7 +++- .../MotorControl/sensorless_estimator.cpp | 33 +++++++++---------- .../MotorControl/sensorless_estimator.hpp | 20 ++++++++--- 3 files changed, 36 insertions(+), 24 deletions(-) diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 2f6c54b11..18b884331 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -10,6 +10,7 @@ BoardConfig_t board_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; +SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; ControllerConfig_t controller_configs[AXIS_COUNT]; MotorConfig_t motor_configs[AXIS_COUNT]; AxisConfig_t axis_configs[AXIS_COUNT]; @@ -22,6 +23,7 @@ Axis *axes[AXIS_COUNT]; typedef Config< BoardConfig_t, Encoder::Config_t[AXIS_COUNT], + SensorlessEstimator::Config_t[AXIS_COUNT], ControllerConfig_t[AXIS_COUNT], MotorConfig_t[AXIS_COUNT], AxisConfig_t[AXIS_COUNT]> ConfigFormat; @@ -30,6 +32,7 @@ void save_configuration(void) { if (ConfigFormat::safe_store_config( &board_config, &encoder_configs, + &sensorless_configs, &controller_configs, &motor_configs, &axis_configs)) { @@ -45,6 +48,7 @@ void load_configuration(void) { ConfigFormat::safe_load_config( &board_config, &encoder_configs, + &sensorless_configs, &controller_configs, &motor_configs, &axis_configs)) { @@ -52,6 +56,7 @@ void load_configuration(void) { board_config = BoardConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { encoder_configs[i] = Encoder::Config_t(); + sensorless_configs[i] = SensorlessEstimator::Config_t(); controller_configs[i] = ControllerConfig_t(); motor_configs[i] = MotorConfig_t(); axis_configs[i] = AxisConfig_t(); @@ -152,7 +157,7 @@ int odrive_main(void) { 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(); + SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]); Controller *controller = new Controller(controller_configs[i]); Motor *motor = new Motor(hw_configs[i].motor_config, hw_configs[i].gate_driver_config, diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index 964391572..e684262a7 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,16 +1,9 @@ #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_); -} +SensorlessEstimator::SensorlessEstimator(Config_t& config) : + config_(config) + {}; bool SensorlessEstimator::update() { // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer @@ -21,12 +14,6 @@ bool SensorlessEstimator::update() { // is the one computed two cycles ago. To get the correct measurement, it was stored twice: // once by final_v_alpha/final_v_beta in the current control reporting, and once by V_alpha_beta_memory. - // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp_ < 1.0f)) { - error_ |= ERROR_UNSTABLE_GAIN; - return false; - } - // Clarke transform float I_alpha_beta[2] = { -axis_->motor_.current_meas_.phB - axis_->motor_.current_meas_.phC, @@ -50,10 +37,10 @@ bool SensorlessEstimator::update() { } // Non-linear observer (see paper eqn 8): - float pm_flux_sqr = pm_flux_linkage_ * pm_flux_linkage_; + float pm_flux_sqr = config_.pm_flux_linkage * config_.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); + float eta_factor = 0.5f * (config_.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); @@ -74,6 +61,16 @@ bool SensorlessEstimator::update() { // PLL // TODO: the PLL part has some code duplication with the encoder PLL + // Pll gains as a function of bandwidth + pll_kp_ = 2.0f * config_.pll_bandwidth; + // Critically damped + pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); + // Check that we don't get problems with discrete time approximation + if (!(current_meas_period * pll_kp_ < 1.0f)) { + error_ |= ERROR_UNSTABLE_GAIN; + return false; + } + // 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 diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index d8590d0e7..df69af4a3 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -8,11 +8,18 @@ class SensorlessEstimator { ERROR_UNSTABLE_GAIN = 0x01, }; - SensorlessEstimator(); + struct Config_t { + float observer_gain = 1000.0f; // [rad/s] + float pll_bandwidth = 1000.0f; // [rad/s] + float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } + }; + + SensorlessEstimator(Config_t& config); bool update(); Axis* axis_ = nullptr; // set by Axis constructor + Config_t& config_; // TODO: expose on protocol Error_t error_ = ERROR_NONE; @@ -21,10 +28,8 @@ class SensorlessEstimator { 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 @@ -34,8 +39,13 @@ class SensorlessEstimator { make_protocol_property("phase", &phase_), make_protocol_property("pll_pos", &pll_pos_), make_protocol_property("pll_vel", &pll_vel_), - make_protocol_property("pll_kp", &pll_kp_), - make_protocol_property("pll_ki", &pll_ki_) + // make_protocol_property("pll_kp", &pll_kp_), + // make_protocol_property("pll_ki", &pll_ki_), + make_protocol_object("config", + make_protocol_property("observer_gain", &config_.observer_gain), + make_protocol_property("pll_bandwidth", &config_.pll_bandwidth), + make_protocol_property("pm_flux_linkage", &config_.pm_flux_linkage) + ) ); } }; From 5253d1ce89248d6df77cbeb439e5745a6ca2b59b Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 23 Jun 2018 22:38:48 -0700 Subject: [PATCH 24/35] Made sensorless control configurable, and also smooth vel_setpoint transition --- Firmware/MotorControl/axis.cpp | 5 +++++ Firmware/MotorControl/sensorless_estimator.cpp | 13 +++++-------- Firmware/MotorControl/sensorless_estimator.hpp | 4 ++-- Firmware/MotorControl/utils.c | 10 ++++++---- 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 3efd8f8d3..088283360 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -148,6 +148,11 @@ bool Axis::run_sensorless_spin_up() { return error_ |= ERROR_MOTOR_FAILED, false; return vel < config_.spin_up_target_vel; }); + + // call to controller.reset() that happend when arming means that vel_setpoint + // is zeroed. So we make the setpoint the spinup target for smooth transition. + controller_.vel_setpoint_ = config_.spin_up_target_vel; + return error_ == ERROR_NONE; } diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index e684262a7..25210f988 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -42,9 +42,6 @@ bool SensorlessEstimator::update() { float bandwidth_factor = 1.0f / pm_flux_sqr; float eta_factor = 0.5f * (config_.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 @@ -62,11 +59,11 @@ bool SensorlessEstimator::update() { // PLL // TODO: the PLL part has some code duplication with the encoder PLL // Pll gains as a function of bandwidth - pll_kp_ = 2.0f * config_.pll_bandwidth; + float pll_kp = 2.0f * config_.pll_bandwidth; // Critically damped - pll_ki_ = 0.25f * (pll_kp_ * pll_kp_); + float pll_ki = 0.25f * (pll_kp * pll_kp); // Check that we don't get problems with discrete time approximation - if (!(current_meas_period * pll_kp_ < 1.0f)) { + if (!(current_meas_period * pll_kp < 1.0f)) { error_ |= ERROR_UNSTABLE_GAIN; return false; } @@ -76,9 +73,9 @@ bool SensorlessEstimator::update() { // 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); + 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; + pll_vel_ += current_meas_period * pll_ki * delta_phase; return true; }; diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index df69af4a3..77f509431 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -26,8 +26,8 @@ class SensorlessEstimator { 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 pll_kp_ = 0.0f; // [rad/s / rad] + // float pll_ki_ = 0.0f; // [(rad/s^2) / rad] float flux_state_[2] = {0.0f, 0.0f}; // [Vs] float V_alpha_beta_memory_[2] = {0.0f, 0.0f}; // [V] bool estimator_good_ = false; diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 72201269e..5d3b92038 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -1,6 +1,7 @@ #include #include +#include #include #include @@ -133,12 +134,13 @@ float fast_atan2(float y, float x) { // a := min (|x|, |y|) / max (|x|, |y|) float abs_y = fabsf(y); float abs_x = fabsf(x); - float a = MACRO_MIN(abs_x, abs_y) / MACRO_MAX(abs_x, abs_y); - //s := a * a + // inject FLT_MIN in denominator to avoid division by zero + float a = MACRO_MIN(abs_x, abs_y) / (MACRO_MAX(abs_x, abs_y) + FLT_MIN); + // s := a * a float s = a * a; - //r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a + // r := ((-0.0464964749 * s + 0.15931422) * s - 0.327622764) * s * a + a float r = ((-0.0464964749f * s + 0.15931422f) * s - 0.327622764f) * s * a + a; - //if |y| > |x| then r := 1.57079637 - r + // if |y| > |x| then r := 1.57079637 - r if (abs_y > abs_x) r = 1.57079637f - r; // if x < 0 then r := 3.14159274 - r From 106cb78903ce20ff8cd928b81b7632ad653ea974 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 23 Jun 2018 22:58:26 -0700 Subject: [PATCH 25/35] fix indentation --- docs/getting-started.md | 93 +++++++++++++++++++++-------------------- 1 file changed, 47 insertions(+), 46 deletions(-) diff --git a/docs/getting-started.md b/docs/getting-started.md index 2036796b5..301282ab9 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -41,11 +41,11 @@ You will need: * A power supply (12V-24V for the 24V board variant, 12V-48V for the 48V board variant). A battery is also fine. -
What voltage variant do I have?
- On all ODrives shipped July 2018 or after have a silkscreen label clearly indicating the voltage variant. - - ODrives before this may or may not have this label. If you don't have a label, then you can look at the bus capacitors (8 gray cylinder components on the underside of the board). If they read 470uF, you have a 24V version; if they read 120uF you have a 48V version. -
+
What voltage variant do I have?
+ On all ODrives shipped July 2018 or after have a silkscreen label clearly indicating the voltage variant. + + ODrives before this may or may not have this label. If you don't have a label, then you can look at the bus capacitors (8 gray cylinder components on the underside of the board). If they read 470uF, you have a 24V version; if they read 120uF you have a 48V version. +
## Wiring up the ODrive @@ -68,12 +68,12 @@ Most instructions in this guide refer to a utility called `odrivetool`, so you s ### Windows 1. Install Python 3. We recommend the Anaconda distribution because it packs a lot of useful scientific tools, however you can also install the standalone python. - * __Anaconda__: Download the installer from [here](https://www.anaconda.com/download/#windows). Execute the downloaded file and follow the instructions. - * __Standalone Python__: Download the installer from [here](https://www.python.org/downloads/). Execute the downloaded file and follow the instructions. - * If you have Python 2 installed alongside Python 3, replace `pip` by `C:\Users\YOUR_USERNAME\AppData\Local\Programs\Python\Python36-32\Scripts\pip`. If you have trouble with this step then refer to [this walkthrough](https://www.youtube.com/watch?v=jnpC_Ib_lbc). + * __Anaconda__: Download the installer from [here](https://www.anaconda.com/download/#windows). Execute the downloaded file and follow the instructions. + * __Standalone Python__: Download the installer from [here](https://www.python.org/downloads/). Execute the downloaded file and follow the instructions. + * If you have Python 2 installed alongside Python 3, replace `pip` by `C:\Users\YOUR_USERNAME\AppData\Local\Programs\Python\Python36-32\Scripts\pip`. If you have trouble with this step then refer to [this walkthrough](https://www.youtube.com/watch?v=jnpC_Ib_lbc). 2. Launch the command prompt. - * __Anaconda__: In the start menu, type `Anaconda Prompt` Enter - * __Standalone Python__: In the start menu, type `cmd` Enter + * __Anaconda__: In the start menu, type `Anaconda Prompt` Enter + * __Standalone Python__: In the start menu, type `cmd` Enter 3. Install dependencies by typing `pip install pywin32==222` Enter 3. Install the ODrive tools by typing `pip install odrive` Enter 4. Plug in a USB cable into the microUSB connector on ODrive, and connect it to your PC. @@ -137,42 +137,43 @@ You can read more about the odrivetool [here](odrivetool.md). 1. Set the limits: -
Wait, how do I set these?
+
Wait, how do I set these?
- In the previous step we started `odrivetool`. In there, you can assign variables directly by name. + In the previous step we started `odrivetool`. In there, you can assign variables directly by name. - For instance, to set the current limit of M0 to 10A you would type: `odrv0.axis0.motor.config.current_lim = 10` Enter + For instance, to set the current limit of M0 to 10A you would type: `odrv0.axis0.motor.config.current_lim = 10` Enter -
+
- * The current limit: `odrv0.axis0.motor.config.current_lim` [A]. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. - * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. -
Ok so tell me how it actually works then...
- The current in the motor is only connected to the current in the power supply _sometimes_ and other times it just cycles out of one phase and back in the other. This is what the modulation magnitude is (sometimes people call this duty cycle, but that's a bit confusing because we use SVM not straight PWM). When the modulation magnitude is 0, the average voltage seen across the motor phases is 0, and the motor current is never connected to the power supply. When the magnitude is 100%, it is always connected, and at 50% it's connected half the time, and cycled in just the motor half the time. + * The current limit: `odrv0.axis0.motor.config.current_lim` [A]. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. + * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. +
Ok so tell me how it actually works then...
+ The current in the motor is only connected to the current in the power supply _sometimes_ and other times it just cycles out of one phase and back in the other. This is what the modulation magnitude is (sometimes people call this duty cycle, but that's a bit confusing because we use SVM not straight PWM). When the modulation magnitude is 0, the average voltage seen across the motor phases is 0, and the motor current is never connected to the power supply. When the magnitude is 100%, it is always connected, and at 50% it's connected half the time, and cycled in just the motor half the time. - The largest effect on modulation magnitude is speed. There are other smaller factors, but in general: if the motor is still it's not unreasonable to have 50A in the motor from 5A on the power supply. When the motor is spinning close to top speed, the power supply current and the motor current will be somewhat close to each other. -
- * The velocity limit: `odrv0.axis0.motor.config.vel_limit` [counts/s]. The motor will be limited to this speed; again the default value is quite slow. - * You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. + The largest effect on modulation magnitude is speed. There are other smaller factors, but in general: if the motor is still it's not unreasonable to have 50A in the motor from 5A on the power supply. When the motor is spinning close to top speed, the power supply current and the motor current will be somewhat close to each other. +
+ * The velocity limit: `odrv0.axis0.motor.config.vel_limit` [counts/s]. The motor will be limited to this speed; again the default value is quite slow. + * You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. 2. Set other hardware parameters: - * `odrv0.config.brake_resistance` [Ohm]: This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. - * `odrv0.axis0.motor.config.pole_pairs`: This is the number of **magnet poles** in the rotor, **divided by two**. You can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ - * `odrv0.axis0.motor.config.motor_type`: This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). - -
Which `motor_type` to choose?
+ * `odrv0.config.brake_resistance` [Ohm]: This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. + * `odrv0.axis0.motor.config.pole_pairs`: This is the number of **magnet poles** in the rotor, **divided by two**. You can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ + * `odrv0.axis0.motor.config.motor_type`: This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). + +
Which `motor_type` to choose?
- If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For low-current gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. Do not use `MOTOR_TYPE_GIMBAL` on a motor that is not a gimbal motor, as it may overheat the motor or the ODrive. + If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For low-current gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. Do not use `MOTOR_TYPE_GIMBAL` on a motor that is not a gimbal motor, as it may overheat the motor or the ODrive. - **Further detail:** - If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. - If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. - If 100's of mA current noise is "large" for you, _and_ you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. + **Further detail:** + If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. + If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. + If 100's of mA current noise is "large" for you, _and_ you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. -
+
+ + * `odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder. - * `odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder. 3. Save configuration. You can save all `.config` parameters to persistent memory such that the ODrive remembers them between power cycles. * `odrv0.save_configuration()` Enter @@ -183,22 +184,22 @@ Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, 1. Type `odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE` Enter. After about 2 seconds should hear a beep. Then the motor will turn slowly in one direction for a few seconds, then back in the other direction. -
What's the point of this?
- This procedure first measures your motor's electrical properties (namely phase resistance and phase inductance) and then the offset between the motor's electrical phase and the encoder position. +
What's the point of this?
+ This procedure first measures your motor's electrical properties (namely phase resistance and phase inductance) and then the offset between the motor's electrical phase and the encoder position. -
+
- The startup procedure is demonstrated [here](https://www.youtube.com/watch?v=VCX1bA2xnuY). + The startup procedure is demonstrated [here](https://www.youtube.com/watch?v=VCX1bA2xnuY). - **Note**: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initalisation, but in the current software the default behaviour is not like that. + **Note**: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initalisation, but in the current software the default behaviour is not like that. -
My motor doesn't beep or doesn't turn
+
My motor doesn't beep or doesn't turn
- Make sure the motor wires are connected firmly. Check the value of `odrv0.axis0.error` and then refer to the [error code documentation](troubleshooting.md#error-codes) for details. + Make sure the motor wires are connected firmly. Check the value of `odrv0.axis0.error` and then refer to the [error code documentation](troubleshooting.md#error-codes) for details. - Once you have understood the error and fixed its cause, you may clear the error state (`odrv0.axis0.error = 0` Enter) and retry. You may also need to clear the error state of other subcomponents (e.g. `odrv0.axis0.motor.error`). + Once you have understood the error and fixed its cause, you may clear the error state (`odrv0.axis0.error = 0` Enter) and retry. You may also need to clear the error state of other subcomponents (e.g. `odrv0.axis0.motor.error`). -
+
2. Type `odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL` Enter. From now on the ODrive will try to hold the motor's position. If you try to turn it by hand, it will fight you gently. That is unless you bump up `odrv0.axis0.motor.config.current_lim`, in which case it will fight you more fiercely. @@ -207,8 +208,8 @@ Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, You can now: - * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance. - * Control the ODrive from your own program or hook it up to an existing system through one of it's [interfaces](interfaces). - * See how you can improve the behavior during the startup procedure, like [bypassing encoder calibration](encoders.md#encoder-with-index-signal). +* See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance. +* Control the ODrive from your own program or hook it up to an existing system through one of it's [interfaces](interfaces). +* See how you can improve the behavior during the startup procedure, like [bypassing encoder calibration](encoders.md#encoder-with-index-signal). If you have any issues or any questions please get in touch. The [ODrive Community](https://discourse.odriverobotics.com/) warmly welcomes you. From 196d672fc00887b2b2a19300021e7d93966cb2fe Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 25 Jun 2018 16:11:22 -0700 Subject: [PATCH 26/35] sensorless docs WIP --- docs/commands.md | 11 +++++++++++ docs/getting-started.md | 5 ++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/docs/commands.md b/docs/commands.md index dc6b2b842..23e6e2669 100644 --- a/docs/commands.md +++ b/docs/commands.md @@ -79,3 +79,14 @@ All variables that are part of a `[...].config` object can be saved to non-volat * `.fw_version_major`, `.fw_version_minor`, `.fw_version_revision`: The firmware version that is currently running. * `.hw_version_major`, `.hw_version_minor`, `.hw_version_revision`: The hardware version of your ODrive. +## Setting up sensorless +The ODrive can run without encoder/hall feedback, but there is a minimum speed, usually around a few hunderd RPM. +However the +``` +odrv0.axis0.controller.config.vel_gain = 0.1 +odrv0.axis0.controller.config.vel_integrator_gain = 0 +odrv0.axis0.controller.config.control_mode = 2 +odrv0.axis0.controller.vel_setpoint = 400 +odrv0.axis0.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / ( * ) +``` + diff --git a/docs/getting-started.md b/docs/getting-started.md index 301282ab9..6b2139706 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -172,7 +172,10 @@ You can read more about the odrivetool [here](odrivetool.md). - * `odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder. + * _if using encoder_: `odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder. + * _if not using encoder_: + * If you wish to run in sensorless mode, please see [Setting up Sensorless](commands.md#setting-up-sensorless). + * If you are using hall sensor feedback, please see the [hoverboard motor example](hoverboard.md). 3. Save configuration. You can save all `.config` parameters to persistent memory such that the ODrive remembers them between power cycles. From b306c35c3fe41fb403ee4a41365afbbd4dd1154d Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Mon, 25 Jun 2018 19:10:46 -0700 Subject: [PATCH 27/35] Update interfaces.md --- docs/interfaces.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/interfaces.md b/docs/interfaces.md index 1d9cd21cf..33bdb3c0b 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -50,6 +50,7 @@ For predictable results, try to have only one feature enabled for any one pin. W ### Hall feedback pinout When the encoder mode is set to hall feedback, the pinout on the encoder port is as follows: + | Label on ODrive | Hall feedback | |-----------------|---------------| | A | Hall A | From 9784608c69e8a165cf20a6cca42c8182f0050d90 Mon Sep 17 00:00:00 2001 From: Nick Winters Date: Tue, 26 Jun 2018 22:41:26 -0700 Subject: [PATCH 28/35] Fix vel_lim/vel_limit typo odrv0.axis0.controller.config.vel_lim should end in vel_limit --- docs/hoverboard.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/hoverboard.md b/docs/hoverboard.md index 881a85c2f..54cd70f5f 100644 --- a/docs/hoverboard.md +++ b/docs/hoverboard.md @@ -33,7 +33,7 @@ Lets also start in velocity control mode since that is probably what you want fo odrv0.axis0.encoder.config.bandwidth = 100 odrv0.axis0.controller.config.pos_gain = 1 odrv0.axis0.controller.config.vel_gain = 0.02 -odrv0.axis0.controller.config.vel_lim = 1000 +odrv0.axis0.controller.config.vel_limit = 1000 odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL ``` From 444f97b96b58c7c3ca5942c98b1832046ed5b08c Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Thu, 28 Jun 2018 19:11:47 -0700 Subject: [PATCH 29/35] add thermistor fit file --- analysis/thermistors.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 analysis/thermistors.py diff --git a/analysis/thermistors.py b/analysis/thermistors.py new file mode 100644 index 000000000..5a5c904c9 --- /dev/null +++ b/analysis/thermistors.py @@ -0,0 +1,32 @@ +#%% +import matplotlib.pyplot as plt +import numpy as np + +Rload = 3300 +R_25 = 10000 +T_25 = 25 + 273.15 #Kelvin +Beta = 3380 +Tmin = 0 +Tmax = 140 + +temps = np.linspace(Tmin, Tmax, 1000) +tempsK = temps + 273.15 + +# https://en.wikipedia.org/wiki/Thermistor#B_or_%CE%B2_parameter_equation +r_inf = R_25 * np.exp(-Beta/T_25) +R_temps = r_inf * np.exp(Beta/tempsK) +V = Rload / (Rload + R_temps) + +fit = np.polyfit(V, temps, 3) +p1 = np.poly1d(fit) +fit_temps = p1(V) + +#%% +print(fit) + +plt.plot(V, temps, label='actual') +plt.plot(V, fit_temps, label='fit') +plt.xlabel('normalized voltage') +plt.ylabel('Temp [C]') +plt.legend(loc=0) +plt.show() \ No newline at end of file From 721132c4dc04908c1d50af503565f65754d9613b Mon Sep 17 00:00:00 2001 From: metanoic <5554798+metanoic@users.noreply.github.com> Date: Fri, 29 Jun 2018 13:56:51 -0500 Subject: [PATCH 30/35] Fix minor spelling/grammar in protocol doc --- docs/protocol.md | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/protocol.md b/docs/protocol.md index 1a3bae4fa..f916b1fe1 100644 --- a/docs/protocol.md +++ b/docs/protocol.md @@ -3,13 +3,13 @@ Communicating with an ODrive consists of a series of endpoint operations. An endpoint can theoretically be any kind data serialized in any way. -There is a default seralization implementation for POD types; for custom types -you must (de)seralize yourself. In the future we may provide a default seralizer -for stucts. +There is a default serialization implementation for POD types; for custom types +you must (de)serialize yourself. In the future we may provide a default serializer +for structs. The available endpoints can be enumerated by reading the JSON from endpoint 0 and can theoretically be different for each communication interface (they are not in practice). -Each endpoint operation can send bytes to one endpoint (referenced by it's ID) +Each endpoint operation can send bytes to one endpoint (referenced by its ID) and at the same time receive bytes from the same endpoint. The semantics of these payloads are specific to each endpoint's type, the name of which is indicated in the JSON. From e30a0642640c3fc0e3e5b5f82d27fb7a0549d8ea Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 1 Jul 2018 00:15:56 -0700 Subject: [PATCH 31/35] add horner_fma to utils --- Firmware/MotorControl/utils.c | 7 +++++++ Firmware/MotorControl/utils.h | 1 + 2 files changed, 8 insertions(+) diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index 5d3b92038..f70f5170f 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -153,6 +153,13 @@ float fast_atan2(float y, float x) { return r; } +float horner_fma(float x, const float *coeffs, size_t count) { + float result = 0.0f; + for (int idx = count-1; idx >= 0; idx--) + result = fmaf(result, x, coeffs[idx]); + return result; +} + // Modulo (as opposed to remainder), per https://stackoverflow.com/a/19288271 int mod(int dividend, int divisor){ int r = dividend % divisor; diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index eeb104b39..1cd63327b 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -94,6 +94,7 @@ static inline float fmodf_pos(float x, float y) { int SVM(float alpha, float beta, float* tA, float* tB, float* tC); float fast_atan2(float y, float x); +float horner_fma(float x, const float *coeffs, size_t count); int mod(int dividend, int divisor); uint32_t deadline_to_timeout(uint32_t deadline_ms); From 5cc1a9546def8dd4809bc8a3427c8d205e735fb0 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 1 Jul 2018 01:11:44 -0700 Subject: [PATCH 32/35] implement axis temp reading --- CHANGELOG.md | 1 + Firmware/MotorControl/axis.cpp | 6 ++++++ Firmware/MotorControl/axis.hpp | 2 ++ Firmware/MotorControl/board_config_v3.h | 15 +++++++++++++++ Firmware/MotorControl/low_level.cpp | 6 ++++-- Firmware/MotorControl/low_level.h | 4 ++++ Firmware/MotorControl/odrive_main.h | 3 --- Firmware/MotorControl/utils.c | 5 ++++- tools/setup.py | 2 +- 9 files changed, 37 insertions(+), 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 359319aed..16cda1ecf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ Please add a note of your changes below this heading if you make a Pull Request. ### Added * Hall sensor feedback * Configurable RC PWM input +* Ability to read axis FET temperature * Config settings for: * `motor.config.requested_current_range` * `motor.config.current_control_bandwidth` and `motor.set_current_control_bandwidth`. Latter required to invoke gain recalculation. diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 088283360..270ab15f8 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -123,6 +123,12 @@ bool Axis::do_updates() { return error_ == ERROR_NONE; } +float Axis::get_temp() { + float adc = adc_measurements_[hw_config_.thermistor_adc_ch]; + float normalized_voltage = adc / adc_full_scale; + return horner_fma(normalized_voltage, thermistor_poly_coeffs, thermistor_num_coeffs); +} + bool Axis::run_sensorless_spin_up() { // Early Spin-up: spiral up current float x = 0.0f; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 8eda28921..cfa1bd28a 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -79,6 +79,7 @@ class Axis { bool check_PSU_brownout(); bool do_checks(); bool do_updates(); + float get_temp(); // @brief Runs the specified update handler at the frequency of the current measurements. // @@ -178,6 +179,7 @@ class Axis { make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration), make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel) ), + make_protocol_function("get_temp", *this, &Axis::get_temp), make_protocol_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), make_protocol_object("encoder", encoder_.make_protocol_definitions()), diff --git a/Firmware/MotorControl/board_config_v3.h b/Firmware/MotorControl/board_config_v3.h index 3885727ac..1e170fedb 100644 --- a/Firmware/MotorControl/board_config_v3.h +++ b/Firmware/MotorControl/board_config_v3.h @@ -25,6 +25,7 @@ typedef struct { uint16_t step_pin; GPIO_TypeDef* dir_port; uint16_t dir_pin; + size_t thermistor_adc_ch; osPriority thread_priority; } AxisHardwareConfig_t; @@ -61,15 +62,23 @@ typedef struct { } BoardHardwareConfig_t; extern const BoardHardwareConfig_t hw_configs[2]; +extern const float thermistor_poly_coeffs[]; +extern const size_t thermistor_num_coeffs; //TODO stick this in a C file #ifdef __MAIN_CPP__ +const float thermistor_poly_coeffs[] = + {363.0172658f, -459.19773008f, 308.29273921f, -28.12731452f}; +const size_t thermistor_num_coeffs = sizeof(thermistor_poly_coeffs)/sizeof(thermistor_poly_coeffs[1]); + const BoardHardwareConfig_t hw_configs[2] = { { + //M0 .axis_config = { .step_port = GPIO_1_GPIO_Port, .step_pin = GPIO_1_Pin, .dir_port = GPIO_2_GPIO_Port, .dir_pin = GPIO_2_Pin, + .thermistor_adc_ch = 15, .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1), }, .encoder_config = { @@ -99,6 +108,7 @@ const BoardHardwareConfig_t hw_configs[2] = { { .nFAULT_pin = nFAULT_Pin, } },{ + //M1 .axis_config = { #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5 .step_port = GPIO_7_GPIO_Port, @@ -110,6 +120,11 @@ const BoardHardwareConfig_t hw_configs[2] = { { .step_pin = GPIO_3_Pin, .dir_port = GPIO_4_GPIO_Port, .dir_pin = GPIO_4_Pin, +#endif +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 + .thermistor_adc_ch = 4, +#else + .thermistor_adc_ch = 1, #endif .thread_priority = osPriorityHigh, }, diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index e39d8a968..43e59bd02 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -28,6 +28,8 @@ /* Private macros ------------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/ /* Global constant data ------------------------------------------------------*/ +const float adc_full_scale = (float)(1 << 12); +const float adc_ref_voltage = 3.3f; /* Global variables ----------------------------------------------------------*/ // This value is updated by the DC-bus reading ADC. @@ -422,7 +424,7 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { channel = 15; } if (channel < ADC_CHANNEL_COUNT) - return ((float)adc_measurements_[channel]) * (3.3f / (float)(1 << 12)); + return ((float)adc_measurements_[channel]) * (adc_ref_voltage / adc_full_scale); else return 0.0f / 0.0f; // NaN } @@ -432,7 +434,7 @@ float get_adc_voltage(GPIO_TypeDef* GPIO_port, uint16_t GPIO_pin) { //-------------------------------- void vbus_sense_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { - static const float voltage_scale = 3.3f * VBUS_S_DIVIDER_RATIO / (float)(1 << 12); + static const float voltage_scale = adc_ref_voltage * VBUS_S_DIVIDER_RATIO / adc_full_scale; // Only one conversion in sequence, so only rank1 uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1); vbus_voltage = ADCValue * voltage_scale; diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 41f7f4b5e..4af6e3e06 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -17,9 +17,13 @@ extern "C" { /* Exported types ------------------------------------------------------------*/ /* Exported constants --------------------------------------------------------*/ +#define ADC_CHANNEL_COUNT 16 +extern const float adc_full_scale; +extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; extern bool brake_resistor_armed; +extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index bf35821f8..4e4db1607 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -36,9 +36,6 @@ extern bool user_config_loaded_; extern uint64_t serial_number; extern char serial_number_str[13]; -#define ADC_CHANNEL_COUNT 16 -extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; - typedef struct { bool fully_booted; uint32_t uptime; // [ms] diff --git a/Firmware/MotorControl/utils.c b/Firmware/MotorControl/utils.c index f70f5170f..b59af789a 100644 --- a/Firmware/MotorControl/utils.c +++ b/Firmware/MotorControl/utils.c @@ -153,9 +153,12 @@ float fast_atan2(float y, float x) { return r; } +// Evaluate polynomials using Fused Multiply Add intrisic instruction. +// coeffs[0] is highest order, as per numpy.polyfit +// p(x) = coeffs[0] * x^deg + ... + coeffs[deg], for some degree "deg" float horner_fma(float x, const float *coeffs, size_t count) { float result = 0.0f; - for (int idx = count-1; idx >= 0; idx--) + for (int idx = 0; idx < count; ++idx) result = fmaf(result, x, coeffs[idx]); return result; } diff --git a/tools/setup.py b/tools/setup.py index d5e38eff3..961ceec6d 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -33,7 +33,7 @@ # Set to true to make an official post-release, rather than dev of new version is_post_release = False -post_rel_num = 5 +post_rel_num = 8 # To test higher numbered releases, bump to the next rev bump_rev = not is_post_release From e15d8386d4a50e7dac9004a0d71a65591b25e326 Mon Sep 17 00:00:00 2001 From: metanoic <5554798+metanoic@users.noreply.github.com> Date: Tue, 17 Jul 2018 10:43:06 -0500 Subject: [PATCH 33/35] Add note for error I received and resolution Adding tip for anyone running into the issue I ran into of openocd not being able to find its scripts directory/.cfg files. --- docs/developer-guide.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/developer-guide.md b/docs/developer-guide.md index 93fde5f2f..440f78ac8 100644 --- a/docs/developer-guide.md +++ b/docs/developer-guide.md @@ -118,6 +118,7 @@ You can also modify the compile-time defaults for all `.config` parameters. You * Connect `GND`, `SWD`, and `SWC` on connector J2 to the programmer. Note: Always plug in `GND` first! * You need to power the board by only **ONE** of the following: VCC(3.3v), 5V, or the main power connection (the DC bus). The USB port (J1) does not power the board. * Run `make flash` in the `Firmware` directory. +__Note__: If you receive the error `can't find target interface/stlink-v2.cfg` or similar, create and set an environment variable named `OPENOCD_SCRIPTS` to the location of the openocd scripts directory. If the flashing worked, you can connect to the board using the [odrivetool](getting-started#start-odrivetool). From bdec727e31513f506b7964d5e2c27acb810e7b76 Mon Sep 17 00:00:00 2001 From: Capo01 <503426+Capo01@users.noreply.github.com> Date: Wed, 25 Jul 2018 21:20:53 +1000 Subject: [PATCH 34/35] Update odrivetool Note sure if this is intentional but currently liveplotter is listed as 'Upgrade the ODrive's Firmware' in the odrivetool help file. That doesn't seem right to me. --- tools/odrivetool | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tools/odrivetool b/tools/odrivetool index e3a3ed22b..c6632e902 100755 --- a/tools/odrivetool +++ b/tools/odrivetool @@ -71,7 +71,7 @@ code_generator_parser.add_argument("-o", "--output", type=argparse.FileType('w') help="path of the generated output") code_generator_parser.set_defaults(template = os.path.join(script_path, 'odrive_header_template.h.in')) -subparsers.add_parser('liveplotter', help="Upgrade the ODrive's Firmware") +subparsers.add_parser('liveplotter', help="For plotting of odrive parameters (i.e. position) in real time") subparsers.add_parser('drv-status', help="Show status of the on-board DRV8301 chips (for debugging only)") subparsers.add_parser('rate-test', help="Estimate the average transmission bandwidth over USB") subparsers.add_parser('udev-setup', help="Linux only: Gives users on your system permission to access the ODrive by installing udev rules") From b37df2616f240cdd101d07620187c61c8140c7b2 Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sat, 4 Aug 2018 22:45:09 -0700 Subject: [PATCH 35/35] update changelog for fw release --- CHANGELOG.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e619a52f5..d801de25f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,7 +1,8 @@ # Unreleased Features Please add a note of your changes below this heading if you make a Pull Request. -## UNRELEASED +# Releases +## [0.4.2] - 2018-07-04 ### Added * Hall sensor feedback * Configurable RC PWM input @@ -11,7 +12,6 @@ Please add a note of your changes below this heading if you make a Pull Request. * `motor.config.current_control_bandwidth` and `motor.set_current_control_bandwidth`. Latter required to invoke gain recalculation. * `encoder.config.bandwidth` -# Releases ## [0.4.1] - 2018-07-01 ### Fixed * Encoder errors would show up as Axis error `ERROR_MOTOR_FAILED` instead of `ERROR_ENCODER_FAILED`.