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

restucture files

Browse Source
This commit is contained in:
Oskar Weigl
2016-11-13 16:58:48 +09:00
parent e64dc5f1bd
commit d83d015222
8 changed files with 225 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+4 -1
View File
@@ -12,4 +12,7 @@ README.html
#Eclipse stuff
.settings/
.project
.project
# STM32CubeMX (in case you put it in this folder, or a symlink)
STM32CubeMX
Makefile
+3 -1
View File
@@ -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
MotorControl/test.c → MotorControl/low_level.c
+94 -172
View File
@@ -1,21 +1,93 @@
#include "test.h"
#include <low_level.h>
#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);
}
MotorControl/low_level.h
+21
View File
@@ -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
Src/freertos.c
+2 -1
View File
@@ -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 */
Src/main.c
-4
View File
@@ -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----------------------------------------------------------*/
Test/test.c
+101
View File
@@ -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);
// }
MotorControl/test.h → Test/test.h
-1
View File
@@ -3,6 +3,5 @@
#define __TEST_H
void test_main(void);
void test_motor_thread(void const * argument);
#endif //__TEST_H