From 1f2568bd8c665f30ec380ec8c76ae71e498170cd Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Wed, 16 Nov 2022 11:49:47 +0100
Subject: [PATCH] Some cleanup, run encoder-routine in thread instead of isr

---
 CHANGELOG                 |   1 +
 conf_general.h            |   2 +-
 driver/spi_bb.c           |  12 ---
 driver/spi_bb.h           |  14 +--
 encoder/enc_tle5012.c     |  31 ++----
 encoder/enc_tle5012.h     |   1 -
 encoder/enc_ts5700n8501.c |   2 +-
 encoder/encoder.c         | 217 ++++++++++++++++++++------------------
 encoder/encoder_cfg.c     |  13 +--
 make/fw.mk                |   2 +-
 10 files changed, 129 insertions(+), 166 deletions(-)

diff --git a/CHANGELOG b/CHANGELOG
index 68a13092..9fcfd028 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -24,6 +24,7 @@
 * APP ADC button bitfield and CC disable support.
 * BissC encoder support: https://github.com/vedderb/bldc/pull/536
 * Better detection failt handling and reporting: https://github.com/vedderb/bldc/pull/533
+* TLE5012-support: https://github.com/vedderb/bldc/pull/551
 
 === FW 5.03 ===
 * Fixed inductance measurement bug.
diff --git a/conf_general.h b/conf_general.h
index 4cb24f4a..13055685 100755
--- a/conf_general.h
+++ b/conf_general.h
@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			6
 #define FW_VERSION_MINOR			00
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		76
+#define FW_TEST_VERSION_NUMBER		77
 
 #include "datatypes.h"
 
diff --git a/driver/spi_bb.c b/driver/spi_bb.c
index ef9a77be..a3a0265a 100644
--- a/driver/spi_bb.c
+++ b/driver/spi_bb.c
@@ -34,9 +34,6 @@ void spi_bb_init(spi_bb_state *s) {
 		palSetPad(s->mosi_gpio, s->mosi_pin);
 		palSetPad(s->nss_gpio, s->nss_pin);
 	}
-
-	s->has_started = false;
-	s->has_error = false;
 }
 
 void spi_bb_deinit(spi_bb_state *s) {
@@ -47,9 +44,6 @@ void spi_bb_deinit(spi_bb_state *s) {
 	if (s->mosi_gpio) {
 		palSetPadMode(s->mosi_gpio, s->mosi_pin, PAL_MODE_INPUT_PULLUP);
 	}
-
-	s->has_started = false;
-	s->has_error = false;
 }
 
 void ssc_bb_init(spi_bb_state *s) {
@@ -65,9 +59,6 @@ void ssc_bb_init(spi_bb_state *s) {
 
 	palClearPad(s->sck_gpio, s->sck_pin);
 	palSetPad(s->nss_gpio, s->nss_pin);
-
-	s->has_started = false;
-	s->has_error = false;
 }
 
 void ssc_bb_deinit(spi_bb_state *s) {
@@ -76,9 +67,6 @@ void ssc_bb_deinit(spi_bb_state *s) {
 	palSetPadMode(s->mosi_gpio, s->miso_pin, PAL_MODE_INPUT_PULLUP);
 	palSetPadMode(s->sck_gpio, s->sck_pin, PAL_MODE_INPUT_PULLUP);
 	palSetPadMode(s->nss_gpio, s->nss_pin, PAL_MODE_INPUT_PULLUP);
-
-	s->has_started = false;
-	s->has_error = false;
 }
 
 uint8_t spi_bb_exchange_8(spi_bb_state *s, uint8_t x) {
diff --git a/driver/spi_bb.h b/driver/spi_bb.h
index 5bb88ff4..cd974394 100644
--- a/driver/spi_bb.h
+++ b/driver/spi_bb.h
@@ -25,25 +25,15 @@
 #include "stdint.h"
 #include "stdbool.h"
 
-enum spi_types{
-	spi_type_sw, // spi = seperate mosi and miso wires, (hw = hw spi pins)
-	spi_type_hw, 
-	ssc_type_sw, // ssc = one data wire using mosi pin
-	ssc_type_hw, 
-	};
-
 typedef struct {
 	stm32_gpio_t *nss_gpio;
 	int nss_pin;
 	stm32_gpio_t *sck_gpio;
 	int sck_pin;
-    stm32_gpio_t *mosi_gpio;
+	stm32_gpio_t *mosi_gpio;
 	int mosi_pin;
-    stm32_gpio_t *miso_gpio;
+	stm32_gpio_t *miso_gpio;
 	int miso_pin;
-	enum spi_types spi_type;
-	bool has_started;
-	bool has_error;
 	mutex_t mutex;
 } spi_bb_state;
 
diff --git a/encoder/enc_tle5012.c b/encoder/enc_tle5012.c
index dd21b9df..457f15d4 100644
--- a/encoder/enc_tle5012.c
+++ b/encoder/enc_tle5012.c
@@ -96,20 +96,6 @@ bool enc_tle5012_init_sw_ssc(TLE5012_config_t *cfg) {
 	return enc_tle5012_setup(cfg);
 }
 
-bool enc_tle5012_init_hw_ssc(TLE5012_config_t *cfg) {
-
-	// software ssc for now using hw spi pins
-	memset(&cfg->state, 0, sizeof(TLE5012_state)); 
-
-	spi_bb_init(&(cfg->sw_spi));
-
-	cfg->state.last_status_error = 0;
-	cfg->state.spi_error_rate = 0.0;
-	cfg->state.encoder_no_magnet_error_rate = 0.0;
-
-	return enc_tle5012_setup(cfg);
-}
-
 void enc_tle5012_deinit(TLE5012_config_t *cfg) {
 	// sw spi
 	spi_bb_deinit(&(cfg->sw_spi)); 
@@ -179,7 +165,7 @@ bool enc_tle5012_setup(TLE5012_config_t *cfg) {
 
 	// set up control registers to be identical across tle5012 variants using above settings
 	tle5012_errortypes errorCheck = 0;
-	uint16_t tleregister;
+	uint16_t tleregister = 0;
 	// Interface Mode1
 	errorCheck = errorCheck && enc_tle5012_transfer(cfg, 0x06, &tleregister, SSC_READ, true);
 	tleregister = tleregister & ~0b110000000010111; // mask (1 = cleared)
@@ -315,26 +301,23 @@ tle5012_errortypes enc_tle5012_transfer(TLE5012_config_t *cfg, uint8_t address,
  * @param length length of data
  * @return returns 8bit CRC
  */
-uint8_t crc8(uint8_t *data, uint8_t length)
-{
+uint8_t crc8(uint8_t *data, uint8_t length) {
 	uint32_t crc;
 	int16_t i, bit;
 
 	crc = TLE5012_CRC_SEED;
-	for (i = 0; i < length; i++)
-	{
+	for (i = 0; i < length; i++) {
 		crc ^= data[i];
-		for (bit = 0; bit < 8; bit++)
-		{
-			if ((crc & 0x80) != 0)
-			{
+		for (bit = 0; bit < 8; bit++) {
+			if ((crc & 0x80) != 0) {
 				crc <<= 1;
 				crc ^= TLE5012_CRC_POLYNOMIAL;
-			}else{
+			} else {
 				crc <<= 1;
 			}
 		}
 	}
+
 	return ((~crc) & TLE5012_CRC_SEED);
 }
 
diff --git a/encoder/enc_tle5012.h b/encoder/enc_tle5012.h
index 5e68611d..b217fcd3 100644
--- a/encoder/enc_tle5012.h
+++ b/encoder/enc_tle5012.h
@@ -26,7 +26,6 @@
 #include "encoder/encoder_datatype.h"
 
 bool enc_tle5012_init_sw_ssc(TLE5012_config_t *cfg);
-bool enc_tle5012_init_hw_ssc(TLE5012_config_t *cfg);
 void enc_tle5012_deinit(TLE5012_config_t *cfg);
 void enc_tle5012_routine(TLE5012_config_t *cfg);
 tle5012_errortypes enc_tle5012_get_temperature(TLE5012_config_t *cfg, double *temperature);
diff --git a/encoder/enc_ts5700n8501.c b/encoder/enc_ts5700n8501.c
index e1a92c2e..f06a674c 100644
--- a/encoder/enc_ts5700n8501.c
+++ b/encoder/enc_ts5700n8501.c
@@ -43,7 +43,7 @@ bool enc_ts5700n8501_init(TS5700N8501_config_t *cfg) {
 	cfg->state.is_running = true;
 
 	chThdCreateStatic(cfg->thread_wa, cfg->thread_wa_size,
-	NORMALPRIO - 10, ts5700n8501_thread, cfg);
+			NORMALPRIO - 10, ts5700n8501_thread, cfg);
 
 	return true;
 }
diff --git a/encoder/encoder.c b/encoder/encoder.c
index a5522663..70491a6a 100644
--- a/encoder/encoder.c
+++ b/encoder/encoder.c
@@ -32,18 +32,30 @@
 
 #include <math.h>
 
-static encoder_type_t encoder_type_now = ENCODER_TYPE_NONE;
+// These rates turn into even multiples of systicks
+typedef enum {
+	routine_rate_1k = 0,
+	routine_rate_2k,
+	routine_rate_5k,
+	routine_rate_10k
+} routine_rate_t;
+
+volatile routine_rate_t m_routine_rate = routine_rate_1k;
+static encoder_type_t m_encoder_type_now = ENCODER_TYPE_NONE;
+
+static THD_WORKING_AREA(routine_thread_wa, 256);
+static THD_FUNCTION(routine_thread, arg);
 
 // Private functions
 static void terminal_encoder(int argc, const char **argv);
 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);
+static void timer_start(routine_rate_t rate);
 
 bool encoder_init(volatile mc_configuration *conf) {
 	bool res = false;
 
-	if (encoder_type_now != ENCODER_TYPE_NONE) {
+	if (m_encoder_type_now != ENCODER_TYPE_NONE) {
 		encoder_deinit();
 	}
 
@@ -58,11 +70,11 @@ bool encoder_init(volatile mc_configuration *conf) {
 		encoder_cfg_ABI.counts = conf->m_encoder_counts;
 
 		if (!enc_abi_init(&encoder_cfg_ABI)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_ABI;
+		m_encoder_type_now = ENCODER_TYPE_ABI;
 		res = true;
 	} break;
 
@@ -73,8 +85,8 @@ bool encoder_init(volatile mc_configuration *conf) {
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_AS504x;
-		timer_start(10000);
+		m_encoder_type_now = ENCODER_TYPE_AS504x;
+		timer_start(routine_rate_10k);
 
 		res = true;
 	} break;
@@ -83,12 +95,12 @@ bool encoder_init(volatile mc_configuration *conf) {
 		SENSOR_PORT_5V();
 
 		if (!enc_mt6816_init(&encoder_cfg_mt6816)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_MT6816;
-		timer_start(10000);
+		m_encoder_type_now = ENCODER_TYPE_MT6816;
+		timer_start(routine_rate_10k);
 
 		res = true;
 	} break;
@@ -103,20 +115,17 @@ bool encoder_init(volatile mc_configuration *conf) {
 					HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, // sck
 					HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, // mosi
 					HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, // miso
-					ssc_type_sw,
-					0, // has_started
-					0, // has_error
 					{{NULL, NULL}, NULL, NULL} // Mutex
 			};
 		encoder_cfg_tle5012.sw_spi = sw_ssc;
 
 		if (!enc_tle5012_init_sw_ssc(&encoder_cfg_tle5012)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_TLE5012;
-		timer_start(4000); // slow down sw spi as transactions long
+		m_encoder_type_now = ENCODER_TYPE_TLE5012;
+		timer_start(routine_rate_5k); // slow down sw spi as transactions long
 
 		res = true;
 	} break;
@@ -131,21 +140,18 @@ bool encoder_init(volatile mc_configuration *conf) {
 					HW_SPI_PORT_SCK, HW_SPI_PIN_SCK, // sck
 					HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, // mosi
 					HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI, // miso (shared dat line)
-					ssc_type_sw,
-					0, // has_started
-					0, // has_error
 					{{NULL, NULL}, NULL, NULL} // Mutex
 			};
 		encoder_cfg_tle5012.sw_spi = sw_ssc;	
 
-		if (!enc_tle5012_init_hw_ssc(&encoder_cfg_tle5012)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+		if (!enc_tle5012_init_sw_ssc(&encoder_cfg_tle5012)) {
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_TLE5012;
+		m_encoder_type_now = ENCODER_TYPE_TLE5012;
 		// timer_start(10000);
-		timer_start(4000);
+		timer_start(routine_rate_10k);
 
 		res = true;
 	} break;
@@ -154,12 +160,12 @@ bool encoder_init(volatile mc_configuration *conf) {
 		SENSOR_PORT_5V();
 
 		if (!enc_ad2s1205_init(&encoder_cfg_ad2s1205)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_AD2S1205_SPI;
-		timer_start(10000);
+		m_encoder_type_now = ENCODER_TYPE_AD2S1205_SPI;
+		timer_start(routine_rate_10k);
 
 		res = true;
 	} break;
@@ -174,11 +180,11 @@ bool encoder_init(volatile mc_configuration *conf) {
 		encoder_cfg_sincos.filter_constant = conf->foc_encoder_sincos_filter_constant;
 
 		if (!enc_sincos_init(&encoder_cfg_sincos)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_SINCOS;
+		m_encoder_type_now = ENCODER_TYPE_SINCOS;
 		res = true;
 	} break;
 
@@ -198,11 +204,11 @@ bool encoder_init(volatile mc_configuration *conf) {
 		mempools_free_appconf(appconf);
 
 		if (!enc_ts5700n8501_init(&encoder_cfg_TS5700N8501)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_TS5700N8501;
+		m_encoder_type_now = ENCODER_TYPE_TS5700N8501;
 		res = true;
 	} break;
 
@@ -213,8 +219,8 @@ bool encoder_init(volatile mc_configuration *conf) {
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_AS5x47U;
-		timer_start(10000);
+		m_encoder_type_now = ENCODER_TYPE_AS5x47U;
+		timer_start(routine_rate_10k);
 
 		res = true;
 	} break;
@@ -225,19 +231,19 @@ bool encoder_init(volatile mc_configuration *conf) {
 		encoder_cfg_bissc.enc_res = conf->m_encoder_counts;
 
 		if (!enc_bissc_init(&encoder_cfg_bissc)) {
-			encoder_type_now = ENCODER_TYPE_NONE;
+			m_encoder_type_now = ENCODER_TYPE_NONE;
 			return false;
 		}
 
-		encoder_type_now = ENCODER_TYPE_BISSC;
-		timer_start(10000);
+		m_encoder_type_now = ENCODER_TYPE_BISSC;
+		timer_start(routine_rate_10k);
 
 		res = true;
 	} break;
 
 	default:
 		SENSOR_PORT_5V();
-		encoder_type_now = ENCODER_TYPE_NONE;
+		m_encoder_type_now = ENCODER_TYPE_NONE;
 		break;
 	}
 
@@ -267,54 +273,54 @@ void encoder_deinit(void) {
 	nvicDisableVector(HW_ENC_TIM_ISR_CH);
 	TIM_DeInit(HW_ENC_TIM);
 
-	if (encoder_type_now == ENCODER_TYPE_AS504x) {
+	if (m_encoder_type_now == ENCODER_TYPE_AS504x) {
 		enc_as504x_deinit(&encoder_cfg_as504x);
-	} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_MT6816) {
 		enc_mt6816_deinit(&encoder_cfg_mt6816);
-	} else if (encoder_type_now == ENCODER_TYPE_TLE5012) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_TLE5012) {
 		enc_tle5012_deinit(&encoder_cfg_tle5012);
-	} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
 		enc_ad2s1205_deinit(&encoder_cfg_ad2s1205);
-	} else if (encoder_type_now == ENCODER_TYPE_ABI) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_ABI) {
 		enc_abi_deinit(&encoder_cfg_ABI);
-	} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_SINCOS) {
 		enc_sincos_deinit(&encoder_cfg_sincos);
-	} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_TS5700N8501) {
 		enc_ts5700n8501_deinit(&encoder_cfg_TS5700N8501);
-	} else if (encoder_type_now == ENCODER_TYPE_AS5x47U) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_AS5x47U) {
 		enc_as5x47u_deinit(&encoder_cfg_as5x47u);
-	} else if (encoder_type_now == ENCODER_TYPE_BISSC) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_BISSC) {
 		enc_bissc_deinit(&encoder_cfg_bissc);
 	}
 
-	encoder_type_now = ENCODER_TYPE_NONE;
+	m_encoder_type_now = ENCODER_TYPE_NONE;
 }
 
 float encoder_read_deg(void) {
-	if (encoder_type_now == ENCODER_TYPE_AS504x) {
+	if (m_encoder_type_now == ENCODER_TYPE_AS504x) {
 		return AS504x_LAST_ANGLE(&encoder_cfg_as504x);
-	} else if (encoder_type_now == ENCODER_TYPE_MT6816) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_MT6816) {
 		return MT6816_LAST_ANGLE(&encoder_cfg_mt6816);
-	} else if (encoder_type_now == ENCODER_TYPE_TLE5012) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_TLE5012) {
 		return TLE5012_LAST_ANGLE(&encoder_cfg_tle5012);
-	} else if (encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_AD2S1205_SPI) {
 		return AD2S1205_LAST_ANGLE(&encoder_cfg_ad2s1205);
-	} else if (encoder_type_now == ENCODER_TYPE_ABI) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_ABI) {
 		return enc_abi_read_deg(&encoder_cfg_ABI);
-	} else if (encoder_type_now == ENCODER_TYPE_SINCOS) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_SINCOS) {
 		return enc_sincos_read_deg(&encoder_cfg_sincos);
-	} else if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_TS5700N8501) {
 		return enc_ts5700n8501_read_deg(&encoder_cfg_TS5700N8501);
-	} else if (encoder_type_now == ENCODER_TYPE_AS5x47U) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_AS5x47U) {
 		return AS5x47U_LAST_ANGLE(&encoder_cfg_as5x47u);
-	} else if (encoder_type_now == ENCODER_TYPE_BISSC) {
+	} else if (m_encoder_type_now == ENCODER_TYPE_BISSC) {
 		return BISSC_LAST_ANGLE(&encoder_cfg_bissc);
 	}
 	return 0.0;
 }
 
 float encoder_read_deg_multiturn(void) {
-	if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
+	if (m_encoder_type_now == ENCODER_TYPE_TS5700N8501) {
 		float ts_mt = (float)enc_ts5700n8501_get_abm(&encoder_cfg_TS5700N8501);
 		if (fabsf(ts_mt) > 5000.0) {
 			ts_mt = 0;
@@ -330,11 +336,11 @@ float encoder_read_deg_multiturn(void) {
 }
 
 encoder_type_t encoder_is_configured(void) {
-	return encoder_type_now;
+	return m_encoder_type_now;
 }
 
 bool encoder_index_found(void) {
-	if (encoder_type_now == ENCODER_TYPE_ABI) {
+	if (m_encoder_type_now == ENCODER_TYPE_ABI) {
 		return encoder_cfg_ABI.state.index_found;
 	} else {
 		return true;
@@ -342,13 +348,13 @@ bool encoder_index_found(void) {
 }
 
 void encoder_reset_multiturn(void) {
-	if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
+	if (m_encoder_type_now == ENCODER_TYPE_TS5700N8501) {
 		return enc_ts5700n8501_reset_multiturn(&encoder_cfg_TS5700N8501);
 	}
 }
 
 void encoder_reset_errors(void) {
-	if (encoder_type_now == ENCODER_TYPE_TS5700N8501) {
+	if (m_encoder_type_now == ENCODER_TYPE_TS5700N8501) {
 		enc_ts5700n8501_reset_errors(&encoder_cfg_TS5700N8501);
 	}
 }
@@ -461,33 +467,7 @@ void encoder_pin_isr(void) {
 }
 
 void encoder_tim_isr(void) {
-	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_TLE5012:
-			enc_tle5012_routine(&encoder_cfg_tle5012);
-			break;
-
-		case ENCODER_TYPE_AD2S1205_SPI:
-			enc_ad2s1205_routine(&encoder_cfg_ad2s1205);
-			break;
-
-		case ENCODER_TYPE_AS5x47U:
-			enc_as5x47u_routine(&encoder_cfg_as5x47u);
-			break;
-
-		case ENCODER_TYPE_BISSC:
-			enc_bissc_routine(&encoder_cfg_bissc);
-
-		default:
-			break;
-	}
+	// Use thread. Maybe use this one for encoders with a higher rate.
 }
 
 static void terminal_encoder(int argc, const char **argv) {
@@ -640,25 +620,56 @@ static void terminal_encoder_clear_multiturn(int argc, const char **argv) {
 	commands_printf("Done!\n");
 }
 
-static void timer_start(float rate) {
-	TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
+static THD_FUNCTION(routine_thread, arg) {
+	(void)arg;
+	chRegSetThreadName("Enc Routine");
 
-	// Enable timer clock
-	HW_ENC_TIM_CLK_EN();
+	for (;;) {
+		switch (m_encoder_type_now) {
+		case ENCODER_TYPE_AS504x:
+			enc_as504x_routine(&encoder_cfg_as504x);
+			break;
 
-	// Time Base configuration
-	TIM_TimeBaseStructure.TIM_Prescaler = 0;
-	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
-	TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / rate) - 1);
-	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
-	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
-	TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
+		case ENCODER_TYPE_MT6816:
+			enc_mt6816_routine(&encoder_cfg_mt6816);
+			break;
 
-	// Enable overflow interrupt
-	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
+		case ENCODER_TYPE_TLE5012:
+			enc_tle5012_routine(&encoder_cfg_tle5012);
+			break;
 
-	// Enable timer
-	TIM_Cmd(HW_ENC_TIM, ENABLE);
+		case ENCODER_TYPE_AD2S1205_SPI:
+			enc_ad2s1205_routine(&encoder_cfg_ad2s1205);
+			break;
 
-	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
+		case ENCODER_TYPE_AS5x47U:
+			enc_as5x47u_routine(&encoder_cfg_as5x47u);
+			break;
+
+		case ENCODER_TYPE_BISSC:
+			enc_bissc_routine(&encoder_cfg_bissc);
+			break;
+
+		default:
+			break;
+		}
+
+		switch (m_routine_rate) {
+		case routine_rate_1k: chThdSleep(CH_CFG_ST_FREQUENCY / 1000); break;
+		case routine_rate_2k: chThdSleep(CH_CFG_ST_FREQUENCY / 2000); break;
+		case routine_rate_5k: chThdSleep(CH_CFG_ST_FREQUENCY / 5000); break;
+		case routine_rate_10k: chThdSleep(CH_CFG_ST_FREQUENCY / 10000); break;
+		default: chThdSleep(5);
+		}
+	}
+}
+
+static void timer_start(routine_rate_t rate) {
+	m_routine_rate = rate;
+
+	static bool routine_running = false;
+	if (!routine_running) {
+		routine_running = true;
+		chThdCreateStatic(routine_thread_wa, sizeof(routine_thread_wa), NORMALPRIO + 5, routine_thread, NULL);
+	}
 }
diff --git a/encoder/encoder_cfg.c b/encoder/encoder_cfg.c
index d2dade10..15690399 100644
--- a/encoder/encoder_cfg.c
+++ b/encoder/encoder_cfg.c
@@ -23,7 +23,7 @@
 #include "hal.h"
 
 // Stack area for the running encoder
-static THD_WORKING_AREA(encoder_thread_wa, 512);
+static THD_WORKING_AREA(encoder_thread_wa, 256);
 
 #define SPI_BaudRatePrescaler_2         ((uint16_t)0x0000) //  42 MHz      21 MHZ
 #define SPI_BaudRatePrescaler_4         ((uint16_t)0x0008) //  21 MHz      10.5 MHz
@@ -46,9 +46,6 @@ AS504x_config_t encoder_cfg_as504x = {
 				0, 0,
 #endif
 				HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2,
-				spi_type_sw,
-				0, // has_started
-				0, // has_error
 				{{NULL, NULL}, NULL, NULL} // Mutex
 		},
 
@@ -65,9 +62,6 @@ AD2S1205_config_t encoder_cfg_ad2s1205 = {
 				0, 0,
 #endif
 				HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2,
-				spi_type_sw,
-				0, // has_started
-				0, // has_error
 				{{NULL, NULL}, NULL, NULL} // Mutex
 		},
 		{0},
@@ -105,9 +99,6 @@ TLE5012_config_t encoder_cfg_tle5012 = {
 				HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1, // sck
 				HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, // mosi
 				HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, // miso
-				ssc_type_sw,
-				0, // has_started
-				0, // has_error
 				{{NULL, NULL}, NULL, NULL} // Mutex
 		}, //ssc
 		{0, 0, 0, 0, 0, 0, 0, 0} // State
@@ -213,4 +204,4 @@ BISSC_config_t encoder_cfg_bissc = {
 		{0}, // crc
 		{0.0, 0, 0.0, 0, 0.0, 0, 0, {0}}
 #endif
-};
\ No newline at end of file
+};
diff --git a/make/fw.mk b/make/fw.mk
index ede6c3fd..b437ae91 100755
--- a/make/fw.mk
+++ b/make/fw.mk
@@ -64,7 +64,7 @@ endif
 # Stack size to be allocated to the Cortex-M process stack. This stack is
 # the stack used by the main() thread.
 ifeq ($(USE_PROCESS_STACKSIZE),)
-  USE_PROCESS_STACKSIZE = 0x800
+  USE_PROCESS_STACKSIZE = 0x400
 endif
 
 # Stack size to the allocated to the Cortex-M main/exceptions stack. This