diff --git a/Makefile b/Makefile
index 698e625c..d70400c4 100755
--- a/Makefile
+++ b/Makefile
@@ -146,7 +146,6 @@ CSRC = $(STARTUPSRC) \
        commands.c \
        timeout.c \
        comm_can.c \
-       encoder.c \
        flash_helper.c \
        mc_interface.c \
        mcpwm_foc.c \
diff --git a/applications/app_custom_template.c b/applications/app_custom_template.c
index 09f9766f..46830a07 100644
--- a/applications/app_custom_template.c
+++ b/applications/app_custom_template.c
@@ -24,7 +24,7 @@
 // Some useful includes
 #include "mc_interface.h"
 #include "utils.h"
-#include "encoder.h"
+#include "encoder/encoders.h"
 #include "terminal.h"
 #include "comm_can.h"
 #include "hw.h"
diff --git a/applications/app_skypuff.c b/applications/app_skypuff.c
index e1dfda3d..a71a3a75 100644
--- a/applications/app_skypuff.c
+++ b/applications/app_skypuff.c
@@ -24,7 +24,7 @@
 // Some useful includes
 #include "comm_can.h"
 #include "commands.h"
-#include "encoder.h"
+#include "encoder/encoders.h"
 #include "hw.h"
 #include "mc_interface.h"
 #include "terminal.h"
@@ -605,4 +605,4 @@ static void terminal_show_skypuff_conf(int argc, const char **argv)
 	commands_printf("  rewinding force: %.2fkg (%.1fA)", (double)(config.rewinding_current / config.kg_to_amps), (double)config.rewinding_current);
 	commands_printf("  slow speed: %.1fms (%.0f ERPM)", (double)erpm_to_ms(config.slow_erpm), (double)config.slow_erpm);
 	commands_printf("  maximum slow force: %.2fkg (%.1fA)", (double)(config.slow_max_current / config.kg_to_amps), (double)config.slow_max_current);
-}
\ No newline at end of file
+}
diff --git a/applications/er/app_erockit_v2.c b/applications/er/app_erockit_v2.c
index 3a9584a1..483c6d18 100755
--- a/applications/er/app_erockit_v2.c
+++ b/applications/er/app_erockit_v2.c
@@ -24,7 +24,7 @@
 #include "conf_general.h"
 #include "mc_interface.h"
 #include "utils.h"
-#include "encoder.h"
+#include "encoder/encoders.h"
 #include "terminal.h"
 #include "comm_can.h"
 #include "hw.h"
diff --git a/applications/finn/app_finn_az.c b/applications/finn/app_finn_az.c
index 562676af..2def98b2 100644
--- a/applications/finn/app_finn_az.c
+++ b/applications/finn/app_finn_az.c
@@ -23,7 +23,7 @@
 
 #include "mc_interface.h"
 #include "utils.h"
-#include "encoder.h"
+#include "encoder/encoders.h"
 #include "terminal.h"
 #include "comm_can.h"
 #include "hw.h"
diff --git a/comm_can.c b/comm_can.c
index c88c75c6..d3ad926b 100644
--- a/comm_can.c
+++ b/comm_can.c
@@ -33,7 +33,6 @@
 #include "packet.h"
 #include "hw.h"
 #include "canard_driver.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "utils.h"
 #include "mempools.h"
@@ -1578,7 +1577,7 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 			case CAN_PACKET_POLL_TS5700N8501_STATUS: {
 				comm_can_transmit_eid_replace(app_get_configuration()->controller_id |
 						((uint32_t)CAN_PACKET_POLL_TS5700N8501_STATUS << 8),
-						encoder_ts5700n8501_get_raw_status(), 8, true);
+						encoders_get_raw_status(), 8, true);
 			} break;
 
 			case CAN_PACKET_CONF_BATTERY_CUT:
diff --git a/commands.c b/commands.c
index fe0a2c61..ce14d76c 100644
--- a/commands.c
+++ b/commands.c
@@ -36,7 +36,6 @@
 #include "flash_helper.h"
 #include "utils.h"
 #include "packet.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "nrf_driver.h"
 #include "gpdrive.h"
diff --git a/conf_general.c b/conf_general.c
index a04d4591..4432a49a 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -27,7 +27,6 @@
 #include "stm32f4xx_conf.h"
 #include "timeout.h"
 #include "commands.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "comm_can.h"
 #include "app.h"
diff --git a/encoder.c b/encoder.c
deleted file mode 100644
index e4f2fb57..00000000
--- a/encoder.c
+++ /dev/null
@@ -1,1166 +0,0 @@
-/*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
-
-	This file is part of the VESC firmware.
-
-	The VESC firmware is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    The VESC firmware is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    */
-
-#include "encoder.h"
-#include "ch.h"
-#include "hal.h"
-#include "stm32f4xx_conf.h"
-#include "hw.h"
-#include "mc_interface.h"
-#include "utils.h"
-#include <math.h>
-
-// Defines
-#define AS5047P_READ_ANGLECOM		(0x3FFF | 0x4000 | 0x8000) // This is just ones
-#define AS5047_SAMPLE_RATE_HZ		20000
-#define AD2S1205_SAMPLE_RATE_HZ		20000		//25MHz max spi clk
-#define MT6816_SAMPLE_RATE_HZ		20000
-#define MT6816_NO_MAGNET_ERROR_MASK	0x0002
-#define SINCOS_SAMPLE_RATE_HZ		20000
-#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
-
-#if (AS5047_USE_HW_SPI_PINS) || (MT6816_USE_HW_SPI_PINS) || (AD2S1205_USE_HW_SPI_PINS)
-#ifdef HW_SPI_DEV
-#define SPI_SW_MISO_GPIO			HW_SPI_PORT_MISO
-#define SPI_SW_MISO_PIN				HW_SPI_PIN_MISO
-#define SPI_SW_MOSI_GPIO			HW_SPI_PORT_MOSI
-#define SPI_SW_MOSI_PIN				HW_SPI_PIN_MOSI
-#define SPI_SW_SCK_GPIO				HW_SPI_PORT_SCK
-#define SPI_SW_SCK_PIN				HW_SPI_PIN_SCK
-#define SPI_SW_CS_GPIO				HW_SPI_PORT_NSS
-#define SPI_SW_CS_PIN				HW_SPI_PIN_NSS
-#else
-// Note: These values are hardcoded.
-#define SPI_SW_MISO_GPIO			GPIOB
-#define SPI_SW_MISO_PIN				4
-#define SPI_SW_MOSI_GPIO			GPIOB
-#define SPI_SW_MOSI_PIN				5
-#define SPI_SW_SCK_GPIO				GPIOB
-#define SPI_SW_SCK_PIN				3
-#define SPI_SW_CS_GPIO				GPIOB
-#define SPI_SW_CS_PIN				0
-#endif
-#else
-#define SPI_SW_MISO_GPIO			HW_HALL_ENC_GPIO2
-#define SPI_SW_MISO_PIN				HW_HALL_ENC_PIN2
-#if AS504x_USE_SW_MOSI_PIN
-#ifdef HW_SPI_SW_MOSI_GPIO
-#define SPI_SW_MOSI_GPIO 			HW_SPI_SW_MOSI_GPIO
-#define SPI_SW_MOSI_PIN 			HW_SPI_SW_MOSI_PIN
-#else
-#define SPI_SW_MOSI_GPIO 			HW_SPI_PORT_MOSI
-#define SPI_SW_MOSI_PIN 			HW_SPI_PIN_MOSI
-#endif
-#endif
-#define SPI_SW_SCK_GPIO				HW_HALL_ENC_GPIO1
-#define SPI_SW_SCK_PIN				HW_HALL_ENC_PIN1
-#define SPI_SW_CS_GPIO				HW_HALL_ENC_GPIO3
-#define SPI_SW_CS_PIN				HW_HALL_ENC_PIN3
-#endif
-
-// Private types
-typedef enum {
-	ENCODER_MODE_NONE = 0,
-	ENCODER_MODE_ABI,
-	ENCODER_MODE_AS5047P_SPI,
-	RESOLVER_MODE_AD2S1205,
-	ENCODER_MODE_SINCOS,
-	ENCODER_MODE_TS5700N8501,
-	ENCODER_MODE_MT6816_SPI
-} encoder_mode;
-
-// Private variables
-static bool index_found = false;
-static uint32_t enc_counts = 10000;
-static encoder_mode mode = ENCODER_MODE_NONE;
-static float last_enc_angle = 0.0;
-static uint32_t spi_val = 0;
-static uint8_t spi_data_err_raised = 0;
-static uint32_t spi_error_cnt = 0;
-static uint32_t encoder_no_magnet_error_cnt = 0;
-static float spi_error_rate = 0.0;
-static float encoder_no_magnet_error_rate = 0.0;
-static float resolver_loss_of_tracking_error_rate = 0.0;
-static float resolver_degradation_of_signal_error_rate = 0.0;
-static float resolver_loss_of_signal_error_rate = 0.0;
-static uint32_t resolver_loss_of_tracking_error_cnt = 0;
-static uint32_t resolver_degradation_of_signal_error_cnt = 0;
-static uint32_t resolver_loss_of_signal_error_cnt = 0;
-static uint32_t AS504x_spi_communication_error_count = 0;
-static AS504x_diag AS504x_sensor_diag = {0};
-
-static float sin_gain = 0.0;
-static float sin_offset = 0.0;
-static float cos_gain = 0.0;
-static float cos_offset = 0.0;
-static float sincos_filter_constant = 0.0;
-static uint32_t sincos_signal_below_min_error_cnt = 0;
-static uint32_t sincos_signal_above_max_error_cnt = 0;
-static float sincos_signal_low_error_rate = 0.0;
-static float sincos_signal_above_max_error_rate = 0.0;
-
-static SerialConfig TS5700N8501_uart_cfg = {
-		2500000,
-		0,
-		USART_CR2_LINEN,
-		0
-};
-
-//                                                             SPI1        SPI2/3
-#define SPI_BaudRatePrescaler_2         ((uint16_t)0x0000) //  42 MHz      21 MHZ
-#define SPI_BaudRatePrescaler_4         ((uint16_t)0x0008) //  21 MHz      10.5 MHz
-#define SPI_BaudRatePrescaler_8         ((uint16_t)0x0010) //  10.5 MHz    5.25 MHz
-#define SPI_BaudRatePrescaler_16        ((uint16_t)0x0018) //  5.25 MHz    2.626 MHz
-#define SPI_BaudRatePrescaler_32        ((uint16_t)0x0020) //  2.626 MHz   1.3125 MHz
-#define SPI_BaudRatePrescaler_64        ((uint16_t)0x0028) //  1.3125 MHz  656.25 KHz
-#define SPI_BaudRatePrescaler_128       ((uint16_t)0x0030) //  656.25 KHz  328.125 KHz
-#define SPI_BaudRatePrescaler_256       ((uint16_t)0x0038) //  328.125 KHz 164.06 KHz
-#define SPI_DATASIZE_16BIT				SPI_CR1_DFF
-
-#ifdef HW_SPI_DEV
-//MT6816 max clk freq: 15.625MHz
-static const SPIConfig mt6816_spi_cfg = {
-		NULL,
-		SPI_SW_CS_GPIO,
-		SPI_SW_CS_PIN,
-		SPI_BaudRatePrescaler_4 | SPI_CR1_CPOL | SPI_CR1_CPHA | SPI_DATASIZE_16BIT};
-#endif
-
-static THD_FUNCTION(ts5700n8501_thread, arg);
-static THD_WORKING_AREA(ts5700n8501_thread_wa, 512);
-static volatile bool ts5700n8501_stop_now = true;
-static volatile bool ts5700n8501_is_running = false;
-static volatile uint8_t ts5700n8501_raw_status[8] = {0};
-static volatile bool ts5700n8501_reset_errors = false;
-static volatile bool ts5700n8501_reset_multiturn = false;
-
-// Private functions
-static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
-static void spi_begin(void);
-static void spi_end(void);
-static void spi_delay(void);
-static void spi_AS5047_cs_delay(void);
-static void TS5700N8501_send_byte(uint8_t b);
-
-static void AS504x_determinate_if_connected(bool was_last_valid);
-
-#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
-static uint16_t AS504x_diag_fetch_now_count = 0;
-
-static uint8_t AS504x_fetch_diag(void);
-static uint8_t AS504x_verify_serial(void);
-static void AS504x_deserialize_diag(void);
-static void AS504x_fetch_clear_err_diag(void);
-static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length);
-#else
-static uint32_t AS504x_data_last_invalid_counter = 0;
-#endif
-
-uint32_t encoder_spi_get_error_cnt(void) {
-	return spi_error_cnt;
-}
-
-uint32_t encoder_spi_get_val(void) {
-	return spi_val;
-}
-
-float encoder_spi_get_error_rate(void) {
-	return spi_error_rate;
-}
-
-uint32_t encoder_get_no_magnet_error_cnt(void) {
-	return encoder_no_magnet_error_cnt;
-}
-
-float encoder_get_no_magnet_error_rate(void) {
-	return encoder_no_magnet_error_rate;
-}
-
-float encoder_resolver_loss_of_tracking_error_rate(void) {
-	return resolver_loss_of_tracking_error_rate;
-}
-
-float encoder_resolver_degradation_of_signal_error_rate(void) {
-	return resolver_degradation_of_signal_error_rate;
-}
-
-float encoder_resolver_loss_of_signal_error_rate(void) {
-	return resolver_loss_of_signal_error_rate;
-}
-
-uint32_t encoder_resolver_loss_of_tracking_error_cnt(void) {
-	return resolver_loss_of_tracking_error_cnt;
-}
-
-uint32_t encoder_resolver_degradation_of_signal_error_cnt(void) {
-	return resolver_degradation_of_signal_error_cnt;
-}
-
-uint32_t encoder_resolver_loss_of_signal_error_cnt(void) {
-	return resolver_loss_of_signal_error_cnt;
-}
-
-uint32_t encoder_sincos_get_signal_below_min_error_cnt(void) {
-	return sincos_signal_below_min_error_cnt;
-}
-
-uint32_t encoder_sincos_get_signal_above_max_error_cnt(void) {
-	return sincos_signal_above_max_error_cnt;
-}
-
-float encoder_sincos_get_signal_below_min_error_rate(void) {
-	return sincos_signal_low_error_rate;
-}
-
-float encoder_sincos_get_signal_above_max_error_rate(void) {
-	return sincos_signal_above_max_error_rate;
-}
-
-uint8_t* encoder_ts5700n8501_get_raw_status(void) {
-	return (uint8_t*)ts5700n8501_raw_status;
-}
-
-int16_t encoder_ts57n8501_get_abm(void) {
-	return (uint16_t)ts5700n8501_raw_status[4] |
-			((uint16_t)ts5700n8501_raw_status[5] << 8);
-}
-
-void encoder_ts57n8501_reset_errors(void) {
-	ts5700n8501_reset_errors = true;
-}
-
-void encoder_ts57n8501_reset_multiturn(void) {
-	ts5700n8501_reset_multiturn = true;
-}
-
-void encoder_deinit(void) {
-	nvicDisableVector(HW_ENC_EXTI_CH);
-	nvicDisableVector(HW_ENC_TIM_ISR_CH);
-
-	TIM_DeInit(HW_ENC_TIM);
-
-	palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT_PULLUP);
-	palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_INPUT_PULLUP);
-	palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_INPUT_PULLUP);
-
-#ifdef HW_SPI_DEV
-	spiStop(&HW_SPI_DEV);
-#endif
-
-	palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP);
-	palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
-
-	if (mode == ENCODER_MODE_TS5700N8501) {
-		ts5700n8501_stop_now = true;
-		while (ts5700n8501_is_running) {
-			chThdSleepMilliseconds(1);
-		}
-
-		palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT_PULLUP);
-		palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT_PULLUP);
-#ifdef HW_ADC_EXT_GPIO
-		palSetPadMode(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN, PAL_MODE_INPUT_ANALOG);
-#endif
-	}
-
-	index_found = false;
-	mode = ENCODER_MODE_NONE;
-	last_enc_angle = 0.0;
-	spi_error_rate = 0.0;
-	sincos_signal_low_error_rate = 0.0;
-	sincos_signal_above_max_error_rate = 0.0;
-}
-
-void encoder_init_abi(uint32_t counts) {
-	EXTI_InitTypeDef   EXTI_InitStructure;
-
-	// Initialize variables
-	index_found = false;
-	enc_counts = counts;
-
-	palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
-	palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
-//	palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_ALTERNATE(HW_ENC_TIM_AF));
-
-	// Enable timer clock
-	HW_ENC_TIM_CLK_EN();
-
-	// 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, enc_counts - 1);
-
-	// Filter
-	HW_ENC_TIM->CCMR1 |= 6 << 12 | 6 << 4;
-	HW_ENC_TIM->CCMR2 |= 6 << 4;
-
-	TIM_Cmd(HW_ENC_TIM, ENABLE);
-
-	// Interrupt on index pulse
-
-	// Connect EXTI Line to pin
-	SYSCFG_EXTILineConfig(HW_ENC_EXTI_PORTSRC, HW_ENC_EXTI_PINSRC);
-
-	// Configure EXTI Line
-	EXTI_InitStructure.EXTI_Line = HW_ENC_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);
-
-	// Enable and set EXTI Line Interrupt to the highest priority
-	nvicEnableVector(HW_ENC_EXTI_CH, 0);
-
-	mode = ENCODER_MODE_ABI;
-}
-
-void encoder_init_as5047p_spi(void) {
-	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-
-	palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
-	palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-
-	// Set MOSI to 1
-#if (AS5047_USE_HW_SPI_PINS || AD2S1205_USE_HW_SPI_PINS || AS504x_USE_SW_MOSI_PIN )
-	palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPad(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN);
-#endif
-
-	// Enable timer clock
-	HW_ENC_TIM_CLK_EN();
-
-	// Time Base configuration
-	TIM_TimeBaseStructure.TIM_Prescaler = 0;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-	TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AS5047_SAMPLE_RATE_HZ) - 1);
-	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
-	TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
-
-	// Enable overflow interrupt
-	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
-
-	// Enable timer
-	TIM_Cmd(HW_ENC_TIM, ENABLE);
-
-	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
-
-	mode = ENCODER_MODE_AS5047P_SPI;
-	index_found = true;
-	spi_error_rate = 0.0;
-
-}
-
-void encoder_init_mt6816_spi(void) {
-#ifdef HW_SPI_DEV
-	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-
-	palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-
-#if (MT6816_USE_HW_SPI_PINS)
-	palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_ALTERNATE(6) | PAL_STM32_OSPEED_HIGHEST);
-#endif
-
-	//Start driver with MT6816 SPI settings
-	spiStart(&HW_SPI_DEV, &mt6816_spi_cfg);
-
-	// Enable timer clock
-	HW_ENC_TIM_CLK_EN();
-
-	// Time Base configuration
-	TIM_TimeBaseStructure.TIM_Prescaler = 0;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-	TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / MT6816_SAMPLE_RATE_HZ) - 1);
-	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
-	TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
-
-	// Enable overflow interrupt
-	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
-
-	// Enable timer
-	TIM_Cmd(HW_ENC_TIM, ENABLE);
-
-	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
-
-	mode = ENCODER_MODE_MT6816_SPI;
-	index_found = true;
-	spi_error_rate = 0.0;
-	encoder_no_magnet_error_rate = 0.0;
-#endif
-}
-
-void encoder_init_ad2s1205_spi(void) {
-	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
-
-	resolver_loss_of_tracking_error_rate = 0.0;
-	resolver_degradation_of_signal_error_rate = 0.0;
-	resolver_loss_of_signal_error_rate = 0.0;
-	resolver_loss_of_tracking_error_cnt = 0;
-	resolver_loss_of_signal_error_cnt = 0;
-
-	palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
-	palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-
-	// Set MOSI to 1
-#if (AS5047_USE_HW_SPI_PINS || AD2S1205_USE_HW_SPI_PINS)
-	palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPad(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN);
-#endif
-
-	// TODO: Choose pins on comm port when these are not defined
-#if defined(AD2S1205_SAMPLE_GPIO)
-	palSetPadMode(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);	// Prepare for a falling edge SAMPLE assertion
-#endif
-#if defined(AD2S1205_RDVEL_GPIO)
-	palSetPadMode(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN);		// Will always read position
-#endif
-
-
-	// Enable timer clock
-	HW_ENC_TIM_CLK_EN();
-
-	// Time Base configuration
-	TIM_TimeBaseStructure.TIM_Prescaler = 0;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-	TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AD2S1205_SAMPLE_RATE_HZ) - 1);
-	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
-	TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
-
-	// Enable overflow interrupt
-	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
-
-	// Enable timer
-	TIM_Cmd(HW_ENC_TIM, ENABLE);
-
-	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
-
-	mode = RESOLVER_MODE_AD2S1205;
-	index_found = true;
-}
-
-void encoder_init_sincos(float s_gain, float s_offset,
-						 float c_gain, float c_offset, float filter_constant) {
-	//ADC inputs are already initialized in hw_init_gpio()
-	sin_gain = s_gain;
-	sin_offset = s_offset;
-	cos_gain = c_gain;
-	cos_offset = c_offset;
-	sincos_filter_constant = filter_constant;
-
-	sincos_signal_below_min_error_cnt = 0;
-	sincos_signal_above_max_error_cnt = 0;
-	sincos_signal_low_error_rate = 0.0;
-	sincos_signal_above_max_error_rate = 0.0;
-
-	// ADC measurements needs to be in sync with motor PWM
-#ifdef HW_HAS_SIN_COS_ENCODER
-	mode = ENCODER_MODE_SINCOS;
-	index_found = true;
-#else
-	mode = ENCODER_MODE_NONE;
-	index_found = false;
-#endif
-}
-
-void encoder_init_ts5700n8501(void) {
-	mode = ENCODER_MODE_TS5700N8501;
-	index_found = true;
-	spi_error_rate = 0.0;
-	spi_error_cnt = 0;
-	ts5700n8501_is_running = true;
-	ts5700n8501_stop_now = false;
-
-	chThdCreateStatic(ts5700n8501_thread_wa, sizeof(ts5700n8501_thread_wa),
-			NORMALPRIO - 10, ts5700n8501_thread, NULL);
-}
-
-bool encoder_is_configured(void) {
-	return mode != ENCODER_MODE_NONE;
-}
-
-/**
- * Read angle from configured encoder.
- *
- * @return
- * The current encoder angle in degrees.
- */
-float encoder_read_deg(void) {
-	static float angle = 0.0;
-
-	switch (mode) {
-	case ENCODER_MODE_ABI:
-		angle = ((float)HW_ENC_TIM->CNT * 360.0) / (float)enc_counts;
-		break;
-
-	case ENCODER_MODE_AS5047P_SPI:
-	case ENCODER_MODE_MT6816_SPI:
-	case RESOLVER_MODE_AD2S1205:
-	case ENCODER_MODE_TS5700N8501:
-		angle = last_enc_angle;
-		break;
-
-#ifdef HW_HAS_SIN_COS_ENCODER
-	case ENCODER_MODE_SINCOS: {
-		float sin = ENCODER_SIN_VOLTS * sin_gain - sin_offset;
-		float cos = ENCODER_COS_VOLTS * cos_gain - cos_offset;
-
-		float module = SQ(sin) + SQ(cos);
-
-		if (module > SQ(SINCOS_MAX_AMPLITUDE) )	{
-			// signals vector outside of the valid area. Increase error count and discard measurement
-			++sincos_signal_above_max_error_cnt;
-			UTILS_LP_FAST(sincos_signal_above_max_error_rate, 1.0, 1./SINCOS_SAMPLE_RATE_HZ);
-			angle = last_enc_angle;
-		} else {
-			if (module < SQ(SINCOS_MIN_AMPLITUDE)) {
-				++sincos_signal_below_min_error_cnt;
-				UTILS_LP_FAST(sincos_signal_low_error_rate, 1.0, 1./SINCOS_SAMPLE_RATE_HZ);
-				angle = last_enc_angle;
-			} else {
-				UTILS_LP_FAST(sincos_signal_above_max_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
-				UTILS_LP_FAST(sincos_signal_low_error_rate, 0.0, 1./SINCOS_SAMPLE_RATE_HZ);
-
-				float angle_tmp = RAD2DEG_f(utils_fast_atan2(sin, cos));
-				UTILS_LP_FAST(angle, angle_tmp, sincos_filter_constant);
-				last_enc_angle = angle;
-			}
-		}
-		break;
-	}
-#endif
-
-	default:
-		break;
-	}
-
-	return angle;
-}
-
-/*
- * Note: This is not a good solution and needs a proper implementation later...
- */
-float encoder_read_deg_multiturn(void) {
-	if (mode == ENCODER_MODE_TS5700N8501) {
-		encoder_ts57n8501_get_abm();
-		float ts_mt = (float)encoder_ts57n8501_get_abm();
-		if (fabsf(ts_mt) > 5000.0) {
-			ts_mt = 0;
-			encoder_ts57n8501_reset_multiturn();
-		}
-
-		ts_mt += 5000;
-
-		return encoder_read_deg() / 10000.0 + (360 * ts_mt) / 10000.0;
-	} else {
-		return encoder_read_deg();
-	}
-}
-
-/**
- * Reset the encoder counter. Should be called from the index interrupt.
- */
-void encoder_reset(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 = enc_counts / 20;
-
-		if (index_found) {
-			// Some plausibility filtering.
-			if (cnt > (enc_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;
-		}
-	}
-}
-
-// returns true for even number of ones (no parity error according to AS5047 datasheet
-static bool spi_check_parity(uint16_t x) {
-	x ^= x >> 8;
-	x ^= x >> 4;
-	x ^= x >> 2;
-	x ^= x >> 1;
-	return (~x) & 1;
-}
-
-#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
-static uint8_t AS504x_fetch_diag(void) {
-	uint16_t recf[2], senf[2] = {AS504x_SPI_READ_DIAG_MSG, AS504x_SPI_READ_MAGN_MSG};
-	uint8_t ret = 0;
-
-
-	spi_begin();
-	spi_transfer(0, senf, 1);
-	spi_end();
-
-	spi_AS5047_cs_delay();
-
-	spi_begin();
-	ret |= AS504x_spi_transfer_err_check(recf, senf + 1, 1);
-	spi_end();
-
-	spi_AS5047_cs_delay();
-
-	spi_begin();
-	ret |= AS504x_spi_transfer_err_check(recf + 1, 0, 1);
-	spi_end();
-
-	if(!ret) {
-		if (spi_check_parity(recf[0]) && spi_check_parity(recf[1])) {
-			AS504x_sensor_diag.serial_diag_flgs = recf[0];
-			AS504x_sensor_diag.serial_magnitude = recf[1];
-		}
-	}
-
-	return ret;
-}
-
-/*
- * This function fetches error flag from AS504x and afterwards clean error flag
- */
-static void AS504x_fetch_clear_err_diag() {
-	uint16_t recf, senf = AS504x_SPI_READ_CLEAR_ERROR_MSG;
-
-	spi_begin();
-	spi_transfer(0, &senf, 1);
-	spi_end();
-
-	spi_AS5047_cs_delay();
-
-	spi_begin();
-	spi_transfer(&recf, 0, 1);
-	spi_end();
-
-	AS504x_sensor_diag.serial_error_flags = recf;
-}
-
-/*
- * Try verify if the diagnostics are not corrupt
- * This function can prevent deserialazing corrupted data if the MISO bus is HIGH or LOW
- */
-static uint8_t AS504x_verify_serial() {
-	uint16_t serial_diag_flgs, serial_magnitude, test_magnitude;
-	uint8_t test_AGC_value, test_is_Comp_high, test_is_Comp_low;
-
-	serial_magnitude = encoder_AS504x_get_diag().serial_magnitude;
-	serial_diag_flgs = encoder_AS504x_get_diag().serial_diag_flgs;
-
-	test_magnitude = serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
-	test_AGC_value = serial_diag_flgs;
-	test_is_Comp_low = (serial_diag_flgs >> AS504x_SPI_DIAG_COMP_LOW_BIT_POS) & 1;
-	test_is_Comp_high = (serial_diag_flgs >> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS) & 1;
-
-	if (test_is_Comp_high && test_is_Comp_low) {
-		return 1;
-	}
-	if((uint32_t)test_magnitude + (uint32_t)test_AGC_value == 0) {
-		return 1;
-	}
-
-	return 0;
-}
-
-static void AS504x_deserialize_diag() {
-	AS504x_sensor_diag.AGC_value = AS504x_sensor_diag.serial_diag_flgs;
-	AS504x_sensor_diag.is_OCF = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_OCF_BIT_POS) & 1;
-	AS504x_sensor_diag.is_COF = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COF_BIT_POS) & 1;
-	AS504x_sensor_diag.is_Comp_low = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COMP_LOW_BIT_POS) & 1;
-	AS504x_sensor_diag.is_Comp_high = (AS504x_sensor_diag.serial_diag_flgs >> AS504x_SPI_DIAG_COMP_HIGH_BIT_POS) & 1;
-	AS504x_sensor_diag.magnitude = AS504x_sensor_diag.serial_magnitude & AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK;
-}
-
-static uint8_t AS504x_spi_transfer_err_check(uint16_t *in_buf, const uint16_t *out_buf, int length) {
-	spi_transfer(in_buf, out_buf, length);
-
-	for(int len_count = 0; len_count < length; len_count++) {
-		if(((in_buf[len_count]) >> 14) & 0b01) {
-			return 1;
-		}
-	}
-
-	return 0;
-}
-#endif
-
-/*
- * Determinate if is connected depending on last retieved data.
- */
-static void AS504x_determinate_if_connected(bool was_last_valid) {
-	if(!was_last_valid) {
-		AS504x_spi_communication_error_count++;
-
-		if(AS504x_spi_communication_error_count >= AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD) {
-			AS504x_spi_communication_error_count = AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD;
-			AS504x_sensor_diag.is_connected = 0;
-		}
-	} else {
-		if(AS504x_spi_communication_error_count) {
-			AS504x_spi_communication_error_count--;
-		} else {
-			AS504x_sensor_diag.is_connected = 1;
-		}
-	}
-}
-
-AS504x_diag encoder_AS504x_get_diag(void) {
-	return AS504x_sensor_diag;
-}
-
-void encoder_tim_isr(void) {
-	uint16_t pos;
-
-	if(mode == ENCODER_MODE_AS5047P_SPI) {
-// if MOSI is defined, use diagnostics
-#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
-		spi_begin();
-		spi_transfer(0, 0, 1);
-		spi_end();
-
-		spi_AS5047_cs_delay();
-
-		spi_begin();
-		spi_data_err_raised = AS504x_spi_transfer_err_check(&pos, 0, 1);
-		spi_end();
-		spi_val = pos;
-
-		// get diagnostic every AS504x_REFRESH_DIAG_AFTER_NSAMPLES
-		AS504x_diag_fetch_now_count++;
-		if(AS504x_diag_fetch_now_count >= AS504x_REFRESH_DIAG_AFTER_NSAMPLES || spi_data_err_raised) {
-			// clear error flags before getting new diagnostics data
-			AS504x_fetch_clear_err_diag();
-
-			if(!AS504x_fetch_diag()) {
-				if(!AS504x_verify_serial()) {
-					AS504x_deserialize_diag();
-					AS504x_determinate_if_connected(true);
-				} else {
-					AS504x_determinate_if_connected(false);
-				}
-			} else {
-				AS504x_determinate_if_connected(false);
-			}
-			AS504x_diag_fetch_now_count = 0;
-		}
-#else
-		spi_begin();
-		spi_transfer(&pos, 0, 1);
-		spi_end();
-		spi_val = pos;
-
-		if(0x0000 == pos || 0xFFFF == pos) {
-			AS504x_data_last_invalid_counter++;
-		} else {
-			AS504x_data_last_invalid_counter = 0;
-			AS504x_determinate_if_connected(true);
-		}
-
-		if (AS504x_data_last_invalid_counter >= AS504x_DATA_INVALID_THRESHOLD) {
-			AS504x_determinate_if_connected(false);
-			AS504x_data_last_invalid_counter = AS504x_DATA_INVALID_THRESHOLD;
-		}
-#endif
-
-		if(spi_check_parity(pos) && !spi_data_err_raised) {
-			pos &= 0x3FFF;
-			last_enc_angle = ((float)pos * 360.0) / 16384.0;
-			UTILS_LP_FAST(spi_error_rate, 0.0, 1./AS5047_SAMPLE_RATE_HZ);
-		} else {
-			++spi_error_cnt;
-			UTILS_LP_FAST(spi_error_rate, 1.0, 1./AS5047_SAMPLE_RATE_HZ);
-		}		
-	}
-
-#ifdef HW_SPI_DEV
-	if(mode == ENCODER_MODE_MT6816_SPI) {
-		uint16_t reg_data_03;
-		uint16_t reg_data_04;
-		uint16_t reg_addr_03 = 0x8300;
-		uint16_t reg_addr_04 = 0x8400;
-
-		spi_begin();
-		reg_data_03 = spiPolledExchange(&HW_SPI_DEV, reg_addr_03);
-		spi_end();
-		spi_delay();
-		spi_begin();
-		reg_data_04 = spiPolledExchange(&HW_SPI_DEV, reg_addr_04);
-		spi_end();
-
-		pos = (reg_data_03 << 8) | reg_data_04;
-		spi_val = pos;
-
-		if( spi_check_parity(pos) ) {
-			if (pos & MT6816_NO_MAGNET_ERROR_MASK) {
-				++encoder_no_magnet_error_cnt;
-				UTILS_LP_FAST(encoder_no_magnet_error_rate, 1.0, 1./MT6816_SAMPLE_RATE_HZ);
-			} else {
-				pos = pos >> 2;
-				last_enc_angle = ((float)pos * 360.0) / 16384.0;
-				UTILS_LP_FAST(spi_error_rate, 0.0, 1./MT6816_SAMPLE_RATE_HZ);
-				UTILS_LP_FAST(encoder_no_magnet_error_rate, 0.0, 1./MT6816_SAMPLE_RATE_HZ);
-			}
-		} else {
-			++spi_error_cnt;
-			UTILS_LP_FAST(spi_error_rate, 1.0, 1./MT6816_SAMPLE_RATE_HZ);
-		}
-	}
-#endif
-
-	if (mode == RESOLVER_MODE_AD2S1205) {
-		// SAMPLE signal should have been be asserted in sync with ADC sampling
-#ifdef AD2S1205_RDVEL_GPIO
-		palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN);	// Always read position
-#endif
-
-		palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
-		spi_delay();
-		spi_begin(); // CS uses the same mcu pin as AS5047
-		spi_delay();
-
-		spi_transfer(&pos, 0, 1);
-		spi_end();
-
-		spi_val = pos;
-
-		uint16_t RDVEL = pos & 0x0008; // 1 means a position read
-
-		if((RDVEL != 0)){
-
-			bool DOS = ((pos & 0x04) == 0);
-			bool LOT = ((pos & 0x02) == 0);
-			bool LOS = DOS && LOT;
-			bool parity_error = spi_check_parity(pos);	//16 bit frame has odd parity
-			bool angle_is_correct = true;
-
-			if(LOS) {
-				LOT = DOS = 0;
-			}
-
-			if(!parity_error) {
-				UTILS_LP_FAST(spi_error_rate, 0.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			} else {
-				angle_is_correct = false;
-				++spi_error_cnt;
-				UTILS_LP_FAST(spi_error_rate, 1.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			}
-
-			pos &= 0xFFF0;
-			pos = pos >> 4;
-			pos &= 0x0FFF;
-
-			if(LOT) {
-				angle_is_correct = false;
-				++resolver_loss_of_tracking_error_cnt;
-				UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 1.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			} else {
-				UTILS_LP_FAST(resolver_loss_of_tracking_error_rate, 0.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			}
-
-			if(DOS) {
-				angle_is_correct = false;
-				++resolver_degradation_of_signal_error_cnt;
-				UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 1.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			} else {
-				UTILS_LP_FAST(resolver_degradation_of_signal_error_rate, 0.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			}
-
-			if(LOS) {
-				angle_is_correct = false;
-				++resolver_loss_of_signal_error_cnt;
-				UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 1.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			} else {
-				UTILS_LP_FAST(resolver_loss_of_signal_error_rate, 0.0, 1./AD2S1205_SAMPLE_RATE_HZ);
-			}
-
-			if(angle_is_correct)
-			{
-				last_enc_angle = ((float)pos * 360.0) / 4096.0;
-			}
-		}
-	}
-}
-
-/**
- * Set the number of encoder counts.
- *
- * @param counts
- * The number of encoder counts
- */
-void encoder_set_counts(uint32_t counts) {
-	if (counts != enc_counts) {
-		enc_counts = counts;
-		TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1);
-		index_found = false;
-	}
-}
-
-/**
- * Check if the index pulse is found.
- *
- * @return
- * True if the index is found, false otherwise.
- */
-bool encoder_index_found(void) {
-	return index_found;
-}
-
-// Software SPI
-static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
-	for (int i = 0;i < length;i++) {
-
-#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
-		uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
-#else
-		(void)out_buf;
-#endif
-
-		uint16_t receive = 0;
-
-		for (int bit = 0;bit < 16;bit++) {
-#if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
-			palWritePad(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, send >> (15 - bit));
-#endif
-
-			palSetPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
-			spi_delay();
-
-			int samples = 0;
-			samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
-			__NOP();
-			samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
-			__NOP();
-			samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
-			__NOP();
-			samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
-			__NOP();
-			samples += palReadPad(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN);
-
-			receive <<= 1;
-			if (samples > 2) {
-				receive |= 1;
-			}
-
-			palClearPad(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN);
-			spi_delay();
-		}
-
-		if (in_buf) {
-			in_buf[i] = receive;
-		}
-	}
-}
-
-static void spi_begin(void) {
-	palClearPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
-	spi_AS5047_cs_delay();
-}
-
-static void spi_end(void) {
-	palSetPad(SPI_SW_CS_GPIO, SPI_SW_CS_PIN);
-	spi_AS5047_cs_delay();
-}
-
-static void spi_delay(void) {
-	__NOP();
-	__NOP();
-	__NOP();
-	__NOP();
-}
-
-static void spi_AS5047_cs_delay(void) {
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();__NOP();__NOP();
-	__NOP();
-}
-
-#pragma GCC push_options
-#pragma GCC optimize ("O0")
-
-void TS5700N8501_delay_uart(void) {
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP();
-}
-
-/*
- * It is important to switch to receive mode immediately after sending the readout command,
- * as the TS5700N8501 starts sending the reply after 3 microseconds. Therefore use software
- * UART on TX so that the enable signal can be controlled manually. This function runs while
- * the system is locked, but it should finish fast enough to not cause problems for other
- * things due to the high baud rate.
- */
-static void TS5700N8501_send_byte(uint8_t b) {
-	utils_sys_lock_cnt();
-#ifdef HW_ADC_EXT_GPIO
-	palSetPad(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN);
-#endif
-	TS5700N8501_delay_uart();
-	palWritePad(HW_UART_TX_PORT, HW_UART_TX_PIN, 0);
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	for (int i = 0;i < 8;i++) {
-		palWritePad(HW_UART_TX_PORT, HW_UART_TX_PIN,
-				(b & (0x80 >> i)) ? PAL_HIGH : PAL_LOW);
-		TS5700N8501_delay_uart();
-	}
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	__NOP(); __NOP(); __NOP();
-	palWritePad(HW_UART_TX_PORT, HW_UART_TX_PIN, 1);
-	TS5700N8501_delay_uart();
-#ifdef HW_ADC_EXT_GPIO
-	palClearPad(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN);
-#endif
-	utils_sys_unlock_cnt();
-}
-
-#pragma GCC pop_options
-
-static THD_FUNCTION(ts5700n8501_thread, arg) {
-	(void)arg;
-
-	chRegSetThreadName("TS5700N8501");
-
-	sdStart(&HW_UART_DEV, &TS5700N8501_uart_cfg);
-	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_OUTPUT_PUSHPULL |
-			PAL_STM32_OSPEED_HIGHEST |
-			PAL_STM32_PUDR_PULLUP);
-	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
-			PAL_STM32_OSPEED_HIGHEST |
-			PAL_STM32_PUDR_PULLUP);
-#ifdef HW_ADC_EXT_GPIO
-	palSetPadMode(HW_ADC_EXT_GPIO, HW_ADC_EXT_PIN, PAL_MODE_OUTPUT_PUSHPULL |
-			PAL_STM32_OSPEED_HIGHEST |
-			PAL_STM32_PUDR_PULLUP);
-#endif
-
-	for(;;) {
-		// Check if it is time to stop.
-		if (ts5700n8501_stop_now) {
-			ts5700n8501_is_running = false;
-			return;
-		}
-
-		if (ts5700n8501_reset_errors) {
-			for (int i = 0;i < 20;i++) {
-				TS5700N8501_send_byte(0b01011101);
-				chThdSleep(2);
-			}
-
-			ts5700n8501_reset_errors = false;
-		}
-
-		if (ts5700n8501_reset_multiturn) {
-			for (int i = 0;i < 20;i++) {
-				TS5700N8501_send_byte(0b01000110);
-				chThdSleep(2);
-			}
-
-			ts5700n8501_reset_multiturn = false;
-		}
-
-		TS5700N8501_send_byte(0b01011000);
-
-		chThdSleep(2);
-
-		uint8_t reply[11];
-		int reply_ind = 0;
-
-		msg_t res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
-		while (res != MSG_TIMEOUT) {
-			if (reply_ind < (int)sizeof(reply)) {
-				reply[reply_ind++] = res;
-			}
-			res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
-		}
-
-		uint8_t crc = 0;
-		for (int i = 0;i < (reply_ind - 1);i++) {
-			crc = (reply[i] ^ crc);
-		}
-
-		if (reply_ind == 11 && crc == reply[reply_ind - 1]) {
-			uint32_t pos = (uint32_t)reply[2] + ((uint32_t)reply[3] << 8) + ((uint32_t)reply[4] << 16);
-			spi_val = pos;
-			last_enc_angle = (float)pos / 131072.0 * 360.0;
-			UTILS_LP_FAST(spi_error_rate, 0.0, 1.0 / AS5047_SAMPLE_RATE_HZ);
-
-			ts5700n8501_raw_status[0] = reply[1]; // SF
-			ts5700n8501_raw_status[1] = reply[2]; // ABS0
-			ts5700n8501_raw_status[2] = reply[3]; // ABS1
-			ts5700n8501_raw_status[3] = reply[4]; // ABS2
-			ts5700n8501_raw_status[4] = reply[6]; // ABM0
-			ts5700n8501_raw_status[5] = reply[7]; // ABM1
-			ts5700n8501_raw_status[6] = reply[8]; // ABM2
-			ts5700n8501_raw_status[7] = reply[9]; // ALMC
-		} else {
-			++spi_error_cnt;
-			UTILS_LP_FAST(spi_error_rate, 1.0, 1.0 / AS5047_SAMPLE_RATE_HZ);
-		}
-	}
-}
-
diff --git a/encoder.h b/encoder.h
deleted file mode 100644
index 3b120d95..00000000
--- a/encoder.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
-
-	This file is part of the VESC firmware.
-
-	The VESC firmware is free software: you can redistribute it and/or modify
-    it under the terms of the GNU General Public License as published by
-    the Free Software Foundation, either version 3 of the License, or
-    (at your option) any later version.
-
-    The VESC firmware is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-    GNU General Public License for more details.
-
-    You should have received a copy of the GNU General Public License
-    along with this program.  If not, see <http://www.gnu.org/licenses/>.
-    */
-
-#ifndef ENCODER_H_
-#define ENCODER_H_
-
-#include "conf_general.h"
-
-// Functions
-void encoder_deinit(void);
-void encoder_init_abi(uint32_t counts);
-void encoder_init_as5047p_spi(void);
-void encoder_init_mt6816_spi(void);
-void encoder_init_ad2s1205_spi(void);
-void encoder_init_sincos(float sin_gain, float sin_offset,
-						 float cos_gain, float cos_offset, float sincos_filter_constant);
-void encoder_init_ts5700n8501(void);
-bool encoder_is_configured(void);
-float encoder_read_deg(void);
-float encoder_read_deg_multiturn(void);
-void encoder_reset(void);
-void encoder_tim_isr(void);
-void encoder_set_counts(uint32_t counts);
-bool encoder_index_found(void);
-
-uint32_t encoder_spi_get_val(void);
-uint32_t encoder_spi_get_error_cnt(void);
-float encoder_spi_get_error_rate(void);
-uint32_t encoder_get_no_magnet_error_cnt(void);
-float encoder_get_no_magnet_error_rate(void);
-uint32_t encoder_resolver_loss_of_tracking_error_cnt(void);
-uint32_t encoder_resolver_degradation_of_signal_error_cnt(void);
-uint32_t encoder_resolver_loss_of_signal_error_cnt(void);
-float encoder_resolver_loss_of_tracking_error_rate(void);
-float encoder_resolver_degradation_of_signal_error_rate(void);
-float encoder_resolver_loss_of_signal_error_rate(void);
-uint32_t encoder_sincos_get_signal_below_min_error_cnt(void);
-uint32_t encoder_sincos_get_signal_above_max_error_cnt(void);
-float encoder_sincos_get_signal_below_min_error_rate(void);
-float encoder_sincos_get_signal_above_max_error_rate(void);
-uint8_t* encoder_ts5700n8501_get_raw_status(void);
-int16_t encoder_ts57n8501_get_abm(void);
-void encoder_ts57n8501_reset_errors(void);
-void encoder_ts57n8501_reset_multiturn(void);
-AS504x_diag encoder_AS504x_get_diag(void);
-
-#define AS504x_SPI_READ_BIT 								0x4000
-#define AS504x_SPI_WRITE_BIT 								0x0000
-
-#define AS504x_SPI_DIAG_OCF_BIT_POS							8
-#define AS504x_SPI_DIAG_COF_BIT_POS							9
-#define AS504x_SPI_DIAG_COMP_LOW_BIT_POS					10
-#define AS504x_SPI_DIAG_COMP_HIGH_BIT_POS					11
-
-
-#define AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK			0x3FFF
-
-
-#define AS504x_SPI_DIAG_ADR									0x3FFD
-#define AS504x_SPI_MAGN_ADR									0x3FFE
-#define AS504x_SPI_CLEAR_ERROR_ADR							0x0001
-
-#define AS504x_SPI_READ_DIAG_MSG							(AS504x_SPI_DIAG_ADR | AS504x_SPI_READ_BIT)
-#define AS504x_SPI_READ_MAGN_MSG							(AS504x_SPI_MAGN_ADR | AS504x_SPI_READ_BIT)
-#define AS504x_SPI_READ_CLEAR_ERROR_MSG						(AS504x_SPI_CLEAR_ERROR_ADR | AS504x_SPI_READ_BIT)
-
-#define AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD		5
-
-#define AS504x_DATA_INVALID_THRESHOLD						20000
-#define AS504x_REFRESH_DIAG_AFTER_NSAMPLES					100
-
-#endif /* ENCODER_H_ */
diff --git a/encoder/ABI.c b/encoder/ABI.c
index e43101cf..485dd4b1 100644
--- a/encoder/ABI.c
+++ b/encoder/ABI.c
@@ -73,11 +73,12 @@ encoders_ret_t ABI_init(ABI_config_t *abi_config) {
 	EXTI_InitStructure.EXTI_LineCmd = ENABLE;
 	EXTI_Init(&EXTI_InitStructure);
 
+	abi_config->is_init = 1;
+	abi_config_now = *abi_config;
+
 	// Enable and set EXTI Line Interrupt to the highest priority
 	nvicEnableVector(HW_ENC_EXTI_CH, 0);
-	abi_config->is_init = 1;
 
-	abi_config_now = *abi_config;
 	return ENCODERS_OK;
 }
 
@@ -86,3 +87,10 @@ float ABI_read_deg(void) {
 	return last_enc_angle;
 }
 
+void ABI_set_counts(uint32_t counts) {
+	if (counts != enc_counts) {
+		enc_counts = counts;
+		TIM_SetAutoreload(HW_ENC_TIM, enc_counts - 1);
+	}
+}
+
diff --git a/encoder/ABI.h b/encoder/ABI.h
index 2a6b8377..840bc896 100644
--- a/encoder/ABI.h
+++ b/encoder/ABI.h
@@ -10,4 +10,6 @@ encoders_ret_t ABI_init(ABI_config_t *abi_config);
 
 float ABI_read_deg(void);
 
+void ABI_set_counts(uint32_t counts);
+
 #endif /* ENCODER_ABI_H_ */
diff --git a/encoder/AD2S1205.c b/encoder/AD2S1205.c
index 903be428..dc7e97be 100644
--- a/encoder/AD2S1205.c
+++ b/encoder/AD2S1205.c
@@ -94,8 +94,6 @@ encoders_ret_t AD2S1205_init(AD2S1205_config_t *AD2S1205_config) {
 	palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN);		// Will always read position
 #endif
 
-	AD2S1205_config_now = *AD2S1205_config;
-
 	// Enable timer clock
 	HW_ENC_TIM_CLK_EN();
 
@@ -110,18 +108,21 @@ encoders_ret_t AD2S1205_init(AD2S1205_config_t *AD2S1205_config) {
 
 	// Enable overflow interrupt
 	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
-
 	// Enable timer
 	TIM_Cmd(HW_ENC_TIM, ENABLE);
 
+	AD2S1205_config->is_init = 1;
+	AD2S1205_config_now = *AD2S1205_config;
+
 	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
 
-	AD2S1205_config_now.is_init = 1;
-	AD2S1205_config->is_init = 1;
-
 	return ENCODERS_OK;
 }
 
+float AD2S1205_read_deg(void) {
+	return last_enc_angle;
+}
+
 void AD2S1205_routine(void) {
 	uint16_t pos;
 	// SAMPLE signal should have been be asserted in sync with ADC sampling
@@ -233,10 +234,6 @@ uint32_t AD2S1205_spi_get_error_cnt(void) {
 	return spi_error_cnt;
 }
 
-float AD2S1205_read_deg(void) {
-	return last_enc_angle;
-}
-
 static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
 	const encoders_gpio_t gpio_miso = AD2S1205_config_now.spi_config.gpio_miso;
 #if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
diff --git a/encoder/AD2S1205.h b/encoder/AD2S1205.h
index a9b67d71..38cf8a4f 100644
--- a/encoder/AD2S1205.h
+++ b/encoder/AD2S1205.h
@@ -11,7 +11,6 @@ encoders_ret_t AD2S1205_init(AD2S1205_config_t *AD2S1205_config);
 float AD2S1205_read_deg(void);
 void AD2S1205_routine(void);
 
-bool AD2S1205_index_found(void);
 float AD2S1205_resolver_loss_of_tracking_error_rate(void);
 float AD2S1205_resolver_degradation_of_signal_error_rate(void);
 float AD2S1205_resolver_loss_of_signal_error_rate(void);
diff --git a/encoder/AS504x.c b/encoder/AS504x.c
index 6b43e158..cceaa1a5 100644
--- a/encoder/AS504x.c
+++ b/encoder/AS504x.c
@@ -9,6 +9,31 @@
 #include "utils.h"
 #include <math.h>
 
+#define AS504x_SPI_READ_BIT 								0x4000
+#define AS504x_SPI_WRITE_BIT 								0x0000
+
+#define AS504x_SPI_DIAG_OCF_BIT_POS							8
+#define AS504x_SPI_DIAG_COF_BIT_POS							9
+#define AS504x_SPI_DIAG_COMP_LOW_BIT_POS					10
+#define AS504x_SPI_DIAG_COMP_HIGH_BIT_POS					11
+
+#define AS5047_SAMPLE_RATE_HZ		20000
+
+#define AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK			0x3FFF
+
+#define AS504x_SPI_DIAG_ADR									0x3FFD
+#define AS504x_SPI_MAGN_ADR									0x3FFE
+#define AS504x_SPI_CLEAR_ERROR_ADR							0x0001
+
+#define AS504x_SPI_READ_DIAG_MSG							(AS504x_SPI_DIAG_ADR | AS504x_SPI_READ_BIT)
+#define AS504x_SPI_READ_MAGN_MSG							(AS504x_SPI_MAGN_ADR | AS504x_SPI_READ_BIT)
+#define AS504x_SPI_READ_CLEAR_ERROR_MSG						(AS504x_SPI_CLEAR_ERROR_ADR | AS504x_SPI_READ_BIT)
+
+#define AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD		5
+
+#define AS504x_DATA_INVALID_THRESHOLD						20000
+#define AS504x_REFRESH_DIAG_AFTER_NSAMPLES					100
+
 //Private variables
 #if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
 static uint16_t AS504x_diag_fetch_now_count = 0;
@@ -181,8 +206,6 @@ encoders_ret_t AS504x_init(AS504x_config_t *AS504x_config) {
 			AS504x_spi_config.gpio_mosi.pin);
 #endif
 
-	AS504x_config_now = *AS504x_config;
-
 	HW_ENC_TIM_CLK_EN();
 
 	// Time Base configuration
@@ -198,12 +221,13 @@ encoders_ret_t AS504x_init(AS504x_config_t *AS504x_config) {
 	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
 	// Enable timer
 	TIM_Cmd(HW_ENC_TIM, ENABLE);
+
+	AS504x_config->is_init = 1;
+	AS504x_config_now = *AS504x_config;
+
 	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
 
 	spi_error_rate = 0.0;
-
-	AS504x_config_now.is_init = 1;
-	AS504x_config->is_init = 1;
 	return ENCODERS_OK;
 }
 #if (AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS)
diff --git a/encoder/AS504x.h b/encoder/AS504x.h
index a5afd9ba..6a9482fc 100644
--- a/encoder/AS504x.h
+++ b/encoder/AS504x.h
@@ -5,39 +5,14 @@
 #include "datatypes.h"
 #include "encoder/encoder_datatype.h"
 
-void AS504x_routine(void);
-float AS504x_read_deg(void);
 void AS504x_deinit(void);
 encoders_ret_t AS504x_init(AS504x_config_t *AS504x_config);
+
+float AS504x_read_deg(void);
+void AS504x_routine(void);
+
 AS504x_diag AS504x_get_diag(void);
-bool AS504x_index_found(void);
 float AS504x_spi_get_error_rate(void);
 uint32_t AS504x_spi_get_val(void);
 uint32_t AS504x_spi_get_error_cnt(void);
-
-#define AS504x_SPI_READ_BIT 								0x4000
-#define AS504x_SPI_WRITE_BIT 								0x0000
-
-#define AS504x_SPI_DIAG_OCF_BIT_POS							8
-#define AS504x_SPI_DIAG_COF_BIT_POS							9
-#define AS504x_SPI_DIAG_COMP_LOW_BIT_POS					10
-#define AS504x_SPI_DIAG_COMP_HIGH_BIT_POS					11
-
-#define AS5047_SAMPLE_RATE_HZ		20000
-
-#define AS504x_SPI_EXCLUDE_PARITY_AND_ERROR_BITMASK			0x3FFF
-
-#define AS504x_SPI_DIAG_ADR									0x3FFD
-#define AS504x_SPI_MAGN_ADR									0x3FFE
-#define AS504x_SPI_CLEAR_ERROR_ADR							0x0001
-
-#define AS504x_SPI_READ_DIAG_MSG							(AS504x_SPI_DIAG_ADR | AS504x_SPI_READ_BIT)
-#define AS504x_SPI_READ_MAGN_MSG							(AS504x_SPI_MAGN_ADR | AS504x_SPI_READ_BIT)
-#define AS504x_SPI_READ_CLEAR_ERROR_MSG						(AS504x_SPI_CLEAR_ERROR_ADR | AS504x_SPI_READ_BIT)
-
-#define AS504x_CONNECTION_DETERMINATOR_ERROR_THRESHOLD		5
-
-#define AS504x_DATA_INVALID_THRESHOLD						20000
-#define AS504x_REFRESH_DIAG_AFTER_NSAMPLES					100
-
 #endif /* ENCODER_AS504X_H_ */
diff --git a/encoder/ENC_SINCOS.c b/encoder/ENC_SINCOS.c
index e8afb8d8..3440dd1d 100644
--- a/encoder/ENC_SINCOS.c
+++ b/encoder/ENC_SINCOS.c
@@ -7,6 +7,7 @@
 #include "mc_interface.h"
 #include "utils.h"
 #include <math.h>
+#include "encoder/encoder_hwconf.h"
 
 ENCSINCOS_config_t enc_sincos_config_now = { 0 };
 
@@ -47,6 +48,7 @@ encoders_ret_t ENC_SINCOS_init(ENCSINCOS_config_t *enc_sincos_config) {
 	// ADC measurements needs to be in sync with motor PWM
 #ifdef HW_HAS_SIN_COS_ENCODER
 	enc_sincos_config->is_init = 1;
+	enc_sincos_config_now = *enc_sincos_config;
 	return ENCODERS_OK;
 #else
 	enc_sincos_config->is_init = 0;
@@ -84,3 +86,19 @@ float ENC_SINCOS_read_deg(void) {
 #endif
 	return last_enc_angle;
 }
+
+uint32_t ENC_SINCOS_get_signal_below_min_error_cnt(void) {
+	return sincos_signal_below_min_error_cnt;
+}
+
+uint32_t ENC_SINCOS_get_signal_above_max_error_cnt(void) {
+	return sincos_signal_above_max_error_cnt;
+}
+
+float ENC_SINCOS_get_signal_below_min_error_rate(void) {
+	return sincos_signal_low_error_rate;
+}
+
+float ENC_SINCOS_get_signal_above_max_error_rate(void) {
+	return sincos_signal_above_max_error_rate;
+}
diff --git a/encoder/ENC_SINCOS.h b/encoder/ENC_SINCOS.h
index 6efc5864..83cc4b7a 100644
--- a/encoder/ENC_SINCOS.h
+++ b/encoder/ENC_SINCOS.h
@@ -10,4 +10,9 @@ encoders_ret_t ENC_SINCOS_init(ENCSINCOS_config_t *enc_sincos_config);
 
 float ENC_SINCOS_read_deg(void);
 
+uint32_t ENC_SINCOS_get_signal_below_min_error_cnt(void);
+uint32_t ENC_SINCOS_get_signal_above_max_error_cnt(void);
+float ENC_SINCOS_get_signal_below_min_error_rate(void);
+float ENC_SINCOS_get_signal_above_max_error_rate(void);
+
 #endif /* ENCODER_ENC_SINCOS_H_ */
diff --git a/encoder/MT6816.c b/encoder/MT6816.c
index 7c9b53c0..12eb28d6 100644
--- a/encoder/MT6816.c
+++ b/encoder/MT6816.c
@@ -47,9 +47,6 @@ void MT6816_deinit(void) {
 	spiStop(&HW_SPI_DEV);
 #endif
 
-	//palSetPadMode(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, PAL_MODE_INPUT_PULLUP); //TODO: is this necessary
-	//palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP); //TODO: is this necessary
-
 	palSetPadMode(mt6816_config_now.spi_config.gpio_miso.port,
 			mt6816_config_now.spi_config.gpio_miso.pin, PAL_MODE_INPUT_PULLUP);
 	palSetPadMode(mt6816_config_now.spi_config.gpio_sck.port,
@@ -112,6 +109,11 @@ encoders_ret_t MT6816_init(MT6816_config_t *mt6816_config) {
 #endif
 	return ENCODERS_OK;
 }
+
+float MT6816_read_deg(void) {
+	return last_enc_angle;
+}
+
 void MT6816_routine(void) {
 	uint16_t pos;
 	uint16_t reg_data_03;
@@ -167,10 +169,6 @@ uint32_t MT6816_get_no_magnet_error_rate(void) {
 	return encoder_no_magnet_error_rate;
 }
 
-float MT6816_read_deg(void) {
-	return last_enc_angle;
-}
-
 static void spi_begin(void) {
 	palClearPad(mt6816_config_now.spi_config.gpio_nss.port,
 			mt6816_config_now.spi_config.gpio_nss.pin);
diff --git a/encoder/MT6816.h b/encoder/MT6816.h
index 276e2ad6..0653eb9c 100644
--- a/encoder/MT6816.h
+++ b/encoder/MT6816.h
@@ -7,9 +7,9 @@
 
 void MT6816_deinit(void);
 encoders_ret_t MT6816_init(MT6816_config_t *mt6816_config);
-void MT6816_routine(void);
 
 float MT6816_read_deg(void);
+void MT6816_routine(void);
 
 uint32_t MT6816_spi_get_val(void);
 uint32_t MT6816_spi_get_error_cnt(void);
diff --git a/encoder/encoder_datatype.h b/encoder/encoder_datatype.h
index c56649a4..480f2694 100644
--- a/encoder/encoder_datatype.h
+++ b/encoder/encoder_datatype.h
@@ -6,6 +6,15 @@
 #include "ch.h"
 #include "hal.h"
 
+#define AS5047_SAMPLE_RATE_HZ       20000
+#define AD2S1205_SAMPLE_RATE_HZ     20000       //25MHz max spi clk
+#define TS5700N8501_SAMPLE_RATE_HZ	20000
+#define MT6816_SAMPLE_RATE_HZ       20000
+#define MT6816_NO_MAGNET_ERROR_MASK 0x0002
+#define SINCOS_SAMPLE_RATE_HZ       20000
+#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
+
 typedef uint8_t encoders_pin_t;
 typedef uint32_t encoders_refresh_rate_hz_t;
 
@@ -88,5 +97,18 @@ typedef struct {
 	ENCSINCOS_config_t encsincos;
 	TS5700N8501_config_t ts5700n8501;
 } encoders_config_t;
-
+/*
+typedef struct {
+	uint8_t is_connected;
+	uint8_t AGC_value;
+	uint16_t magnitude;
+	uint8_t is_OCF;
+	uint8_t is_COF;
+	uint8_t is_Comp_low;
+	uint8_t is_Comp_high;
+	uint16_t serial_diag_flgs;
+	uint16_t serial_magnitude;
+	uint16_t serial_error_flags;
+}AS504x_diag;
+*/
 #endif /* ENCODER_ENCODER_DATATYPE_H_ */
diff --git a/encoder/encoder_hwconf.h b/encoder/encoder_hwconf.h
index eb914e6b..34badffa 100644
--- a/encoder/encoder_hwconf.h
+++ b/encoder/encoder_hwconf.h
@@ -4,15 +4,6 @@
 #include "encoder/encoder_datatype.h"
 #include "hw.h"
 
-#define AS5047_SAMPLE_RATE_HZ       20000
-#define AD2S1205_SAMPLE_RATE_HZ     20000       //25MHz max spi clk
-#define TS5700N8501_SAMPLE_RATE_HZ	20000
-#define MT6816_SAMPLE_RATE_HZ       20000
-#define MT6816_NO_MAGNET_ERROR_MASK 0x0002
-#define SINCOS_SAMPLE_RATE_HZ       20000
-#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
-
 extern encoders_config_t conf_AS5047;
 extern encoders_config_t conf_MT6816;
 extern encoders_config_t conf_AD2S1205;
diff --git a/encoder/encoders.c b/encoder/encoders.c
index a3b0a898..9a340336 100644
--- a/encoder/encoders.c
+++ b/encoder/encoders.c
@@ -1,12 +1,42 @@
 #include "encoders.h"
 
-static encoders_type_t encoder_type_now = ENCODERS_TYPE_NONE; // TODO: CHANGE TO encoder_config_now
+#include "encoder/AS504x.h"
+#include "encoder/MT6816.h"
+#include "encoder/AD2S1205.h"
+#include "encoder/encoder_hwconf.h"
+#include "encoder/ABI.h"
+#include "encoder/ENC_SINCOS.h"
+#include "encoder/TS5700N8501.h"
+
+#include "math.h"
+
+static bool encoders_is_uart_defined(encoders_config_t *encoder_config);
+static bool encoders_is_spi_defined(encoders_config_t *encoder_config);
+static bool encoders_is_incremental_defined(encoders_config_t *encoder_config);
+
+static encoders_type_t encoder_type_now = ENCODERS_TYPE_NONE;
 static uint32_t enc_counts = 10000;
 static bool index_found = false;
 
-encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
+void encoders_deinit(void) {
+	if (encoder_type_now == ENCODERS_TYPE_AS504x) {
+		AS504x_deinit();
+	} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
+		MT6816_deinit();
+	} else if (encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI) {
+		AD2S1205_deinit();
+	} else if (encoder_type_now == ENCODERS_TYPE_ABI) {
+		ABI_deinit();
+	} else if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
+		ENC_SINCOS_deinit();
+	} else if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
+		TS5700N8501_deinit();
+	}
 
-	//TODO: ASSERTS
+	encoder_type_now = ENCODERS_TYPE_NONE;
+}
+
+encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
 
 	if (encoder_type_now != ENCODERS_TYPE_NONE) {
 		return ENCODERS_ERROR;
@@ -16,6 +46,11 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
 		AS504x_config_t *as504x_config;
 		encoders_ret_t encoder_ret;
 
+		if(!encoders_is_spi_defined(encoder_config))
+		{
+			return ENCODERS_ERROR;
+		}
+
 		as504x_config = &(encoder_config->encspi);
 		as504x_config->is_init = 0;
 
@@ -33,6 +68,11 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
 		MT6816_config_t *mt6816_config;
 		encoders_ret_t encoder_ret;
 
+		if(!encoders_is_spi_defined(encoder_config))
+		{
+			return ENCODERS_ERROR;
+		}
+
 		mt6816_config = &(encoder_config->encspi);
 		mt6816_config->is_init = 0;
 
@@ -50,6 +90,11 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
 		AD2S1205_config_t *ad2s1205_config;
 		encoders_ret_t encoder_ret;
 
+		if(!encoders_is_spi_defined(encoder_config))
+		{
+			return ENCODERS_ERROR;
+		}
+
 		ad2s1205_config = &(encoder_config->encspi);
 		ad2s1205_config->is_init = 0;
 
@@ -67,6 +112,11 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
 		ABI_config_t *abi_config;
 		encoders_ret_t encoder_ret;
 
+		if(!encoders_is_incremental_defined(encoder_config))
+		{
+			return ENCODERS_ERROR;
+		}
+
 		abi_config = &(encoder_config->abi);
 		abi_config->is_init = 0;
 
@@ -101,6 +151,11 @@ encoders_ret_t encoders_init(encoders_config_t *encoder_config) {
 		TS5700N8501_config_t *ts5700N8501_config;
 		encoders_ret_t encoder_ret;
 
+		if(!encoders_is_uart_defined(encoder_config))
+		{
+			return ENCODERS_ERROR;
+		}
+
 		ts5700N8501_config = &(encoder_config->ts5700n8501);
 
 		encoder_ret = TS5700N8501_init(ts5700N8501_config);
@@ -137,6 +192,23 @@ float encoders_read_deg(void) {
 	return 0.0;
 }
 
+float encoders_read_deg_multiturn(void) {
+	if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
+		encoders_get_abm();
+		float ts_mt = (float)encoders_get_abm();
+		if (fabsf(ts_mt) > 5000.0) {
+			ts_mt = 0;
+			encoders_reset_multiturn();
+		}
+
+		ts_mt += 5000;
+
+		return encoders_read_deg() / 10000.0 + (360 * ts_mt) / 10000.0;
+	} else {
+		return encoders_read_deg();
+	}
+}
+
 encoders_type_t encoders_is_configured(void) {
 	return encoder_type_now;
 }
@@ -145,24 +217,6 @@ bool encoders_index_found(void) {
 	return index_found;
 }
 
-void encoders_deinit(void) {
-	if (encoder_type_now == ENCODERS_TYPE_AS504x) {
-		AS504x_deinit();
-	} else if (encoder_type_now == ENCODERS_TYPE_MT6816) {
-		MT6816_deinit();
-	} else if (encoder_type_now == ENCODERS_TYPE_AD2S1205_SPI) {
-		AD2S1205_deinit();
-	} else if (encoder_type_now == ENCODERS_TYPE_ABI) {
-		ABI_deinit();
-	} else if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
-		ENC_SINCOS_deinit();
-	} else if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
-		TS5700N8501_deinit();
-	}
-
-	encoder_type_now = ENCODERS_TYPE_NONE;
-}
-
 float encoders_spi_get_error_rate(void) {
 	if (encoder_type_now == ENCODERS_TYPE_AS504x) {
 		return AS504x_spi_get_error_rate();
@@ -216,6 +270,14 @@ uint32_t encoders_resolver_loss_of_signal_error_cnt(void) {
 	return AD2S1205_resolver_loss_of_signal_error_cnt();
 }
 
+void encoders_set_counts(uint32_t counts) {
+	if (encoder_type_now == ENCODERS_TYPE_ABI)
+	{
+		ABI_set_counts(counts);
+		index_found = false;
+	}
+}
+
 uint32_t encoders_spi_get_val(void) {
 	if (encoder_type_now == ENCODERS_TYPE_AS504x) {
 		return AS504x_spi_get_val();
@@ -257,6 +319,65 @@ void encoders_reset(void) {
 	}
 }
 
+AS504x_diag encoders_get_diag(void) {
+	AS504x_diag diag = {0};
+	if (encoder_type_now == ENCODERS_TYPE_AS504x) {
+		diag = AS504x_get_diag();
+	}
+	return diag;
+}
+
+uint8_t* encoders_get_raw_status(void) {
+	if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
+		return TS5700N8501_get_raw_status();
+	}
+	return NULL;
+}
+
+int16_t encoders_get_abm(void) {
+	if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
+		return TS5700N8501_get_abm();
+	}
+	return 0;
+}
+
+void encoders_reset_errors(void) {
+	if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
+		return TS5700N8501_reset_errors();
+	}
+}
+
+void encoders_reset_multiturn(void) {
+	if (encoder_type_now == ENCODERS_TYPE_TS5700N8501) {
+		return TS5700N8501_reset_multiturn();
+	}
+}
+
+uint32_t encoders_get_signal_below_min_error_cnt(void) {
+	if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
+		return ENC_SINCOS_get_signal_below_min_error_cnt();
+	}
+	return 0;
+}
+uint32_t encoders_get_signal_above_max_error_cnt(void) {
+	if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
+		return ENC_SINCOS_get_signal_above_max_error_cnt();
+	}
+	return 0;
+}
+float encoders_get_signal_below_min_error_rate(void) {
+	if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
+		return ENC_SINCOS_get_signal_below_min_error_rate();
+	}
+	return 0.0;
+}
+float encoders_get_signal_above_max_error_rate(void) {
+	if (encoder_type_now == ENCODERS_TYPE_SINCOS) {
+		return ENC_SINCOS_get_signal_above_max_error_rate();
+	}
+	return 0.0;
+}
+
 void encoders_tim_isr(void) {
 	if (encoder_type_now == ENCODERS_TYPE_AS504x) {
 		AS504x_routine();
@@ -267,3 +388,34 @@ void encoders_tim_isr(void) {
 	}
 }
 
+static bool encoders_is_uart_defined(encoders_config_t *encoder_config)
+{
+	if (!encoder_config->ts5700n8501.uart_config.gpio_RX.port
+			|| !encoder_config->ts5700n8501.uart_config.gpio_TX.port)
+	{
+		return false;
+	}
+	return true;
+}
+
+static bool encoders_is_spi_defined(encoders_config_t *encoder_config)
+{
+	if (!encoder_config->encspi.spi_config.gpio_miso.port
+			|| !encoder_config->encspi.spi_config.gpio_nss.port
+			|| !encoder_config->encspi.spi_config.gpio_sck.port)
+	{
+		return false;
+	}
+	return true;
+}
+
+static bool encoders_is_incremental_defined(encoders_config_t *encoder_config)
+{
+	if (!encoder_config->abi.incremental_config.gpio_A.port
+			|| !encoder_config->abi.incremental_config.gpio_B.port)
+	{
+		return false;
+	}
+	return true;
+}
+
diff --git a/encoder/encoders.h b/encoder/encoders.h
index 2e29e375..5d4cb7d7 100644
--- a/encoder/encoders.h
+++ b/encoder/encoders.h
@@ -2,36 +2,55 @@
 #define ENCODER_ENCODERS_H_
 
 #include "encoder_datatype.h"
-#include "encoder/AS504x.h"
-#include "encoder/MT6816.h"
-#include "encoder/AD2S1205.h"
 #include "encoder/encoder_hwconf.h"
-#include "encoder/ABI.h"
-#include "encoder/ENC_SINCOS.h"
-#include "encoder/TS5700N8501.h"
 #include "hal.h"
+
 // GENERIC GLOBAL
 void encoders_deinit(void);
 encoders_ret_t encoders_init(encoders_config_t *encoder_config);
+
 float encoders_read_deg(void);
+float encoders_read_deg_multiturn(void);
 encoders_type_t encoders_is_configured(void);
+bool encoders_index_found(void);
+void encoders_reset_multiturn(void);
 
 // SPECIFIC GLOBAL
-bool encoders_index_found(void);
-float encoders_spi_get_error_rate(void);
-uint32_t encoders_spi_get_error_cnt(void);
-uint32_t encoders_get_no_magnet_error_cnt(void);
-
+//AD2S1205
 float encoders_resolver_loss_of_tracking_error_rate(void);
 float encoders_resolver_degradation_of_signal_error_rate(void);
 float encoders_resolver_loss_of_signal_error_rate(void);
 uint32_t encoders_resolver_loss_of_tracking_error_cnt(void);
 uint32_t encoders_resolver_degradation_of_signal_error_cnt(void);
 uint32_t encoders_resolver_loss_of_signal_error_cnt(void);
-float encoders_get_no_magnet_error_rate(void);
-uint32_t encoders_spi_get_val(void);
 
-void encoders_tim_isr(void);
+//ABI
+void encoders_set_counts(uint32_t counts);
+
+//MT6816
+float encoders_get_no_magnet_error_rate(void);
+uint32_t encoders_get_no_magnet_error_cnt(void);
+
+//AS504x
+AS504x_diag encoders_get_diag(void);
+
+//SINCOS
+uint32_t encoders_get_signal_below_min_error_cnt(void);
+uint32_t encoders_get_signal_above_max_error_cnt(void);
+float encoders_get_signal_below_min_error_rate(void);
+float encoders_get_signal_above_max_error_rate(void);
+
+//TS5700N8501
+uint8_t* encoders_get_raw_status(void);
+int16_t encoders_get_abm(void);
+void encoders_reset_errors(void);
+
+//SPI ENCODERS
+uint32_t encoders_spi_get_val(void);
+float encoders_spi_get_error_rate(void);
+uint32_t encoders_spi_get_error_cnt(void);
+
 void encoders_reset(void);
+void encoders_tim_isr(void);
 
 #endif /* ENCODER_ENCODERS_H_ */
diff --git a/irq_handlers.c b/irq_handlers.c
index f8c7a4be..f5141c28 100644
--- a/irq_handlers.c
+++ b/irq_handlers.c
@@ -24,7 +24,6 @@
 #include "mc_interface.h"
 #include "mcpwm_foc.h"
 #include "hw.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 
 CH_IRQ_HANDLER(ADC1_2_3_IRQHandler) {
diff --git a/main.c b/main.c
index 95d32881..66b540d0 100644
--- a/main.c
+++ b/main.c
@@ -39,7 +39,6 @@
 #include "commands.h"
 #include "timeout.h"
 #include "comm_can.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "servo_simple.h"
 #include "utils.h"
diff --git a/mc_interface.c b/mc_interface.c
index 7920a87a..5b37aa33 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -28,7 +28,6 @@
 #include "ch.h"
 #include "hal.h"
 #include "commands.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "buffer.h"
 #include "gpdrive.h"
@@ -348,7 +347,8 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 		switch (configuration->m_sensor_port_mode) {
 		case SENSOR_PORT_MODE_ABI:
 		  // TODO: integrate with encoder/encoders
-			encoder_init_abi(configuration->m_encoder_counts);
+			conf_ABI.abi.counts = configuration->m_encoder_counts;
+			encoders_init(&conf_ABI);
 			break;
 
 		case SENSOR_PORT_MODE_AS5047_SPI:
@@ -364,12 +364,14 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 			break;
 
 		case SENSOR_PORT_MODE_SINCOS:
-		   // TODO: integrate with encoder/encoders
-			encoder_init_sincos(motor->m_conf.foc_encoder_sin_gain, motor->m_conf.foc_encoder_sin_offset,
-								motor->m_conf.foc_encoder_cos_gain, motor->m_conf.foc_encoder_cos_offset,
-								motor->m_conf.foc_encoder_sincos_filter_constant);
+			conf_SINCOS.encsincos.s_gain = motor->m_conf.foc_encoder_sin_gain;
+			conf_SINCOS.encsincos.s_offset = motor->m_conf.foc_encoder_sin_offset;
+			conf_SINCOS.encsincos.c_gain = motor->m_conf.foc_encoder_cos_gain;
+			conf_SINCOS.encsincos.c_offset = motor->m_conf.foc_encoder_cos_offset;
+			conf_SINCOS.encsincos.filter_constant = motor->m_conf.foc_encoder_sincos_filter_constant;
+
+			encoders_init(&conf_SINCOS);
 			break;
-			// TODO: integrate with encoder/encoders
 		case SENSOR_PORT_MODE_TS5700N8501:
 		case SENSOR_PORT_MODE_TS5700N8501_MULTITURN: {
 			app_configuration *appconf = mempools_alloc_appconf();
@@ -392,7 +394,7 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 	}
 
 	if (configuration->m_sensor_port_mode == SENSOR_PORT_MODE_ABI) {
-		encoder_set_counts(configuration->m_encoder_counts);
+		encoders_set_counts(configuration->m_encoder_counts);
 	}
 
 #ifdef HW_HAS_DRV8301
@@ -2473,22 +2475,22 @@ static void run_timer_tasks(volatile motor_if_state_t *motor) {
 			motor->m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
 			motor->m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
 
-		if (encoder_sincos_get_signal_below_min_error_rate() > 0.05)
+		if (encoders_get_signal_below_min_error_rate() > 0.05)
 			mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_BELOW_MIN_AMPLITUDE, !is_motor_1, false);
-		if (encoder_sincos_get_signal_above_max_error_rate() > 0.05)
+		if (encoders_get_signal_above_max_error_rate() > 0.05)
 			mc_interface_fault_stop(FAULT_CODE_ENCODER_SINCOS_ABOVE_MAX_AMPLITUDE, !is_motor_1, false);
 	}
 
 	if (motor->m_conf.motor_type == MOTOR_TYPE_FOC &&
 				motor->m_conf.foc_sensor_mode == FOC_SENSOR_MODE_ENCODER &&
 				motor->m_conf.m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI) {
-		if (!AS504x_get_diag().is_connected) {
+		if (!encoders_get_diag().is_connected) {
 			mc_interface_fault_stop(FAULT_CODE_ENCODER_SPI, !is_motor_1, false);
 		}
 
-		if (AS504x_get_diag().is_Comp_high) {
+		if (encoders_get_diag().is_Comp_high) {
 			mc_interface_fault_stop(FAULT_CODE_ENCODER_NO_MAGNET, !is_motor_1, false);
-		} else if(AS504x_get_diag().is_Comp_low) {
+		} else if(encoders_get_diag().is_Comp_low) {
 			mc_interface_fault_stop(FAULT_CODE_ENCODER_MAGNET_TOO_STRONG, !is_motor_1, false);
 		}
 	}
diff --git a/mcpwm.c b/mcpwm.c
index 14c79c32..080f21fe 100644
--- a/mcpwm.c
+++ b/mcpwm.c
@@ -31,7 +31,6 @@
 #include "ledpwm.h"
 #include "terminal.h"
 #include "timeout.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "timer.h"
 
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index c66158db..782f3e11 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -30,7 +30,6 @@
 #include "utils.h"
 #include "ledpwm.h"
 #include "terminal.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "commands.h"
 #include "timeout.h"
@@ -3074,7 +3073,7 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	float angle_now = 0.0;
 	if (encoders_is_configured()) {
 		if (conf_now->m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501_MULTITURN) {
-			angle_now = encoder_read_deg_multiturn();
+			angle_now = encoders_read_deg_multiturn();
 		} else {
 			angle_now = enc_ang;
 		}
diff --git a/terminal.c b/terminal.c
index ffff8f20..01ed6211 100644
--- a/terminal.c
+++ b/terminal.c
@@ -28,7 +28,6 @@
 #include "comm_can.h"
 #include "utils.h"
 #include "timeout.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 #include "app.h"
 #include "comm_usb.h"
@@ -880,22 +879,22 @@ void terminal_process_string(char *str) {
 						(unsigned int)encoders_spi_get_val(),
 						encoders_spi_get_error_cnt(),
 						(double)encoders_spi_get_error_rate() * (double)100.0,
-						AS504x_get_diag().is_connected);
+						encoders_get_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(encoder_ts5700n8501_get_raw_status()[0], sf);
-				utils_byte_to_binary(encoder_ts5700n8501_get_raw_status()[7], almc);
-				commands_printf("TS5700N8501 ABM: %d, SF: %s, ALMC: %s\n", encoder_ts57n8501_get_abm(), sf, almc);
+				utils_byte_to_binary(encoders_get_raw_status()[0], sf);
+				utils_byte_to_binary(encoders_get_raw_status()[7], almc);
+				commands_printf("TS5700N8501 ABM: %d, SF: %s, ALMC: %s\n", encoders_get_abm(), 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 %%",
 						encoders_get_no_magnet_error_cnt(),
-						(double)encoder_get_no_magnet_error_rate() * (double)100.0);
+						(double)encoders_get_no_magnet_error_rate() * (double)100.0);
 			}
 
 #if AS504x_USE_SW_MOSI_PIN || AS5047_USE_HW_SPI_PINS
@@ -907,22 +906,22 @@ void terminal_process_string(char *str) {
 						"OCF       : %u\n"
 						"COMP_low  : %u\n"
 						"COMP_high : %u\n",
-						AS504x_get_diag().AGC_value, AS504x_get_diag().magnitude,
-						AS504x_get_diag().is_COF, AS504x_get_diag().is_OCF,
-						AS504x_get_diag().is_Comp_low,
-						AS504x_get_diag().is_Comp_high);
+						encoders_get_diag().AGC_value, encoders_get_diag().magnitude,
+						encoders_get_diag().is_COF, encoders_get_diag().is_OCF,
+						encoders_get_diag().is_Comp_low,
+						encoders_get_diag().is_Comp_high);
 			}
 #endif
 		}
 
 		if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_SINCOS) {
 			commands_printf("Sin/Cos encoder signal below minimum amplitude: errors: %d, error rate: %.3f %%",
-					encoder_sincos_get_signal_below_min_error_cnt(),
-					(double)encoder_sincos_get_signal_below_min_error_rate() * (double)100.0);
+					encoders_get_signal_below_min_error_cnt(),
+					(double)encoders_get_signal_below_min_error_rate() * (double)100.0);
 
 			commands_printf("Sin/Cos encoder signal above maximum amplitude: errors: %d, error rate: %.3f %%",
-					encoder_sincos_get_signal_above_max_error_cnt(),
-					(double)encoder_sincos_get_signal_above_max_error_rate() * (double)100.0);
+					encoders_get_signal_above_max_error_cnt(),
+					(double)encoders_get_signal_above_max_error_rate() * (double)100.0);
 		}
 
 		if (mcconf->m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
@@ -937,10 +936,10 @@ void terminal_process_string(char *str) {
 					(double)encoders_resolver_loss_of_signal_error_rate() * (double)100.0);
 		}
 	} else if (strcmp(argv[0], "encoder_clear_errors") == 0) {
-		encoder_ts57n8501_reset_errors();
+		encoders_reset_errors();
 		commands_printf("Done!\n");
 	} else if (strcmp(argv[0], "encoder_clear_multiturn") == 0) {
-		encoder_ts57n8501_reset_multiturn();
+		encoders_reset_multiturn();
 		commands_printf("Done!\n");
 	} else if (strcmp(argv[0], "uptime") == 0) {
 		commands_printf("Uptime: %.2f s\n", (double)chVTGetSystemTimeX() / (double)CH_CFG_ST_FREQUENCY);
diff --git a/virtual_motor.c b/virtual_motor.c
index 04034174..02be85e7 100644
--- a/virtual_motor.c
+++ b/virtual_motor.c
@@ -24,7 +24,6 @@
 #include "math.h"
 #include "stdio.h"
 #include "commands.h"
-#include "encoder.h"
 #include "encoder/encoders.h"
 
 typedef struct{
@@ -290,9 +289,13 @@ static void disconnect_virtual_motor( void ){
 				break;
 
 			case SENSOR_PORT_MODE_SINCOS:
-				encoder_init_sincos(m_conf->foc_encoder_sin_gain, m_conf->foc_encoder_sin_offset,
-									m_conf->foc_encoder_cos_gain, m_conf->foc_encoder_cos_offset,
-									m_conf->foc_encoder_sincos_filter_constant);
+
+				conf_SINCOS.encsincos.s_gain = m_conf->foc_encoder_sin_gain;
+				conf_SINCOS.encsincos.s_offset = m_conf->foc_encoder_sin_offset;
+				conf_SINCOS.encsincos.c_gain = m_conf->foc_encoder_cos_gain;
+				conf_SINCOS.encsincos.c_offset = m_conf->foc_encoder_cos_offset;
+				conf_SINCOS.encsincos.filter_constant = m_conf->foc_encoder_sincos_filter_constant;
+				encoders_init(&conf_SINCOS);
 				break;
 
 			default: