From d83d01522237b2395b844a54e13ac40bc7300ffd Mon Sep 17 00:00:00 2001 From: Oskar Weigl Date: Sun, 13 Nov 2016 16:58:48 +0900 Subject: [PATCH] restucture files --- .gitignore | 5 +- Makefile | 4 +- MotorControl/{test.c => low_level.c} | 266 ++++++++++----------------- MotorControl/low_level.h | 21 +++ Src/freertos.c | 3 +- Src/main.c | 4 - Test/test.c | 101 ++++++++++ {MotorControl => Test}/test.h | 1 - 8 files changed, 225 insertions(+), 180 deletions(-) rename MotorControl/{test.c => low_level.c} (68%) create mode 100644 MotorControl/low_level.h create mode 100644 Test/test.c rename {MotorControl => Test}/test.h (62%) diff --git a/.gitignore b/.gitignore index b8c4c782..c60ca775 100644 --- a/.gitignore +++ b/.gitignore @@ -12,4 +12,7 @@ README.html #Eclipse stuff .settings/ -.project \ No newline at end of file +.project + +# STM32CubeMX (in case you put it in this folder, or a symlink) +STM32CubeMX \ No newline at end of file diff --git a/Makefile b/Makefile index f43ffdff..9fdecec8 100644 --- a/Makefile +++ b/Makefile @@ -34,6 +34,7 @@ C_SOURCES = \ Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c \ Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c \ Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS/cmsis_os.c \ + Test/test.c \ Src/stm32f4xx_it.c \ Src/tim.c \ Src/gpio.c \ @@ -63,7 +64,7 @@ C_SOURCES = \ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c \ Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c \ Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c \ - MotorControl/test.c + MotorControl/low_level.c ASM_SOURCES = \ Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/gcc/startup_stm32f405xx.s @@ -89,6 +90,7 @@ AS_INCLUDES = C_INCLUDES = -IMiddlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F C_INCLUDES += -IMiddlewares/Third_Party/FreeRTOS/Source/include C_INCLUDES += -IMiddlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS +C_INCLUDES += -ITest C_INCLUDES += -IDrivers/DRV8301 C_INCLUDES += -IDrivers/STM32F4xx_HAL_Driver/Inc C_INCLUDES += -IDrivers/STM32F4xx_HAL_Driver/Inc/Legacy diff --git a/MotorControl/test.c b/MotorControl/low_level.c similarity index 68% rename from MotorControl/test.c rename to MotorControl/low_level.c index ae575a2c..d8bc124a 100644 --- a/MotorControl/test.c +++ b/MotorControl/low_level.c @@ -1,21 +1,93 @@ -#include "test.h" +#include -#include "stm32f4xx_hal.h" -#include "stm32f405xx.h" -#include "assert.h" #include "cmsis_os.h" #include "adc.h" #include "tim.h" #include "spi.h" -#include "drv8301.h" -#include "math.h" -#include "stdint.h" -void start_adc_pwm(){ +// Global variables +Motor_t motor_configs[] = { + { //M0 + .gate_driver = { + .spiHandle = &hspi3, + //Note: this board has the EN_Gate pin shared! + .EngpioHandle = EN_GATE_GPIO_Port, + .EngpioNumber = EN_GATE_Pin, + .nCSgpioHandle = M0_nCS_GPIO_Port, + .nCSgpioNumber = M0_nCS_Pin, + .RxTimeOut = false, + .enableTimeOut = false + }, + .shunt_conductance = 1.0f/0.0005f, //[S] + .maxcurrent = 75.0f //[A] //Note: consistent with 40v/v gain + } +}; +const int num_motors = sizeof(motor_configs)/sizeof(motor_configs[0]); +// Private variables +//Local view of DRV registers +static DRV_SPI_8301_Vars_t gate_driver_regs[1/*num_motors*/]; + +// current sense queue from ADC to motor control task +typedef struct { + float current_phB; + float current_phC; +} Iph_BC_queue_item_t; +osMailQDef (Iph_queue_def, 2, Iph_BC_queue_item_t); +osMailQId (M0_Iph_queue); + +// Private function prototypes +static void DRV8301_setup(); +static void start_adc_pwm(); +static float phase_current_from_adcval(uint32_t ADCValue, int motornum); +static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc); + + +//Special function name for ADC callback. +//Automatically registered if defined. +void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) { + pwm_trig_adc_cb(hadc); +} + + +void init_motor_control() { + //Allocate the queues + M0_Iph_queue = osMailCreate(osMailQ(Iph_queue_def), NULL); + + //Init gate drivers + DRV8301_setup(); + + osDelay(1000); + + // Start PWM and enable adc interrupts/callbacks + start_adc_pwm(); +} + +// Set up the gate drivers +static void DRV8301_setup() { + for (int i = 0; i < num_motors; ++i) { + DRV8301_enable(&motor_configs[i].gate_driver); + DRV8301_setupSpi(&motor_configs[i].gate_driver, &gate_driver_regs[i]); + + //@TODO we can use reporting only if we actually wire up the nOCTW pin + gate_driver_regs[i].Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; + //Overcurrent set to approximately 150A at 100degC. This may need tweaking. + gate_driver_regs[i].Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; + //20V/V on 500uOhm gives a range of +/- 150A + //40V/V on 500uOhm gives a range of +/- 75A + gate_driver_regs[i].Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; + + gate_driver_regs[i].SndCmd = true; + DRV8301_writeData(&motor_configs[i].gate_driver, &gate_driver_regs[i]); + gate_driver_regs[i].RcvCmd = true; + DRV8301_readData(&motor_configs[i].gate_driver, &gate_driver_regs[i]); + } +} + +static void start_adc_pwm(){ //Enable ADC and interrupts __HAL_ADC_ENABLE(&hadc2); __HAL_ADC_ENABLE(&hadc3); @@ -24,6 +96,10 @@ void start_adc_pwm(){ __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); __HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC); + //Ensure that debug halting of the core doesn't leave the motor PWM running + __HAL_DBGMCU_FREEZE_TIM1(); + __HAL_DBGMCU_FREEZE_TIM8(); + //Init PWM int half_load = htim1.Instance->ARR/2; htim1.Instance->CCR1 = half_load; @@ -43,134 +119,9 @@ void start_adc_pwm(){ //Turn off output //__HAL_TIM_MOE_DISABLE(&htim1); - } -typedef struct Motor_s { - DRV8301_Obj gate_driver; - float shunt_conductance; - float maxcurrent; -} Motor_t; - -Motor_t motor_configs[] = { - { //M0 - .gate_driver = { - .spiHandle = &hspi3, - //Note: this board has the EN_Gate pin shared! - .EngpioHandle = EN_GATE_GPIO_Port, - .EngpioNumber = EN_GATE_Pin, - .nCSgpioHandle = M0_nCS_GPIO_Port, - .nCSgpioNumber = M0_nCS_Pin, - .RxTimeOut = false, - .enableTimeOut = false - }, - .shunt_conductance = 1.0f/0.0005f, //[S] - .maxcurrent = 75.0f //[A] //Note: consistent with 40v/v gain - } -}; - -static const int num_motors = sizeof(motor_configs)/sizeof(motor_configs[0]); - -//Local view of DRV registers -static DRV_SPI_8301_Vars_t gate_driver_regs[1/*num_motors*/]; - -void test_DRV8301_setup() { - for (int i = 0; i < num_motors; ++i) { - DRV8301_enable(&motor_configs[i].gate_driver); - DRV8301_setupSpi(&motor_configs[i].gate_driver, &gate_driver_regs[i]); - - //@TODO we can use reporting only if we actually wire up the nOCTW pin - gate_driver_regs[i].Ctrl_Reg_1.OC_MODE = DRV8301_OcMode_LatchShutDown; - //Overcurrent set to approximately 150A at 100degC. This may need tweaking. - gate_driver_regs[i].Ctrl_Reg_1.OC_ADJ_SET = DRV8301_VdsLevel_0p730_V; - //20V/V on 500uOhm gives a range of +/- 150A - //40V/V on 500uOhm gives a range of +/- 75A - gate_driver_regs[i].Ctrl_Reg_2.GAIN = DRV8301_ShuntAmpGain_40VpV; - - gate_driver_regs[i].SndCmd = true; - DRV8301_writeData(&motor_configs[i].gate_driver, &gate_driver_regs[i]); - gate_driver_regs[i].RcvCmd = true; - DRV8301_readData(&motor_configs[i].gate_driver, &gate_driver_regs[i]); - } -} - -///////////////////////////////////////////////// -//Test adc conversion latency and triggering - -void test_adc_trigger() { - //Set trigger to mid phase to check for trigger polarity - htim1.Instance->CCR4 = 2048; - - HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4); - __HAL_ADC_ENABLE(&hadc2); - - //Warp field stabilize. - osDelay(2); - __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); -} - -static int test = 0; -static int test2 = 0; -static uint32_t testcnt[16]; -static int tcidx = 0; -void test_adc_trigger_cb() { - uint32_t cnt = htim1.Instance->CNT; - int dir = htim1.Instance->CR1 & TIM_CR1_DIR; - if(dir){ - test++; - } else { - test2++; - testcnt[tcidx] = cnt - 2048; - if(++tcidx == 16) - tcidx = 0; - } -} -///////////////////////////////////////////////// - -static int cbcnt = 0; -void test_cb_count(){ - ++cbcnt; -} - - -///////////////////////////////////////////////// -//Histogram test -static float alpha = 1/(5000.0f); -static float avg = 2048.0f; -static float var = 0.0f; -static uint32_t hist_countdown = 40000; -static uint32_t errhist[20]; -static uint32_t neg_errhist[20]; -void test_adc_hist_cb(ADC_HandleTypeDef* hadc) { - //float unknown_ch_volts = read_ADC_volts(hadc, 1); - - uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, 1); - float val = (float)ADCValue; - avg *= (1.0f - alpha); - avg += alpha * val; - - float dval = val-avg; - var *= (1.0f - alpha); - var += alpha * (dval * dval); - - if (hist_countdown) { - --hist_countdown; - } else { - int idval = (int)dval; - int pos = (idval >= 0); - if (!pos) - idval = -idval; - if (idval >= 20) - idval = 19; - if (pos) - ++errhist[idval]; - else - ++neg_errhist[idval]; - } -} -///////////////////////////////////////////////// - -float phase_current_from_adcval(uint32_t ADCValue, int motornum) { +static float phase_current_from_adcval(uint32_t ADCValue, int motornum) { float rev_gain; switch (gate_driver_regs[motornum].Ctrl_Reg_2.GAIN) { case DRV8301_ShuntAmpGain_10VpV: @@ -194,21 +145,16 @@ float phase_current_from_adcval(uint32_t ADCValue, int motornum) { return current; } -void assertt(int arg) { +//@TODO implement +void safe_assert(int arg) { if(!arg) { for(;;); } } -// current sense queue from ADC to motor control task -typedef struct { - float current_phB; - float current_phC; -} Iph_BC_queue_item_t; -osMailQDef (Iph_queue_def, 2, Iph_BC_queue_item_t); -osMailQId (M0_Iph_queue); - -void test_pwm_from_adc_cb(ADC_HandleTypeDef* hadc) { +// This is the callback from the ADC that we expect after the PWM has triggered an ADC conversion. +//@TODO: Document how the phasing is done +static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) { // ADC2 and ADC3 record the phB and phC currents concurrently, // and their interrupts should arrive on the same clock cycle. @@ -230,7 +176,7 @@ void test_pwm_from_adc_cb(ADC_HandleTypeDef* hadc) { phC_current = phase_current_from_adcval(ADCValue, 0); } else { //hadc is something else, not expected - assertt(0); + safe_assert(0); } //Allocate mail queue storage @@ -248,18 +194,9 @@ void test_pwm_from_adc_cb(ADC_HandleTypeDef* hadc) { } -void test_motor_thread(void const * argument) { +void motor_thread(void const * argument) { - //Allocate the queues - M0_Iph_queue = osMailCreate(osMailQ(Iph_queue_def), NULL); - - //Init gate drivers - test_DRV8301_setup(); - - osDelay(1000); - - // Start PWM and enable adc interrupts/callbacks - start_adc_pwm(); + init_motor_control(); for(;;) { //Current measurements not occurring in a timely manner can be handled by the watchdog @@ -268,7 +205,7 @@ void test_motor_thread(void const * argument) { osEvent evt = osMailGet(M0_Iph_queue, osWaitForever); //Since we wait forever, we do not expect timeouts here. - assertt(evt.status == osEventMail); + safe_assert(evt.status == osEventMail); Iph_BC_queue_item_t* mail_ptr = evt.value.p; float M0_phB_current = mail_ptr->current_phB; @@ -283,18 +220,3 @@ void test_motor_thread(void const * argument) { } } - -//Test setup: Setup tests in main, and set callbacks -void test_main(void) { - - //test_adc_trigger(); - -} - -void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) { - //test_adc_trigger_cb(hadc); - //test_cb_count(); - //test_adc_hist_cb(hadc); - test_pwm_from_adc_cb(hadc); -} - diff --git a/MotorControl/low_level.h b/MotorControl/low_level.h new file mode 100644 index 00000000..7dad0b79 --- /dev/null +++ b/MotorControl/low_level.h @@ -0,0 +1,21 @@ + +#ifndef __LOW_LEVEL_H +#define __LOW_LEVEL_H + +#include "drv8301.h" + +typedef struct Motor_s { + DRV8301_Obj gate_driver; + float shunt_conductance; + float maxcurrent; +} Motor_t; + +extern Motor_t motor_configs[]; +extern const int num_motors; + +void init_motor_control(); + +//@TODO move motor thread to high level file +void motor_thread(void const * argument); + +#endif //__LOW_LEVEL_H \ No newline at end of file diff --git a/Src/freertos.c b/Src/freertos.c index 10c5b9a2..02f4df7a 100644 --- a/Src/freertos.c +++ b/Src/freertos.c @@ -48,6 +48,7 @@ /* USER CODE BEGIN Includes */ #include "test.h" +#include "low_level.h" /* USER CODE END Includes */ /* Variables -----------------------------------------------------------------*/ @@ -94,7 +95,7 @@ void MX_FREERTOS_Init(void) { defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL); /* USER CODE BEGIN RTOS_THREADS */ - osThreadDef(task_motor_0, test_motor_thread, osPriorityHigh, 0, 512); + osThreadDef(task_motor_0, motor_thread, osPriorityHigh, 0, 512); motor0_TaskHandle = osThreadCreate(osThread(task_motor_0), NULL); /* USER CODE END RTOS_THREADS */ diff --git a/Src/main.c b/Src/main.c index b0b6eca5..a6d9eeee 100644 --- a/Src/main.c +++ b/Src/main.c @@ -81,10 +81,6 @@ int main(void) /* USER CODE BEGIN 1 */ - //Ensure that debug halting of the core doesn't leave the motor PWM running - __HAL_DBGMCU_FREEZE_TIM1(); - __HAL_DBGMCU_FREEZE_TIM8(); - /* USER CODE END 1 */ /* MCU Configuration----------------------------------------------------------*/ diff --git a/Test/test.c b/Test/test.c new file mode 100644 index 00000000..a3c43d57 --- /dev/null +++ b/Test/test.c @@ -0,0 +1,101 @@ + +#include "test.h" + +#include "adc.h" +#include "tim.h" + +#include "math.h" +#include "stdint.h" + +///////////////////////////////////////////////// +//Test adc conversion latency and triggering + +void test_adc_trigger() { + //Set trigger to mid phase to check for trigger polarity + htim1.Instance->CCR4 = 2048; + + HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4); + __HAL_ADC_ENABLE(&hadc2); + + //Warp field stabilize. + osDelay(2); + __HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC); +} + +static int test = 0; +static int test2 = 0; +static uint32_t testcnt[16]; +static int tcidx = 0; +void test_adc_trigger_cb() { + uint32_t cnt = htim1.Instance->CNT; + int dir = htim1.Instance->CR1 & TIM_CR1_DIR; + if(dir){ + test++; + } else { + test2++; + testcnt[tcidx] = cnt - 2048; + if(++tcidx == 16) + tcidx = 0; + } +} +///////////////////////////////////////////////// + +static int cbcnt = 0; +void test_cb_count(){ + ++cbcnt; +} + + +///////////////////////////////////////////////// +//Histogram test +static float alpha = 1/(5000.0f); +static float avg = 2048.0f; +static float var = 0.0f; +static uint32_t hist_countdown = 40000; +static uint32_t errhist[20]; +static uint32_t neg_errhist[20]; +void test_adc_hist_cb(ADC_HandleTypeDef* hadc) { + //float unknown_ch_volts = read_ADC_volts(hadc, 1); + + uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, 1); + float val = (float)ADCValue; + avg *= (1.0f - alpha); + avg += alpha * val; + + float dval = val-avg; + var *= (1.0f - alpha); + var += alpha * (dval * dval); + + if (hist_countdown) { + --hist_countdown; + } else { + int idval = (int)dval; + int pos = (idval >= 0); + if (!pos) + idval = -idval; + if (idval >= 20) + idval = 19; + if (pos) + ++errhist[idval]; + else + ++neg_errhist[idval]; + } +} +///////////////////////////////////////////////// + + +//Test setup: Setup tests in main, and set callbacks +void test_main(void) { + + //test_adc_trigger(); + +} + +//Uncomment to define the ADC callback. Ordinarily it is used by motor control +// void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc) { +// //test_adc_trigger_cb(hadc); +// //test_cb_count(); +// //test_adc_hist_cb(hadc); +// test_pwm_from_adc_cb(hadc); +// } + diff --git a/MotorControl/test.h b/Test/test.h similarity index 62% rename from MotorControl/test.h rename to Test/test.h index 8033e23b..cf6db149 100644 --- a/MotorControl/test.h +++ b/Test/test.h @@ -3,6 +3,5 @@ #define __TEST_H void test_main(void); -void test_motor_thread(void const * argument); #endif //__TEST_H