diff --git a/encoder/enc_abi.c b/encoder/enc_abi.c
index 7d6b294c..538cb86f 100644
--- a/encoder/enc_abi.c
+++ b/encoder/enc_abi.c
@@ -26,35 +26,18 @@
 #include "hw.h"
 #include "mc_interface.h"
 #include "utils.h"
+
+#include <string.h>
 #include <math.h>
 
-static float last_enc_angle = 0.0;
-static ABI_config_t ABI_config_now = { 0 };
-
-void enc_abi_deinit(void) {
-	nvicDisableVector(HW_ENC_EXTI_CH);
-
-	TIM_DeInit(HW_ENC_TIM);
-
-	palSetPadMode(ABI_config_now.A_gpio,
-			ABI_config_now.A_pin,
-			PAL_MODE_INPUT_PULLUP);
-	palSetPadMode(ABI_config_now.B_gpio,
-			ABI_config_now.B_pin,
-			PAL_MODE_INPUT_PULLUP);
-
-	last_enc_angle = 0.0;
-}
-
-encoder_ret_t enc_abi_init(ABI_config_t *abi_config) {
+bool enc_abi_init(ABI_config_t *cfg) {
 	EXTI_InitTypeDef EXTI_InitStructure;
 
-	// Initialize variables
-	ABI_config_now = *abi_config;
+	memset(&cfg->state, 0, sizeof(ABI_state));
 
-	palSetPadMode(ABI_config_now.A_gpio, ABI_config_now.A_pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
-	palSetPadMode(ABI_config_now.B_gpio, ABI_config_now.B_pin, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
-	palSetPadMode(ABI_config_now.I_gpio, ABI_config_now.I_pin, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(cfg->A_gpio, cfg->A_pin, PAL_MODE_ALTERNATE(cfg->tim_af));
+	palSetPadMode(cfg->B_gpio, cfg->B_pin, PAL_MODE_ALTERNATE(cfg->tim_af));
+	palSetPadMode(cfg->I_gpio, cfg->I_pin, PAL_MODE_INPUT_PULLUP);
 
 	// Enable timer clock
 	HW_ENC_TIM_CLK_EN();
@@ -62,39 +45,75 @@ encoder_ret_t enc_abi_init(ABI_config_t *abi_config) {
 	// Enable SYSCFG clock
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
 
-	TIM_EncoderInterfaceConfig(HW_ENC_TIM, TIM_EncoderMode_TI12,
-	TIM_ICPolarity_Rising,
-	TIM_ICPolarity_Rising);
-	TIM_SetAutoreload(HW_ENC_TIM, ABI_config_now.counts - 1);
+	TIM_EncoderInterfaceConfig(cfg->timer,
+			TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising);
+	TIM_SetAutoreload(cfg->timer, cfg->counts - 1);
 
 	// Filter
-	HW_ENC_TIM->CCMR1 |= 6 << 12 | 6 << 4;
-	HW_ENC_TIM->CCMR2 |= 6 << 4;
+	cfg->timer->CCMR1 |= 6 << 12 | 6 << 4;
+	cfg->timer->CCMR2 |= 6 << 4;
 
-	TIM_Cmd(HW_ENC_TIM, ENABLE);
+	TIM_Cmd(cfg->timer, ENABLE);
 
 	// Interrupt on index pulse
 
 	// Connect EXTI Line to pin
-	SYSCFG_EXTILineConfig(HW_ENC_EXTI_PORTSRC, HW_ENC_EXTI_PINSRC);
+	SYSCFG_EXTILineConfig(cfg->exti_portsrc, cfg->exti_pinsrc);
 
 	// Configure EXTI Line
-	EXTI_InitStructure.EXTI_Line = HW_ENC_EXTI_LINE;
+	EXTI_InitStructure.EXTI_Line = cfg->exti_line;
 	EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
 	EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
 	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
 	EXTI_Init(&EXTI_InitStructure);
 
-	ABI_config_now = *abi_config;
-
 	// Enable and set EXTI Line Interrupt to the highest priority
-	nvicEnableVector(HW_ENC_EXTI_CH, 0);
+	nvicEnableVector(cfg->exti_ch, 0);
 
-	return ENCODER_OK;
+	return true;
 }
 
-float enc_abi_read_deg(void) {
-	last_enc_angle = ((float) HW_ENC_TIM->CNT * 360.0) / (float) ABI_config_now.counts;
-	return last_enc_angle;
+void enc_abi_deinit(ABI_config_t *cfg) {
+	nvicDisableVector(cfg->exti_ch);
+	TIM_DeInit(cfg->timer);
+	palSetPadMode(cfg->A_gpio, cfg->A_pin, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(cfg->B_gpio, cfg->B_pin, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(cfg->I_gpio, cfg->I_pin, PAL_MODE_INPUT_PULLUP);
+	cfg->state.last_enc_angle = 0.0;
 }
 
+float enc_abi_read_deg(ABI_config_t *cfg) {
+	cfg->state.last_enc_angle = ((float) cfg->timer->CNT * 360.0) / (float) cfg->counts;
+	return cfg->state.last_enc_angle;
+}
+
+void enc_abi_pin_isr(ABI_config_t *cfg) {
+	// Only reset if the pin is still high to avoid too short pulses, which
+	// most likely are noise.
+	__NOP();
+	__NOP();
+	__NOP();
+	__NOP();
+	if (palReadPad(cfg->I_gpio, cfg->I_pin)) {
+		const unsigned int cnt = cfg->timer->CNT;
+		const unsigned int lim = cfg->counts / 20;
+
+		if (cfg->state.index_found) {
+			// Some plausibility filtering.
+			if (cnt > (cfg->counts - lim) || cnt < lim) {
+				cfg->timer->CNT = 0;
+				cfg->state.bad_pulses = 0;
+			} else {
+				cfg->state.bad_pulses++;
+
+				if (cfg->state.bad_pulses > 5) {
+					cfg->state.index_found = 0;
+				}
+			}
+		} else {
+			cfg->timer->CNT = 0;
+			cfg->state.index_found = true;
+			cfg->state.bad_pulses = 0;
+		}
+	}
+}
diff --git a/encoder/enc_abi.h b/encoder/enc_abi.h
index 146f3e82..443cb16e 100644
--- a/encoder/enc_abi.h
+++ b/encoder/enc_abi.h
@@ -24,9 +24,12 @@
 #include "datatypes.h"
 #include "encoder/encoder_datatype.h"
 
-void enc_abi_deinit(void);
-encoder_ret_t enc_abi_init(ABI_config_t *abi_config);
+// Functions
+bool enc_abi_init(ABI_config_t *cfg);
+void enc_abi_deinit(ABI_config_t *cfg);
+float enc_abi_read_deg(ABI_config_t *cfg);
 
-float enc_abi_read_deg(void);
+// Call this functions on index pin change interrupts
+void enc_abi_pin_isr(ABI_config_t *cfg);
 
 #endif /* ENC_ABI_H_ */
diff --git a/encoder/enc_ad2s1205.c b/encoder/enc_ad2s1205.c
index f1609168..e03d4eba 100644
--- a/encoder/enc_ad2s1205.c
+++ b/encoder/enc_ad2s1205.c
@@ -33,7 +33,7 @@
 #include <string.h>
 #include <math.h>
 
-encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *cfg) {
+bool enc_ad2s1205_init(AD2S1205_config_t *cfg) {
 	spi_bb_init(&(cfg->sw_spi));
 
 	memset(&cfg->state, 0, sizeof(AD2S1205_state));
@@ -49,7 +49,7 @@ encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *cfg) {
 	palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN);		// Will always read position
 #endif
 
-	return ENCODER_OK;
+	return true;
 }
 
 void enc_ad2s1205_deinit(AD2S1205_config_t *cfg) {
diff --git a/encoder/enc_ad2s1205.h b/encoder/enc_ad2s1205.h
index 6ebcfab2..81877535 100644
--- a/encoder/enc_ad2s1205.h
+++ b/encoder/enc_ad2s1205.h
@@ -26,7 +26,7 @@
 #include "encoder/encoder_datatype.h"
 
 // Functions
-encoder_ret_t enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config);
+bool enc_ad2s1205_init(AD2S1205_config_t *AD2S1205_config);
 void enc_ad2s1205_deinit(AD2S1205_config_t *cfg);
 void enc_ad2s1205_routine(AD2S1205_config_t *cfg);
 
diff --git a/encoder/enc_as504x.c b/encoder/enc_as504x.c
index 5af841dc..5deca987 100644
--- a/encoder/enc_as504x.c
+++ b/encoder/enc_as504x.c
@@ -65,10 +65,10 @@ static uint8_t AS504x_spi_transfer_err_check(spi_bb_state *sw_spi,
 		uint16_t *in_buf, const uint16_t *out_buf, int length);
 static void AS504x_determinate_if_connected(AS504x_config_t *cfg, bool was_last_valid);
 
-encoder_ret_t enc_as504x_init(AS504x_config_t *cfg) {
+bool enc_as504x_init(AS504x_config_t *cfg) {
 	memset(&cfg->state, 0, sizeof(AS504x_state));
 	spi_bb_init(&(cfg->sw_spi));
-	return ENCODER_OK;
+	return true;
 }
 
 void enc_as504x_deinit(AS504x_config_t *cfg) {
diff --git a/encoder/enc_as504x.h b/encoder/enc_as504x.h
index dc735ae9..561597d3 100644
--- a/encoder/enc_as504x.h
+++ b/encoder/enc_as504x.h
@@ -24,7 +24,7 @@
 #include "encoder/encoder_datatype.h"
 
 // Functions
-encoder_ret_t enc_as504x_init(AS504x_config_t *AS504x_config);
+bool enc_as504x_init(AS504x_config_t *AS504x_config);
 void enc_as504x_deinit(AS504x_config_t *cfg);
 void enc_as504x_routine(AS504x_config_t *cfg);
 
diff --git a/encoder/enc_mt6816.c b/encoder/enc_mt6816.c
index 64953f29..5e1f26cf 100644
--- a/encoder/enc_mt6816.c
+++ b/encoder/enc_mt6816.c
@@ -35,9 +35,9 @@
 
 #define MT6816_NO_MAGNET_ERROR_MASK		0x0002
 
-encoder_ret_t enc_mt6816_init(MT6816_config_t *cfg) {
+bool enc_mt6816_init(MT6816_config_t *cfg) {
 	if (cfg->spi_dev == NULL) {
-		return ENCODER_ERROR;
+		return false;
 	}
 
 	memset(&cfg->state, 0, sizeof(MT6816_state));
@@ -52,7 +52,7 @@ encoder_ret_t enc_mt6816_init(MT6816_config_t *cfg) {
 	cfg->state.spi_error_rate = 0.0;
 	cfg->state.encoder_no_magnet_error_rate = 0.0;
 
-	return ENCODER_OK;
+	return true;
 }
 
 void enc_mt6816_deinit(MT6816_config_t *cfg) {
diff --git a/encoder/enc_mt6816.h b/encoder/enc_mt6816.h
index 8c4763b6..fd002a8d 100644
--- a/encoder/enc_mt6816.h
+++ b/encoder/enc_mt6816.h
@@ -25,7 +25,7 @@
 #include "datatypes.h"
 #include "encoder/encoder_datatype.h"
 
-encoder_ret_t enc_mt6816_init(MT6816_config_t *mt6816_config);
+bool enc_mt6816_init(MT6816_config_t *mt6816_config);
 void enc_mt6816_deinit(MT6816_config_t *cfg);
 void enc_mt6816_routine(MT6816_config_t *cfg);
 
diff --git a/encoder/enc_sincos.c b/encoder/enc_sincos.c
index 34b965ea..2a69421a 100644
--- a/encoder/enc_sincos.c
+++ b/encoder/enc_sincos.c
@@ -35,9 +35,9 @@
 #define SINCOS_MIN_AMPLITUDE        1.0         // sqrt(sin^2 + cos^2) has to be larger than this
 #define SINCOS_MAX_AMPLITUDE        1.65        // sqrt(sin^2 + cos^2) has to be smaller than this
 
-encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *cfg) {
+bool enc_sincos_init(ENCSINCOS_config_t *cfg) {
 	memset(&cfg->state, 0, sizeof(ENCSINCOS_state));
-	return ENCODER_OK;
+	return true;
 }
 
 void enc_sincos_deinit(ENCSINCOS_config_t *cfg) {
diff --git a/encoder/enc_sincos.h b/encoder/enc_sincos.h
index a85936de..a0e90413 100644
--- a/encoder/enc_sincos.h
+++ b/encoder/enc_sincos.h
@@ -26,7 +26,7 @@
 #include "encoder/encoder_datatype.h"
 
 // Functions
-encoder_ret_t enc_sincos_init(ENCSINCOS_config_t *cfg);
+bool enc_sincos_init(ENCSINCOS_config_t *cfg);
 void enc_sincos_deinit(ENCSINCOS_config_t *cfg);
 float enc_sincos_read_deg(ENCSINCOS_config_t *cfg);
 
diff --git a/encoder/enc_ts5700n8501.c b/encoder/enc_ts5700n8501.c
index 25221ec9..57c4b873 100644
--- a/encoder/enc_ts5700n8501.c
+++ b/encoder/enc_ts5700n8501.c
@@ -32,9 +32,9 @@
 // Private functions
 static THD_FUNCTION(ts5700n8501_thread, arg);
 
-encoder_ret_t enc_ts5700n8501_init(TS5700N8501_config_t *cfg) {
+bool enc_ts5700n8501_init(TS5700N8501_config_t *cfg) {
 	if (cfg->sd == NULL) {
-		return ENCODER_ERROR;
+		return false;
 	}
 
 	memset(&cfg->state, 0, sizeof(TS5700N8501_state));
@@ -43,7 +43,7 @@ encoder_ret_t enc_ts5700n8501_init(TS5700N8501_config_t *cfg) {
 	chThdCreateStatic(cfg->thread_wa, cfg->thread_wa_size,
 	NORMALPRIO - 10, ts5700n8501_thread, cfg);
 
-	return ENCODER_OK;
+	return true;
 }
 
 void enc_ts5700n8501_deinit(TS5700N8501_config_t *cfg) {
diff --git a/encoder/enc_ts5700n8501.h b/encoder/enc_ts5700n8501.h
index e6b957f9..f5792b89 100644
--- a/encoder/enc_ts5700n8501.h
+++ b/encoder/enc_ts5700n8501.h
@@ -24,7 +24,7 @@
 #include "datatypes.h"
 #include "encoder/encoder_datatype.h"
 
-encoder_ret_t enc_ts5700n8501_init(TS5700N8501_config_t *cfg);
+bool enc_ts5700n8501_init(TS5700N8501_config_t *cfg);
 void enc_ts5700n8501_deinit(TS5700N8501_config_t *cfg);
 
 inline float enc_ts5700n8501_read_deg(TS5700N8501_config_t *cfg) {
diff --git a/encoder/encoder.c b/encoder/encoder.c
index 4f506222..e26ade78 100644
--- a/encoder/encoder.c
+++ b/encoder/encoder.c
@@ -32,8 +32,6 @@
 #include <math.h>
 
 static encoder_type_t encoder_type_now = ENCODER_TYPE_NONE;
-static bool index_found = false;
-static float timer_rate_now = 1.0;
 
 // Private functions
 static void terminal_encoder(int argc, const char **argv);
@@ -41,83 +39,71 @@ static void terminal_encoder_clear_errors(int argc, const char **argv);
 static void terminal_encoder_clear_multiturn(int argc, const char **argv);
 static void timer_start(float rate);
 
-encoder_ret_t encoder_init(volatile mc_configuration *conf) {
-	encoder_ret_t res = ENCODER_ERROR;
+bool encoder_init(volatile mc_configuration *conf) {
+	bool res = false;
 
 	if (encoder_type_now != ENCODER_TYPE_NONE) {
-		return res;
+		encoder_deinit();
 	}
 
+	nvicDisableVector(HW_ENC_EXTI_CH);
+	nvicDisableVector(HW_ENC_TIM_ISR_CH);
+	TIM_DeInit(HW_ENC_TIM);
+
 	switch (conf->m_sensor_port_mode) {
 	case SENSOR_PORT_MODE_ABI: {
 		SENSOR_PORT_5V();
 
 		encoder_cfg_ABI.counts = conf->m_encoder_counts;
-		encoder_ret_t encoder_ret = enc_abi_init(&encoder_cfg_ABI);
 
-		if (ENCODER_OK != encoder_ret) {
+		if (!enc_abi_init(&encoder_cfg_ABI)) {
 			encoder_type_now = ENCODER_TYPE_NONE;
-			index_found = false;
-			return ENCODER_ERROR;
+			return false;
 		}
+
 		encoder_type_now = ENCODER_TYPE_ABI;
-		index_found = true;
-		res = ENCODER_OK;
+		res = true;
 	} break;
 
 	case SENSOR_PORT_MODE_AS5047_SPI: {
 		SENSOR_PORT_3V3();
 
-		encoder_ret_t encoder_ret = enc_as504x_init(&encoder_cfg_as504x);
-
-		if (ENCODER_OK != encoder_ret) {
-			index_found = false;
-			return ENCODER_ERROR;
+		if (!enc_as504x_init(&encoder_cfg_as504x)) {
+			return false;
 		}
 
 		encoder_type_now = ENCODER_TYPE_AS504x;
-		index_found = true;
-
 		timer_start(10000);
 
-		res = ENCODER_OK;
+		res = true;
 	} break;
 
 	case SENSOR_PORT_MODE_MT6816_SPI: {
 		SENSOR_PORT_5V();
 
-		encoder_ret_t encoder_ret = enc_mt6816_init(&encoder_cfg_mt6816);
-
-		if (ENCODER_OK != encoder_ret) {
+		if (!enc_mt6816_init(&encoder_cfg_mt6816)) {
 			encoder_type_now = ENCODER_TYPE_NONE;
-			index_found = false;
-			return ENCODER_ERROR;
+			return false;
 		}
 
 		encoder_type_now = ENCODER_TYPE_MT6816;
-		index_found = true;
-
 		timer_start(10000);
 
-		res = ENCODER_OK;
+		res = true;
 	} break;
 
 	case SENSOR_PORT_MODE_AD2S1205: {
 		SENSOR_PORT_5V();
 
-		encoder_ret_t encoder_ret = enc_ad2s1205_init(&encoder_cfg_ad2s1205);
-
-		if (ENCODER_OK != encoder_ret) {
+		if (!enc_ad2s1205_init(&encoder_cfg_ad2s1205)) {
 			encoder_type_now = ENCODER_TYPE_NONE;
-			index_found = false;
-			return ENCODER_ERROR;
+			return false;
 		}
-		encoder_type_now = ENCODER_TYPE_AD2S1205_SPI;
-		index_found = true;
 
+		encoder_type_now = ENCODER_TYPE_AD2S1205_SPI;
 		timer_start(10000);
 
-		res = ENCODER_OK;
+		res = true;
 	} break;
 
 	case SENSOR_PORT_MODE_SINCOS: {
@@ -129,16 +115,13 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) {
 		encoder_cfg_sincos.c_offset =  conf->foc_encoder_cos_offset;
 		encoder_cfg_sincos.filter_constant = conf->foc_encoder_sincos_filter_constant;
 
-		encoder_ret_t encoder_ret = enc_sincos_init(&encoder_cfg_sincos);
-
-		if (ENCODER_OK != encoder_ret) {
+		if (!enc_sincos_init(&encoder_cfg_sincos)) {
 			encoder_type_now = ENCODER_TYPE_NONE;
-			index_found = false;
-			return ENCODER_ERROR;
+			return false;
 		}
+
 		encoder_type_now = ENCODER_TYPE_SINCOS;
-		index_found = true;
-		res = ENCODER_OK;
+		res = true;
 	} break;
 
 	case SENSOR_PORT_MODE_TS5700N8501:
@@ -155,16 +138,13 @@ encoder_ret_t encoder_init(volatile mc_configuration *conf) {
 		}
 		mempools_free_appconf(appconf);
 
-		encoder_ret_t encoder_ret = enc_ts5700n8501_init(&encoder_cfg_TS5700N8501);
-
-		if (ENCODER_OK != encoder_ret) {
+		if (!enc_ts5700n8501_init(&encoder_cfg_TS5700N8501)) {
 			encoder_type_now = ENCODER_TYPE_NONE;
-			index_found = false;
-			return ENCODER_ERROR;
+			return false;
 		}
+
 		encoder_type_now = ENCODER_TYPE_TS5700N8501;
-		index_found = true;
-		res = ENCODER_OK;
+		res = true;
 	} break;
 
 	default:
@@ -206,7 +186,7 @@ void encoder_deinit(void) {
 	} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
 		enc_ad2s1205_deinit(&encoder_cfg_ad2s1205);
 	} else if (encoder_type_now == ENCODER_TYPE_ABI) {
-		enc_abi_deinit();
+		enc_abi_deinit(&encoder_cfg_ABI);
 	} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
 		enc_sincos_deinit(&encoder_cfg_sincos);
 	} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
@@ -224,7 +204,7 @@ float encoder_read_deg(void) {
 	} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
 		return AD2S1205_LAST_ANGLE(&encoder_cfg_ad2s1205);
 	} else if (encoder_type_now == ENCODER_TYPE_ABI) {
-		return enc_abi_read_deg();
+		return enc_abi_read_deg(&encoder_cfg_ABI);
 	} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
 		return enc_sincos_read_deg(&encoder_cfg_sincos);
 	} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
@@ -254,7 +234,11 @@ encoder_type_t encoder_is_configured(void) {
 }
 
 bool encoder_index_found(void) {
-	return index_found;
+	if (encoder_type_now == ENCODER_TYPE_ABI) {
+		return encoder_cfg_ABI.state.index_found;
+	} else {
+		return true;
+	}
 }
 
 void encoder_reset_multiturn(void) {
@@ -269,99 +253,89 @@ void encoder_reset_errors(void) {
 	}
 }
 
+// Check for encoder faults that should stop the motor with a fault code.
 void encoder_check_faults(volatile mc_configuration *m_conf, bool is_second_motor) {
-	// Trigger encoder error rate fault, using 5% errors as threshold.
-	// Relevant only in FOC mode with encoder enabled
 
+	// Only generate fault code when the encoder is being used. Note that encoder faults
+	// that occur above the sensorless ERPM won't stop the motor.
 	bool is_foc_encoder = m_conf->motor_type == MOTOR_TYPE_FOC &&
 			m_conf->foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
 			mcpwm_foc_is_using_encoder();
 
-	if (is_foc_encoder &&
-			m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI &&
-			encoder_cfg_as504x.state.spi_error_rate > 0.05) {
-		mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
-	}
+	if (is_foc_encoder) {
+		switch (m_conf->m_sensor_port_mode) {
+		case SENSOR_PORT_MODE_AS5047_SPI:
+			if (encoder_cfg_as504x.state.spi_error_rate > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
+			}
 
-	if (is_foc_encoder &&
-			m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_MT6816_SPI &&
-			encoder_cfg_mt6816.state.encoder_no_magnet_error_rate > 0.05) {
-		mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
-	}
+			if (encoder_cfg_as504x.sw_spi.mosi_gpio != NULL) {
+				AS504x_diag diag = encoder_cfg_as504x.state.sensor_diag;
+				if (!diag.is_connected) {
+					mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
+				}
 
-	if (is_foc_encoder && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
-		if (encoder_cfg_sincos.state.signal_low_error_rate > 0.05)
-			mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, is_second_motor, false);
-		if (encoder_cfg_sincos.state.signal_above_max_error_rate > 0.05)
-			mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, is_second_motor, false);
-	}
+				if (diag.is_Comp_high) {
+					mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
+				} else if(diag.is_Comp_low) {
+					mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false);
+				}
+			}
+			break;
 
-	if (is_foc_encoder && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) {
-		AS504x_diag diag = encoder_cfg_as504x.state.sensor_diag;
+		case SENSOR_PORT_MODE_MT6816_SPI:
+			if (encoder_cfg_mt6816.state.encoder_no_magnet_error_rate > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
+			}
+			break;
 
-		if (!diag.is_connected) {
-			mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, is_second_motor, false);
-		}
+		case SENSOR_PORT_MODE_SINCOS:
+			if (encoder_cfg_sincos.state.signal_low_error_rate > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, is_second_motor, false);
+			}
+			if (encoder_cfg_sincos.state.signal_above_max_error_rate > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, is_second_motor, false);
+			}
+			break;
 
-		if (diag.is_Comp_high) {
-			mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, is_second_motor, false);
-		} else if(diag.is_Comp_low) {
-			mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, is_second_motor, false);
-		}
-	}
+		case SENSOR_PORT_MODE_AD2S1205:
+			if (encoder_cfg_ad2s1205.state.resolver_loss_of_tracking_error_rate > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, is_second_motor, false);
+			}
+			if (encoder_cfg_ad2s1205.state.resolver_degradation_of_signal_error_rate > 0.05) {
+				mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS, is_second_motor, false);
+			}
+			if (encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate > 0.04) {
+				mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS, is_second_motor, false);
+			}
+			break;
 
-	if (is_foc_encoder && m_conf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
-		if (encoder_cfg_ad2s1205.state.resolver_loss_of_tracking_error_rate > 0.05) {
-			mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOT, is_second_motor, false);
-		}
-		if (encoder_cfg_ad2s1205.state.resolver_degradation_of_signal_error_rate > 0.05) {
-			mc_interface_fault_stop(FAULT_CODE_RESOLVER_DOS, is_second_motor, false);
-		}
-		if (encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate > 0.04) {
-			mc_interface_fault_stop(FAULT_CODE_RESOLVER_LOS, is_second_motor, false);
+		default:
+			break;
 		}
 	}
 }
 
 void encoder_pin_isr(void) {
-	// Only reset if the pin is still high to avoid too short pulses, which
-	// most likely are noise.
-	__NOP();
-	__NOP();
-	__NOP();
-	__NOP();
-	if (palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)) {
-		const unsigned int cnt = HW_ENC_TIM->CNT;
-		static int bad_pulses = 0;
-		const unsigned int lim = encoder_cfg_ABI.counts / 20;
-
-		if (encoder_index_found()) {
-			// Some plausibility filtering.
-			if (cnt > (encoder_cfg_ABI.counts - lim) || cnt < lim) {
-				HW_ENC_TIM->CNT = 0;
-				bad_pulses = 0;
-			} else {
-				bad_pulses++;
-
-				if (bad_pulses > 5) {
-					index_found = 0;
-				}
-			}
-		} else {
-			HW_ENC_TIM->CNT = 0;
-			index_found = true;
-			bad_pulses = 0;
-		}
-	}
+	enc_abi_pin_isr(&encoder_cfg_ABI);
 }
 
 void encoder_tim_isr(void) {
-	if (encoder_type_now == ENCODER_TYPE_AS504x) {
-		enc_as504x_routine(&encoder_cfg_as504x);
-	} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
-		enc_mt6816_routine(&encoder_cfg_mt6816);
-	} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
-		enc_ad2s1205_routine(&encoder_cfg_ad2s1205);
+	switch (encoder_type_now) {
+		case ENCODER_TYPE_AS504x:
+			enc_as504x_routine(&encoder_cfg_as504x);
+			break;
+
+		case ENCODER_TYPE_MT6816:
+			enc_mt6816_routine(&encoder_cfg_mt6816);
+			break;
+
+		case ENCODER_TYPE_AD2S1205_SPI:
+			enc_ad2s1205_routine(&encoder_cfg_ad2s1205);
+			break;
+
+		default:
+			break;
 	}
 }
 
@@ -370,72 +344,47 @@ static void terminal_encoder(int argc, const char **argv) {
 
 	const volatile mc_configuration *mcconf = mc_interface_get_configuration();
 
-	if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI ||
-			mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_MT6816_SPI ||
-			mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205 ||
-			mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501 ||
-			mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) {
+	switch (mcconf->m_sensor_port_mode) {
+	case SENSOR_PORT_MODE_AS5047_SPI:
+		commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%",
+				encoder_cfg_as504x.state.spi_val, encoder_cfg_as504x.state.spi_communication_error_count,
+				(double)(encoder_cfg_as504x.state.spi_error_rate * 100.0));
 
-		unsigned int spi_val = 0;
-		float error_rate = 0.0;
-		unsigned int error_cnt = 0;
-		AS504x_diag diag = { 0 };
-
-		if (encoder_is_configured() == ENCODER_TYPE_AS504x) {
-			spi_val = encoder_cfg_as504x.state.spi_val;
-			error_rate = encoder_cfg_as504x.state.spi_error_rate;
-			error_cnt = encoder_cfg_as504x.state.spi_communication_error_count;
-			diag = encoder_cfg_as504x.state.sensor_diag;
-		} else if (encoder_is_configured() == ENCODER_TYPE_MT6816) {
-			spi_val = encoder_cfg_mt6816.state.spi_val;
-			error_cnt = encoder_cfg_mt6816.state.spi_error_cnt;
-		}
-
-		if (mcconf->m_sensor_port_mode != SENSOR_PORT_MODE_AS5047_SPI) {
-			commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%",
-					spi_val,
-					error_cnt,
-					(double)(error_rate * 100.0));
-		} else {
-			commands_printf("SPI encoder value: %d, errors: %d, error rate: %.3f %%, Connected: %u",
-					spi_val,
-					error_cnt,
-					(double)(error_rate * 100.0),
-					diag.is_connected);
-		}
-
-		if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501 ||
-				mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) {
-			char sf[9];
-			char almc[9];
-			utils_byte_to_binary(enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501)[0], sf);
-			utils_byte_to_binary(enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501)[7], almc);
-			commands_printf("TS5700N8501 ABM: %d, SF: %s, ALMC: %s\n", enc_ts5700n8501_get_abm(&encoder_cfg_TS5700N8501), sf, almc);
-		}
-
-		if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_MT6816_SPI) {
-			commands_printf("Low flux error (no magnet): errors: %d, error rate: %.3f %%",
-					encoder_cfg_mt6816.state.encoder_no_magnet_error_cnt,
-					(double)(encoder_cfg_mt6816.state.encoder_no_magnet_error_rate * 100.0));
-		}
-
-		if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI &&
-				encoder_cfg_as504x.sw_spi.mosi_gpio != NULL) {
+		if (encoder_cfg_as504x.sw_spi.mosi_gpio != NULL) {
 			commands_printf("\nAS5047 DIAGNOSTICS:\n"
+					"Connected : %u\n"
 					"AGC       : %u\n"
 					"Magnitude : %u\n"
 					"COF       : %u\n"
 					"OCF       : %u\n"
 					"COMP_low  : %u\n"
-					"COMP_high : %u\n",
-					diag.AGC_value, diag.magnitude,
-					diag.is_COF, diag.is_OCF,
-					diag.is_Comp_low,
-					diag.is_Comp_high);
+					"COMP_high : %u",
+					encoder_cfg_as504x.state.sensor_diag.is_connected,
+					encoder_cfg_as504x.state.sensor_diag.AGC_value,
+					encoder_cfg_as504x.state.sensor_diag.magnitude,
+					encoder_cfg_as504x.state.sensor_diag.is_COF,
+					encoder_cfg_as504x.state.sensor_diag.is_OCF,
+					encoder_cfg_as504x.state.sensor_diag.is_Comp_low,
+					encoder_cfg_as504x.state.sensor_diag.is_Comp_high);
 		}
-	}
+		break;
 
-	if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
+	case SENSOR_PORT_MODE_MT6816_SPI:
+		commands_printf("Low flux error (no magnet): errors: %d, error rate: %.3f %%",
+				encoder_cfg_mt6816.state.encoder_no_magnet_error_cnt,
+				(double)(encoder_cfg_mt6816.state.encoder_no_magnet_error_rate * 100.0));
+		break;
+
+	case SENSOR_PORT_MODE_TS5700N8501:
+	case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: {
+		char sf[9];
+		char almc[9];
+		utils_byte_to_binary(enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501)[0], sf);
+		utils_byte_to_binary(enc_ts5700n8501_get_raw_status(&encoder_cfg_TS5700N8501)[7], almc);
+		commands_printf("TS5700N8501 ABM: %d, SF: %s, ALMC: %s", enc_ts5700n8501_get_abm(&encoder_cfg_TS5700N8501), sf, almc);
+	} break;
+
+	case SENSOR_PORT_MODE_SINCOS:
 		commands_printf("Sin/Cos encoder signal below minimum amplitude: errors: %d, error rate: %.3f %%",
 				encoder_cfg_sincos.state.signal_below_min_error_cnt,
 				(double)(encoder_cfg_sincos.state.signal_low_error_rate * 100.0));
@@ -443,9 +392,9 @@ static void terminal_encoder(int argc, const char **argv) {
 		commands_printf("Sin/Cos encoder signal above maximum amplitude: errors: %d, error rate: %.3f %%",
 				encoder_cfg_sincos.state.signal_above_max_error_cnt,
 				(double)(encoder_cfg_sincos.state.signal_above_max_error_rate * 100.0));
-	}
+		break;
 
-	if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
+	case SENSOR_PORT_MODE_AD2S1205:
 		commands_printf("Resolver Loss Of Tracking (>5%c error): errors: %d, error rate: %.3f %%", 0xB0,
 				encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_cnt,
 				(double)(encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate * 100.0));
@@ -455,11 +404,18 @@ static void terminal_encoder(int argc, const char **argv) {
 		commands_printf("Resolver Loss Of Signal (>57%c error): errors: %d, error rate: %.3f %%", 0xB0,
 				encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_cnt,
 				(double)(encoder_cfg_ad2s1205.state.resolver_loss_of_signal_error_rate * 100.0));
+		break;
+
+	case SENSOR_PORT_MODE_ABI:
+		commands_printf("Index found: %d", encoder_index_found());
+		break;
+
+	default:
+		commands_printf("No encoder debug info available.");
+		break;
 	}
 
-	if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
-		commands_printf("Index found: %d\n", encoder_index_found());
-	}
+	commands_printf(" ");
 }
 
 static void terminal_encoder_clear_errors(int argc, const char **argv) {
@@ -475,8 +431,6 @@ static void terminal_encoder_clear_multiturn(int argc, const char **argv) {
 }
 
 static void timer_start(float rate) {
-	timer_rate_now = rate;
-
 	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
 
 	// Enable timer clock
diff --git a/encoder/encoder.h b/encoder/encoder.h
index ecf184b6..59de2fc9 100644
--- a/encoder/encoder.h
+++ b/encoder/encoder.h
@@ -31,7 +31,7 @@
 #include "enc_abi.h"
 
 // Functions
-encoder_ret_t encoder_init(volatile mc_configuration *conf);
+bool encoder_init(volatile mc_configuration *conf);
 void encoder_deinit(void);
 
 float encoder_read_deg(void);
diff --git a/encoder/encoder_cfg.c b/encoder/encoder_cfg.c
index ad49e873..c62174ec 100644
--- a/encoder/encoder_cfg.c
+++ b/encoder/encoder_cfg.c
@@ -94,12 +94,18 @@ MT6816_config_t encoder_cfg_mt6816 = {
 #endif
 };
 
-ABI_config_t encoder_cfg_ABI =
-{
+ABI_config_t encoder_cfg_ABI = {
 		10000, // counts
 		HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1,
 		HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2,
-		HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3
+		HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3,
+		HW_ENC_TIM,
+		HW_ENC_TIM_AF,
+		HW_ENC_EXTI_PORTSRC,
+		HW_ENC_EXTI_PINSRC,
+		HW_ENC_EXTI_LINE,
+		HW_ENC_EXTI_CH,
+		{0, 0, 0}, // State
 };
 
 ENCSINCOS_config_t encoder_cfg_sincos = {0};
diff --git a/encoder/encoder_cfg.h b/encoder/encoder_cfg.h
index a81d5d59..5ea07e68 100644
--- a/encoder/encoder_cfg.h
+++ b/encoder/encoder_cfg.h
@@ -17,8 +17,8 @@
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#ifndef ENCODER_ENCODER_CFG_H_
-#define ENCODER_ENCODER_CFG_H_
+#ifndef ENCODER_CFG_H_
+#define ENCODER_CFG_H_
 
 #include "encoder_datatype.h"
 
@@ -30,4 +30,4 @@ extern ABI_config_t encoder_cfg_ABI;
 extern ENCSINCOS_config_t encoder_cfg_sincos;
 extern TS5700N8501_config_t encoder_cfg_TS5700N8501;
 
-#endif /* ENCODER_ENCODER_CFG_H_ */
+#endif /* ENCODER_CFG_H_ */
diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h
index 866dd317..50b0a06a 100644
--- a/encoder/encoder_datatype.h
+++ b/encoder/encoder_datatype.h
@@ -27,10 +27,6 @@
 #include "hal.h"
 #include "spi_bb.h"
 
-typedef enum {
-	ENCODER_OK = 0, ENCODER_NONE, ENCODER_ERROR
-} encoder_ret_t;
-
 typedef enum {
 	ENCODER_TYPE_NONE = 0,
 	ENCODER_TYPE_AS504x,
@@ -85,14 +81,28 @@ typedef struct {
 	MT6816_state state;
 } MT6816_config_t;
 
+typedef struct {
+	volatile bool index_found;
+	volatile float last_enc_angle;
+	volatile int bad_pulses;
+} ABI_state;
+
 typedef struct {
 	uint32_t counts;
-	stm32_gpio_t *A_gpio;
-	uint8_t A_pin;
-	stm32_gpio_t *B_gpio;
-	uint8_t B_pin;
-	stm32_gpio_t *I_gpio;
-	uint8_t I_pin;
+
+	stm32_gpio_t *A_gpio; uint8_t A_pin;
+	stm32_gpio_t *B_gpio; uint8_t B_pin;
+	stm32_gpio_t *I_gpio; uint8_t I_pin;
+
+	TIM_TypeDef *timer;
+	uint8_t tim_af;
+
+	uint8_t exti_portsrc;
+	uint8_t exti_pinsrc;
+	uint32_t exti_line;
+	uint32_t exti_ch;
+
+	ABI_state state;
 } ABI_config_t;
 
 typedef struct {