/*
* Called every time new ADC values are available. Note that
* the ADC is initialized from mcpwm.c
*/
void main_dma_adc_handler(void) {
ledpwm_update_pwm();
if (sample_at_start && (mc_interface_get_state() == MC_STATE_RUNNING ||
start_comm != mcpwm_get_comm_step())) {
sample_now = 0;
sample_ready = 0;
sample_at_start = 0;
}
static int a = 0;
if (!sample_ready) {
a++;
if (a >= sample_int) {
a = 0;
if (mc_interface_get_state() == MC_STATE_DETECTING) {
curr0_samples[sample_now] = (int16_t)mcpwm_detect_currents[mcpwm_get_comm_step() - 1];
curr1_samples[sample_now] = (int16_t)mcpwm_detect_currents_diff[mcpwm_get_comm_step() - 1];
ph1_samples[sample_now] = (int16_t)mcpwm_detect_voltages[0];
ph2_samples[sample_now] = (int16_t)mcpwm_detect_voltages[1];
ph3_samples[sample_now] = (int16_t)mcpwm_detect_voltages[2];
} else {
curr0_samples[sample_now] = ADC_curr_norm_value[0];
curr1_samples[sample_now] = ADC_curr_norm_value[1];
ph1_samples[sample_now] = ADC_V_L1 - mcpwm_vzero;
ph2_samples[sample_now] = ADC_V_L2 - mcpwm_vzero;
ph3_samples[sample_now] = ADC_V_L3 - mcpwm_vzero;
}
vzero_samples[sample_now] = mcpwm_vzero;
curr_fir_samples[sample_now] = (int16_t)(mc_interface_get_tot_current() * 100.0);
f_sw_samples[sample_now] = (int16_t)(mc_interface_get_switching_frequency_now() / 10.0);
status_samples[sample_now] = mcpwm_get_comm_step() | (mcpwm_read_hall_phase() << 3);
sample_now++;
if (sample_now == sample_len) {
sample_ready = 1;
sample_now = 0;
chSysLockFromISR();
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
chSysUnlockFromISR();
}
main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
}
}
}
/*
* Called every time new ADC values are available. Note that
* the ADC is initialized from mcpwm.c
*/
void main_dma_adc_handler(void) {
ledpwm_update_pwm();
if (sample_at_start && mcpwm_get_state() == MC_STATE_STARTING) {
sample_now = 0;
sample_ready = 0;
was_start_wample = 1;
sample_at_start = 0;
}
static int a = 0;
if (!sample_ready) {
a++;
if (a >= sample_int) {
a = 0;
curr0_samples[sample_now] = ADC_curr_norm_value[0];
curr1_samples[sample_now] = ADC_curr_norm_value[1];
ph1_samples[sample_now] = ADC_V_L1;
ph2_samples[sample_now] = ADC_V_L2;
ph3_samples[sample_now] = ADC_V_L3;
vzero_samples[sample_now] = ADC_V_ZERO * MCPWM_VZERO_FACT;
if (mcpwm_get_state() == MC_STATE_DETECTING && 0) {
status_samples[sample_now] = mcpwm_get_detect_top();
} else {
uint8_t tmp;
if (was_start_wample) {
if (mcpwm_get_state() == MC_STATE_STARTING) {
tmp = 1;
} else if (mcpwm_get_state() == MC_STATE_RUNNING) {
tmp = 2;
} else {
tmp = 3;
}
} else {
tmp = mcpwm_read_hall_phase();
}
status_samples[sample_now] = mcpwm_get_comm_step() | (tmp << 3);
}
curr_fir_samples[sample_now] = (int16_t)(mcpwm_get_tot_current_filtered() * 100);
sample_now++;
if (sample_now == sample_len) {
sample_ready = 1;
sample_now = 0;
was_start_wample = 0;
chSysLockFromIsr();
chEvtSignalI(sample_send_tp, (eventmask_t) 1);
chSysUnlockFromIsr();
}
main_last_adc_duration = mcpwm_get_last_adc_isr_duration();
}
}
}
void main_sample_print_data(bool at_start, uint16_t len, uint8_t decimation) {
if (len > ADC_SAMPLE_MAX_LEN) {
len = ADC_SAMPLE_MAX_LEN;
}
sample_len = len;
sample_int = decimation;
if (at_start) {
sample_at_start = 1;
start_comm = mcpwm_get_comm_step();
} else {
sample_now = 0;
sample_ready = 0;
}
}
void mc_interface_fault_stop(mc_fault_code fault) {
if (mc_interface_dccal_done() && m_fault_now == FAULT_CODE_NONE) {
// Sent to terminal fault logger so that all faults and their conditions
// can be printed for debugging.
chSysLock();
volatile int val_samp = TIM8->CCR1;
volatile int current_samp = TIM1->CCR4;
volatile int tim_top = TIM1->ARR;
chSysUnlock();
fault_data fdata;
fdata.fault = fault;
fdata.current = mc_interface_get_tot_current();
fdata.current_filtered = mc_interface_get_tot_current_filtered();
fdata.voltage = GET_INPUT_VOLTAGE();
fdata.duty = mc_interface_get_duty_cycle_now();
fdata.rpm = mc_interface_get_rpm();
fdata.tacho = mc_interface_get_tachometer_value(false);
fdata.cycles_running = m_cycles_running;
fdata.tim_val_samp = val_samp;
fdata.tim_current_samp = current_samp;
fdata.tim_top = tim_top;
fdata.comm_step = mcpwm_get_comm_step();
fdata.temperature = NTC_TEMP(ADC_IND_TEMP_MOS1);
terminal_add_fault_data(&fdata);
}
m_ignore_iterations = m_conf.m_fault_stop_time_ms;
switch (m_conf.motor_type) {
case MOTOR_TYPE_BLDC:
case MOTOR_TYPE_DC:
mcpwm_stop_pwm();
break;
case MOTOR_TYPE_FOC:
mcpwm_foc_stop_pwm();
break;
default:
break;
}
m_fault_now = fault;
}
void main_process_packet(unsigned char *data, unsigned char len) {
if (!len) {
return;
}
int16_t value16;
int32_t value32;
switch (data[0]) {
case 1:
// Sample and print data
value16 = (int)data[1] << 8 | (int)data[2];
if (value16 > ADC_SAMPLE_MAX_LEN) {
value16 = ADC_SAMPLE_MAX_LEN;
}
sample_len = value16;
sample_int = data[3];
sample_now = 0;
sample_ready = 0;
break;
case 2:
// Duty Control
value16 = (int)data[1] << 8 | (int)data[2];
mcpwm_set_duty((float)value16 / 1000.0);
break;
case 3:
// PID Control
value32 = (int)data[1] << 24 | (int)data[2] << 16 | (int)data[3] << 8 | (int)data[4];
mcpwm_set_pid_speed((float)value32);
break;
case 4:
// Detect
mcpwm_set_detect();
break;
case 5:
// Reset Fault
// TODO
break;
case 6:
// Sample and print data at start
value16 = (int)data[1] << 8 | (int)data[2];
if (value16 > ADC_SAMPLE_MAX_LEN) {
value16 = ADC_SAMPLE_MAX_LEN;
}
sample_len = value16;
sample_int = data[3];
sample_at_start = 1;
start_comm = mcpwm_get_comm_step();
break;
case 7:
// Current Control
value16 = (int)data[1] << 8 | (int)data[2];
mcpwm_set_current((float)value16 / 100.0);
break;
case 8:
// Brake current control
value16 = (int)data[1] << 8 | (int)data[2];
mcpwm_set_brake_current((float)value16 / 100.0);
break;
default:
break;
}
}
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();
}
}
}
