static THD_FUNCTION(timer_thread, arg) {
(void)arg;
chRegSetThreadName("mcif timer");
for(;;) {
// Check if the DRV8302 indicates any fault
if (IS_DRV_FAULT()) {
mc_interface_fault_stop(FAULT_CODE_DRV8302);
}
// Decrease fault iterations
if (m_ignore_iterations > 0) {
m_ignore_iterations--;
} else {
if (!IS_DRV_FAULT()) {
m_fault_now = FAULT_CODE_NONE;
}
}
update_override_limits(&m_conf);
chThdSleepMilliseconds(1);
}
}
/**
* Update the override limits for a configuration based on MOSFET temperature etc.
*
* @param conf
* The configaration to update.
*/
static void update_override_limits(volatile mc_configuration *conf) {
const float temp = NTC_TEMP(ADC_IND_TEMP_MOS1);
const float v_in = GET_INPUT_VOLTAGE();
// Temperature
if (temp < conf->l_temp_fet_start) {
conf->lo_current_min = conf->l_current_min;
conf->lo_current_max = conf->l_current_max;
} else if (temp > conf->l_temp_fet_end) {
conf->lo_current_min = 0.0;
conf->lo_current_max = 0.0;
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET);
} else {
float maxc = fabsf(conf->l_current_max);
if (fabsf(conf->l_current_min) > maxc) {
maxc = fabsf(conf->l_current_min);
}
maxc = utils_map(temp, conf->l_temp_fet_start, conf->l_temp_fet_end, maxc, 0.0);
if (fabsf(conf->l_current_max) > maxc) {
conf->lo_current_max = SIGN(conf->l_current_max) * maxc;
}
if (fabsf(conf->l_current_min) > maxc) {
conf->lo_current_min = SIGN(conf->l_current_min) * maxc;
}
}
// Battery cutoff
if (v_in > conf->l_battery_cut_start) {
conf->lo_in_current_max = conf->l_in_current_max;
} else if (v_in < conf->l_battery_cut_end) {
conf->lo_in_current_max = 0.0;
} else {
conf->lo_in_current_max = utils_map(v_in, conf->l_battery_cut_start,
conf->l_battery_cut_end, conf->l_in_current_max, 0.0);
}
conf->lo_in_current_min = conf->l_in_current_min;
}
void gpdrive_init(volatile mc_configuration *configuration) {
utils_sys_lock_cnt();
m_init_done = false;
// Restore timers
TIM_DeInit(TIM1);
TIM1->CNT = 0;
// Disable channel 2 pins
palSetPadMode(GPIOA, 9, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(GPIOA, 9);
palSetPadMode(GPIOB, 14, PAL_MODE_OUTPUT_PUSHPULL);
palClearPad(GPIOB, 14);
m_conf = configuration;
m_fsw_now = 40000;
m_mod_now = 0.0;
m_current_now = 0.0;
m_current_now_filtered = 0.0;
m_output_mode = GPD_OUTPUT_MODE_NONE;
memset((void*)&m_sample_buffer, 0, sizeof(m_sample_buffer));
m_buffer_int_scale = 1.0 / 128.0;
m_is_running = false;
m_output_now = 0.0;
m_curr0_sum = 0;
m_curr1_sum = 0;
m_curr_samples = 0;
m_curr0_offset = 0;
m_curr1_offset = 0;
m_dccal_done = false;
#ifdef HW_HAS_3_SHUNTS
m_curr2_sum = 0;
m_curr2_offset = 0;
#endif
m_last_adc_isr_duration = 0;
memset((void*)&m_current_state, 0, sizeof(m_current_state));
TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
TIM_OCInitTypeDef TIM_OCInitStructure;
TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
TIM_TimeBaseStructure.TIM_Prescaler = 0;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_Period = SYSTEM_CORE_CLOCK / (int)m_fsw_now;
TIM_TimeBaseStructure.TIM_ClockDivision = 0;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
TIM_OCInitStructure.TIM_Pulse = TIM1->ARR / 2;
TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
TIM_OC1Init(TIM1, &TIM_OCInitStructure);
TIM_OC2Init(TIM1, &TIM_OCInitStructure);
TIM_OC3Init(TIM1, &TIM_OCInitStructure);
TIM_OC4Init(TIM1, &TIM_OCInitStructure);
TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
TIM_BDTRInitStructure.TIM_DeadTime = conf_general_calculate_deadtime(HW_DEAD_TIME_NSEC, SYSTEM_CORE_CLOCK);
TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
TIM_CCPreloadControl(TIM1, ENABLE);
TIM_ARRPreloadConfig(TIM1, ENABLE);
ADC_CommonInitTypeDef ADC_CommonInitStructure;
DMA_InitTypeDef DMA_InitStructure;
ADC_InitTypeDef ADC_InitStructure;
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC, ENABLE);
RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE);
dmaStreamAllocate(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)),
3,
(stm32_dmaisr_t)adc_int_handler,
(void *)0);
DMA_InitStructure.DMA_Channel = DMA_Channel_0;
DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value;
DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStructure.DMA_BufferSize = HW_ADC_CHANNELS;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(DMA2_Stream4, &DMA_InitStructure);
DMA_Cmd(DMA2_Stream4, ENABLE);
DMA_ITConfig(DMA2_Stream4, DMA_IT_TC, ENABLE);
// Note that the ADC is running at 42MHz, which is higher than the
// specified 36MHz in the data sheet, but it works.
ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult;
ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2;
ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
ADC_CommonInit(&ADC_CommonInitStructure);
ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC2;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV;
ADC_Init(ADC1, &ADC_InitStructure);
ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
ADC_InitStructure.ADC_ExternalTrigConv = 0;
ADC_Init(ADC2, &ADC_InitStructure);
ADC_Init(ADC3, &ADC_InitStructure);
ADC_TempSensorVrefintCmd(ENABLE);
ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
hw_setup_adc_channels();
ADC_Cmd(ADC1, ENABLE);
ADC_Cmd(ADC2, ENABLE);
ADC_Cmd(ADC3, ENABLE);
TIM_Cmd(TIM1, ENABLE);
TIM_CtrlPWMOutputs(TIM1, ENABLE);
// Always sample ADC in the beginning of the PWM cycle
TIM1->CCR2 = 200;
utils_sys_unlock_cnt();
ENABLE_GATE();
DCCAL_OFF();
do_dc_cal();
// Start threads
timer_thd_stop = false;
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
stop_pwm_hw();
// Check if the system has resumed from IWDG reset
if (timeout_had_IWDG_reset()) {
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET);
}
m_init_done = true;
}
static void adc_int_handler(void *p, uint32_t flags) {
(void)p;
(void)flags;
uint32_t t_start = timer_time_now();
// Reset the watchdog
timeout_feed_WDT(THREAD_MCPWM);
int curr0 = GET_CURRENT1();
int curr1 = GET_CURRENT2();
#ifdef HW_HAS_3_SHUNTS
int curr2 = GET_CURRENT3();
#endif
m_curr0_sum += curr0;
m_curr1_sum += curr1;
#ifdef HW_HAS_3_SHUNTS
m_curr2_sum += curr2;
#endif
curr0 -= m_curr0_offset;
curr1 -= m_curr1_offset;
#ifdef HW_HAS_3_SHUNTS
curr2 -= m_curr2_offset;
#endif
m_curr_samples++;
// Update current
#ifdef HW_HAS_3_SHUNTS
float i1 = -(float)curr2;
#else
float i1 = -(float)curr1;
#endif
float i2 = (float)curr0;
m_current_now = utils_max_abs(i1, i2) * FAC_CURRENT;
UTILS_LP_FAST(m_current_now_filtered, m_current_now, m_conf->gpd_current_filter_const);
// Check for most critical faults here, as doing it in mc_interface can be too slow
// for high switching frequencies.
const float input_voltage = GET_INPUT_VOLTAGE();
static int wrong_voltage_iterations = 0;
if (input_voltage < m_conf->l_min_vin ||
input_voltage > m_conf->l_max_vin) {
wrong_voltage_iterations++;
if ((wrong_voltage_iterations >= 8)) {
mc_interface_fault_stop(input_voltage < m_conf->l_min_vin ?
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
}
} else {
wrong_voltage_iterations = 0;
}
if (m_conf->l_slow_abs_current) {
if (fabsf(m_current_now) > m_conf->l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
} else {
if (fabsf(m_current_now_filtered) > m_conf->l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
}
// Buffer handling
static bool buffer_was_empty = true;
static int interpol = 0;
static float buffer_last = 0.0;
static float buffer_next = 0.0;
interpol++;
if (interpol > m_conf->gpd_buffer_interpol) {
interpol = 0;
if (m_sample_buffer.read != m_sample_buffer.write) {
buffer_last = buffer_next;
buffer_next = m_sample_buffer.buffer[m_sample_buffer.read++];
m_sample_buffer.read %= SAMPLE_BUFFER_SIZE;
m_output_now = buffer_last;
m_is_running = true;
buffer_was_empty = false;
} else {
if (!buffer_was_empty) {
stop_pwm_hw();
}
buffer_was_empty = true;
}
} else if (!buffer_was_empty) {
m_output_now = utils_map((float)interpol,
0.0, (float)m_conf->gpd_buffer_interpol + 1.0,
buffer_last, buffer_next);
m_is_running = true;
}
if (m_is_running) {
gpdrive_output_sample(m_output_now);
if (m_output_mode == GPD_OUTPUT_MODE_CURRENT) {
float v_in = GET_INPUT_VOLTAGE();
float err = m_current_state.current_set - m_current_now_filtered;
m_current_state.voltage_now = m_current_state.voltage_int + err * m_conf->gpd_current_kp;
m_current_state.voltage_int += err * m_conf->gpd_current_ki * (1.0 / m_fsw_now);
utils_truncate_number_abs((float*)&m_current_state.voltage_int, v_in);
set_modulation(m_current_state.voltage_now / v_in);
}
}
ledpwm_update_pwm();
m_last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
}
void mc_interface_mc_timer_isr(void) {
ledpwm_update_pwm(); // LED PWM Driver update
const float input_voltage = GET_INPUT_VOLTAGE();
// Check for faults that should stop the motor
static int wrong_voltage_iterations = 0;
if (input_voltage < m_conf.l_min_vin ||
input_voltage > m_conf.l_max_vin) {
wrong_voltage_iterations++;
if ((wrong_voltage_iterations >= 8)) {
mc_interface_fault_stop(input_voltage < m_conf.l_min_vin ?
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
}
} else {
wrong_voltage_iterations = 0;
}
if (mc_interface_get_state() == MC_STATE_RUNNING) {
m_cycles_running++;
} else {
m_cycles_running = 0;
}
if (pwn_done_func) {
pwn_done_func();
}
const float current = mc_interface_get_tot_current_filtered();
const float current_in = mc_interface_get_tot_current_in_filtered();
m_motor_current_sum += current;
m_input_current_sum += current_in;
m_motor_current_iterations++;
m_input_current_iterations++;
float abs_current = mc_interface_get_tot_current();
float abs_current_filtered = current;
if (m_conf.motor_type == MOTOR_TYPE_FOC) {
// TODO: Make this more general
abs_current = mcpwm_foc_get_abs_motor_current();
abs_current_filtered = mcpwm_foc_get_abs_motor_current_filtered();
}
// Current fault code
if (m_conf.l_slow_abs_current) {
if (fabsf(abs_current_filtered) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
} else {
if (fabsf(abs_current) > m_conf.l_abs_current_max) {
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
}
}
// Watt and ah counters
const float f_sw = mc_interface_get_switching_frequency_now();
if (fabsf(current) > 1.0) {
// Some extra filtering
static float curr_diff_sum = 0.0;
static float curr_diff_samples = 0;
curr_diff_sum += current_in / f_sw;
curr_diff_samples += 1.0 / f_sw;
if (curr_diff_samples >= 0.01) {
if (curr_diff_sum > 0.0) {
m_amp_seconds += curr_diff_sum;
m_watt_seconds += curr_diff_sum * input_voltage;
} else {
m_amp_seconds_charged -= curr_diff_sum;
m_watt_seconds_charged -= curr_diff_sum * input_voltage;
}
curr_diff_samples = 0.0;
curr_diff_sum = 0.0;
}
}
// Sample collection
if (m_sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
m_start_comm != mcpwm_get_comm_step())) {
m_sample_now = 0;
m_sample_ready = 0;
m_sample_at_start = 0;
}
static int a = 0;
if (!m_sample_ready) {
a++;
if (a >= m_sample_int) {
a = 0;
if (mc_interface_get_state() == MC_STATE_DETECTING) {
m_curr0_samples[m_sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
m_curr1_samples[m_sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
m_ph1_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[0];
m_ph2_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[1];
m_ph3_samples[m_sample_now] = (int16_t)mcpwm_detect_voltages[2];
} else {
m_curr0_samples[m_sample_now] = ADC_curr_norm_value[0];
m_curr1_samples[m_sample_now] = ADC_curr_norm_value[1];
m_ph1_samples[m_sample_now] = ADC_V_L1 - mcpwm_vzero;
m_ph2_samples[m_sample_now] = ADC_V_L2 - mcpwm_vzero;
m_ph3_samples[m_sample_now] = ADC_V_L3 - mcpwm_vzero;
}
m_vzero_samples[m_sample_now] = mcpwm_vzero;
m_curr_fir_samples[m_sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
m_f_sw_samples[m_sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
m_status_samples[m_sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
m_sample_now++;
if (m_sample_now == m_sample_len) {
m_sample_ready = 1;
m_sample_now = 0;
chSysLockFromISR();
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
chSysUnlockFromISR();
}
m_last_adc_duration_sample = mcpwm_get_last_adc_isr_duration();
}
}
}
