Added mutex to DRV SPI driver, moved fault stop to thread, smooth current ramping for sensor and flux linkage detection

Author: vedderb

CHANGELOG

@@ -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).

build_all/100_250/VESC_default.bin

@@ -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) {

build_all/100_250/VESC_default_no_hw_limits.bin

@@ -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,19 +547,19 @@ 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;
 	}
 
 	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);
 		}
 	}
 

build_all/410_o_411_o_412/VESC_0005ohm.bin

@@ -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

build_all/410_o_411_o_412/VESC_default.bin

@@ -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

build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin

@@ -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

build_all/46_o_47/VESC_0005ohm.bin

@@ -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

build_all/46_o_47/VESC_33k.bin

@@ -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);