mirror of https://github.com/rusefi/bldc.git
Added mutex to DRV SPI driver, moved fault stop to thread, smooth current ramping for sensor and flux linkage detection
This commit is contained in:
parent
c0f75f14ac
commit
88d74ae0b6
|
@ -14,6 +14,8 @@
|
|||
* Updated ChibiOS to version 3.0.5.
|
||||
* Increased USB thread priority to avoid freeze during connect/disconnect on windows.
|
||||
* Smooth current ramping during resistance measurement.
|
||||
* Moved fault stop to thread, and added SPI mutexes to DRV drivers.
|
||||
* Smooth current ramping in flux linkage measurement and sensor detection.
|
||||
|
||||
=== FW 4.02 ===
|
||||
* Position PID fix (most notable on multiturn encoders).
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -840,7 +840,10 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
|
|||
float rpm_now = 0;
|
||||
|
||||
// Start by locking the motor
|
||||
mcpwm_foc_set_openloop(current, rpm_now);
|
||||
for (int i = 0;i < 200;i++) {
|
||||
mcpwm_foc_set_openloop((float)i * current / 200.0, rpm_now);
|
||||
chThdSleepMilliseconds(1);
|
||||
}
|
||||
|
||||
float duty_still = 0;
|
||||
float samples = 0;
|
||||
|
@ -1015,12 +1018,15 @@ int conf_general_autodetect_apply_sensors_foc(float current,
|
|||
}
|
||||
|
||||
// AS5047 encoder
|
||||
#ifndef HW_HAS_DUAL_MOTORS
|
||||
if (!res) {
|
||||
mcconf->m_sensor_port_mode = SENSOR_PORT_MODE_AS5047_SPI;
|
||||
mc_interface_set_configuration(mcconf);
|
||||
|
||||
mcpwm_foc_set_openloop_phase(current, 0.0);
|
||||
chThdSleepMilliseconds(1000);
|
||||
for (int i = 0;i < 1000;i++) {
|
||||
mcpwm_foc_set_openloop_phase((float)i * current / 1000.0, 0.0);
|
||||
chThdSleepMilliseconds(1);
|
||||
}
|
||||
|
||||
float phase_start = encoder_read_deg();
|
||||
float phase_mid = 0.0;
|
||||
|
@ -1053,6 +1059,7 @@ int conf_general_autodetect_apply_sensors_foc(float current,
|
|||
result = 2;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// Sensorless
|
||||
if (!res) {
|
||||
|
|
|
@ -251,7 +251,7 @@ void gpdrive_init(volatile mc_configuration *configuration) {
|
|||
|
||||
// Check if the system has resumed from IWDG reset
|
||||
if (timeout_had_IWDG_reset()) {
|
||||
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false);
|
||||
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false, false);
|
||||
}
|
||||
|
||||
m_init_done = true;
|
||||
|
@ -547,7 +547,7 @@ static void adc_int_handler(void *p, uint32_t flags) {
|
|||
|
||||
if ((wrong_voltage_iterations >= 8)) {
|
||||
mc_interface_fault_stop(input_voltage < m_conf->l_min_vin ?
|
||||
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE, false);
|
||||
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE, false, true);
|
||||
}
|
||||
} else {
|
||||
wrong_voltage_iterations = 0;
|
||||
|
@ -555,11 +555,11 @@ static void adc_int_handler(void *p, uint32_t flags) {
|
|||
|
||||
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, false);
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, false, true);
|
||||
}
|
||||
} else {
|
||||
if (fabsf(m_current_now_filtered) > m_conf->l_abs_current_max) {
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, false);
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, false, true);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -45,8 +45,11 @@ static void terminal_reset_faults(int argc, const char **argv);
|
|||
|
||||
// Private variables
|
||||
static char m_fault_print_buffer[120];
|
||||
static mutex_t m_spi_mutex;
|
||||
|
||||
void drv8301_init(void) {
|
||||
chMtxObjectInit(&m_spi_mutex);
|
||||
|
||||
// DRV8301 SPI
|
||||
palSetPadMode(DRV8301_MISO_GPIO, DRV8301_MISO_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(DRV8301_SCK_GPIO, DRV8301_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||
|
@ -221,6 +224,8 @@ unsigned int drv8301_read_reg(int reg) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= 0x807F;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
|
||||
if (reg != 0) {
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
|
@ -231,6 +236,8 @@ unsigned int drv8301_read_reg(int reg) {
|
|||
uint16_t res = spi_exchange(0xFFFF);
|
||||
spi_end();
|
||||
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
@ -239,9 +246,11 @@ void drv8301_write_reg(int reg, int data) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= data & 0x7FF;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
spi_end();
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
}
|
||||
|
||||
// Software SPI
|
||||
|
|
|
@ -41,8 +41,11 @@ static void terminal_write_reg(int argc, const char **argv);
|
|||
|
||||
// Private variables
|
||||
static char m_fault_print_buffer[120];
|
||||
static mutex_t m_spi_mutex;
|
||||
|
||||
void drv8305_init(void) {
|
||||
chMtxObjectInit(&m_spi_mutex);
|
||||
|
||||
// DRV8305 SPI
|
||||
palSetPadMode(DRV8305_MISO_GPIO, DRV8305_MISO_PIN, PAL_MODE_INPUT);
|
||||
palSetPadMode(DRV8305_SCK_GPIO, DRV8305_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||
|
@ -165,6 +168,8 @@ unsigned int drv8305_read_reg(int reg) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= 0x807F;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
|
||||
if (reg != 0) {
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
|
@ -175,6 +180,8 @@ unsigned int drv8305_read_reg(int reg) {
|
|||
uint16_t res = spi_exchange(0xFFFF);
|
||||
spi_end();
|
||||
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
@ -183,9 +190,11 @@ void drv8305_write_reg(int reg, int data) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= data & 0x7FF;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
spi_end();
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
}
|
||||
|
||||
// Software SPI
|
||||
|
|
|
@ -44,8 +44,11 @@ static void terminal_reset_faults(int argc, const char **argv);
|
|||
|
||||
// Private variables
|
||||
static char m_fault_print_buffer[120];
|
||||
static mutex_t m_spi_mutex;
|
||||
|
||||
void drv8320s_init(void) {
|
||||
chMtxObjectInit(&m_spi_mutex);
|
||||
|
||||
// DRV8320S SPI
|
||||
palSetPadMode(DRV8320S_MISO_GPIO, DRV8320S_MISO_PIN, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(DRV8320S_SCK_GPIO, DRV8320S_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||
|
@ -265,6 +268,8 @@ unsigned int drv8320s_read_reg(int reg) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= 0x807F;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
|
||||
if (reg != 0) {
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
|
@ -275,6 +280,8 @@ unsigned int drv8320s_read_reg(int reg) {
|
|||
uint16_t res = spi_exchange(out);
|
||||
spi_end();
|
||||
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
@ -283,9 +290,11 @@ void drv8320s_write_reg(int reg, int data) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= data & 0x7FF;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
spi_end();
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
}
|
||||
|
||||
// Software SPI
|
||||
|
|
|
@ -45,8 +45,11 @@ static void terminal_reset_faults(int argc, const char **argv);
|
|||
|
||||
// Private variables
|
||||
static char m_fault_print_buffer[120];
|
||||
static mutex_t m_spi_mutex;
|
||||
|
||||
void drv8323s_init(void) {
|
||||
chMtxObjectInit(&m_spi_mutex);
|
||||
|
||||
// DRV8323S SPI
|
||||
palSetPadMode(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN, PAL_MODE_INPUT_PULLUP);
|
||||
palSetPadMode(DRV8323S_SCK_GPIO, DRV8323S_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
|
||||
|
@ -318,6 +321,8 @@ unsigned int drv8323s_read_reg(int reg) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= 0x807F;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
|
||||
if (reg != 0) {
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
|
@ -328,6 +333,8 @@ unsigned int drv8323s_read_reg(int reg) {
|
|||
uint16_t res = spi_exchange(out);
|
||||
spi_end();
|
||||
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
|
@ -336,9 +343,11 @@ void drv8323s_write_reg(int reg, int data) {
|
|||
out |= (reg & 0x0F) << 11;
|
||||
out |= data & 0x7FF;
|
||||
|
||||
chMtxLock(&m_spi_mutex);
|
||||
spi_begin();
|
||||
spi_exchange(out);
|
||||
spi_end();
|
||||
chMtxUnlock(&m_spi_mutex);
|
||||
}
|
||||
|
||||
// Software SPI
|
||||
|
|
|
@ -65,7 +65,7 @@ CH_IRQ_HANDLER(PVD_IRQHandler) {
|
|||
if (EXTI_GetITStatus(EXTI_Line16) != RESET) {
|
||||
// Log the fault. Supply voltage dropped below 2.9V,
|
||||
// could corrupt an ongoing flash programming
|
||||
mc_interface_fault_stop(FAULT_CODE_MCU_UNDER_VOLTAGE, false);
|
||||
mc_interface_fault_stop(FAULT_CODE_MCU_UNDER_VOLTAGE, false, true);
|
||||
|
||||
// Clear the PVD pending bit
|
||||
EXTI_ClearITPendingBit(EXTI_Line16);
|
||||
|
|
213
mc_interface.c
213
mc_interface.c
|
@ -110,6 +110,8 @@ static volatile int m_sample_now;
|
|||
static volatile int m_sample_trigger;
|
||||
static volatile float m_last_adc_duration_sample;
|
||||
static volatile bool m_sample_is_second_motor;
|
||||
static volatile mc_fault_code m_fault_stop_fault;
|
||||
static volatile bool m_fault_stop_is_second_motor;
|
||||
|
||||
// Private functions
|
||||
static void update_override_limits(volatile motor_if_state_t *motor, volatile mc_configuration *conf);
|
||||
|
@ -122,9 +124,12 @@ static void(*pwn_done_func)(void) = 0;
|
|||
// Threads
|
||||
static THD_WORKING_AREA(timer_thread_wa, 1024);
|
||||
static THD_FUNCTION(timer_thread, arg);
|
||||
static THD_WORKING_AREA(sample_send_thread_wa, 1024);
|
||||
static THD_WORKING_AREA(sample_send_thread_wa, 512);
|
||||
static THD_FUNCTION(sample_send_thread, arg);
|
||||
static thread_t *sample_send_tp;
|
||||
static THD_WORKING_AREA(fault_stop_thread_wa, 512);
|
||||
static THD_FUNCTION(fault_stop_thread, arg);
|
||||
static thread_t *fault_stop_tp;
|
||||
|
||||
void mc_interface_init(void) {
|
||||
memset((void*)&m_motor_1, 0, sizeof(motor_if_state_t));
|
||||
|
@ -154,6 +159,7 @@ void mc_interface_init(void) {
|
|||
// Start threads
|
||||
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
|
||||
chThdCreateStatic(sample_send_thread_wa, sizeof(sample_send_thread_wa), NORMALPRIO - 1, sample_send_thread, NULL);
|
||||
chThdCreateStatic(fault_stop_thread_wa, sizeof(fault_stop_thread_wa), HIGHPRIO - 3, fault_stop_thread, NULL);
|
||||
|
||||
int motor_old = mc_interface_get_motor_thread();
|
||||
mc_interface_select_motor_thread(1);
|
||||
|
@ -1496,81 +1502,17 @@ int mc_interface_try_input(void) {
|
|||
return retval;
|
||||
}
|
||||
|
||||
void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor) {
|
||||
#ifdef HW_HAS_DUAL_MOTORS
|
||||
volatile motor_if_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
|
||||
#else
|
||||
volatile motor_if_state_t *motor = &m_motor_1;
|
||||
(void)is_second_motor;
|
||||
#endif
|
||||
void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor, bool is_isr) {
|
||||
m_fault_stop_fault = fault;
|
||||
m_fault_stop_is_second_motor = is_second_motor;
|
||||
|
||||
if (motor->m_fault_now == fault) {
|
||||
motor->m_ignore_iterations = motor->m_conf.m_fault_stop_time_ms;
|
||||
return;
|
||||
if (is_isr) {
|
||||
chSysLockFromISR();
|
||||
chEvtSignalI(fault_stop_tp, (eventmask_t) 1);
|
||||
chSysUnlockFromISR();
|
||||
} else {
|
||||
chEvtSignal(fault_stop_tp, (eventmask_t) 1);
|
||||
}
|
||||
|
||||
if (mc_interface_dccal_done() && motor->m_fault_now == FAULT_CODE_NONE) {
|
||||
// Sent to terminal fault logger so that all faults and their conditions
|
||||
// can be printed for debugging.
|
||||
utils_sys_lock_cnt();
|
||||
volatile int val_samp = TIM8->CCR1;
|
||||
volatile int current_samp = TIM1->CCR4;
|
||||
volatile int tim_top = TIM1->ARR;
|
||||
utils_sys_unlock_cnt();
|
||||
|
||||
fault_data fdata;
|
||||
fdata.motor = is_second_motor ? 2 : 1;
|
||||
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.gate_driver_voltage = motor->m_gate_driver_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 = motor->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_MOS);
|
||||
#ifdef HW_HAS_DRV8301
|
||||
if (fault == FAULT_CODE_DRV) {
|
||||
fdata.drv8301_faults = drv8301_read_faults();
|
||||
}
|
||||
#elif defined(HW_HAS_DRV8320S)
|
||||
if (fault == FAULT_CODE_DRV) {
|
||||
fdata.drv8301_faults = drv8320s_read_faults();
|
||||
}
|
||||
#elif defined(HW_HAS_DRV8323S)
|
||||
if (fault == FAULT_CODE_DRV) {
|
||||
fdata.drv8301_faults = drv8323s_read_faults();
|
||||
}
|
||||
#endif
|
||||
terminal_add_fault_data(&fdata);
|
||||
}
|
||||
|
||||
motor->m_ignore_iterations = motor->m_conf.m_fault_stop_time_ms;
|
||||
|
||||
switch (motor->m_conf.motor_type) {
|
||||
case MOTOR_TYPE_BLDC:
|
||||
case MOTOR_TYPE_DC:
|
||||
mcpwm_stop_pwm();
|
||||
break;
|
||||
|
||||
case MOTOR_TYPE_FOC:
|
||||
mcpwm_foc_stop_pwm(is_second_motor);
|
||||
break;
|
||||
|
||||
case MOTOR_TYPE_GPD:
|
||||
gpdrive_set_mode(GPD_OUTPUT_MODE_NONE);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
motor->m_fault_now = fault;
|
||||
}
|
||||
|
||||
void mc_interface_mc_timer_isr(bool is_second_motor) {
|
||||
|
@ -1594,7 +1536,7 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
|
|||
|
||||
if ((wrong_voltage_iterations >= 8)) {
|
||||
mc_interface_fault_stop(input_voltage < conf_now->l_min_vin ?
|
||||
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE, is_second_motor);
|
||||
FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE, is_second_motor, true);
|
||||
}
|
||||
} else {
|
||||
wrong_voltage_iterations = 0;
|
||||
|
@ -1652,11 +1594,11 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
|
|||
// Current fault code
|
||||
if (conf_now->l_slow_abs_current) {
|
||||
if (fabsf(abs_current_filtered) > conf_now->l_abs_current_max) {
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, is_second_motor);
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, is_second_motor, true);
|
||||
}
|
||||
} else {
|
||||
if (fabsf(abs_current) > conf_now->l_abs_current_max) {
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, is_second_motor);
|
||||
mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT, is_second_motor, true);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -1666,13 +1608,13 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
|
|||
#else
|
||||
if (is_second_motor ? IS_DRV_FAULT_2() : IS_DRV_FAULT()) {
|
||||
#endif
|
||||
mc_interface_fault_stop(FAULT_CODE_DRV, is_second_motor);
|
||||
mc_interface_fault_stop(FAULT_CODE_DRV, is_second_motor, true);
|
||||
}
|
||||
|
||||
#ifdef HW_USE_BRK
|
||||
// BRK fault code
|
||||
if (TIM_GetFlagStatus(TIM1, TIM_FLAG_Break) != RESET) {
|
||||
mc_interface_fault_stop(FAULT_CODE_BRK, is_second_motor);
|
||||
mc_interface_fault_stop(FAULT_CODE_BRK, is_second_motor, true);
|
||||
// latch the BRK/FAULT pin to low until next MCU reset
|
||||
palSetPadMode(BRK_GPIO, BRK_PIN, PAL_MODE_OUTPUT_PUSHPULL);
|
||||
palClearPad(BRK_GPIO, BRK_PIN);
|
||||
|
@ -1681,11 +1623,11 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
|
|||
|
||||
#ifdef HW_HAS_GATE_DRIVER_SUPPLY_MONITOR
|
||||
if(motor->m_gate_driver_voltage > HW_GATE_DRIVER_SUPPLY_MAX_VOLTAGE) {
|
||||
mc_interface_fault_stop(FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, is_second_motor);
|
||||
mc_interface_fault_stop(FAULT_CODE_GATE_DRIVER_OVER_VOLTAGE, is_second_motor, true);
|
||||
}
|
||||
|
||||
if(motor->m_gate_driver_voltage < HW_GATE_DRIVER_SUPPLY_MIN_VOLTAGE) {
|
||||
mc_interface_fault_stop(FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, is_second_motor);
|
||||
mc_interface_fault_stop(FAULT_CODE_GATE_DRIVER_UNDER_VOLTAGE, is_second_motor, true);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -1951,7 +1893,7 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
} else if (motor->m_temp_fet > conf->l_temp_fet_end) {
|
||||
lo_min_mos = 0.0;
|
||||
lo_max_mos = 0.0;
|
||||
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET, !is_motor_1, false);
|
||||
} else {
|
||||
float maxc = fabsf(l_current_max_tmp);
|
||||
if (fabsf(l_current_min_tmp) > maxc) {
|
||||
|
@ -1977,7 +1919,7 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
|
|||
} else if (motor->m_temp_motor > conf->l_temp_motor_end) {
|
||||
lo_min_mot = 0.0;
|
||||
lo_max_mot = 0.0;
|
||||
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_MOTOR, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_MOTOR, !is_motor_1, false);
|
||||
} else {
|
||||
float maxc = fabsf(l_current_max_tmp);
|
||||
if (fabsf(l_current_min_tmp) > maxc) {
|
||||
|
@ -2167,7 +2109,7 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) {
|
|||
motor->m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
mcpwm_foc_is_using_encoder() &&
|
||||
encoder_spi_get_error_rate() > 0.05) {
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, !is_motor_1, false);
|
||||
}
|
||||
|
||||
if(motor->m_conf.motor_type == MOTOR_TYPE_FOC &&
|
||||
|
@ -2175,20 +2117,20 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) {
|
|||
motor->m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
|
||||
|
||||
if (encoder_sincos_get_signal_below_min_error_rate() > 0.05)
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, !is_motor_1, false);
|
||||
if (encoder_sincos_get_signal_above_max_error_rate() > 0.05)
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, !is_motor_1, false);
|
||||
}
|
||||
|
||||
if(motor->m_conf.motor_type == MOTOR_TYPE_FOC &&
|
||||
motor->m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
|
||||
motor->m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
|
||||
if (encoder_resolver_loss_of_tracking_error_rate() > 0.05)
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, !is_motor_1, false);
|
||||
if (encoder_resolver_degradation_of_signal_error_rate() > 0.05)
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS, !is_motor_1, false);
|
||||
if (encoder_resolver_loss_of_signal_error_rate() > 0.04)
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS, !is_motor_1, false);
|
||||
}
|
||||
// TODO: Implement for BLDC and GPDRIVE
|
||||
if(motor->m_conf.motor_type == MOTOR_TYPE_FOC) {
|
||||
|
@ -2209,14 +2151,14 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) {
|
|||
#endif
|
||||
|
||||
if (abs(curr0_offset - MIDDLE_ADC) > HW_MAX_CURRENT_OFFSET) {
|
||||
mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_1, !is_motor_1, false);
|
||||
}
|
||||
if (abs(curr1_offset - MIDDLE_ADC) > HW_MAX_CURRENT_OFFSET) {
|
||||
mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_2, !is_motor_1, false);
|
||||
}
|
||||
#ifdef HW_HAS_3_SHUNTS
|
||||
if (abs(curr2_offset - MIDDLE_ADC) > HW_MAX_CURRENT_OFFSET) {
|
||||
mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_HIGH_OFFSET_CURRENT_SENSOR_3, !is_motor_1, false);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
@ -2233,7 +2175,7 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) {
|
|||
}
|
||||
|
||||
if (motor->m_motor_current_unbalance_error_rate > MCCONF_MAX_CURRENT_UNBALANCE_RATE) {
|
||||
mc_interface_fault_stop(FAULT_CODE_UNBALANCED_CURRENTS, !is_motor_1);
|
||||
mc_interface_fault_stop(FAULT_CODE_UNBALANCED_CURRENTS, !is_motor_1, false);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -2258,7 +2200,6 @@ static THD_FUNCTION(sample_send_thread, arg) {
|
|||
(void)arg;
|
||||
|
||||
chRegSetThreadName("SampleSender");
|
||||
|
||||
sample_send_tp = chThdGetSelfX();
|
||||
|
||||
for(;;) {
|
||||
|
@ -2314,3 +2255,87 @@ static THD_FUNCTION(sample_send_thread, arg) {
|
|||
}
|
||||
}
|
||||
}
|
||||
|
||||
static THD_FUNCTION(fault_stop_thread, arg) {
|
||||
(void)arg;
|
||||
|
||||
chRegSetThreadName("Fault Stop");
|
||||
fault_stop_tp = chThdGetSelfX();
|
||||
|
||||
for(;;) {
|
||||
chEvtWaitAny((eventmask_t) 1);
|
||||
#ifdef HW_HAS_DUAL_MOTORS
|
||||
volatile motor_if_state_t *motor = m_fault_stop_is_second_motor ? &m_motor_2 : &m_motor_1;
|
||||
#else
|
||||
volatile motor_if_state_t *motor = &m_motor_1;
|
||||
#endif
|
||||
|
||||
if (motor->m_fault_now == m_fault_stop_fault) {
|
||||
motor->m_ignore_iterations = motor->m_conf.m_fault_stop_time_ms;
|
||||
continue;
|
||||
}
|
||||
|
||||
if (mc_interface_dccal_done() && motor->m_fault_now == FAULT_CODE_NONE) {
|
||||
// Sent to terminal fault logger so that all faults and their conditions
|
||||
// can be printed for debugging.
|
||||
utils_sys_lock_cnt();
|
||||
volatile int val_samp = TIM8->CCR1;
|
||||
volatile int current_samp = TIM1->CCR4;
|
||||
volatile int tim_top = TIM1->ARR;
|
||||
utils_sys_unlock_cnt();
|
||||
|
||||
fault_data fdata;
|
||||
fdata.motor = m_fault_stop_is_second_motor ? 2 : 1;
|
||||
fdata.fault = m_fault_stop_fault;
|
||||
fdata.current = mc_interface_get_tot_current();
|
||||
fdata.current_filtered = mc_interface_get_tot_current_filtered();
|
||||
fdata.voltage = GET_INPUT_VOLTAGE();
|
||||
fdata.gate_driver_voltage = motor->m_gate_driver_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 = motor->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_MOS);
|
||||
#ifdef HW_HAS_DRV8301
|
||||
if (m_fault_stop_fault == FAULT_CODE_DRV) {
|
||||
fdata.drv8301_faults = drv8301_read_faults();
|
||||
}
|
||||
#elif defined(HW_HAS_DRV8320S)
|
||||
if (m_fault_stop_fault == FAULT_CODE_DRV) {
|
||||
fdata.drv8301_faults = drv8320s_read_faults();
|
||||
}
|
||||
#elif defined(HW_HAS_DRV8323S)
|
||||
if (m_fault_stop_fault == FAULT_CODE_DRV) {
|
||||
fdata.drv8301_faults = drv8323s_read_faults();
|
||||
}
|
||||
#endif
|
||||
terminal_add_fault_data(&fdata);
|
||||
}
|
||||
|
||||
motor->m_ignore_iterations = motor->m_conf.m_fault_stop_time_ms;
|
||||
|
||||
switch (motor->m_conf.motor_type) {
|
||||
case MOTOR_TYPE_BLDC:
|
||||
case MOTOR_TYPE_DC:
|
||||
mcpwm_stop_pwm();
|
||||
break;
|
||||
|
||||
case MOTOR_TYPE_FOC:
|
||||
mcpwm_foc_stop_pwm(m_fault_stop_is_second_motor);
|
||||
break;
|
||||
|
||||
case MOTOR_TYPE_GPD:
|
||||
gpdrive_set_mode(GPD_OUTPUT_MODE_NONE);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
motor->m_fault_now = m_fault_stop_fault;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -86,7 +86,7 @@ float mc_interface_get_distance_abs(void);
|
|||
setup_values mc_interface_get_setup_values(void);
|
||||
|
||||
// MC implementation functions
|
||||
void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor);
|
||||
void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor, bool is_isr);
|
||||
int mc_interface_try_input(void);
|
||||
void mc_interface_mc_timer_isr(bool is_second_motor);
|
||||
|
||||
|
|
2
mcpwm.c
2
mcpwm.c
|
@ -447,7 +447,7 @@ void mcpwm_init(volatile mc_configuration *configuration) {
|
|||
|
||||
// Check if the system has resumed from IWDG reset
|
||||
if (timeout_had_IWDG_reset()) {
|
||||
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false);
|
||||
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false, false);
|
||||
}
|
||||
|
||||
// Reset tachometers again
|
||||
|
|
13
mcpwm_foc.c
13
mcpwm_foc.c
|
@ -633,7 +633,7 @@ void mcpwm_foc_init(volatile mc_configuration *conf_m1, volatile mc_configuratio
|
|||
|
||||
// Check if the system has resumed from IWDG reset
|
||||
if (timeout_had_IWDG_reset()) {
|
||||
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false);
|
||||
mc_interface_fault_stop(FAULT_CODE_BOOTING_FROM_WATCHDOG_RESET, false, false);
|
||||
}
|
||||
|
||||
terminal_register_command_callback(
|
||||
|
@ -1953,7 +1953,7 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
|||
mc_interface_lock();
|
||||
|
||||
motor->m_phase_override = true;
|
||||
motor->m_id_set = current;
|
||||
motor->m_id_set = 0.0;
|
||||
motor->m_iq_set = 0.0;
|
||||
motor->m_control_mode = CONTROL_MODE_CURRENT;
|
||||
motor->m_state = MC_STATE_RUNNING;
|
||||
|
@ -1966,7 +1966,11 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
|
|||
|
||||
// Lock the motor
|
||||
motor->m_phase_now_override = 0;
|
||||
chThdSleepMilliseconds(1000);
|
||||
|
||||
for (int i = 0;i < 1000;i++) {
|
||||
motor->m_id_set = (float)i * current / 1000.0;
|
||||
chThdSleepMilliseconds(1);
|
||||
}
|
||||
|
||||
float sin_hall[8];
|
||||
float cos_hall[8];
|
||||
|
@ -3647,6 +3651,9 @@ static void run_pid_control_speed(float dt, volatile motor_all_state_t *motor) {
|
|||
}
|
||||
|
||||
static void stop_pwm_hw(volatile motor_all_state_t *motor) {
|
||||
motor->m_id_set = 0.0;
|
||||
motor->m_iq_set = 0.0;
|
||||
|
||||
if (motor == &m_motor_1) {
|
||||
TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive);
|
||||
TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
|
||||
|
|
Loading…
Reference in New Issue