diff --git a/CHANGELOG b/CHANGELOG
index 18542ca8b..bc7114b6f 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,11 @@
+=== FW 3.63 ===
+* NRF remote power meter is now unaffected by temperature decrease and speed limits.
+* Added LZO compression support to firmware upload, making firmware updates 30% - 50% faster.
+* Added LZO compression support to SWD upload.
+* Made serial interrupts priority higher than PWM so that higher speed UART is possible.
+* Added support for TS5700N8501 encoder (via COMM port).
+* Better observer gain calculation.
+
 === FW 3.62 ===
 * Added COMM_BM_MEM_READ.
 * Merged EUC app (experimental).
diff --git a/applications/app_custom_template.c b/applications/app_custom_template.c
index 25110b760..09f9766fd 100644
--- a/applications/app_custom_template.c
+++ b/applications/app_custom_template.c
@@ -29,6 +29,7 @@
 #include "comm_can.h"
 #include "hw.h"
 #include "commands.h"
+#include "timeout.h"
 
 #include <math.h>
 #include <string.h>
@@ -109,9 +110,11 @@ static THD_FUNCTION(my_thread, arg) {
 			return;
 		}
 
+		timeout_reset(); // Reset timeout if everything is OK.
+
 		// Run your logic here. A lot of functionality is available in mc_interface.h.
 
-		chThdSleepMicroseconds(10);
+		chThdSleepMilliseconds(10);
 	}
 }
 
diff --git a/applications/app_uartcomm.c b/applications/app_uartcomm.c
index f55919152..7f7279b8d 100644
--- a/applications/app_uartcomm.c
+++ b/applications/app_uartcomm.c
@@ -129,10 +129,12 @@ void app_uartcomm_start_permanent(void) {
 }
 
 void app_uartcomm_stop(void) {
-	sdStop(&HW_UART_DEV);
-	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);
-	uart_is_running = false;
+	if (uart_is_running) {
+		sdStop(&HW_UART_DEV);
+		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);
+		uart_is_running = false;
+	}
 
 	// Notice that the processing thread is kept running in case this call is made from it.
 }
@@ -151,9 +153,8 @@ void app_uartcomm_send_packet(unsigned char *data, unsigned int len) {
 void app_uartcomm_configure(uint32_t baudrate, bool permanent_enabled) {
 	uart_cfg.speed = baudrate;
 
-	if (thread_is_running) {
+	if (thread_is_running && uart_is_running) {
 		sdStart(&HW_UART_DEV, &uart_cfg);
-		uart_is_running = true;
 	}
 
 #ifdef HW_UART_P_DEV
@@ -193,17 +194,19 @@ static THD_FUNCTION(packet_process_thread, arg) {
 		while (rx) {
 			rx = false;
 
-			msg_t res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
-			if (res != MSG_TIMEOUT) {
+			if (uart_is_running) {
+				msg_t res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
+				if (res != MSG_TIMEOUT) {
 #ifdef HW_UART_P_DEV
-				from_p_uart = false;
+					from_p_uart = false;
 #endif
-				packet_process_byte(res, PACKET_HANDLER);
-				rx = true;
+					packet_process_byte(res, PACKET_HANDLER);
+					rx = true;
+				}
 			}
 
 #ifdef HW_UART_P_DEV
-			res = sdGetTimeout(&HW_UART_P_DEV, TIME_IMMEDIATE);
+			msg_t res = sdGetTimeout(&HW_UART_P_DEV, TIME_IMMEDIATE);
 			if (res != MSG_TIMEOUT) {
 				from_p_uart = true;
 				packet_process_byte(res, PACKET_HANDLER_P);
diff --git a/build_all/410_o_411_o_412/VESC_0005ohm.bin b/build_all/410_o_411_o_412/VESC_0005ohm.bin
index a8fdcd537..c8292204e 100755
Binary files a/build_all/410_o_411_o_412/VESC_0005ohm.bin and b/build_all/410_o_411_o_412/VESC_0005ohm.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_default.bin b/build_all/410_o_411_o_412/VESC_default.bin
index a22008732..f7416e1eb 100755
Binary files a/build_all/410_o_411_o_412/VESC_default.bin and b/build_all/410_o_411_o_412/VESC_default.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin b/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin
index 0748eaea9..025b6776a 100755
Binary files a/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin and b/build_all/410_o_411_o_412/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_servoout.bin b/build_all/410_o_411_o_412/VESC_servoout.bin
index 33e2ee3db..5b84e504e 100755
Binary files a/build_all/410_o_411_o_412/VESC_servoout.bin and b/build_all/410_o_411_o_412/VESC_servoout.bin differ
diff --git a/build_all/410_o_411_o_412/VESC_ws2811.bin b/build_all/410_o_411_o_412/VESC_ws2811.bin
index a2c01074e..93f2e4ab7 100755
Binary files a/build_all/410_o_411_o_412/VESC_ws2811.bin and b/build_all/410_o_411_o_412/VESC_ws2811.bin differ
diff --git a/build_all/46_o_47/VESC_0005ohm.bin b/build_all/46_o_47/VESC_0005ohm.bin
index 22303a894..137465692 100755
Binary files a/build_all/46_o_47/VESC_0005ohm.bin and b/build_all/46_o_47/VESC_0005ohm.bin differ
diff --git a/build_all/46_o_47/VESC_33k.bin b/build_all/46_o_47/VESC_33k.bin
index 04edf01ff..265521d2d 100755
Binary files a/build_all/46_o_47/VESC_33k.bin and b/build_all/46_o_47/VESC_33k.bin differ
diff --git a/build_all/46_o_47/VESC_default.bin b/build_all/46_o_47/VESC_default.bin
index dff2182bc..b60357374 100755
Binary files a/build_all/46_o_47/VESC_default.bin and b/build_all/46_o_47/VESC_default.bin differ
diff --git a/build_all/46_o_47/VESC_servoout.bin b/build_all/46_o_47/VESC_servoout.bin
index 62536e3ba..65583be30 100755
Binary files a/build_all/46_o_47/VESC_servoout.bin and b/build_all/46_o_47/VESC_servoout.bin differ
diff --git a/build_all/46_o_47/VESC_ws2811.bin b/build_all/46_o_47/VESC_ws2811.bin
index 6a4bcfaf2..f5b2f1d5e 100755
Binary files a/build_all/46_o_47/VESC_ws2811.bin and b/build_all/46_o_47/VESC_ws2811.bin differ
diff --git a/build_all/46_o_47/VESC_ws2811_33k.bin b/build_all/46_o_47/VESC_ws2811_33k.bin
index 52bca6185..77617bafb 100755
Binary files a/build_all/46_o_47/VESC_ws2811_33k.bin and b/build_all/46_o_47/VESC_ws2811_33k.bin differ
diff --git a/build_all/48/VESC_0005ohm.bin b/build_all/48/VESC_0005ohm.bin
index 9e5451a52..c3122654c 100755
Binary files a/build_all/48/VESC_0005ohm.bin and b/build_all/48/VESC_0005ohm.bin differ
diff --git a/build_all/48/VESC_default.bin b/build_all/48/VESC_default.bin
index 24fb0a7a0..bd17ef8ed 100755
Binary files a/build_all/48/VESC_default.bin and b/build_all/48/VESC_default.bin differ
diff --git a/build_all/48/VESC_servoout.bin b/build_all/48/VESC_servoout.bin
index b4407d582..f9d61093e 100755
Binary files a/build_all/48/VESC_servoout.bin and b/build_all/48/VESC_servoout.bin differ
diff --git a/build_all/48/VESC_ws2811.bin b/build_all/48/VESC_ws2811.bin
index 91e33bd0d..54d20e395 100755
Binary files a/build_all/48/VESC_ws2811.bin and b/build_all/48/VESC_ws2811.bin differ
diff --git a/build_all/60/VESC_default.bin b/build_all/60/VESC_default.bin
index c6b783f74..f0828598c 100755
Binary files a/build_all/60/VESC_default.bin and b/build_all/60/VESC_default.bin differ
diff --git a/build_all/60/VESC_default_no_hw_limits.bin b/build_all/60/VESC_default_no_hw_limits.bin
index c186f610d..d47f04edb 100755
Binary files a/build_all/60/VESC_default_no_hw_limits.bin and b/build_all/60/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/60/VESC_servoout.bin b/build_all/60/VESC_servoout.bin
index 23053c3da..c3d0b3e54 100755
Binary files a/build_all/60/VESC_servoout.bin and b/build_all/60/VESC_servoout.bin differ
diff --git a/build_all/60/VESC_ws2811.bin b/build_all/60/VESC_ws2811.bin
index 49c2127ac..4b3f43774 100755
Binary files a/build_all/60/VESC_ws2811.bin and b/build_all/60/VESC_ws2811.bin differ
diff --git a/build_all/75_300/VESC_default.bin b/build_all/75_300/VESC_default.bin
index ed00fcb4d..440688250 100755
Binary files a/build_all/75_300/VESC_default.bin and b/build_all/75_300/VESC_default.bin differ
diff --git a/build_all/75_300/VESC_default_no_hw_limits.bin b/build_all/75_300/VESC_default_no_hw_limits.bin
index e26b959bb..855c3770f 100755
Binary files a/build_all/75_300/VESC_default_no_hw_limits.bin and b/build_all/75_300/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/75_300/VESC_servoout.bin b/build_all/75_300/VESC_servoout.bin
index d3b978eb2..554d3c5ce 100755
Binary files a/build_all/75_300/VESC_servoout.bin and b/build_all/75_300/VESC_servoout.bin differ
diff --git a/build_all/75_300/VESC_ws2811.bin b/build_all/75_300/VESC_ws2811.bin
index 622427cc3..e38567b06 100755
Binary files a/build_all/75_300/VESC_ws2811.bin and b/build_all/75_300/VESC_ws2811.bin differ
diff --git a/build_all/75_300_R2/VESC_default.bin b/build_all/75_300_R2/VESC_default.bin
index eac6727b4..72c7d0973 100755
Binary files a/build_all/75_300_R2/VESC_default.bin and b/build_all/75_300_R2/VESC_default.bin differ
diff --git a/build_all/75_300_R2/VESC_default_no_hw_limits.bin b/build_all/75_300_R2/VESC_default_no_hw_limits.bin
index d076d016b..189b9ba6f 100755
Binary files a/build_all/75_300_R2/VESC_default_no_hw_limits.bin and b/build_all/75_300_R2/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/75_300_R2/VESC_servoout.bin b/build_all/75_300_R2/VESC_servoout.bin
index 521d48411..6f74ca3bb 100755
Binary files a/build_all/75_300_R2/VESC_servoout.bin and b/build_all/75_300_R2/VESC_servoout.bin differ
diff --git a/build_all/75_300_R2/VESC_ws2811.bin b/build_all/75_300_R2/VESC_ws2811.bin
index 54326dc2e..6838ac10a 100755
Binary files a/build_all/75_300_R2/VESC_ws2811.bin and b/build_all/75_300_R2/VESC_ws2811.bin differ
diff --git a/build_all/A200S_V21/VESC_default.bin b/build_all/A200S_V21/VESC_default.bin
index 00111f9ac..ec0112128 100755
Binary files a/build_all/A200S_V21/VESC_default.bin and b/build_all/A200S_V21/VESC_default.bin differ
diff --git a/build_all/A200S_V22/VESC_default.bin b/build_all/A200S_V22/VESC_default.bin
index f17dbea7e..dccea7405 100755
Binary files a/build_all/A200S_V22/VESC_default.bin and b/build_all/A200S_V22/VESC_default.bin differ
diff --git a/build_all/AXIOM/VESC_default.bin b/build_all/AXIOM/VESC_default.bin
index 213831470..cef67464e 100755
Binary files a/build_all/AXIOM/VESC_default.bin and b/build_all/AXIOM/VESC_default.bin differ
diff --git a/build_all/DAS_RS/VESC_default.bin b/build_all/DAS_RS/VESC_default.bin
index 503e55284..8159a64a3 100755
Binary files a/build_all/DAS_RS/VESC_default.bin and b/build_all/DAS_RS/VESC_default.bin differ
diff --git a/build_all/HD/VESC_default.bin b/build_all/HD/VESC_default.bin
index dd59d24c7..37392ec69 100755
Binary files a/build_all/HD/VESC_default.bin and b/build_all/HD/VESC_default.bin differ
diff --git a/build_all/HD/VESC_default_no_hw_limits.bin b/build_all/HD/VESC_default_no_hw_limits.bin
index 3a13a46d6..659bf2b5c 100755
Binary files a/build_all/HD/VESC_default_no_hw_limits.bin and b/build_all/HD/VESC_default_no_hw_limits.bin differ
diff --git a/build_all/HD/VESC_servoout.bin b/build_all/HD/VESC_servoout.bin
index c13b18b40..73254f047 100755
Binary files a/build_all/HD/VESC_servoout.bin and b/build_all/HD/VESC_servoout.bin differ
diff --git a/build_all/HD/VESC_ws2811.bin b/build_all/HD/VESC_ws2811.bin
index a745d0178..9495d5450 100755
Binary files a/build_all/HD/VESC_ws2811.bin and b/build_all/HD/VESC_ws2811.bin differ
diff --git a/build_all/UAVC_OMEGA/VESC_default.bin b/build_all/UAVC_OMEGA/VESC_default.bin
index 26e67926b..7876c9a2f 100755
Binary files a/build_all/UAVC_OMEGA/VESC_default.bin and b/build_all/UAVC_OMEGA/VESC_default.bin differ
diff --git a/chconf.h b/chconf.h
index 72ca57518..dfb939117 100644
--- a/chconf.h
+++ b/chconf.h
@@ -29,7 +29,7 @@
 #define _CHCONF_H_
 
 #define CHPRINTF_USE_FLOAT				TRUE
-#define CORTEX_SIMPLIFIED_PRIORITY		FALSE
+#define CORTEX_SIMPLIFIED_PRIORITY		TRUE
 #define PORT_IDLE_THREAD_STACK_SIZE		64
 #define PORT_INT_REQUIRED_STACK			128
 
diff --git a/commands.c b/commands.c
index db7fa2167..217d094a8 100644
--- a/commands.c
+++ b/commands.c
@@ -45,6 +45,7 @@
 #if HAS_BLACKMAGIC
 #include "bm_if.h"
 #endif
+#include "minilzo.h"
 
 #include <math.h>
 #include <string.h>
@@ -222,16 +223,39 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		reply_func(send_buffer, ind);
 	} break;
 
+	case COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO:
 	case COMM_WRITE_NEW_APP_DATA_ALL_CAN:
+		if (packet_id == COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO) {
+			chMtxLock(&send_buffer_mutex);
+			memcpy(send_buffer_global, data + 6, len - 6);
+			int32_t ind = 4;
+			lzo_uint decompressed_len = buffer_get_uint16(data, &ind);
+			lzo1x_decompress_safe(send_buffer_global, len - 6, data + 4, &decompressed_len, NULL);
+			chMtxUnlock(&send_buffer_mutex);
+			len = decompressed_len + 4;
+		}
+
 		if (nrf_driver_ext_nrf_running()) {
 			nrf_driver_pause(2000);
 		}
 
 		data[-1] = COMM_WRITE_NEW_APP_DATA;
+
 		comm_can_send_buffer(255, data - 1, len + 1, 2);
 		/* Falls through. */
 		/* no break */
+	case COMM_WRITE_NEW_APP_DATA_LZO:
 	case COMM_WRITE_NEW_APP_DATA: {
+		if (packet_id == COMM_WRITE_NEW_APP_DATA_LZO) {
+			chMtxLock(&send_buffer_mutex);
+			memcpy(send_buffer_global, data + 6, len - 6);
+			int32_t ind = 4;
+			lzo_uint decompressed_len = buffer_get_uint16(data, &ind);
+			lzo1x_decompress_safe(send_buffer_global, len - 6, data + 4, &decompressed_len, NULL);
+			chMtxUnlock(&send_buffer_mutex);
+			len = decompressed_len + 4;
+		}
+
 		int32_t ind = 0;
 		uint32_t new_app_offset = buffer_get_uint32(data, &ind);
 
@@ -948,6 +972,7 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 	case COMM_PING_CAN:
 	case COMM_BM_CONNECT:
 	case COMM_BM_ERASE_FLASH_ALL:
+	case COMM_BM_WRITE_FLASH_LZO:
 	case COMM_BM_WRITE_FLASH:
 	case COMM_BM_REBOOT:
 	case COMM_BM_DISCONNECT:
@@ -1162,7 +1187,7 @@ static THD_FUNCTION(blocking_thread, arg) {
 
 		COMM_PACKET_ID packet_id;
 		static mc_configuration mcconf, mcconf_old;
-		static uint8_t send_buffer[384];
+		static uint8_t send_buffer[512];
 
 		packet_id = data[0];
 		data++;
@@ -1410,7 +1435,16 @@ static THD_FUNCTION(blocking_thread, arg) {
 			}
 		} break;
 
+		case COMM_BM_WRITE_FLASH_LZO:
 		case COMM_BM_WRITE_FLASH: {
+			if (packet_id == COMM_BM_WRITE_FLASH_LZO) {
+				memcpy(send_buffer, data + 6, len - 6);
+				int32_t ind = 4;
+				lzo_uint decompressed_len = buffer_get_uint16(data, &ind);
+				lzo1x_decompress_safe(send_buffer, len - 6, data + 4, &decompressed_len, NULL);
+				len = decompressed_len + 4;
+			}
+
 			int32_t ind = 0;
 			uint32_t addr = buffer_get_uint32(data, &ind);
 
diff --git a/conf_general.c b/conf_general.c
index cc2feefb2..9cad59e83 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -798,8 +798,8 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
 	const int max_time = 15000;
 
 	while (fabsf(mc_interface_get_duty_cycle_now()) < duty) {
-		mcpwm_foc_set_openloop(current, mcconf.m_invert_direction ? -rpm_now : rpm_now);
 		rpm_now += erpm_per_sec / 1000.0;
+		mcpwm_foc_set_openloop(current, mcconf.m_invert_direction ? -rpm_now : rpm_now);
 
 		chThdSleepMilliseconds(1);
 		cnt++;
@@ -841,13 +841,13 @@ bool conf_general_measure_flux_linkage_openloop(float current, float duty,
 		float id_avg = 0.0;
 		float samples2 = 0.0;
 
-		for (int i = 0;i < 1000;i++) {
+		for (int i = 0;i < 30000;i++) {
 			vq_avg += mcpwm_foc_get_vq();
 			vd_avg += mcpwm_foc_get_vd();
 			iq_avg += mcpwm_foc_get_iq();
 			id_avg += mcpwm_foc_get_id();
 			samples2 += 1.0;
-			chThdSleepMilliseconds(1);
+			chThdSleep(1);
 		}
 
 		vq_avg /= samples2;
@@ -1131,7 +1131,7 @@ int conf_general_detect_apply_all_foc(float max_power_loss,
 		float bw = 1.0 / (tc * 1e-6);
 		float kp = l * bw;
 		float ki = r * bw;
-		float gain = 0.001 / (lambda * lambda);
+		float gain = (0.00001 / r) / (lambda * lambda);
 
 		mcconf_old.motor_type = MOTOR_TYPE_FOC;
 		mcconf_old.foc_motor_r = r;
diff --git a/conf_general.h b/conf_general.h
index 1363a766e..413ee4f79 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		62
+#define FW_VERSION_MINOR		63
 
 #include "datatypes.h"
 
@@ -151,14 +151,15 @@
 //#include			"appconf_custom.h"
 //#include			"appconf_ellwee.h"
 
-#include "hw.h"
-#include "mcconf_default.h"
-#include "appconf_default.h"
-
 /*
  * Set APP_CUSTOM_TO_USE to the name of the main C file of the custom application.
  */
 //#define APP_CUSTOM_TO_USE			"app_custom_template.c"
+//#include "app_erockit_conf.h"
+
+#include "hw.h"
+#include "mcconf_default.h"
+#include "appconf_default.h"
 
 /*
  * Enable blackmagic probe output on SWD port
diff --git a/confgenerator.h b/confgenerator.h
index e4062e42c..61e50be84 100644
--- a/confgenerator.h
+++ b/confgenerator.h
@@ -8,7 +8,7 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		503309878
+#define MCCONF_SIGNATURE		3875075384
 #define APPCONF_SIGNATURE		783041200
 
 // Functions
diff --git a/datatypes.h b/datatypes.h
index 4ee7d3e6e..aa760876d 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -129,7 +129,8 @@ typedef enum {
 	SENSOR_PORT_MODE_ABI,
 	SENSOR_PORT_MODE_AS5047_SPI,
 	SENSOR_PORT_MODE_AD2S1205,
-	SENSOR_PORT_MODE_SINCOS
+	SENSOR_PORT_MODE_SINCOS,
+	SENSOR_PORT_MODE_TS5700N8501
 } sensor_port_mode;
 
 typedef struct {
@@ -687,6 +688,9 @@ typedef enum {
 	COMM_PLOT_SET_GRAPH,
 	COMM_GET_DECODED_BALANCE,
 	COMM_BM_MEM_READ,
+	COMM_WRITE_NEW_APP_DATA_LZO,
+	COMM_WRITE_NEW_APP_DATA_ALL_CAN_LZO,
+	COMM_BM_WRITE_FLASH_LZO
 } COMM_PACKET_ID;
 
 // CAN commands
diff --git a/encoder.c b/encoder.c
index 492b4dc9e..dd06a1f27 100644
--- a/encoder.c
+++ b/encoder.c
@@ -71,7 +71,8 @@ typedef enum {
 	ENCODER_MODE_ABI,
 	ENCODER_MODE_AS5047P_SPI,
 	RESOLVER_MODE_AD2S1205,
-	ENCODER_MODE_SINCOS
+	ENCODER_MODE_SINCOS,
+	ENCODER_MODE_TS5700N8501
 } encoder_mode;
 
 // Private variables
@@ -79,26 +80,38 @@ 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;
-uint16_t spi_val = 0;
-uint32_t spi_error_cnt = 0;
-float spi_error_rate = 0.0;
-
-float sin_gain = 0.0;
-float sin_offset = 0.0;
-float cos_gain = 0.0;
-float cos_offset = 0.0;
-float sincos_filter_constant = 0.0;
-uint32_t sincos_signal_below_min_error_cnt = 0;
-uint32_t sincos_signal_above_max_error_cnt = 0;
-float sincos_signal_low_error_rate = 0.0;
-float sincos_signal_above_max_error_rate = 0.0;
+static uint16_t spi_val = 0;
+static uint32_t spi_error_cnt = 0;
+static float spi_error_rate = 0.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
+};
+
+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;
 
 // 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);
-
+void TS5700N8501_send_byte(uint8_t b);
 
 uint32_t encoder_spi_get_error_cnt(void) {
 	return spi_error_cnt;
@@ -141,6 +154,19 @@ void encoder_deinit(void) {
 	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;
@@ -302,6 +328,18 @@ void encoder_init_sincos(float s_gain, float s_offset,
 #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;
 }
@@ -322,6 +360,7 @@ float encoder_read_deg(void) {
 
 	case ENCODER_MODE_AS5047P_SPI:
 	case RESOLVER_MODE_AD2S1205:
+	case ENCODER_MODE_TS5700N8501:
 		angle = last_enc_angle;
 		break;
 
@@ -533,3 +572,112 @@ static void spi_delay(void) {
 	__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.
+ */
+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;
+		}
+
+		TS5700N8501_send_byte(0b01000000);
+
+		chThdSleep(1);
+
+		uint8_t reply[6];
+		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 == 6 && crc == reply[reply_ind - 1]) {
+			uint32_t pos = (uint32_t)reply[2] + ((uint32_t)reply[3] << 8) + ((uint32_t)reply[4] << 16);
+			last_enc_angle = (float)pos / 131072.0 * 360.0;
+			UTILS_LP_FAST(spi_error_rate, 0.0, 1.0 / AS5047_SAMPLE_RATE_HZ);
+		} 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
index 93a121a49..dd40783aa 100644
--- a/encoder.h
+++ b/encoder.h
@@ -29,6 +29,7 @@ void encoder_init_as5047p_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);
 void encoder_reset(void);
diff --git a/hwconf/hw_40.h b/hwconf/hw_40.h
index c727edbb5..b9030e61b 100644
--- a/hwconf/hw_40.h
+++ b/hwconf/hw_40.h
@@ -111,6 +111,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/hwconf/hw_45.h b/hwconf/hw_45.h
index 124bb7ea3..795ab39da 100644
--- a/hwconf/hw_45.h
+++ b/hwconf/hw_45.h
@@ -113,6 +113,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/hwconf/hw_46.h b/hwconf/hw_46.h
index ea3adbcb1..b106952c9 100644
--- a/hwconf/hw_46.h
+++ b/hwconf/hw_46.h
@@ -113,6 +113,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/hwconf/hw_48.h b/hwconf/hw_48.h
index d086f371c..905dc90d8 100644
--- a/hwconf/hw_48.h
+++ b/hwconf/hw_48.h
@@ -107,6 +107,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/hwconf/hw_49.h b/hwconf/hw_49.h
index a3fe9bf29..076ff30b1 100644
--- a/hwconf/hw_49.h
+++ b/hwconf/hw_49.h
@@ -107,6 +107,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h
index b43ad0ebd..79c478971 100644
--- a/hwconf/hw_60.h
+++ b/hwconf/hw_60.h
@@ -140,6 +140,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_75_300.h b/hwconf/hw_75_300.h
index 60542ba52..a5dc3e7e8 100644
--- a/hwconf/hw_75_300.h
+++ b/hwconf/hw_75_300.h
@@ -171,6 +171,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_a200s_v2.h b/hwconf/hw_a200s_v2.h
index e9e8ee357..6a601c7dd 100644
--- a/hwconf/hw_a200s_v2.h
+++ b/hwconf/hw_a200s_v2.h
@@ -143,6 +143,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_axiom.h b/hwconf/hw_axiom.h
index 066dd1122..e015b1d18 100644
--- a/hwconf/hw_axiom.h
+++ b/hwconf/hw_axiom.h
@@ -194,6 +194,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOB
+#define HW_ADC_EXT2_PIN			0
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_das_mini.h b/hwconf/hw_das_mini.h
index abd540a77..e58b1f354 100644
--- a/hwconf/hw_das_mini.h
+++ b/hwconf/hw_das_mini.h
@@ -124,6 +124,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_das_rs.h b/hwconf/hw_das_rs.h
index f50989b98..c0aeda1ff 100644
--- a/hwconf/hw_das_rs.h
+++ b/hwconf/hw_das_rs.h
@@ -139,6 +139,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_hd.h b/hwconf/hw_hd.h
index da91b4672..d9066518e 100644
--- a/hwconf/hw_hd.h
+++ b/hwconf/hw_hd.h
@@ -133,6 +133,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_mini4.h b/hwconf/hw_mini4.h
index cb7b18464..fa9d59fbe 100644
--- a/hwconf/hw_mini4.h
+++ b/hwconf/hw_mini4.h
@@ -123,6 +123,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_r2.h b/hwconf/hw_r2.h
index b30c7398f..da651b004 100644
--- a/hwconf/hw_r2.h
+++ b/hwconf/hw_r2.h
@@ -123,6 +123,10 @@
 #define CURR2_DOUBLE_SAMPLE	0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_rh.h b/hwconf/hw_rh.h
index 535775a22..f712740a2 100644
--- a/hwconf/hw_rh.h
+++ b/hwconf/hw_rh.h
@@ -114,6 +114,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			4
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/hwconf/hw_tp.h b/hwconf/hw_tp.h
index ca00776fa..1e346ac44 100644
--- a/hwconf/hw_tp.h
+++ b/hwconf/hw_tp.h
@@ -122,6 +122,10 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_uavc_omega.h b/hwconf/hw_uavc_omega.h
index 008467b8e..a9777d0ae 100644
--- a/hwconf/hw_uavc_omega.h
+++ b/hwconf/hw_uavc_omega.h
@@ -126,6 +126,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_uavc_qcube.h b/hwconf/hw_uavc_qcube.h
index 8947e278c..8888591d3 100644
--- a/hwconf/hw_uavc_qcube.h
+++ b/hwconf/hw_uavc_qcube.h
@@ -126,6 +126,12 @@
 #define CURR3_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOA
+#define HW_ADC_EXT_PIN			5
+#define HW_ADC_EXT2_GPIO		GPIOA
+#define HW_ADC_EXT2_PIN			6
+
 // UART Peripheral
 #define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
diff --git a/hwconf/hw_victor_r1a.h b/hwconf/hw_victor_r1a.h
index a70d168bc..071eca1fc 100644
--- a/hwconf/hw_victor_r1a.h
+++ b/hwconf/hw_victor_r1a.h
@@ -110,6 +110,10 @@
 #define CURR2_DOUBLE_SAMPLE		0
 #endif
 
+// COMM-port ADC GPIOs
+#define HW_ADC_EXT_GPIO			GPIOC
+#define HW_ADC_EXT_PIN			5
+
 // UART Peripheral
 #define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
diff --git a/mc_interface.c b/mc_interface.c
index 29b878ec3..6aaf4ed30 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -36,6 +36,8 @@
 #include "gpdrive.h"
 #include "comm_can.h"
 #include "shutdown.h"
+#include "app.h"
+
 #include <math.h>
 #include <stdlib.h>
 
@@ -97,6 +99,10 @@ static volatile int m_sample_now;
 static volatile int m_sample_trigger;
 static volatile float m_last_adc_duration_sample;
 
+#if !WS2811_ENABLE
+static app_configuration m_tmp_appconf;
+#endif
+
 // Private functions
 static void update_override_limits(volatile mc_configuration *conf);
 
@@ -184,6 +190,18 @@ void mc_interface_init(mc_configuration *configuration) {
 							m_conf.foc_encoder_sincos_filter_constant);
 		break;
 
+	case SENSOR_PORT_MODE_TS5700N8501:
+		conf_general_read_app_configuration(&m_tmp_appconf);
+		if (m_tmp_appconf.app_to_use == APP_ADC ||
+				m_tmp_appconf.app_to_use == APP_UART ||
+				m_tmp_appconf.app_to_use == APP_PPM_UART ||
+				m_tmp_appconf.app_to_use == APP_ADC_UART) {
+			m_tmp_appconf.app_to_use = APP_NONE;
+			conf_general_store_app_configuration(&m_tmp_appconf);
+		}
+		encoder_init_ts5700n8501();
+		break;
+
 	default:
 		break;
 	}
@@ -236,6 +254,19 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 								m_conf.foc_encoder_sincos_filter_constant);
 			break;
 
+		case SENSOR_PORT_MODE_TS5700N8501: {
+			m_tmp_appconf = *app_get_configuration();
+			if (m_tmp_appconf.app_to_use == APP_ADC ||
+					m_tmp_appconf.app_to_use == APP_UART ||
+					m_tmp_appconf.app_to_use == APP_PPM_UART ||
+					m_tmp_appconf.app_to_use == APP_ADC_UART) {
+				m_tmp_appconf.app_to_use = APP_NONE;
+				conf_general_store_app_configuration(&m_tmp_appconf);
+				app_set_configuration(&m_tmp_appconf);
+			}
+			encoder_init_ts5700n8501();
+		} break;
+
 		default:
 			break;
 		}
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index ada07fa9f..21f69f044 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -307,7 +307,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE);
 
 	dmaStreamAllocate(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)),
-			3,
+			4,
 			(stm32_dmaisr_t)mcpwm_foc_adc_int_handler,
 			(void *)0);
 
diff --git a/mcuconf.h b/mcuconf.h
index f356b6207..e7e0dfec3 100644
--- a/mcuconf.h
+++ b/mcuconf.h
@@ -277,12 +277,12 @@
 #define STM32_SERIAL_USE_UART4              TRUE
 #define STM32_SERIAL_USE_UART5              FALSE
 #define STM32_SERIAL_USE_USART6             TRUE
-#define STM32_SERIAL_USART1_PRIORITY        12
-#define STM32_SERIAL_USART2_PRIORITY        12
-#define STM32_SERIAL_USART3_PRIORITY        12
-#define STM32_SERIAL_UART4_PRIORITY         12
-#define STM32_SERIAL_UART5_PRIORITY         12
-#define STM32_SERIAL_USART6_PRIORITY        12
+#define STM32_SERIAL_USART1_PRIORITY        3
+#define STM32_SERIAL_USART2_PRIORITY        3
+#define STM32_SERIAL_USART3_PRIORITY        3
+#define STM32_SERIAL_UART4_PRIORITY         3
+#define STM32_SERIAL_UART5_PRIORITY         3
+#define STM32_SERIAL_USART6_PRIORITY        3
 
 /*
  * SPI driver system settings.
diff --git a/nrf/nrf_driver.c b/nrf/nrf_driver.c
index 3c316cbf6..200b91e65 100644
--- a/nrf/nrf_driver.c
+++ b/nrf/nrf_driver.c
@@ -215,7 +215,8 @@ static THD_FUNCTION(tx_thread, arg) {
 			buffer_append_float32(pl, val.wh_tot, 1e4, &index);
 			buffer_append_float32(pl, val.wh_charge_tot, 1e4, &index);
 			pl[index++] = (uint8_t)((int8_t)(mc_interface_get_tot_current_directional_filtered() /
-					mc_interface_get_configuration()->lo_current_motor_max_now * 100.0));
+					(mc_interface_get_configuration()->l_current_max *
+							mc_interface_get_configuration()->l_current_max_scale) * 100.0));
 
 			if (driver_paused == 0) {
 				rf_tx_wrapper((char*)pl, index);
diff --git a/terminal.c b/terminal.c
index 831ac9825..537b7bda9 100644
--- a/terminal.c
+++ b/terminal.c
@@ -667,7 +667,8 @@ void terminal_process_string(char *str) {
 		}
 	} else if (strcmp(argv[0], "encoder") == 0) {
 		if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_AS5047_SPI ||
-			mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205) {
+			mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_AD2S1205 ||
+			mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_TS5700N8501) {
 			commands_printf("SPI encoder value: %x, errors: %d, error rate: %.3f %%",
 				(unsigned int)encoder_spi_get_val(),
 				encoder_spi_get_error_cnt(),
@@ -796,7 +797,7 @@ void terminal_process_string(char *str) {
 		commands_printf("  initiates detection in all VESCs found on the CAN-bus.");
 		
 		commands_printf("encoder");
-		commands_printf("  Prints the status of the AS5047, AD2S1205, or Sin/Cos encoder.");
+		commands_printf("  Prints the status of the AS5047, AD2S1205, or TS5700N8501 encoder.");
 
 		for (int i = 0;i < callback_write;i++) {
 			if (callbacks[i].cbf == 0) {
diff --git a/timeout.c b/timeout.c
index 2a3ba3104..1239d3df5 100644
--- a/timeout.c
+++ b/timeout.c
@@ -22,6 +22,7 @@
 #include "stm32f4xx_conf.h"
 
 // Private variables
+static volatile bool init_done = false;
 static volatile systime_t timeout_msec;
 static volatile systime_t last_update_time;
 static volatile float timeout_brake_current;
@@ -37,6 +38,7 @@ void timeout_init(void) {
 	last_update_time = 0;
 	timeout_brake_current = 0.0;
 	has_timeout = false;
+	init_done = true;
 
 	IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable);
 
@@ -100,6 +102,10 @@ void timeout_feed_WDT(uint8_t index) {
 }
 
 void timeout_configure_IWDT_slowest(void) {
+	if (!init_done) {
+		return;
+	}
+
 	while(((IWDG->SR & IWDG_SR_RVU) != 0) || ((IWDG->SR & IWDG_SR_PVU) != 0)) {
 		// Continue to kick the dog
 		IWDG_ReloadCounter();
@@ -119,6 +125,10 @@ void timeout_configure_IWDT_slowest(void) {
 }
 
 void timeout_configure_IWDT(void) {
+	if (!init_done) {
+		return;
+	}
+
 	while(((IWDG->SR & IWDG_SR_RVU) != 0) || ((IWDG->SR & IWDG_SR_PVU) != 0)) {
 		// Continue to kick the dog
 		IWDG_ReloadCounter();