mirror of
https://github.com/odriverobotics/ODrive.git
synced 2026-08-23 17:08:15 +08:00
320 lines
9.0 KiB
C
320 lines
9.0 KiB
C
|
|
#include "test.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(){
|
|
|
|
//Enable ADC and interrupts
|
|
__HAL_ADC_ENABLE(&hadc2);
|
|
__HAL_ADC_ENABLE(&hadc3);
|
|
//Warp field stabilize.
|
|
osDelay(2);
|
|
__HAL_ADC_ENABLE_IT(&hadc2, ADC_IT_JEOC);
|
|
__HAL_ADC_ENABLE_IT(&hadc3, ADC_IT_JEOC);
|
|
|
|
//Init PWM
|
|
int half_load = htim1.Instance->ARR/2;
|
|
htim1.Instance->CCR1 = half_load;
|
|
htim1.Instance->CCR2 = half_load;
|
|
htim1.Instance->CCR3 = half_load;
|
|
|
|
//This hardware obfustication layer really is getting on my nerves
|
|
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
|
|
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_1);
|
|
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
|
|
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_2);
|
|
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
|
|
HAL_TIMEx_PWMN_Start(&htim1, TIM_CHANNEL_3);
|
|
|
|
htim1.Instance->CCR4 = 1;
|
|
HAL_TIM_PWM_Start_IT(&htim1, TIM_CHANNEL_4);
|
|
|
|
//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) {
|
|
float rev_gain;
|
|
switch (gate_driver_regs[motornum].Ctrl_Reg_2.GAIN) {
|
|
case DRV8301_ShuntAmpGain_10VpV:
|
|
rev_gain = 1.0f/10.0f;
|
|
break;
|
|
case DRV8301_ShuntAmpGain_20VpV:
|
|
rev_gain = 1.0f/20.0f;
|
|
break;
|
|
case DRV8301_ShuntAmpGain_40VpV:
|
|
rev_gain = 1.0f/40.0f;
|
|
break;
|
|
case DRV8301_ShuntAmpGain_80VpV:
|
|
rev_gain = 1.0f/80.0f;
|
|
break;
|
|
}
|
|
|
|
int adcval_bal = (int)ADCValue - (1<<11);
|
|
float amp_out_volt = (3.3f/(float)(1<<12)) * (float)adcval_bal;
|
|
float shunt_volt = amp_out_volt * rev_gain;
|
|
float current = shunt_volt * motor_configs[motornum].shunt_conductance;
|
|
return current;
|
|
}
|
|
|
|
void assertt(int arg) {
|
|
if(!arg) {
|
|
int test = 3;
|
|
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) {
|
|
|
|
// ADC2 and ADC3 record the phB and phC currents concurrently,
|
|
// and their interrupts have the same priorities so they can complete
|
|
// in any order. Because they cannot preempt each other, it is safe
|
|
// to store the result of the first interrupt without risk of a race condition.
|
|
// Then we send both into the queue.
|
|
typedef enum ADC_sync_e {NONE_STORED, PHB_STORED, PHC_STORED} ADC_sync_t;
|
|
static ADC_sync_t adc_sync = NONE_STORED;
|
|
static float stored_current;
|
|
|
|
//Only one conversion in sequence, so only rank1
|
|
uint32_t ADCValue = HAL_ADCEx_InjectedGetValue(hadc, ADC_INJECTED_RANK_1);
|
|
|
|
//Store and return, or fetch and continue
|
|
float M0_phB_current, M0_phC_current;
|
|
if (hadc == &hadc2) {
|
|
M0_phB_current = phase_current_from_adcval(ADCValue, 0);
|
|
if (adc_sync == NONE_STORED) {
|
|
stored_current = M0_phB_current;
|
|
adc_sync = PHB_STORED;
|
|
return;
|
|
} else {
|
|
assertt(adc_sync == PHC_STORED);
|
|
M0_phC_current = stored_current;
|
|
adc_sync = NONE_STORED;
|
|
}
|
|
} else if (hadc == &hadc3) {
|
|
M0_phC_current = phase_current_from_adcval(ADCValue, 0);
|
|
if (adc_sync == NONE_STORED) {
|
|
stored_current = M0_phC_current;
|
|
adc_sync = PHC_STORED;
|
|
return;
|
|
} else {
|
|
assertt(adc_sync == PHB_STORED);
|
|
M0_phB_current = stored_current;
|
|
adc_sync = NONE_STORED;
|
|
}
|
|
} else {
|
|
//hadc is something else, not expected
|
|
assertt(0);
|
|
}
|
|
|
|
//Allocate mail queue storage
|
|
Iph_BC_queue_item_t* mail_ptr;
|
|
mail_ptr = (Iph_BC_queue_item_t*) osMailAlloc(M0_Iph_queue, 0);
|
|
if (mail_ptr == NULL) {
|
|
return;
|
|
}
|
|
|
|
//Write contents and send mail
|
|
mail_ptr->current_phB = M0_phB_current;
|
|
mail_ptr->current_phC = M0_phC_current;
|
|
osMailPut(M0_Iph_queue, mail_ptr);
|
|
|
|
}
|
|
|
|
|
|
void test_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();
|
|
|
|
for(;;) {
|
|
//Current measurements not occurring in a timely manner can be handled by the watchdog
|
|
//@TODO Actually make watchdog
|
|
//Hence we can use osWaitForever
|
|
osEvent evt = osMailGet(M0_Iph_queue, osWaitForever);
|
|
|
|
//Since we wait forever, we do not expect timeouts here.
|
|
assertt(evt.status == osEventMail);
|
|
|
|
Iph_BC_queue_item_t* mail_ptr = evt.value.p;
|
|
float M0_phB_current = mail_ptr->current_phB;
|
|
float M0_phC_current = mail_ptr->current_phC;
|
|
osMailFree(M0_Iph_queue, mail_ptr);
|
|
|
|
// WARNING: Only gimbal motors!!
|
|
int half_load = htim1.Instance->ARR/2;
|
|
htim1.Instance->CCR1 = half_load - 400;
|
|
htim1.Instance->CCR2 = half_load + 400;
|
|
htim1.Instance->CCR3 = half_load + 400;
|
|
}
|
|
}
|
|
|
|
|
|
//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);
|
|
}
|
|
|