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

implement current meas test and DC_CAL WIP

Browse Source
This commit is contained in:
Oskar Weigl
2016-11-19 22:12:36 +09:00
parent ce4bd018bc
commit b5beb0164a
1 changed files with 53 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotorControl/low_level.c
+53 -23
View File
@@ -145,9 +145,11 @@ static float phase_current_from_adcval(uint32_t ADCValue, int motornum) {
return current;
}
//@TODO implement
//@TODO make available from anywhere
void safe_assert(int arg) {
if(!arg) {
__HAL_TIM_MOE_DISABLE(&htim1);
__HAL_TIM_MOE_DISABLE(&htim8);
for(;;);
}
}
@@ -156,6 +158,17 @@ void safe_assert(int arg) {
//@TODO: Document how the phasing is done
static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) {
uint32_t trig_src = hadc->Instance->CR2 & ADC_CR2_JEXTSEL;
if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_CC4) {
hadc->Instance->CR2 &= ~(ADC_CR2_JEXTSEL);
hadc->Instance->CR2 |= ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
} else if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) {
hadc->Instance->CR2 &= ~(ADC_CR2_JEXTSEL);
hadc->Instance->CR2 |= ADC_EXTERNALTRIGINJECCONV_T1_CC4;
} else {
safe_assert(0);
}
// ADC2 and ADC3 record the phB and phC currents concurrently,
// and their interrupts should arrive on the same clock cycle.
// The HAL issues the callbacks in order, so ADC2 will always be processed before ADC3.
@@ -208,36 +221,53 @@ void motor_thread(void const * argument) {
init_motor_control();
float test_voltages[] = {0.3f, 0.7f};
float test_currentsB[] = {0.0f, 0.0f};
float test_currentsC[] = {0.0f, 0.0f};
float test_sum[] = {0.0f, 0.0f};
static const int num_test = sizeof(test_voltages)/sizeof(test_voltages[0]);
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);
for (int i = 0; i < num_test; ++i) {
for (int rep = 0; rep < 10000; ++rep) {
mark_timing();
//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.
safe_assert(evt.status == osEventMail);
mark_timing();
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);
//Since we wait forever, we do not expect timeouts here.
safe_assert(evt.status == osEventMail);
int full_load = htim1.Instance->ARR;
int half_load = full_load/2;
float DC_bus_voltage = 12.0f;
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);
float test_voltage = 0.5f;
float test_modulation = test_voltage/DC_bus_voltage;
int timing = (int)(test_modulation * (float)half_load);
//Test voltage along phase A
htim1.Instance->CCR1 = half_load - timing;
htim1.Instance->CCR2 = half_load + timing;
htim1.Instance->CCR3 = half_load + timing;
test_currentsB[i] += M0_phB_current;
test_currentsC[i] += M0_phC_current;
test_sum[i] += 1.0f;
mark_timing();
int full_load = htim1.Instance->ARR;
int half_load = full_load/2;
float DC_bus_voltage = 12.0f;
// float test_voltage = 0.5f;
float test_modulation = test_voltages[i]/DC_bus_voltage;
int timing = (int)(test_modulation * (float)half_load);
//Test voltage along phase A
htim1.Instance->CCR1 = half_load - timing;
htim1.Instance->CCR2 = half_load + timing;
htim1.Instance->CCR3 = half_load + timing;
mark_timing();
}
}
}
}