Merge remote-tracking branch 'upstream/dev_fw_5_00' into dev_fw_5_00

CHANGELOG

@@ -2,6 +2,7 @@
 * Dual motor support. VESC-based controllers such as the focbox unity will now work.
 * Fixed bug in cross BEMF decoupling.
 * Disable CC decoupling during flux linkage measurement.
+* Balance app updates. See: https://github.com/vedderb/bldc/pull/141
 
 === FW 4.02 ===
 * Position PID fix (most notable on multiturn encoders).

confgenerator.h

@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		3632471335
-#define APPCONF_SIGNATURE		1232755601
+#define APPCONF_SIGNATURE		2460147246
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);

hwconf/drv8301.c

@@ -29,6 +29,7 @@
 #include "commands.h"
 #include <string.h>
 #include <stdio.h>
+#include "mc_interface.h"
 
 // Private functions
 static uint16_t spi_exchange(uint16_t x);
@@ -44,7 +45,6 @@ static void terminal_reset_faults(int argc, const char **argv);
 
 // Private variables
 static char m_fault_print_buffer[120];
-static volatile bool m_is_motor2;
 
 void drv8301_init(void) {
 	// DRV8301 SPI
@@ -57,8 +57,6 @@ void drv8301_init(void) {
 	palSetPadMode(DRV8301_CS_GPIO2, DRV8301_CS_PIN2, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
 #endif
 
-	m_is_motor2 = false;
-
 	chThdSleepMilliseconds(100);
 
 	// Disable OC
@@ -96,10 +94,6 @@ void drv8301_init(void) {
 			terminal_reset_faults);
 }
 
-void drv8301_select_second_motor(bool select_second_motor) {
-	m_is_motor2 = select_second_motor;
-}
-
 /**
  * Set the threshold of the over current protection of the DRV8301. It works by measuring
  * the voltage drop across drain-source of the MOSFETs and activates when it is higher than
@@ -294,7 +288,7 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
 
 static void spi_begin(void) {
 #ifdef DRV8301_CS_GPIO2
-	if (m_is_motor2) {
+	if (mc_interface_motor_now() == 2) {
 		palClearPad(DRV8301_CS_GPIO2, DRV8301_CS_PIN2);
 	} else {
 		palClearPad(DRV8301_CS_GPIO, DRV8301_CS_PIN);
@@ -306,7 +300,7 @@ static void spi_begin(void) {
 
 static void spi_end(void) {
 #ifdef DRV8301_CS_GPIO2
-	if (m_is_motor2) {
+	if (mc_interface_motor_now() == 2) {
 		palSetPad(DRV8301_CS_GPIO2, DRV8301_CS_PIN2);
 	} else {
 		palSetPad(DRV8301_CS_GPIO, DRV8301_CS_PIN);

hwconf/drv8301.h

@@ -22,7 +22,6 @@
 
 // Functions
 void drv8301_init(void);
-void drv8301_select_second_motor(bool select_second_motor);
 void drv8301_set_oc_adj(int val);
 void drv8301_set_oc_mode(drv8301_oc_mode mode);
 int drv8301_read_faults(void);

hwconf/hw_unity.c

@@ -160,11 +160,11 @@ void hw_init_gpio(void) {
 	palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
 	palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG);
 
-	drv8301_select_second_motor(false);
+	mc_interface_select_motor_thread(1);
 	drv8301_init();
-	drv8301_select_second_motor(true);
+	mc_interface_select_motor_thread(2);
 	drv8301_init();
-	drv8301_select_second_motor(false);
+	mc_interface_select_motor_thread(1);
 }
 
 void hw_setup_adc_channels(void) {

mc_interface.c

@@ -45,7 +45,7 @@
 #include <string.h>
 
 // Macros
-#define DIR_MULT		(m_motor_now->m_conf.m_invert_direction ? -1.0 : 1.0)
+#define DIR_MULT		(motor_now()->m_conf.m_invert_direction ? -1.0 : 1.0)
 
 // Global variables
 volatile uint16_t ADC_Value[HW_ADC_CHANNELS + HW_ADC_CHANNELS_EXTRA];
@@ -87,7 +87,6 @@ static volatile motor_if_state_t m_motor_1;
 #ifdef HW_HAS_DUAL_MOTORS
 static volatile motor_if_state_t m_motor_2;
 #endif
-static volatile motor_if_state_t *m_motor_now;
 
 // Sampling variables
 #define ADC_SAMPLE_MAX_LEN		2000
@@ -114,6 +113,7 @@ static volatile bool m_sample_is_second_motor;
 // Private functions
 static void update_override_limits(volatile motor_if_state_t *motor, volatile mc_configuration *conf);
 static void run_timer_tasks(volatile motor_if_state_t *motor);
+static volatile motor_if_state_t *motor_now(void);
 
 // Function pointers
 static void(*pwn_done_func)(void) = 0;
@@ -130,7 +130,6 @@ void mc_interface_init(void) {
 #ifdef HW_HAS_DUAL_MOTORS
 	memset((void*)&m_motor_2, 0, sizeof(motor_if_state_t));
 #endif
-	m_motor_now = &m_motor_1;
 
 	conf_general_read_mc_configuration((mc_configuration*)&m_motor_1.m_conf, false);
 #ifdef HW_HAS_DUAL_MOTORS
@@ -155,28 +154,39 @@ void mc_interface_init(void) {
 	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);
 
+	int motor_old = mc_interface_get_motor_thread();
+	mc_interface_select_motor_thread(1);
 #ifdef HW_HAS_DRV8301
-	drv8301_select_second_motor(false);
-	drv8301_set_oc_mode(m_motor_1.m_conf.m_drv8301_oc_mode);
-	drv8301_set_oc_adj(m_motor_1.m_conf.m_drv8301_oc_adj);
-#ifdef HW_HAS_DUAL_MOTORS
-	drv8301_select_second_motor(true);
-	drv8301_set_oc_mode(m_motor_2.m_conf.m_drv8301_oc_mode);
-	drv8301_set_oc_adj(m_motor_2.m_conf.m_drv8301_oc_adj);
-#endif
+	drv8301_set_oc_mode(motor_now()->m_conf.m_drv8301_oc_mode);
+	drv8301_set_oc_adj(motor_now()->m_conf.m_drv8301_oc_adj);
 #elif defined(HW_HAS_DRV8320S)
-	drv8320s_set_oc_mode(m_motor_now->m_conf.m_drv8301_oc_mode);
-	drv8320s_set_oc_adj(m_motor_now->m_conf.m_drv8301_oc_adj);
+	drv8320s_set_oc_mode(motor_now()->m_conf.m_drv8301_oc_mode);
+	drv8320s_set_oc_adj(motor_now()->m_conf.m_drv8301_oc_adj);
 #elif defined(HW_HAS_DRV8323S)
-	drv8323s_set_oc_mode(m_motor_now->m_conf.m_drv8301_oc_mode);
-	drv8323s_set_oc_adj(m_motor_now->m_conf.m_drv8301_oc_adj);
+	drv8323s_set_oc_mode(motor_now()->m_conf.m_drv8301_oc_mode);
+	drv8323s_set_oc_adj(motor_now()->m_conf.m_drv8301_oc_adj);
 #endif
 
+#ifdef HW_HAS_DUAL_MOTORS
+	mc_interface_select_motor_thread(2);
+#ifdef HW_HAS_DRV8301
+	drv8301_set_oc_mode(motor_now()->m_conf.m_drv8301_oc_mode);
+	drv8301_set_oc_adj(motor_now()->m_conf.m_drv8301_oc_adj);
+#elif defined(HW_HAS_DRV8320S)
+	drv8320s_set_oc_mode(motor_now()->m_conf.m_drv8301_oc_mode);
+	drv8320s_set_oc_adj(motor_now()->m_conf.m_drv8301_oc_adj);
+#elif defined(HW_HAS_DRV8323S)
+	drv8323s_set_oc_mode(motor_now()->m_conf.m_drv8301_oc_mode);
+	drv8323s_set_oc_adj(motor_now()->m_conf.m_drv8301_oc_adj);
+#endif
+#endif
+	mc_interface_select_motor_thread(motor_old);
+
 	// Initialize encoder
 #if !WS2811_ENABLE
-	switch (m_motor_now->m_conf.m_sensor_port_mode) {
+	switch (motor_now()->m_conf.m_sensor_port_mode) {
 	case SENSOR_PORT_MODE_ABI:
-		encoder_init_abi(m_motor_now->m_conf.m_encoder_counts);
+		encoder_init_abi(motor_now()->m_conf.m_encoder_counts);
 		break;
 
 	case SENSOR_PORT_MODE_AS5047_SPI:
@@ -188,9 +198,9 @@ void mc_interface_init(void) {
 		break;
 
 	case SENSOR_PORT_MODE_SINCOS:
-		encoder_init_sincos(m_motor_now->m_conf.foc_encoder_sin_gain, m_motor_now->m_conf.foc_encoder_sin_offset,
-							m_motor_now->m_conf.foc_encoder_cos_gain, m_motor_now->m_conf.foc_encoder_cos_offset,
-							m_motor_now->m_conf.foc_encoder_sincos_filter_constant);
+		encoder_init_sincos(motor_now()->m_conf.foc_encoder_sin_gain, motor_now()->m_conf.foc_encoder_sin_offset,
+							motor_now()->m_conf.foc_encoder_cos_gain, motor_now()->m_conf.foc_encoder_cos_offset,
+							motor_now()->m_conf.foc_encoder_sincos_filter_constant);
 		break;
 
 	case SENSOR_PORT_MODE_TS5700N8501:
@@ -214,10 +224,10 @@ void mc_interface_init(void) {
 #endif
 
 	// Initialize selected implementation
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
-		mcpwm_init(&m_motor_now->m_conf);
+		mcpwm_init(&motor_now()->m_conf);
 		break;
 
 	case MOTOR_TYPE_FOC:
@@ -229,7 +239,7 @@ void mc_interface_init(void) {
 		break;
 
 	case MOTOR_TYPE_GPD:
-		gpdrive_init(&m_motor_now->m_conf);
+		gpdrive_init(&motor_now()->m_conf);
 		break;
 
 	default:
@@ -237,6 +247,23 @@ void mc_interface_init(void) {
 	}
 }
 
+int mc_interface_motor_now(void) {
+#ifdef HW_HAS_DUAL_MOTORS
+	int isr_motor = mcpwm_foc_isr_motor();
+	int thd_motor = chThdGetSelfX()->motor_selected;
+
+	if (isr_motor > 0) {
+		return isr_motor;
+	} else if (thd_motor > 0) {
+		return thd_motor;
+	} else {
+		return 1;
+	}
+#else
+	return 1;
+#endif
+}
+
 /**
  * Select motor for current thread. When a thread has a motor selected,
  * the mc_interface functions will use that motor for that thread. This
@@ -269,36 +296,12 @@ int mc_interface_get_motor_thread(void) {
 	return chThdGetSelfX()->motor_selected;
 }
 
-void mc_interface_update_selected_motor(void) {
-	if (mcpwm_foc_is_isr()) {
-		return;
-	}
-
-#ifdef HW_HAS_DUAL_MOTORS
-	if (chThdGetSelfX()->motor_selected == 1) {
-		mcpwm_foc_select_second_motor(false);
-#ifdef HW_HAS_DRV8301
-		drv8301_select_second_motor(false);
-#endif
-		m_motor_now = &m_motor_1;
-	} else if (chThdGetSelfX()->motor_selected == 2) {
-		mcpwm_foc_select_second_motor(true);
-#ifdef HW_HAS_DRV8301
-		drv8301_select_second_motor(true);
-#endif
-		m_motor_now = &m_motor_2;
-	}
-#endif
-}
-
 const volatile mc_configuration* mc_interface_get_configuration(void) {
-	mc_interface_update_selected_motor();
-	return &m_motor_now->m_conf;
+	return &motor_now()->m_conf;
 }
 
 void mc_interface_set_configuration(mc_configuration *configuration) {
-	mc_interface_update_selected_motor();
-	volatile motor_if_state_t *motor = m_motor_now;
+	volatile motor_if_state_t *motor = motor_now();
 
 #ifdef HW_HAS_DUAL_MOTORS
 	configuration->motor_type = MOTOR_TYPE_FOC;
@@ -353,9 +356,7 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 #endif
 
 #ifdef HW_HAS_DRV8301
-	mc_interface_update_selected_motor();
 	drv8301_set_oc_mode(configuration->m_drv8301_oc_mode);
-	mc_interface_update_selected_motor();
 	drv8301_set_oc_adj(configuration->m_drv8301_oc_adj);
 #elif defined(HW_HAS_DRV8320S)
 	drv8320s_set_oc_mode(configuration->m_drv8301_oc_mode);
@@ -413,7 +414,6 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 			m_motor_1.m_conf.foc_f_sw = motor->m_conf.foc_f_sw;
 		}
 #endif
-		mc_interface_update_selected_motor();
 		mcpwm_foc_set_configuration(&motor->m_conf);
 		break;
 
@@ -427,10 +427,8 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 }
 
 bool mc_interface_dccal_done(void) {
-	mc_interface_update_selected_motor();
-
 	bool ret = false;
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_is_dccal_done();
@@ -467,29 +465,25 @@ void mc_interface_set_pwm_callback(void (*p_func)(void)) {
  * Lock the control by disabling all control commands.
  */
 void mc_interface_lock(void) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_lock_enabled = true;
+	motor_now()->m_lock_enabled = true;
 }
 
 /**
  * Unlock all control commands.
  */
 void mc_interface_unlock(void) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_lock_enabled = false;
+	motor_now()->m_lock_enabled = false;
 }
 
 /**
  * Allow just one motor control command in the locked state.
  */
 void mc_interface_lock_override_once(void) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_lock_override_once = true;
+	motor_now()->m_lock_override_once = true;
 }
 
 mc_fault_code mc_interface_get_fault(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_fault_now;
+	return motor_now()->m_fault_now;
 }
 
 const char* mc_interface_fault_to_string(mc_fault_code fault) {
@@ -519,9 +513,8 @@ const char* mc_interface_fault_to_string(mc_fault_code fault) {
 }
 
 mc_state mc_interface_get_state(void) {
-	mc_interface_update_selected_motor();
 	mc_state ret = MC_STATE_OFF;
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_state();
@@ -539,7 +532,6 @@ mc_state mc_interface_get_state(void) {
 }
 
 void mc_interface_set_duty(float dutyCycle) {
-	mc_interface_update_selected_motor();
 	if (fabsf(dutyCycle) > 0.001) {
 		SHUTDOWN_RESET();
 	}
@@ -548,7 +540,7 @@ void mc_interface_set_duty(float dutyCycle) {
 		return;
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		mcpwm_set_duty(DIR_MULT * dutyCycle);
@@ -564,8 +556,6 @@ void mc_interface_set_duty(float dutyCycle) {
 }
 
 void mc_interface_set_duty_noramp(float dutyCycle) {
-	mc_interface_update_selected_motor();
-
 	if (fabsf(dutyCycle) > 0.001) {
 		SHUTDOWN_RESET();
 	}
@@ -574,7 +564,7 @@ void mc_interface_set_duty_noramp(float dutyCycle) {
 		return;
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		mcpwm_set_duty_noramp(DIR_MULT * dutyCycle);
@@ -590,8 +580,6 @@ void mc_interface_set_duty_noramp(float dutyCycle) {
 }
 
 void mc_interface_set_pid_speed(float rpm) {
-	mc_interface_update_selected_motor();
-
 	if (fabsf(rpm) > 0.001) {
 		SHUTDOWN_RESET();
 	}
@@ -600,7 +588,7 @@ void mc_interface_set_pid_speed(float rpm) {
 		return;
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		mcpwm_set_pid_speed(DIR_MULT * rpm);
@@ -616,19 +604,18 @@ void mc_interface_set_pid_speed(float rpm) {
 }
 
 void mc_interface_set_pid_pos(float pos) {
-	mc_interface_update_selected_motor();
 	SHUTDOWN_RESET();
 
 	if (mc_interface_try_input()) {
 		return;
 	}
 
-	m_motor_now->m_position_set = pos;
+	motor_now()->m_position_set = pos;
 
 	pos *= DIR_MULT;
 	utils_norm_angle(&pos);
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		mcpwm_set_pid_pos(pos);
@@ -644,7 +631,6 @@ void mc_interface_set_pid_pos(float pos) {
 }
 
 void mc_interface_set_current(float current) {
-	mc_interface_update_selected_motor();
 	if (fabsf(current) > 0.001) {
 		SHUTDOWN_RESET();
 	}
@@ -653,7 +639,7 @@ void mc_interface_set_current(float current) {
 		return;
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		mcpwm_set_current(DIR_MULT * current);
@@ -669,7 +655,6 @@ void mc_interface_set_current(float current) {
 }
 
 void mc_interface_set_brake_current(float current) {
-	mc_interface_update_selected_motor();
 	if (fabsf(current) > 0.001) {
 		SHUTDOWN_RESET();
 	}
@@ -678,7 +663,7 @@ void mc_interface_set_brake_current(float current) {
 		return;
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		mcpwm_set_brake_current(DIR_MULT * current);
@@ -705,12 +690,11 @@ void mc_interface_set_brake_current(float current) {
  * The relative current value, range [-1.0 1.0]
  */
 void mc_interface_set_current_rel(float val) {
-	mc_interface_update_selected_motor();
 	if (fabsf(val) > 0.001) {
 		SHUTDOWN_RESET();
 	}
 
-	mc_interface_set_current(val * m_motor_now->m_conf.lo_current_motor_max_now);
+	mc_interface_set_current(val * motor_now()->m_conf.lo_current_motor_max_now);
 }
 
 /**
@@ -720,12 +704,11 @@ void mc_interface_set_current_rel(float val) {
  * The relative current value, range [0.0 1.0]
  */
 void mc_interface_set_brake_current_rel(float val) {
-	mc_interface_update_selected_motor();
 	if (fabsf(val) > 0.001) {
 		SHUTDOWN_RESET();
 	}
 
-	mc_interface_set_brake_current(val * fabsf(m_motor_now->m_conf.lo_current_motor_min_now));
+	mc_interface_set_brake_current(val * fabsf(motor_now()->m_conf.lo_current_motor_min_now));
 }
 
 /**
@@ -735,7 +718,6 @@ void mc_interface_set_brake_current_rel(float val) {
  * The current value.
  */
 void mc_interface_set_handbrake(float current) {
-	mc_interface_update_selected_motor();
 	if (fabsf(current) > 0.001) {
 		SHUTDOWN_RESET();
 	}
@@ -744,7 +726,7 @@ void mc_interface_set_handbrake(float current) {
 		return;
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		// TODO: Not implemented yet, use brake mode for now.
@@ -767,16 +749,14 @@ void mc_interface_set_handbrake(float current) {
  * The relative current value, range [0.0 1.0]
  */
 void mc_interface_set_handbrake_rel(float val) {
-	mc_interface_update_selected_motor();
 	if (fabsf(val) > 0.001) {
 		SHUTDOWN_RESET();
 	}
 
-	mc_interface_set_handbrake(val * fabsf(m_motor_now->m_conf.lo_current_motor_min_now));
+	mc_interface_set_handbrake(val * fabsf(motor_now()->m_conf.lo_current_motor_min_now));
 }
 
 void mc_interface_brake_now(void) {
-	mc_interface_update_selected_motor();
 	SHUTDOWN_RESET();
 
 	mc_interface_set_duty(0.0);
@@ -786,7 +766,6 @@ void mc_interface_brake_now(void) {
  * Disconnect the motor and let it turn freely.
  */
 void mc_interface_release_motor(void) {
-	mc_interface_update_selected_motor();
 	mc_interface_set_current(0.0);
 }
 
@@ -794,10 +773,9 @@ void mc_interface_release_motor(void) {
  * Stop the motor and use braking.
  */
 float mc_interface_get_duty_cycle_set(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_duty_cycle_set();
@@ -815,10 +793,9 @@ float mc_interface_get_duty_cycle_set(void) {
 }
 
 float mc_interface_get_duty_cycle_now(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_duty_cycle_now();
@@ -836,10 +813,9 @@ float mc_interface_get_duty_cycle_now(void) {
 }
 
 float mc_interface_get_sampling_frequency_now(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_switching_frequency_now();
@@ -861,10 +837,9 @@ float mc_interface_get_sampling_frequency_now(void) {
 }
 
 float mc_interface_get_rpm(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_rpm();
@@ -891,11 +866,10 @@ float mc_interface_get_rpm(void) {
  * The amount of amp hours drawn.
  */
 float mc_interface_get_amp_hours(bool reset) {
-	mc_interface_update_selected_motor();
-	float val = m_motor_now->m_amp_seconds / 3600;
+	float val = motor_now()->m_amp_seconds / 3600;
 
 	if (reset) {
-		m_motor_now->m_amp_seconds = 0.0;
+		motor_now()->m_amp_seconds = 0.0;
 	}
 
 	return val;
@@ -911,11 +885,10 @@ float mc_interface_get_amp_hours(bool reset) {
  * The amount of amp hours fed back.
  */
 float mc_interface_get_amp_hours_charged(bool reset) {
-	mc_interface_update_selected_motor();
-	float val = m_motor_now->m_amp_seconds_charged / 3600;
+	float val = motor_now()->m_amp_seconds_charged / 3600;
 
 	if (reset) {
-		m_motor_now->m_amp_seconds_charged = 0.0;
+		motor_now()->m_amp_seconds_charged = 0.0;
 	}
 
 	return val;
@@ -931,11 +904,10 @@ float mc_interface_get_amp_hours_charged(bool reset) {
  * The amount of watt hours drawn.
  */
 float mc_interface_get_watt_hours(bool reset) {
-	mc_interface_update_selected_motor();
-	float val = m_motor_now->m_watt_seconds / 3600;
+	float val = motor_now()->m_watt_seconds / 3600;
 
 	if (reset) {
-		m_motor_now->m_watt_seconds = 0.0;
+		motor_now()->m_watt_seconds = 0.0;
 	}
 
 	return val;
@@ -951,21 +923,19 @@ float mc_interface_get_watt_hours(bool reset) {
  * The amount of watt hours fed back.
  */
 float mc_interface_get_watt_hours_charged(bool reset) {
-	mc_interface_update_selected_motor();
-	float val = m_motor_now->m_watt_seconds_charged / 3600;
+	float val = motor_now()->m_watt_seconds_charged / 3600;
 
 	if (reset) {
-		m_motor_now->m_watt_seconds_charged = 0.0;
+		motor_now()->m_watt_seconds_charged = 0.0;
 	}
 
 	return val;
 }
 
 float mc_interface_get_tot_current(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tot_current();
@@ -983,10 +953,9 @@ float mc_interface_get_tot_current(void) {
 }
 
 float mc_interface_get_tot_current_filtered(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tot_current_filtered();
@@ -1004,10 +973,9 @@ float mc_interface_get_tot_current_filtered(void) {
 }
 
 float mc_interface_get_tot_current_directional(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tot_current_directional();
@@ -1025,10 +993,9 @@ float mc_interface_get_tot_current_directional(void) {
 }
 
 float mc_interface_get_tot_current_directional_filtered(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tot_current_directional_filtered();
@@ -1046,10 +1013,9 @@ float mc_interface_get_tot_current_directional_filtered(void) {
 }
 
 float mc_interface_get_tot_current_in(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tot_current_in();
@@ -1067,10 +1033,9 @@ float mc_interface_get_tot_current_in(void) {
 }
 
 float mc_interface_get_tot_current_in_filtered(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tot_current_in_filtered();
@@ -1088,11 +1053,10 @@ float mc_interface_get_tot_current_in_filtered(void) {
 }
 
 float mc_interface_get_abs_motor_current_unbalance(void) {
-	mc_interface_update_selected_motor();
 	float ret = 0.0;
 
 #ifdef HW_HAS_3_SHUNTS
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		break;
@@ -1109,9 +1073,8 @@ float mc_interface_get_abs_motor_current_unbalance(void) {
 }
 
 int mc_interface_set_tachometer_value(int steps) {
-	mc_interface_update_selected_motor();
 	int ret = 0;
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_set_tachometer_value(DIR_MULT * steps);
@@ -1129,10 +1092,9 @@ int mc_interface_set_tachometer_value(int steps) {
 }
 
 int mc_interface_get_tachometer_value(bool reset) {
-	mc_interface_update_selected_motor();
 	int ret = 0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tachometer_value(reset);
@@ -1150,10 +1112,9 @@ int mc_interface_get_tachometer_value(bool reset) {
 }
 
 int mc_interface_get_tachometer_abs_value(bool reset) {
-	mc_interface_update_selected_motor();
 	int ret = 0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_tachometer_abs_value(reset);
@@ -1173,7 +1134,7 @@ int mc_interface_get_tachometer_abs_value(bool reset) {
 float mc_interface_get_last_inj_adc_isr_duration(void) {
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = mcpwm_get_last_inj_adc_isr_duration();
@@ -1195,28 +1156,24 @@ float mc_interface_get_last_inj_adc_isr_duration(void) {
 }
 
 float mc_interface_read_reset_avg_motor_current(void) {
-	mc_interface_update_selected_motor();
-
-	if (m_motor_now->m_conf.motor_type == MOTOR_TYPE_GPD) {
+	if (motor_now()->m_conf.motor_type == MOTOR_TYPE_GPD) {
 		return gpdrive_get_current_filtered();
 	}
 
-	float res = m_motor_now->m_motor_current_sum / m_motor_now->m_motor_current_iterations;
-	m_motor_now->m_motor_current_sum = 0.0;
-	m_motor_now->m_motor_current_iterations = 0.0;
+	float res = motor_now()->m_motor_current_sum / motor_now()->m_motor_current_iterations;
+	motor_now()->m_motor_current_sum = 0.0;
+	motor_now()->m_motor_current_iterations = 0.0;
 	return res;
 }
 
 float mc_interface_read_reset_avg_input_current(void) {
-	mc_interface_update_selected_motor();
-
-	if (m_motor_now->m_conf.motor_type == MOTOR_TYPE_GPD) {
+	if (motor_now()->m_conf.motor_type == MOTOR_TYPE_GPD) {
 		return gpdrive_get_current_filtered() * gpdrive_get_modulation();
 	}
 
-	float res = m_motor_now->m_input_current_sum / m_motor_now->m_input_current_iterations;
-	m_motor_now->m_input_current_sum = 0.0;
-	m_motor_now->m_input_current_iterations = 0.0;
+	float res = motor_now()->m_input_current_sum / motor_now()->m_input_current_iterations;
+	motor_now()->m_input_current_sum = 0.0;
+	motor_now()->m_input_current_iterations = 0.0;
 	return res;
 }
 
@@ -1227,11 +1184,9 @@ float mc_interface_read_reset_avg_input_current(void) {
  * The average D axis current.
  */
 float mc_interface_read_reset_avg_id(void) {
-	mc_interface_update_selected_motor();
-
-	float res = m_motor_now->m_motor_id_sum / m_motor_now->m_motor_id_iterations;
-	m_motor_now->m_motor_id_sum = 0.0;
-	m_motor_now->m_motor_id_iterations = 0.0;
+	float res = motor_now()->m_motor_id_sum / motor_now()->m_motor_id_iterations;
+	motor_now()->m_motor_id_sum = 0.0;
+	motor_now()->m_motor_id_iterations = 0.0;
 	return DIR_MULT * res; // TODO: DIR_MULT?
 }
 
@@ -1242,11 +1197,9 @@ float mc_interface_read_reset_avg_id(void) {
  * The average Q axis current.
  */
 float mc_interface_read_reset_avg_iq(void) {
-	mc_interface_update_selected_motor();
-
-	float res = m_motor_now->m_motor_iq_sum / m_motor_now->m_motor_iq_iterations;
-	m_motor_now->m_motor_iq_sum = 0.0;
-	m_motor_now->m_motor_iq_iterations = 0.0;
+	float res = motor_now()->m_motor_iq_sum / motor_now()->m_motor_iq_iterations;
+	motor_now()->m_motor_iq_sum = 0.0;
+	motor_now()->m_motor_iq_iterations = 0.0;
 	return DIR_MULT * res;
 }
 
@@ -1257,11 +1210,9 @@ float mc_interface_read_reset_avg_iq(void) {
  * The average D axis voltage.
  */
 float mc_interface_read_reset_avg_vd(void) {
-	mc_interface_update_selected_motor();
-
-	float res = m_motor_now->m_motor_vd_sum / m_motor_now->m_motor_vd_iterations;
-	m_motor_now->m_motor_vd_sum = 0.0;
-	m_motor_now->m_motor_vd_iterations = 0.0;
+	float res = motor_now()->m_motor_vd_sum / motor_now()->m_motor_vd_iterations;
+	motor_now()->m_motor_vd_sum = 0.0;
+	motor_now()->m_motor_vd_iterations = 0.0;
 	return DIR_MULT * res;
 }
 
@@ -1272,25 +1223,20 @@ float mc_interface_read_reset_avg_vd(void) {
  * The average Q axis voltage.
  */
 float mc_interface_read_reset_avg_vq(void) {
-	mc_interface_update_selected_motor();
-
-	float res = m_motor_now->m_motor_vq_sum / m_motor_now->m_motor_vq_iterations;
-	m_motor_now->m_motor_vq_sum = 0.0;
-	m_motor_now->m_motor_vq_iterations = 0.0;
+	float res = motor_now()->m_motor_vq_sum / motor_now()->m_motor_vq_iterations;
+	motor_now()->m_motor_vq_sum = 0.0;
+	motor_now()->m_motor_vq_iterations = 0.0;
 	return DIR_MULT * res;
 }
 
 float mc_interface_get_pid_pos_set(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_position_set;
+	return motor_now()->m_position_set;
 }
 
 float mc_interface_get_pid_pos_now(void) {
-	mc_interface_update_selected_motor();
-
 	float ret = 0.0;
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		ret = encoder_read_deg();
@@ -1315,8 +1261,6 @@ float mc_interface_get_last_sample_adc_isr_duration(void) {
 }
 
 void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation) {
-	mc_interface_update_selected_motor();
-
 	if (len > ADC_SAMPLE_MAX_LEN) {
 		len = ADC_SAMPLE_MAX_LEN;
 	}
@@ -1330,7 +1274,7 @@ void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint
 		m_sample_int = decimation;
 		m_sample_mode = mode;
 #ifdef HW_HAS_DUAL_MOTORS
-		m_sample_is_second_motor = m_motor_now == &m_motor_2;
+		m_sample_is_second_motor = motor_now() == &m_motor_2;
 #endif
 	}
 }
@@ -1343,8 +1287,7 @@ void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint
  * The filtered MOSFET temperature.
  */
 float mc_interface_temp_fet_filtered(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_temp_fet;
+	return motor_now()->m_temp_fet;
 }
 
 /**
@@ -1355,8 +1298,7 @@ float mc_interface_temp_fet_filtered(void) {
  * The filtered motor temperature.
  */
 float mc_interface_temp_motor_filtered(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_temp_motor;
+	return motor_now()->m_temp_motor;
 }
 
 /**
@@ -1499,25 +1441,23 @@ setup_values mc_interface_get_setup_values(void) {
  *
  */
 int mc_interface_try_input(void) {
-	mc_interface_update_selected_motor();
-
 	// TODO: Remove this later
 	if (mc_interface_get_state() == MC_STATE_DETECTING) {
 		mcpwm_stop_pwm();
-		m_motor_now->m_ignore_iterations = MCPWM_DETECT_STOP_TIME;
+		motor_now()->m_ignore_iterations = MCPWM_DETECT_STOP_TIME;
 	}
 
-	int retval = m_motor_now->m_ignore_iterations;
+	int retval = motor_now()->m_ignore_iterations;
 
-	if (!m_motor_now->m_ignore_iterations && m_motor_now->m_lock_enabled) {
-		if (!m_motor_now->m_lock_override_once) {
+	if (!motor_now()->m_ignore_iterations && motor_now()->m_lock_enabled) {
+		if (!motor_now()->m_lock_override_once) {
 			retval = 1;
 		} else {
-			m_motor_now->m_lock_override_once = false;
+			motor_now()->m_lock_override_once = false;
 		}
 	}
 
-	switch (m_motor_now->m_conf.motor_type) {
+	switch (motor_now()->m_conf.motor_type) {
 	case MOTOR_TYPE_BLDC:
 	case MOTOR_TYPE_DC:
 		if (!mcpwm_init_done()) {
@@ -1578,7 +1518,6 @@ void mc_interface_fault_stop(mc_fault_code fault, bool is_second_motor) {
 		fdata.temperature = NTC_TEMP(ADC_IND_TEMP_MOS);
 #ifdef HW_HAS_DRV8301
 		if (fault == FAULT_CODE_DRV) {
-			drv8301_select_second_motor(is_second_motor);
 			fdata.drv8301_faults = drv8301_read_faults();
 		}
 #elif defined(HW_HAS_DRV8320S)
@@ -1621,8 +1560,6 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
 
 #ifdef HW_HAS_DUAL_MOTORS
 	volatile motor_if_state_t *motor = is_second_motor ? &m_motor_2 : &m_motor_1;
-	volatile motor_if_state_t *motor_last = m_motor_now;
-	m_motor_now = motor;
 #else
 	volatile motor_if_state_t *motor = &m_motor_1;
 	(void)is_second_motor;
@@ -1914,10 +1851,6 @@ void mc_interface_mc_timer_isr(bool is_second_motor) {
 			m_last_adc_duration_sample = mc_interface_get_last_inj_adc_isr_duration();
 		}
 	}
-
-#ifdef HW_HAS_DUAL_MOTORS
-	m_motor_now = motor_last;
-#endif
 }
 
 void mc_interface_adc_inj_int_handler(void) {
@@ -2154,6 +2087,14 @@ static void update_override_limits(volatile motor_if_state_t *motor, volatile mc
 	conf->lo_current_motor_min_now = conf->lo_current_min;
 }
 
+static volatile motor_if_state_t *motor_now(void) {
+#ifdef HW_HAS_DUAL_MOTORS
+	return mc_interface_motor_now() == 1 ? &m_motor_1 : &m_motor_2;
+#else
+	return &m_motor_1;
+#endif
+}
+
 static void run_timer_tasks(volatile motor_if_state_t *motor) {
 	bool is_motor_1 = motor == &m_motor_1;
 

mc_interface.h

@@ -25,9 +25,9 @@
 
 // Functions
 void mc_interface_init(void);
+int mc_interface_motor_now(void);
 void mc_interface_select_motor_thread(int motor);
 int mc_interface_get_motor_thread(void);
-void mc_interface_update_selected_motor(void);
 const volatile mc_configuration* mc_interface_get_configuration(void);
 void mc_interface_set_configuration(mc_configuration *configuration);
 void mc_interface_set_pwm_callback(void (*p_func)(void));

mcpwm_foc.c

@@ -170,8 +170,7 @@ static volatile motor_all_state_t m_motor_1;
 #ifdef HW_HAS_DUAL_MOTORS
 static volatile motor_all_state_t m_motor_2;
 #endif
-static volatile motor_all_state_t *m_motor_now;
-static volatile bool m_is_isr = false;
+static volatile int m_isr_motor = 0;
 
 // Private functions
 static void do_dc_cal(void);
@@ -494,8 +493,7 @@ void mcpwm_foc_init(volatile mc_configuration *conf_m1, volatile mc_configuratio
 
 	// Initialize variables
 	memset((void*)&m_motor_1, 0, sizeof(motor_all_state_t));
-	m_motor_now = &m_motor_1;
-	m_is_isr = false;
+	m_isr_motor = 0;
 
 	m_motor_1.m_conf = conf_m1;
 	m_motor_1.m_state = MC_STATE_OFF;
@@ -664,14 +662,12 @@ void mcpwm_foc_deinit(void) {
 	dmaStreamRelease(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)));
 }
 
-void mcpwm_foc_select_second_motor(bool select_second_motor) {
-	if (select_second_motor) {
+static volatile motor_all_state_t *motor_now(void) {
 #ifdef HW_HAS_DUAL_MOTORS
-		m_motor_now = &m_motor_2;
+	return mc_interface_motor_now() == 1 ? &m_motor_1 : &m_motor_2;
+#else
+	return &m_motor_1;
 #endif
-	} else {
-		m_motor_now = &m_motor_1;
-	}
 }
 
 bool mcpwm_foc_init_done(void) {
@@ -679,9 +675,7 @@ bool mcpwm_foc_init_done(void) {
 }
 
 void mcpwm_foc_set_configuration(volatile mc_configuration *configuration) {
-	mc_interface_update_selected_motor();
-
-	m_motor_now->m_conf = configuration;
+	motor_now()->m_conf = configuration;
 
 	// Below we check if anything in the configuration changed that requires stopping the motor.
 
@@ -698,32 +692,39 @@ void mcpwm_foc_set_configuration(volatile mc_configuration *configuration) {
 
 		timer_reinit((int)configuration->foc_f_sw);
 #else
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		TIMER_UPDATE_SAMP_TOP_M1(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
 #endif
 	}
 
-	if (((1 << m_motor_now->m_conf->foc_hfi_samples) * 8) != m_motor_now->m_hfi.samples) {
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+	if (((1 << motor_now()->m_conf->foc_hfi_samples) * 8) != motor_now()->m_hfi.samples) {
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		update_hfi_samples(m_motor_1.m_conf->foc_hfi_samples, &m_motor_1);
 	}
 }
 
 mc_state mcpwm_foc_get_state(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_state;
+	return motor_now()->m_state;
 }
 
 bool mcpwm_foc_is_dccal_done(void) {
 	return m_dccal_done;
 }
 
-bool mcpwm_foc_is_isr(void) {
-	return m_is_isr;
+/**
+ * Get the current motor used in the mcpwm ISR
+ *
+ * @return
+ * 0: Not in ISR
+ * 1: Motor 1
+ * 2: Motor 2
+ */
+int mcpwm_foc_isr_motor(void) {
+	return m_isr_motor;
 }
 
 /**
@@ -750,12 +751,11 @@ void mcpwm_foc_stop_pwm(bool is_second_motor) {
  * The duty cycle to use.
  */
 void mcpwm_foc_set_duty(float dutyCycle) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_control_mode = CONTROL_MODE_DUTY;
-	m_motor_now->m_duty_cycle_set = dutyCycle;
+	motor_now()->m_control_mode = CONTROL_MODE_DUTY;
+	motor_now()->m_duty_cycle_set = dutyCycle;
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -782,12 +782,11 @@ void mcpwm_foc_set_duty_noramp(float dutyCycle) {
  * The electrical RPM goal value to use.
  */
 void mcpwm_foc_set_pid_speed(float rpm) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_control_mode = CONTROL_MODE_SPEED;
-	m_motor_now->m_speed_pid_set_rpm = rpm;
+	motor_now()->m_control_mode = CONTROL_MODE_SPEED;
+	motor_now()->m_speed_pid_set_rpm = rpm;
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -799,12 +798,11 @@ void mcpwm_foc_set_pid_speed(float rpm) {
  * The desired position of the motor in degrees.
  */
 void mcpwm_foc_set_pid_pos(float pos) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_control_mode = CONTROL_MODE_POS;
-	m_motor_now->m_pos_pid_set = pos;
+	motor_now()->m_control_mode = CONTROL_MODE_POS;
+	motor_now()->m_pos_pid_set = pos;
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -817,19 +815,18 @@ void mcpwm_foc_set_pid_pos(float pos) {
  * The current to use.
  */
 void mcpwm_foc_set_current(float current) {
-	mc_interface_update_selected_motor();
-	if (fabsf(current) < m_motor_now->m_conf->cc_min_current) {
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+	if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		return;
 	}
 
-	m_motor_now->m_control_mode = CONTROL_MODE_CURRENT;
-	m_motor_now->m_iq_set = current;
+	motor_now()->m_control_mode = CONTROL_MODE_CURRENT;
+	motor_now()->m_iq_set = current;
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -841,19 +838,18 @@ void mcpwm_foc_set_current(float current) {
  * The current to use. Positive and negative values give the same effect.
  */
 void mcpwm_foc_set_brake_current(float current) {
-	mc_interface_update_selected_motor();
-	if (fabsf(current) < m_motor_now->m_conf->cc_min_current) {
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+	if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		return;
 	}
 
-	m_motor_now->m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
-	m_motor_now->m_iq_set = current;
+	motor_now()->m_control_mode = CONTROL_MODE_CURRENT_BRAKE;
+	motor_now()->m_iq_set = current;
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -865,19 +861,18 @@ void mcpwm_foc_set_brake_current(float current) {
  * The brake current to use.
  */
 void mcpwm_foc_set_handbrake(float current) {
-	mc_interface_update_selected_motor();
-	if (fabsf(current) < m_motor_now->m_conf->cc_min_current) {
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+	if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		return;
 	}
 
-	m_motor_now->m_control_mode = CONTROL_MODE_HANDBRAKE;
-	m_motor_now->m_iq_set = current;
+	motor_now()->m_control_mode = CONTROL_MODE_HANDBRAKE;
+	motor_now()->m_iq_set = current;
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -891,23 +886,22 @@ void mcpwm_foc_set_handbrake(float current) {
  * The RPM to use.
  */
 void mcpwm_foc_set_openloop(float current, float rpm) {
-	mc_interface_update_selected_motor();
-	if (fabsf(current) < m_motor_now->m_conf->cc_min_current) {
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+	if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		return;
 	}
 
-	utils_truncate_number(&current, -m_motor_now->m_conf->l_current_max * m_motor_now->m_conf->l_current_max_scale,
-			m_motor_now->m_conf->l_current_max * m_motor_now->m_conf->l_current_max_scale);
+	utils_truncate_number(&current, -motor_now()->m_conf->l_current_max * motor_now()->m_conf->l_current_max_scale,
+			motor_now()->m_conf->l_current_max * motor_now()->m_conf->l_current_max_scale);
 
-	m_motor_now->m_control_mode = CONTROL_MODE_OPENLOOP;
-	m_motor_now->m_iq_set = current;
-	m_motor_now->m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
+	motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP;
+	motor_now()->m_iq_set = current;
+	motor_now()->m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -921,25 +915,24 @@ void mcpwm_foc_set_openloop(float current, float rpm) {
  * The phase to use in degrees, range [0.0 360.0]
  */
 void mcpwm_foc_set_openloop_phase(float current, float phase) {
-	mc_interface_update_selected_motor();
-	if (fabsf(current) < m_motor_now->m_conf->cc_min_current) {
-		m_motor_now->m_control_mode = CONTROL_MODE_NONE;
-		m_motor_now->m_state = MC_STATE_OFF;
-		stop_pwm_hw(m_motor_now);
+	if (fabsf(current) < motor_now()->m_conf->cc_min_current) {
+		motor_now()->m_control_mode = CONTROL_MODE_NONE;
+		motor_now()->m_state = MC_STATE_OFF;
+		stop_pwm_hw(motor_now());
 		return;
 	}
 
-	utils_truncate_number(&current, -m_motor_now->m_conf->l_current_max * m_motor_now->m_conf->l_current_max_scale,
-			m_motor_now->m_conf->l_current_max * m_motor_now->m_conf->l_current_max_scale);
+	utils_truncate_number(&current, -motor_now()->m_conf->l_current_max * motor_now()->m_conf->l_current_max_scale,
+			motor_now()->m_conf->l_current_max * motor_now()->m_conf->l_current_max_scale);
 
-	m_motor_now->m_control_mode = CONTROL_MODE_OPENLOOP_PHASE;
-	m_motor_now->m_iq_set = current;
+	motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_PHASE;
+	motor_now()->m_iq_set = current;
 
-	m_motor_now->m_openloop_phase = phase * M_PI / 180.0;
-	utils_norm_angle_rad((float*)&m_motor_now->m_openloop_phase);
+	motor_now()->m_openloop_phase = phase * M_PI / 180.0;
+	utils_norm_angle_rad((float*)&motor_now()->m_openloop_phase);
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -951,10 +944,9 @@ void mcpwm_foc_set_openloop_phase(float current, float phase) {
 void mcpwm_foc_set_current_offsets(volatile int curr0_offset,
 									volatile int curr1_offset,
 									volatile int curr2_offset) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_curr_ofs[0] = curr0_offset;
-	m_motor_now->m_curr_ofs[1] = curr1_offset;
-	m_motor_now->m_curr_ofs[2] = curr2_offset;
+	motor_now()->m_curr_ofs[0] = curr0_offset;
+	motor_now()->m_curr_ofs[1] = curr1_offset;
+	motor_now()->m_curr_ofs[2] = curr2_offset;
 }
 
 /**
@@ -967,13 +959,12 @@ void mcpwm_foc_set_current_offsets(volatile int curr0_offset,
  * The RPM to use.
  */
 void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY;
-	m_motor_now->m_duty_cycle_set = dutyCycle;
-	m_motor_now->m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
+	motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY;
+	motor_now()->m_duty_cycle_set = dutyCycle;
+	motor_now()->m_openloop_speed = rpm * ((2.0 * M_PI) / 60.0);
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
@@ -987,35 +978,30 @@ void mcpwm_foc_set_openloop_duty(float dutyCycle, float rpm) {
  * The phase to use in degrees, range [0.0 360.0]
  */
 void mcpwm_foc_set_openloop_duty_phase(float dutyCycle, float phase) {
-	mc_interface_update_selected_motor();
-	m_motor_now->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY_PHASE;
-	m_motor_now->m_duty_cycle_set = dutyCycle;
-	m_motor_now->m_openloop_phase = phase * M_PI / 180.0;
-	utils_norm_angle_rad((float*)&m_motor_now->m_openloop_phase);
+	motor_now()->m_control_mode = CONTROL_MODE_OPENLOOP_DUTY_PHASE;
+	motor_now()->m_duty_cycle_set = dutyCycle;
+	motor_now()->m_openloop_phase = phase * M_PI / 180.0;
+	utils_norm_angle_rad((float*)&motor_now()->m_openloop_phase);
 
-	if (m_motor_now->m_state != MC_STATE_RUNNING) {
-		m_motor_now->m_state = MC_STATE_RUNNING;
+	if (motor_now()->m_state != MC_STATE_RUNNING) {
+		motor_now()->m_state = MC_STATE_RUNNING;
 	}
 }
 
 float mcpwm_foc_get_duty_cycle_set(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_duty_cycle_set;
+	return motor_now()->m_duty_cycle_set;
 }
 
 float mcpwm_foc_get_duty_cycle_now(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.duty_now;
+	return motor_now()->m_motor_state.duty_now;
 }
 
 float mcpwm_foc_get_pid_pos_set(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_pos_pid_set;
+	return motor_now()->m_pos_pid_set;
 }
 
 float mcpwm_foc_get_pid_pos_now(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_pos_pid_now;
+	return motor_now()->m_pos_pid_now;
 }
 
 /**
@@ -1025,8 +1011,7 @@ float mcpwm_foc_get_pid_pos_now(void) {
  * The switching frequency in Hz.
  */
 float mcpwm_foc_get_switching_frequency_now(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_conf->foc_f_sw;
+	return motor_now()->m_conf->foc_f_sw;
 }
 
 /**
@@ -1036,15 +1021,14 @@ float mcpwm_foc_get_switching_frequency_now(void) {
  * The sampling frequency in Hz.
  */
 float mcpwm_foc_get_sampling_frequency_now(void) {
-	mc_interface_update_selected_motor();
 #ifdef HW_HAS_PHASE_SHUNTS
-	if (m_motor_now->m_conf->foc_sample_v0_v7) {
-		return m_motor_now->m_conf->foc_f_sw;
+	if (motor_now()->m_conf->foc_sample_v0_v7) {
+		return motor_now()->m_conf->foc_f_sw;
 	} else {
-		return m_motor_now->m_conf->foc_f_sw / 2.0;
+		return motor_now()->m_conf->foc_f_sw / 2.0;
 	}
 #else
-	return m_motor_now->m_conf->foc_f_sw / 2.0;
+	return motor_now()->m_conf->foc_f_sw / 2.0;
 #endif
 }
 
@@ -1052,21 +1036,19 @@ float mcpwm_foc_get_sampling_frequency_now(void) {
  * Returns Ts used for virtual motor sync
  */
 float mcpwm_foc_get_ts(void) {
-	mc_interface_update_selected_motor();
 #ifdef HW_HAS_PHASE_SHUNTS
-	if (m_motor_now->m_conf->foc_sample_v0_v7) {
-		return (1.0 / m_motor_now->m_conf->foc_f_sw) ;
+	if (motor_now()->m_conf->foc_sample_v0_v7) {
+		return (1.0 / motor_now()->m_conf->foc_f_sw) ;
 	} else {
-		return (1.0 / (m_motor_now->m_conf->foc_f_sw / 2.0));
+		return (1.0 / (motor_now()->m_conf->foc_f_sw / 2.0));
 	}
 #else
-	return (1.0 / m_motor_now->m_conf->foc_f_sw) ;
+	return (1.0 / motor_now()->m_conf->foc_f_sw) ;
 #endif
 }
 
 bool mcpwm_foc_is_using_encoder(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_using_encoder;
+	return motor_now()->m_using_encoder;
 }
 
 /**
@@ -1078,9 +1060,8 @@ bool mcpwm_foc_is_using_encoder(void) {
  * The RPM value.
  */
 float mcpwm_foc_get_rpm(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.speed_rad_s / ((2.0 * M_PI) / 60.0);
-//	return m_motor_now->m_speed_est_fast / ((2.0 * M_PI) / 60.0);
+	return motor_now()->m_motor_state.speed_rad_s / ((2.0 * M_PI) / 60.0);
+//	return motor_now()->m_speed_est_fast / ((2.0 * M_PI) / 60.0);
 }
 
 /**
@@ -1092,8 +1073,7 @@ float mcpwm_foc_get_rpm(void) {
  * The motor current.
  */
 float mcpwm_foc_get_tot_current(void) {
-	mc_interface_update_selected_motor();
-	return SIGN(m_motor_now->m_motor_state.vq) * m_motor_now->m_motor_state.iq;
+	return SIGN(motor_now()->m_motor_state.vq) * motor_now()->m_motor_state.iq;
 }
 
 /**
@@ -1105,8 +1085,7 @@ float mcpwm_foc_get_tot_current(void) {
  * The filtered motor current.
  */
 float mcpwm_foc_get_tot_current_filtered(void) {
-	mc_interface_update_selected_motor();
-	return SIGN(m_motor_now->m_motor_state.vq) * m_motor_now->m_motor_state.iq_filter;
+	return SIGN(motor_now()->m_motor_state.vq) * motor_now()->m_motor_state.iq_filter;
 }
 
 /**
@@ -1117,8 +1096,7 @@ float mcpwm_foc_get_tot_current_filtered(void) {
  * The magnitude of the motor current.
  */
 float mcpwm_foc_get_abs_motor_current(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.i_abs;
+	return motor_now()->m_motor_state.i_abs;
 }
 
 /**
@@ -1128,8 +1106,7 @@ float mcpwm_foc_get_abs_motor_current(void) {
  * The magnitude of the phase currents unbalance.
  */
 float mcpwm_foc_get_abs_motor_current_unbalance(void) {
-	mc_interface_update_selected_motor();
-	return (float)(m_motor_now->m_curr_unbalance) * FAC_CURRENT;
+	return (float)(motor_now()->m_curr_unbalance) * FAC_CURRENT;
 }
 
 /**
@@ -1139,9 +1116,8 @@ float mcpwm_foc_get_abs_motor_current_unbalance(void) {
  * The magnitude of the motor voltage.
  */
 float mcpwm_foc_get_abs_motor_voltage(void) {
-	mc_interface_update_selected_motor();
-	const float vd_tmp = m_motor_now->m_motor_state.vd;
-	const float vq_tmp = m_motor_now->m_motor_state.vq;
+	const float vd_tmp = motor_now()->m_motor_state.vd;
+	const float vq_tmp = motor_now()->m_motor_state.vq;
 	return sqrtf(SQ(vd_tmp) + SQ(vq_tmp));
 }
 
@@ -1153,8 +1129,7 @@ float mcpwm_foc_get_abs_motor_voltage(void) {
  * The magnitude of the motor current.
  */
 float mcpwm_foc_get_abs_motor_current_filtered(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.i_abs_filter;
+	return motor_now()->m_motor_state.i_abs_filter;
 }
 
 /**
@@ -1165,8 +1140,7 @@ float mcpwm_foc_get_abs_motor_current_filtered(void) {
  * The motor current.
  */
 float mcpwm_foc_get_tot_current_directional(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.iq;
+	return motor_now()->m_motor_state.iq;
 }
 
 /**
@@ -1177,8 +1151,7 @@ float mcpwm_foc_get_tot_current_directional(void) {
  * The filtered motor current.
  */
 float mcpwm_foc_get_tot_current_directional_filtered(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.iq_filter;
+	return motor_now()->m_motor_state.iq_filter;
 }
 
 /**
@@ -1188,8 +1161,7 @@ float mcpwm_foc_get_tot_current_directional_filtered(void) {
  * The D axis current.
  */
 float mcpwm_foc_get_id(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.id;
+	return motor_now()->m_motor_state.id;
 }
 
 /**
@@ -1199,8 +1171,7 @@ float mcpwm_foc_get_id(void) {
  * The Q axis current.
  */
 float mcpwm_foc_get_iq(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.iq;
+	return motor_now()->m_motor_state.iq;
 }
 
 /**
@@ -1210,8 +1181,7 @@ float mcpwm_foc_get_iq(void) {
  * The input current.
  */
 float mcpwm_foc_get_tot_current_in(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.i_bus;
+	return motor_now()->m_motor_state.i_bus;
 }
 
 /**
@@ -1221,8 +1191,7 @@ float mcpwm_foc_get_tot_current_in(void) {
  * The filtered input current.
  */
 float mcpwm_foc_get_tot_current_in_filtered(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.i_bus; // TODO: Calculate filtered current?
+	return motor_now()->m_motor_state.i_bus; // TODO: Calculate filtered current?
 }
 
 /**
@@ -1237,9 +1206,8 @@ float mcpwm_foc_get_tot_current_in_filtered(void) {
  * be this number divided by (3 * MOTOR_POLE_NUMBER).
  */
 int mcpwm_foc_set_tachometer_value(int steps) {
-	mc_interface_update_selected_motor();
-	int val = m_motor_now->m_tachometer;
-	m_motor_now->m_tachometer = steps;
+	int val = motor_now()->m_tachometer;
+	motor_now()->m_tachometer = steps;
 	return val;
 }
 
@@ -1255,11 +1223,10 @@ int mcpwm_foc_set_tachometer_value(int steps) {
  * be this number divided by (3 * MOTOR_POLE_NUMBER).
  */
 int mcpwm_foc_get_tachometer_value(bool reset) {
-	mc_interface_update_selected_motor();
-	int val = m_motor_now->m_tachometer;
+	int val = motor_now()->m_tachometer;
 
 	if (reset) {
-		m_motor_now->m_tachometer = 0;
+		motor_now()->m_tachometer = 0;
 	}
 
 	return val;
@@ -1276,11 +1243,10 @@ int mcpwm_foc_get_tachometer_value(bool reset) {
  * be this number divided by (3 * MOTOR_POLE_NUMBER).
  */
 int mcpwm_foc_get_tachometer_abs_value(bool reset) {
-	mc_interface_update_selected_motor();
-	int val = m_motor_now->m_tachometer_abs;
+	int val = motor_now()->m_tachometer_abs;
 
 	if (reset) {
-		m_motor_now->m_tachometer_abs = 0;
+		motor_now()->m_tachometer_abs = 0;
 	}
 
 	return val;
@@ -1293,8 +1259,7 @@ int mcpwm_foc_get_tachometer_abs_value(bool reset) {
  * The phase angle in degrees.
  */
 float mcpwm_foc_get_phase(void) {
-	mc_interface_update_selected_motor();
-	float angle = m_motor_now->m_motor_state.phase * (180.0 / M_PI);
+	float angle = motor_now()->m_motor_state.phase * (180.0 / M_PI);
 	utils_norm_angle(&angle);
 	return angle;
 }
@@ -1306,8 +1271,7 @@ float mcpwm_foc_get_phase(void) {
  * The phase angle in degrees.
  */
 float mcpwm_foc_get_phase_observer(void) {
-	mc_interface_update_selected_motor();
-	float angle = m_motor_now->m_phase_now_observer * (180.0 / M_PI);
+	float angle = motor_now()->m_phase_now_observer * (180.0 / M_PI);
 	utils_norm_angle(&angle);
 	return angle;
 }
@@ -1319,20 +1283,17 @@ float mcpwm_foc_get_phase_observer(void) {
  * The phase angle in degrees.
  */
 float mcpwm_foc_get_phase_encoder(void) {
-	mc_interface_update_selected_motor();
-	float angle = m_motor_now->m_phase_now_encoder * (180.0 / M_PI);
+	float angle = motor_now()->m_phase_now_encoder * (180.0 / M_PI);
 	utils_norm_angle(&angle);
 	return angle;
 }
 
 float mcpwm_foc_get_vd(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.vd;
+	return motor_now()->m_motor_state.vd;
 }
 
 float mcpwm_foc_get_vq(void) {
-	mc_interface_update_selected_motor();
-	return m_motor_now->m_motor_state.vq;
+	return motor_now()->m_motor_state.vq;
 }
 
 /**
@@ -1345,7 +1306,6 @@ void mcpwm_foc_get_current_offsets(
 		volatile int *curr1_offset,
 		volatile int *curr2_offset,
 		bool is_second_motor) {
-	mc_interface_update_selected_motor();
 #ifdef HW_HAS_DUAL_MOTORS
 	volatile motor_all_state_t *motor = is_second_motor ? &m_motor_1 : &m_motor_2;
 #else
@@ -1373,10 +1333,9 @@ void mcpwm_foc_get_current_offsets(
  * The detected direction.
  */
 void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted) {
-	mc_interface_update_selected_motor();
 	mc_interface_lock();
 
-	volatile motor_all_state_t *motor = m_motor_now;
+	volatile motor_all_state_t *motor = motor_now();
 
 	motor->m_phase_override = true;
 	motor->m_id_set = current;
@@ -1605,10 +1564,9 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
  * The calculated motor resistance.
  */
 float mcpwm_foc_measure_resistance(float current, int samples) {
-	mc_interface_update_selected_motor();
 	mc_interface_lock();
 
-	volatile motor_all_state_t *motor = m_motor_now;
+	volatile motor_all_state_t *motor = motor_now();
 
 	motor->m_phase_override = true;
 	motor->m_phase_now_override = 0.0;
@@ -1690,8 +1648,7 @@ float mcpwm_foc_measure_resistance(float current, int samples) {
  * The average d and q axis inductance in uH.
  */
 float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *ld_lq_diff) {
-	mc_interface_update_selected_motor();
-	volatile motor_all_state_t *motor = m_motor_now;
+	volatile motor_all_state_t *motor = motor_now();
 
 	mc_sensor_mode sensor_mode_old = motor->m_conf->sensor_mode;
 	float f_sw_old = motor->m_conf->foc_f_sw;
@@ -1722,7 +1679,6 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr, float *
 	chThdSleepMilliseconds(1);
 
 	timeout_reset();
-	mc_interface_update_selected_motor();
 	mcpwm_foc_set_duty(0.0);
 	chThdSleepMilliseconds(1);
 
@@ -1839,8 +1795,7 @@ float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *
  * True if the measurement succeeded, false otherwise.
  */
 bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
-	mc_interface_update_selected_motor();
-	volatile motor_all_state_t *motor = m_motor_now;
+	volatile motor_all_state_t *motor = motor_now();
 
 	const float f_sw_old = motor->m_conf->foc_f_sw;
 	const float kp_old = motor->m_conf->foc_current_kp;
@@ -1899,8 +1854,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
  * false: Something went wrong
  */
 bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
-	mc_interface_update_selected_motor();
-	volatile motor_all_state_t *motor = m_motor_now;
+	volatile motor_all_state_t *motor = motor_now();
 
 	mc_interface_lock();
 
@@ -1985,26 +1939,26 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
 }
 
 void mcpwm_foc_print_state(void) {
-	commands_printf("Mod d:        %.2f", (double)m_motor_now->m_motor_state.mod_d);
-	commands_printf("Mod q:        %.2f", (double)m_motor_now->m_motor_state.mod_q);
-	commands_printf("Duty:         %.2f", (double)m_motor_now->m_motor_state.duty_now);
-	commands_printf("Vd:           %.2f", (double)m_motor_now->m_motor_state.vd);
-	commands_printf("Vq:           %.2f", (double)m_motor_now->m_motor_state.vq);
-	commands_printf("Phase:        %.2f", (double)m_motor_now->m_motor_state.phase);
-	commands_printf("V_alpha:      %.2f", (double)m_motor_now->m_motor_state.v_alpha);
-	commands_printf("V_beta:       %.2f", (double)m_motor_now->m_motor_state.v_beta);
-	commands_printf("id:           %.2f", (double)m_motor_now->m_motor_state.id);
-	commands_printf("iq:           %.2f", (double)m_motor_now->m_motor_state.iq);
-	commands_printf("id_filter:    %.2f", (double)m_motor_now->m_motor_state.id_filter);
-	commands_printf("iq_filter:    %.2f", (double)m_motor_now->m_motor_state.iq_filter);
-	commands_printf("id_target:    %.2f", (double)m_motor_now->m_motor_state.id_target);
-	commands_printf("iq_target:    %.2f", (double)m_motor_now->m_motor_state.iq_target);
-	commands_printf("i_abs:        %.2f", (double)m_motor_now->m_motor_state.i_abs);
-	commands_printf("i_abs_filter: %.2f", (double)m_motor_now->m_motor_state.i_abs_filter);
-	commands_printf("Obs_x1:       %.2f", (double)m_motor_now->m_observer_x1);
-	commands_printf("Obs_x2:       %.2f", (double)m_motor_now->m_observer_x2);
-	commands_printf("vd_int:       %.2f", (double)m_motor_now->m_motor_state.vd_int);
-	commands_printf("vq_int:       %.2f", (double)m_motor_now->m_motor_state.vq_int);
+	commands_printf("Mod d:        %.2f", (double)motor_now()->m_motor_state.mod_d);
+	commands_printf("Mod q:        %.2f", (double)motor_now()->m_motor_state.mod_q);
+	commands_printf("Duty:         %.2f", (double)motor_now()->m_motor_state.duty_now);
+	commands_printf("Vd:           %.2f", (double)motor_now()->m_motor_state.vd);
+	commands_printf("Vq:           %.2f", (double)motor_now()->m_motor_state.vq);
+	commands_printf("Phase:        %.2f", (double)motor_now()->m_motor_state.phase);
+	commands_printf("V_alpha:      %.2f", (double)motor_now()->m_motor_state.v_alpha);
+	commands_printf("V_beta:       %.2f", (double)motor_now()->m_motor_state.v_beta);
+	commands_printf("id:           %.2f", (double)motor_now()->m_motor_state.id);
+	commands_printf("iq:           %.2f", (double)motor_now()->m_motor_state.iq);
+	commands_printf("id_filter:    %.2f", (double)motor_now()->m_motor_state.id_filter);
+	commands_printf("iq_filter:    %.2f", (double)motor_now()->m_motor_state.iq_filter);
+	commands_printf("id_target:    %.2f", (double)motor_now()->m_motor_state.id_target);
+	commands_printf("iq_target:    %.2f", (double)motor_now()->m_motor_state.iq_target);
+	commands_printf("i_abs:        %.2f", (double)motor_now()->m_motor_state.i_abs);
+	commands_printf("i_abs_filter: %.2f", (double)motor_now()->m_motor_state.i_abs_filter);
+	commands_printf("Obs_x1:       %.2f", (double)motor_now()->m_observer_x1);
+	commands_printf("Obs_x2:       %.2f", (double)motor_now()->m_observer_x2);
+	commands_printf("vd_int:       %.2f", (double)motor_now()->m_motor_state.vd_int);
+	commands_printf("vq_int:       %.2f", (double)motor_now()->m_motor_state.vq_int);
 }
 
 float mcpwm_foc_get_last_adc_isr_duration(void) {
@@ -2017,7 +1971,9 @@ void mcpwm_foc_tim_sample_int_handler(void) {
 		TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
 		TIM_GenerateEvent(TIM8, TIM_EventSource_COM);
 
-		virtual_motor_int_handler(m_motor_now->m_motor_state.v_alpha, m_motor_now->m_motor_state.v_beta);
+		virtual_motor_int_handler(
+				m_motor_1.m_motor_state.v_alpha,
+				m_motor_1.m_motor_state.v_beta);
 	}
 }
 
@@ -2032,7 +1988,6 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 		return;
 	}
 
-	m_is_isr = true;
 	bool is_v7 = !(TIM1->CR1 & TIM_CR1_DIR);
 	int norm_curr_ofs = 0;
 
@@ -2040,14 +1995,17 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	bool is_second_motor = is_v7;
 	norm_curr_ofs = is_second_motor ? 3 : 0;
 	volatile motor_all_state_t *motor_other = is_second_motor ? &m_motor_1 : &m_motor_2;
-	volatile motor_all_state_t *motor_now_last = m_motor_now;
-	m_motor_now = is_second_motor ? &m_motor_2 : &m_motor_1;
+	volatile motor_all_state_t *motor_now_last = motor_now();
+	volatile motor_all_state_t *m_motor_now = is_second_motor ? &m_motor_2 : &m_motor_1;
+	m_isr_motor = is_second_motor ? 2 : 1;
 #ifdef HW_HAS_3_SHUNTS
 	volatile TIM_TypeDef *tim = is_second_motor ? TIM8 : TIM1;
 #endif
 #else
 	volatile motor_all_state_t *motor = &m_motor_1;
 	volatile motor_all_state_t *motor_other = &m_motor_1;
+	volatile motor_all_state_t *m_motor_now = &m_motor_1;;
+	m_isr_motor = 1;
 #ifdef HW_HAS_3_SHUNTS
 	volatile TIM_TypeDef *tim = TIM1;
 #endif
@@ -2682,7 +2640,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	m_motor_now = motor_now_last;
 #endif
 
-	m_is_isr = false;
+	m_isr_motor = 0;
 	m_last_adc_isr_duration = timer_seconds_elapsed_since(t_start);
 }
 
@@ -3785,23 +3743,23 @@ static void terminal_plot_hfi(int argc, const char **argv) {
 		sscanf(argv[1], "%d", &d);
 
 		if (d == 0 || d == 1 || d == 2) {
-			m_motor_now->m_hfi_plot_en = d;
-			if (m_motor_now->m_hfi_plot_en == 1) {
-				m_motor_now->m_hfi_plot_sample = 0.0;
+			motor_now()->m_hfi_plot_en = d;
+			if (motor_now()->m_hfi_plot_en == 1) {
+				motor_now()->m_hfi_plot_sample = 0.0;
 				commands_init_plot("Sample", "Value");
 				commands_plot_add_graph("Phase");
 				commands_plot_add_graph("Phase bin2");
 				commands_plot_add_graph("Ld - Lq (uH");
 				commands_plot_add_graph("L Diff Sat (uH)");
 				commands_plot_add_graph("L Avg (uH)");
-			} else if (m_motor_now->m_hfi_plot_en == 2) {
-				m_motor_now->m_hfi_plot_sample = 0.0;
+			} else if (motor_now()->m_hfi_plot_en == 2) {
+				motor_now()->m_hfi_plot_sample = 0.0;
 				commands_init_plot("Sample Index", "Value");
 				commands_plot_add_graph("Current (A)");
 				commands_plot_add_graph("Inductance (uH)");
 			}
 
-			commands_printf(m_motor_now->m_hfi_plot_en ?
+			commands_printf(motor_now()->m_hfi_plot_en ?
 						"HFI plot enabled" :
 						"HFI plot disabled");
 		} else {

mcpwm_foc.h

@@ -27,12 +27,11 @@
 // Functions
 void mcpwm_foc_init(volatile mc_configuration *conf_m1, volatile mc_configuration *conf_m2);
 void mcpwm_foc_deinit(void);
-void mcpwm_foc_select_second_motor(bool select_second_motor);
 bool mcpwm_foc_init_done(void);
 void mcpwm_foc_set_configuration(volatile mc_configuration *configuration);
 mc_state mcpwm_foc_get_state(void);
 bool mcpwm_foc_is_dccal_done(void);
-bool mcpwm_foc_is_isr(void);
+int mcpwm_foc_isr_motor(void);
 void mcpwm_foc_stop_pwm(bool is_second_motor);
 void mcpwm_foc_set_duty(float dutyCycle);
 void mcpwm_foc_set_duty_noramp(float dutyCycle);