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

only push to queue when measuring real current

Browse Source
This commit is contained in:
Oskar Weigl
2016-11-19 22:12:36 +09:00
parent 9385ee9c8f
commit 5133db4961
2 changed files with 47 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MotorControl/low_level.c
+46 -40
View File
@@ -158,60 +158,65 @@ void safe_assert(int arg) {
//@TODO: Document how the phasing is done
static void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc) {
//@TODO get rid of statics when using more than one motor
static float phB_DC_calib = 0.0f;
static float phC_DC_calib = 0.0f;
// static const float calib_filter_k =
// Check if this trigger was the CC4 channel, used for actual current measurement at SVM vector 0
// or the update trigger, which is used for DC_CAL measurement at SVM vector 7
uint32_t trig_src = hadc->Instance->CR2 & ADC_CR2_JEXTSEL;
if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_CC4) {
//We should be measuring current here
//We are measuring current here
//Set up next measurement to be DC_CAL measurement
hadc->Instance->CR2 &= ~(ADC_CR2_JEXTSEL);
hadc->Instance->CR2 |= ADC_EXTERNALTRIGINJECCONV_T1_TRGO;
} else if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) {
//We should be measuring DC_CAL here
// 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.
// Therefore we store the value from ADC2 and push them both into the queue
// when ADC3 is ready.
// @TODO: don't use statics, will only work for 1 motor chanel
static float phB_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 phC_current;
if (hadc == &hadc2) {
phB_current = phase_current_from_adcval(ADCValue, 0);
return;
} else if (hadc == &hadc3) {
phC_current = phase_current_from_adcval(ADCValue, 0);
} else {
//hadc is something else, not expected
safe_assert(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 = phB_current - phB_DC_calib;
mail_ptr->current_phC = phC_current - phC_DC_calib;
osMailPut(M0_Iph_queue, mail_ptr);
} else if (trig_src == ADC_EXTERNALTRIGINJECCONV_T1_TRGO) {
//We are measuring DC_CAL here
//Set up next measurement to be current measurement
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.
// Therefore we store the value from ADC2 and push them both into the queue
// when ADC3 is ready.
// @TODO: don't use statics, will only work for 1 motor chanel
static float phB_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 phC_current;
if (hadc == &hadc2) {
phB_current = phase_current_from_adcval(ADCValue, 0);
return;
} else if (hadc == &hadc3) {
phC_current = phase_current_from_adcval(ADCValue, 0);
} else {
//hadc is something else, not expected
safe_assert(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 = phB_current;
mail_ptr->current_phC = phC_current;
osMailPut(M0_Iph_queue, mail_ptr);
}
void mark_timing() {
@@ -249,6 +254,7 @@ void motor_thread(void const * argument) {
//Since we wait forever, we do not expect timeouts here.
safe_assert(evt.status == osEventMail);
//Fetch current out of the mail queue
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;
Odrive.ioc
+1 -1
View File
@@ -153,7 +153,7 @@ Mcu.Pin7=PC2
Mcu.Pin8=PC3
Mcu.Pin9=PA0-WKUP
Mcu.PinsNb=55
Mcu.UserConstants=
Mcu.UserConstants=TIM_CLOCK_HZ,84000000
Mcu.UserName=STM32F405RGTx
MxCube.Version=4.17.0
MxDb.Version=DB.4.0.170