From c77f92470bac22d43e9f35f40df05685da03b0b5 Mon Sep 17 00:00:00 2001
From: Benjamin Vedder <benjamin@vedder.se>
Date: Fri, 9 Oct 2020 21:08:48 +0200
Subject: [PATCH] IO board support, initial bms support, more openloop
 parameters, removed D current injection, added new HWs

---
 CHANGELOG               |   6 +
 Makefile                |   7 +-
 comm_can.c              | 161 ++++++++++++++++++++++-
 comm_can.h              |  12 +-
 commands.c              |  29 +++-
 commands.h              |   1 +
 conf_general.c          |   7 +-
 conf_general.h          |  25 ++--
 confgenerator.c         |  26 ++--
 confgenerator.h         |   2 +-
 datatypes.h             |  40 +++++-
 hwconf/hw.h             |   8 ++
 hwconf/hw_60.c          |  22 +++-
 hwconf/hw_60.h          |  29 ++--
 hwconf/hw_75_300.h      |   2 +-
 hwconf/hw_es19.c        | 262 ++++++++++++++++++++++++++++++++++++
 hwconf/hw_es19.h        | 284 ++++++++++++++++++++++++++++++++++++++++
 hwconf/hwconf.mk        |   3 +-
 hwconf/si8900.c         | 143 ++++++++++++++++++++
 hwconf/si8900.h         |  39 ++++++
 mc_interface.c          |   8 +-
 mcconf/mcconf_default.h |  18 ++-
 mcpwm_foc.c             | 138 ++++++++++++-------
 terminal.c              |  84 +++++++++++-
 utils.c                 |  62 +++++----
 utils.h                 |   4 +-
 26 files changed, 1287 insertions(+), 135 deletions(-)
 create mode 100644 hwconf/hw_es19.c
 create mode 100644 hwconf/hw_es19.h
 create mode 100644 hwconf/si8900.c
 create mode 100644 hwconf/si8900.h

diff --git a/CHANGELOG b/CHANGELOG
index 35b790b2..01b27d41 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -13,6 +13,12 @@
 * Added bm support for STM32F30x and STM32L47x.
 * App Balance updates. See https://github.com/vedderb/bldc/pull/193
 * Motor current now based on magnitude of both axes.
+* Initial VESC BMS support.
+* Hall sensor interpolation improvement.
+* Made hall sensor filter configurable.
+* Added locking time and ramp up time parameters to sensorless startup.
+* Removed D axis current injection.
+* Initial VESC IO-board support
 
 === FW 5.01 ===
 * Fixed PPM bug in previous release.
diff --git a/Makefile b/Makefile
index 1f636a06..759c1e45 100644
--- a/Makefile
+++ b/Makefile
@@ -287,9 +287,10 @@ build/$(PROJECT).bin: build/$(PROJECT).elf
 
 # Program
 upload: build/$(PROJECT).bin
-#	qstlink2 --cli --erase --write build/$(PROJECT).bin
-#	openocd -f interface/stlink-v2.cfg -c "set WORKAREASIZE 0x2000" -f target/stm32f4x_stlink.cfg -c "program build/$(PROJECT).elf verify reset" # Older openocd
-	openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit" # For openocd 0.9
+	openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit"
+
+upload_only:
+	openocd -f board/stm32f4discovery.cfg -c "reset_config trst_only combined" -c "program build/$(PROJECT).elf verify reset exit"
 
 clear_option_bytes:
 	openocd -f board/stm32f4discovery.cfg -c "init" -c "stm32f2x unlock 0" -c "mww 0x40023C08 0x08192A3B; mww 0x40023C08 0x4C5D6E7F; mww 0x40023C14 0x0fffaaed" -c "exit"
diff --git a/comm_can.c b/comm_can.c
index c3786b45..bf192918 100644
--- a/comm_can.c
+++ b/comm_can.c
@@ -65,6 +65,7 @@ static int rx_frame_read;
 static int rx_frame_write;
 static thread_t *process_tp = 0;
 static thread_t *ping_tp = 0;
+static volatile HW_TYPE ping_hw_last = HW_TYPE_VESC;
 #endif
 
 // Variables
@@ -73,6 +74,9 @@ static can_status_msg_2 stat_msgs_2[CAN_STATUS_MSGS_TO_STORE];
 static can_status_msg_3 stat_msgs_3[CAN_STATUS_MSGS_TO_STORE];
 static can_status_msg_4 stat_msgs_4[CAN_STATUS_MSGS_TO_STORE];
 static can_status_msg_5 stat_msgs_5[CAN_STATUS_MSGS_TO_STORE];
+static io_board_adc_values io_board_adc_1_4[CAN_STATUS_MSGS_TO_STORE];
+static io_board_adc_values io_board_adc_5_8[CAN_STATUS_MSGS_TO_STORE];
+static io_board_digial_inputs io_board_digital_in[CAN_STATUS_MSGS_TO_STORE];
 static unsigned int detect_all_foc_res_index = 0;
 static int8_t detect_all_foc_res[50];
 
@@ -110,6 +114,10 @@ void comm_can_init(void) {
 		stat_msgs_3[i].id = -1;
 		stat_msgs_4[i].id = -1;
 		stat_msgs_5[i].id = -1;
+
+		io_board_adc_1_4[i].id = -1;
+		io_board_adc_5_8[i].id = -1;
+		io_board_digital_in[i].id = -1;
 	}
 
 #if CAN_ENABLE
@@ -460,10 +468,13 @@ void comm_can_set_handbrake_rel(uint8_t controller_id, float current_rel) {
  * @param controller_id
  * The ID of the VESC.
  *
+ * @param hw_type
+ * The hardware type of the CAN device.
+ *
  * @return
  * True for success, false otherwise.
  */
-bool comm_can_ping(uint8_t controller_id) {
+bool comm_can_ping(uint8_t controller_id, HW_TYPE *hw_type) {
 #if CAN_ENABLE
 	if (app_get_configuration()->can_mode != CAN_MODE_VESC) {
 		return false;
@@ -485,6 +496,13 @@ bool comm_can_ping(uint8_t controller_id) {
 
 	int ret = chEvtWaitAnyTimeout(1 << 29, MS2ST(10));
 	ping_tp = 0;
+
+	if (ret != 0) {
+		if (hw_type) {
+			*hw_type = ping_hw_last;
+		}
+	}
+
 	return ret != 0;
 #else
 	(void)controller_id;
@@ -806,6 +824,83 @@ can_status_msg_5 *comm_can_get_status_msg_5_id(int id) {
 	return 0;
 }
 
+io_board_adc_values *comm_can_get_io_board_adc_1_4_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &io_board_adc_1_4[index];
+	} else {
+		return 0;
+	}
+}
+
+io_board_adc_values *comm_can_get_io_board_adc_1_4_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (io_board_adc_1_4[i].id == id) {
+			return &io_board_adc_1_4[i];
+		}
+	}
+
+	return 0;
+}
+
+io_board_adc_values *comm_can_get_io_board_adc_5_8_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &io_board_adc_5_8[index];
+	} else {
+		return 0;
+	}
+}
+
+io_board_adc_values *comm_can_get_io_board_adc_5_8_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (io_board_adc_5_8[i].id == id) {
+			return &io_board_adc_1_4[i];
+		}
+	}
+
+	return 0;
+}
+
+io_board_digial_inputs *comm_can_get_io_board_digital_in_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &io_board_digital_in[index];
+	} else {
+		return 0;
+	}
+}
+
+io_board_digial_inputs *comm_can_get_io_board_digital_in_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (io_board_digital_in[i].id == id) {
+			return &io_board_digital_in[i];
+		}
+	}
+
+	return 0;
+}
+
+void comm_can_io_board_set_output_digital(int id, int channel, bool on) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+
+	buffer[send_index++] = channel;
+	buffer[send_index++] = 1;
+	buffer[send_index++] = on ? 1 : 0;
+
+	comm_can_transmit_eid(id | ((uint32_t)CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL << 8),
+			buffer, send_index);
+}
+
+void comm_can_io_board_set_output_pwm(int id, int channel, float duty) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+
+	buffer[send_index++] = channel;
+	buffer_append_float16(buffer, duty, 1e3, &send_index);
+
+	comm_can_transmit_eid(id | ((uint32_t)CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM << 8),
+			buffer, send_index);
+}
+
 CANRxFrame *comm_can_get_rx_frame(void) {
 #if CAN_ENABLE
 	chMtxLock(&can_rx_mtx);
@@ -1178,15 +1273,21 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 				break;
 
 			case CAN_PACKET_PING: {
-				uint8_t buffer[1];
+				uint8_t buffer[2];
 				buffer[0] = app_get_configuration()->controller_id;
+				buffer[1] = HW_TYPE_VESC;
 				comm_can_transmit_eid(data8[0] |
-						((uint32_t)CAN_PACKET_PONG << 8), buffer, 1);
+						((uint32_t)CAN_PACKET_PONG << 8), buffer, 2);
 			} break;
 
 			case CAN_PACKET_PONG:
 				// data8[0]; // Sender ID
 				if (ping_tp) {
+					if (len >= 2) {
+						ping_hw_last = data8[1];
+					} else {
+						ping_hw_last = HW_TYPE_VESC;
+					}
 					chEvtSignal(ping_tp, 1 << 29);
 				}
 				break;
@@ -1424,6 +1525,59 @@ static void decode_msg(uint32_t eid, uint8_t *data8, int len, bool is_replaced)
 		}
 		break;
 
+	case CAN_PACKET_IO_BOARD_ADC_1_TO_4:
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			io_board_adc_values *msg = &io_board_adc_1_4[i];
+			if (msg->id == id || msg->id == -1) {
+				ind = 0;
+				msg->id = id;
+				msg->rx_time = chVTGetSystemTime();
+				ind = 0;
+				int j = 0;
+				while (ind < len) {
+					msg->adc_voltages[j++] = buffer_get_float16(data8, 1e2, &ind);
+				}
+				break;
+			}
+		}
+		break;
+
+	case CAN_PACKET_IO_BOARD_ADC_5_TO_8:
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			io_board_adc_values *msg = &io_board_adc_5_8[i];
+			if (msg->id == id || msg->id == -1) {
+				ind = 0;
+				msg->id = id;
+				msg->rx_time = chVTGetSystemTime();
+				ind = 0;
+				int j = 0;
+				while (ind < len) {
+					msg->adc_voltages[j++] = buffer_get_float16(data8, 1e2, &ind);
+				}
+				break;
+			}
+		}
+		break;
+
+	case CAN_PACKET_IO_BOARD_DIGITAL_IN:
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			io_board_digial_inputs *msg = &io_board_digital_in[i];
+			if (msg->id == id || msg->id == -1) {
+				ind = 0;
+				msg->id = id;
+				msg->rx_time = chVTGetSystemTime();
+				msg->inputs = 0;
+				ind = 0;
+				while (ind < len) {
+					msg->inputs |= (uint64_t)data8[ind] << (ind * 8);
+					ind++;
+				}
+				break;
+			}
+		}
+		break;
+		break;
+
 	default:
 		break;
 	}
@@ -1481,6 +1635,7 @@ static void send_status5(uint8_t id, bool replace) {
 	comm_can_transmit_eid_replace(id | ((uint32_t)CAN_PACKET_STATUS_5 << 8),
 			buffer, send_index, replace);
 }
+
 #endif
 
 /**
diff --git a/comm_can.h b/comm_can.h
index 94398173..b2f6274a 100644
--- a/comm_can.h
+++ b/comm_can.h
@@ -42,7 +42,7 @@ void comm_can_set_rpm(uint8_t controller_id, float rpm);
 void comm_can_set_pos(uint8_t controller_id, float pos);
 void comm_can_set_current_rel(uint8_t controller_id, float current_rel);
 void comm_can_set_current_brake_rel(uint8_t controller_id, float current_rel);
-bool comm_can_ping(uint8_t controller_id);
+bool comm_can_ping(uint8_t controller_id, HW_TYPE *hw_type);
 void comm_can_detect_apply_all_foc(uint8_t controller_id, bool activate_status_msgs, float max_power_loss);
 void comm_can_conf_current_limits(uint8_t controller_id,
 		bool store, float min, float max);
@@ -66,6 +66,16 @@ can_status_msg_4 *comm_can_get_status_msg_4_index(int index);
 can_status_msg_4 *comm_can_get_status_msg_4_id(int id);
 can_status_msg_5 *comm_can_get_status_msg_5_index(int index);
 can_status_msg_5 *comm_can_get_status_msg_5_id(int id);
+
+io_board_adc_values *comm_can_get_io_board_adc_1_4_index(int index);
+io_board_adc_values *comm_can_get_io_board_adc_1_4_id(int id);
+io_board_adc_values *comm_can_get_io_board_adc_5_8_index(int index);
+io_board_adc_values *comm_can_get_io_board_adc_5_8_id(int id);
+io_board_digial_inputs *comm_can_get_io_board_digital_in_index(int index);
+io_board_digial_inputs *comm_can_get_io_board_digital_in_id(int id);
+void comm_can_io_board_set_output_digital(int id, int channel, bool on);
+void comm_can_io_board_set_output_pwm(int id, int channel, float duty);
+
 CANRxFrame *comm_can_get_rx_frame(void);
 
 #endif /* COMM_CAN_H_ */
diff --git a/commands.c b/commands.c
index 4acfbf11..0c20a6ac 100644
--- a/commands.c
+++ b/commands.c
@@ -72,6 +72,7 @@ static disp_pos_mode display_position_mode;
 static mutex_t print_mutex;
 static mutex_t send_buffer_mutex;
 static mutex_t terminal_mutex;
+static volatile int fw_version_sent_cnt = 0;
 
 void commands_init(void) {
 	chMtxObjectInit(&print_mutex);
@@ -190,6 +191,12 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 		send_buffer[ind++] = app_get_configuration()->pairing_done;
 		send_buffer[ind++] = FW_TEST_VERSION_NUMBER;
 
+		send_buffer[ind++] = HW_TYPE_VESC;
+
+		send_buffer[ind++] = 0; // No custom config
+
+		fw_version_sent_cnt++;
+
 		reply_func(send_buffer, ind);
 	} break;
 
@@ -1260,25 +1267,40 @@ void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf)
 	chMtxUnlock(&send_buffer_mutex);
 }
 
+inline static float hw_lim_upper(float l, float h) {(void)l; return h;}
+
 void commands_apply_mcconf_hw_limits(mc_configuration *mcconf) {
 	utils_truncate_number(&mcconf->l_current_max_scale, 0.0, 1.0);
 	utils_truncate_number(&mcconf->l_current_min_scale, 0.0, 1.0);
 
+	float ctrl_loop_freq = 0.0;
+
 	// This limit should always be active, as starving the threads never
 	// makes sense.
 #ifdef HW_LIM_FOC_CTRL_LOOP_FREQ
     if (mcconf->foc_sample_v0_v7 == true) {
     	//control loop executes twice per pwm cycle when sampling in v0 and v7
 		utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ);
+		ctrl_loop_freq = mcconf->foc_f_sw;
     } else {
 #ifdef HW_HAS_DUAL_MOTORS
     	utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ);
+    	ctrl_loop_freq = mcconf->foc_f_sw;
 #else
 		utils_truncate_number(&mcconf->foc_f_sw, HW_LIM_FOC_CTRL_LOOP_FREQ * 2.0);
+		ctrl_loop_freq = mcconf->foc_f_sw / 2.0;
 #endif
     }
 #endif
 
+    if (ctrl_loop_freq >= (hw_lim_upper(HW_LIM_FOC_CTRL_LOOP_FREQ) * 0.9)) {
+    	utils_truncate_number_int(&mcconf->m_hall_extra_samples, 0, 2);
+    } else if (ctrl_loop_freq >= (hw_lim_upper(HW_LIM_FOC_CTRL_LOOP_FREQ) * 0.7)) {
+    	utils_truncate_number_int(&mcconf->m_hall_extra_samples, 0, 4);
+    } else {
+    	utils_truncate_number_int(&mcconf->m_hall_extra_samples, 0, 10);
+    }
+
 #ifndef DISABLE_HW_LIMITS
 #ifdef HW_LIM_CURRENT
 	utils_truncate_number(&mcconf->l_current_max, HW_LIM_CURRENT);
@@ -1357,6 +1379,10 @@ void commands_send_plot_points(float x, float y) {
 	commands_send_packet(buffer, ind);
 }
 
+int commands_get_fw_version_sent_cnt(void) {
+	return fw_version_sent_cnt;
+}
+
 // TODO: The commands_set_ble_name and commands_set_ble_pin are not
 // tested. Test them, and remove this comment when done!
 
@@ -1665,7 +1691,8 @@ static THD_FUNCTION(blocking_thread, arg) {
 			send_buffer[ind++] = COMM_PING_CAN;
 
 			for (uint8_t i = 0;i < 255;i++) {
-				if (comm_can_ping(i)) {
+				HW_TYPE hw_type;
+				if (comm_can_ping(i, &hw_type)) {
 					send_buffer[ind++] = i;
 				}
 			}
diff --git a/commands.h b/commands.h
index 29f941cd..abde494c 100644
--- a/commands.h
+++ b/commands.h
@@ -46,5 +46,6 @@ void commands_plot_set_graph(int graph);
 void commands_set_ble_name(char* name);
 void commands_set_ble_pin(char* pin);
 void commands_send_plot_points(float x, float y);
+int commands_get_fw_version_sent_cnt(void);
 
 #endif /* COMMANDS_H_ */
diff --git a/conf_general.c b/conf_general.c
index a7ba4ea7..d1cf8d79 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -1642,13 +1642,18 @@ int conf_general_detect_apply_all_foc_can(bool detect_can, float max_power_loss,
 				continue;
 			}
 #endif
+			HW_TYPE hw_type;
+			if (comm_can_ping(i, &hw_type)) {
+				if (hw_type != HW_TYPE_VESC) {
+					continue;
+				}
 
-			if (comm_can_ping(i)) {
 				comm_can_conf_current_limits_in(i, false, mcconf->l_in_current_min, mcconf->l_in_current_max);
 				comm_can_conf_foc_erpms(i, false, mcconf->foc_openloop_rpm, mcconf->foc_sl_erpm);
 				comm_can_detect_apply_all_foc(i, true, max_power_loss);
 				can_devs++;
 
+				// If some other controller has the same ID, change the local one.
 				if (i == id_new) {
 					// Add 2 in case this was a dual controller
 					id_new++;
diff --git a/conf_general.h b/conf_general.h
index 5be19748..f18c8af6 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2017 - 2019 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2017 - 2020 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			02
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		8
+#define FW_TEST_VERSION_NUMBER		10
 
 #include "datatypes.h"
 
@@ -72,9 +72,10 @@
 // Mark3 version of HW60 with power switch and separate NRF UART.
 //#define HW60_IS_MK3
 //#define HW60_IS_MK4
+//#define HW60_IS_MK5
 
-//#define HW_SOURCE "hw_60.c"
-//#define HW_HEADER "hw_60.h"
+#define HW_SOURCE "hw_60.c"
+#define HW_HEADER "hw_60.h"
 
 //#define HW_SOURCE "hw_r2.c"
 //#define HW_HEADER "hw_r2.h"
@@ -101,8 +102,8 @@
 //#define HW75_300_VEDDER_FIRST_PCB
 
 // Second revision with separate UART for NRF51
-//#define HW75_300_REV_2
-//#define HW75_300_REV_3
+#define HW75_300_REV_2
+#define HW75_300_REV_3
 
 //#define HW_SOURCE "hw_75_300.c"
 //#define HW_HEADER "hw_75_300.h"
@@ -153,8 +154,11 @@
 //#define HW_SOURCE "hw_stormcore_100s.c"
 //#define HW_HEADER "hw_stormcore_100s.h"
 
-#define HW_SOURCE "hw_140_300.c"
-#define HW_HEADER "hw_140_300.h"
+//#define HW_SOURCE "hw_140_300.c"
+//#define HW_HEADER "hw_140_300.h"
+
+//#define HW_SOURCE "hw_es19.c"
+//#define HW_HEADER "hw_es19.h"
 #endif
 
 #ifndef HW_SOURCE
@@ -196,6 +200,11 @@
 //#define APP_CUSTOM_TO_USE			"app_motor_heater.c"
 //#include "app_erockit_conf_v2.h"
 
+// CAN-plotter
+//#define APP_CUSTOM_TO_USE			"app_plot_can.c"
+//#define APPCONF_APP_TO_USE			APP_CUSTOM
+//#define APPCONF_CAN_BAUD_RATE		CAN_BAUD_75K
+
 #include "hw.h"
 #include "mcconf_default.h"
 #include "appconf_default.h"
diff --git a/confgenerator.c b/confgenerator.c
index b19a442b..996ee8de 100644
--- a/confgenerator.c
+++ b/confgenerator.c
@@ -80,10 +80,11 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer_append_float32_auto(buffer, conf->foc_duty_dowmramp_kp, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_duty_dowmramp_ki, &ind);
 	buffer_append_float32_auto(buffer, conf->foc_openloop_rpm, &ind);
-	buffer_append_float32_auto(buffer, conf->foc_sl_openloop_hyst, &ind);
-	buffer_append_float32_auto(buffer, conf->foc_sl_openloop_time, &ind);
-	buffer_append_float32_auto(buffer, conf->foc_sl_d_current_duty, &ind);
-	buffer_append_float32_auto(buffer, conf->foc_sl_d_current_factor, &ind);
+	buffer_append_float16(buffer, conf->foc_openloop_rpm_low, 1000, &ind);
+	buffer_append_float16(buffer, conf->foc_sl_openloop_hyst, 100, &ind);
+	buffer_append_float16(buffer, conf->foc_sl_openloop_time_lock, 100, &ind);
+	buffer_append_float16(buffer, conf->foc_sl_openloop_time_ramp, 100, &ind);
+	buffer_append_float16(buffer, conf->foc_sl_openloop_time, 100, &ind);
 	buffer[ind++] = (uint8_t)conf->foc_hall_table[0];
 	buffer[ind++] = (uint8_t)conf->foc_hall_table[1];
 	buffer[ind++] = (uint8_t)conf->foc_hall_table[2];
@@ -144,6 +145,7 @@ int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *
 	buffer[ind++] = conf->m_out_aux_mode;
 	buffer[ind++] = conf->m_motor_temp_sens_type;
 	buffer_append_float32_auto(buffer, conf->m_ptc_motor_coeff, &ind);
+	buffer[ind++] = (uint8_t)conf->m_hall_extra_samples;
 	buffer[ind++] = (uint8_t)conf->si_motor_poles;
 	buffer_append_float32_auto(buffer, conf->si_gear_ratio, &ind);
 	buffer_append_float32_auto(buffer, conf->si_wheel_diameter, &ind);
@@ -379,10 +381,11 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->foc_duty_dowmramp_kp = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_duty_dowmramp_ki = buffer_get_float32_auto(buffer, &ind);
 	conf->foc_openloop_rpm = buffer_get_float32_auto(buffer, &ind);
-	conf->foc_sl_openloop_hyst = buffer_get_float32_auto(buffer, &ind);
-	conf->foc_sl_openloop_time = buffer_get_float32_auto(buffer, &ind);
-	conf->foc_sl_d_current_duty = buffer_get_float32_auto(buffer, &ind);
-	conf->foc_sl_d_current_factor = buffer_get_float32_auto(buffer, &ind);
+	conf->foc_openloop_rpm_low = buffer_get_float16(buffer, 1000, &ind);
+	conf->foc_sl_openloop_hyst = buffer_get_float16(buffer, 100, &ind);
+	conf->foc_sl_openloop_time_lock = buffer_get_float16(buffer, 100, &ind);
+	conf->foc_sl_openloop_time_ramp = buffer_get_float16(buffer, 100, &ind);
+	conf->foc_sl_openloop_time = buffer_get_float16(buffer, 100, &ind);
 	conf->foc_hall_table[0] = buffer[ind++];
 	conf->foc_hall_table[1] = buffer[ind++];
 	conf->foc_hall_table[2] = buffer[ind++];
@@ -443,6 +446,7 @@ bool confgenerator_deserialize_mcconf(const uint8_t *buffer, mc_configuration *c
 	conf->m_out_aux_mode = buffer[ind++];
 	conf->m_motor_temp_sens_type = buffer[ind++];
 	conf->m_ptc_motor_coeff = buffer_get_float32_auto(buffer, &ind);
+	conf->m_hall_extra_samples = buffer[ind++];
 	conf->si_motor_poles = buffer[ind++];
 	conf->si_gear_ratio = buffer_get_float32_auto(buffer, &ind);
 	conf->si_wheel_diameter = buffer_get_float32_auto(buffer, &ind);
@@ -674,10 +678,11 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->foc_duty_dowmramp_kp = MCCONF_FOC_DUTY_DOWNRAMP_KP;
 	conf->foc_duty_dowmramp_ki = MCCONF_FOC_DUTY_DOWNRAMP_KI;
 	conf->foc_openloop_rpm = MCCONF_FOC_OPENLOOP_RPM;
+	conf->foc_openloop_rpm_low = MCCONF_FOC_OPENLOOP_RPM_LOW;
 	conf->foc_sl_openloop_hyst = MCCONF_FOC_SL_OPENLOOP_HYST;
+	conf->foc_sl_openloop_time_lock = MCCONF_FOC_SL_OPENLOOP_T_LOCK;
+	conf->foc_sl_openloop_time_ramp = MCCONF_FOC_SL_OPENLOOP_T_RAMP;
 	conf->foc_sl_openloop_time = MCCONF_FOC_SL_OPENLOOP_TIME;
-	conf->foc_sl_d_current_duty = MCCONF_FOC_SL_D_CURRENT_DUTY;
-	conf->foc_sl_d_current_factor = MCCONF_FOC_SL_D_CURRENT_FACTOR;
 	conf->foc_hall_table[0] = MCCONF_FOC_HALL_TAB_0;
 	conf->foc_hall_table[1] = MCCONF_FOC_HALL_TAB_1;
 	conf->foc_hall_table[2] = MCCONF_FOC_HALL_TAB_2;
@@ -738,6 +743,7 @@ void confgenerator_set_defaults_mcconf(mc_configuration *conf) {
 	conf->m_out_aux_mode = MCCONF_M_OUT_AUX_MODE;
 	conf->m_motor_temp_sens_type = MCCONF_M_MOTOR_TEMP_SENS_TYPE;
 	conf->m_ptc_motor_coeff = MCCONF_M_PTC_MOTOR_COEFF;
+	conf->m_hall_extra_samples = MCCONF_M_HALL_EXTRA_SAMPLES;
 	conf->si_motor_poles = MCCONF_SI_MOTOR_POLES;
 	conf->si_gear_ratio = MCCONF_SI_GEAR_RATIO;
 	conf->si_wheel_diameter = MCCONF_SI_WHEEL_DIAMETER;
diff --git a/confgenerator.h b/confgenerator.h
index 4bfda383..765d0972 100644
--- a/confgenerator.h
+++ b/confgenerator.h
@@ -8,7 +8,7 @@
 #include <stdbool.h>
 
 // Constants
-#define MCCONF_SIGNATURE		1358025204
+#define MCCONF_SIGNATURE		2209777634
 #define APPCONF_SIGNATURE		664237692
 
 // Functions
diff --git a/datatypes.h b/datatypes.h
index 72369ceb..afd58577 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -25,6 +25,12 @@
 #include "ch.h"
 
 // Data types
+typedef enum {
+	HW_TYPE_VESC = 0,
+	HW_TYPE_VESC_BMS,
+	HW_TYPE_CUSTOM_MODULE
+} HW_TYPE;
+
 typedef enum {
    MC_STATE_OFF = 0,
    MC_STATE_DETECTING,
@@ -287,10 +293,11 @@ typedef struct {
 	float foc_duty_dowmramp_kp;
 	float foc_duty_dowmramp_ki;
 	float foc_openloop_rpm;
+	float foc_openloop_rpm_low;
 	float foc_sl_openloop_hyst;
 	float foc_sl_openloop_time;
-	float foc_sl_d_current_duty;
-	float foc_sl_d_current_factor;
+	float foc_sl_openloop_time_lock;
+	float foc_sl_openloop_time_ramp;
 	mc_foc_sensor_mode foc_sensor_mode;
 	uint8_t foc_hall_table[8];
 	float foc_sl_erpm;
@@ -350,6 +357,7 @@ typedef struct {
 	out_aux_mode m_out_aux_mode;
 	temp_sensor_type m_motor_temp_sens_type;
 	float m_ptc_motor_coeff;
+	int m_hall_extra_samples;
 	// Setup info
 	uint8_t si_motor_poles;
 	float si_gear_ratio;
@@ -814,7 +822,21 @@ typedef enum {
 	CAN_PACKET_POLL_TS5700N8501_STATUS,
 	CAN_PACKET_CONF_BATTERY_CUT,
 	CAN_PACKET_CONF_STORE_BATTERY_CUT,
-	CAN_PACKET_SHUTDOWN
+	CAN_PACKET_SHUTDOWN,
+	CAN_PACKET_IO_BOARD_ADC_1_TO_4,
+	CAN_PACKET_IO_BOARD_ADC_5_TO_8,
+	CAN_PACKET_IO_BOARD_ADC_9_TO_12,
+	CAN_PACKET_IO_BOARD_DIGITAL_IN,
+	CAN_PACKET_IO_BOARD_SET_OUTPUT_DIGITAL,
+	CAN_PACKET_IO_BOARD_SET_OUTPUT_PWM,
+	CAN_PACKET_BMS_V_TOT,
+	CAN_PACKET_BMS_I,
+	CAN_PACKET_BMS_AH_WH,
+	CAN_PACKET_BMS_V_CELL,
+	CAN_PACKET_BMS_BAL,
+	CAN_PACKET_BMS_TEMPS,
+	CAN_PACKET_BMS_HUM,
+	CAN_PACKET_BMS_SOC_SOH_TEMP
 } CAN_PACKET_ID;
 
 // Logged fault data
@@ -899,6 +921,18 @@ typedef struct {
 	int32_t tacho_value;
 } can_status_msg_5;
 
+typedef struct {
+	int id;
+	systime_t rx_time;
+	float adc_voltages[4];
+} io_board_adc_values;
+
+typedef struct {
+	int id;
+	systime_t rx_time;
+	uint64_t inputs;
+} io_board_digial_inputs;
+
 typedef struct {
 	uint8_t js_x;
 	uint8_t js_y;
diff --git a/hwconf/hw.h b/hwconf/hw.h
index 4942f039..da835a07 100644
--- a/hwconf/hw.h
+++ b/hwconf/hw.h
@@ -321,6 +321,14 @@
 #define ADC_IND_EXT2 			ADC_IND_EXT
 #endif
 
+// Adc voltage scaling on phases and input
+#ifndef ADC_VOLTS_PH_FACTOR
+#define ADC_VOLTS_PH_FACTOR		1.0
+#endif
+#ifndef ADC_VOLTS_INPUT_FACTOR
+#define ADC_VOLTS_INPUT_FACTOR	1.0
+#endif
+
 // NRF SW SPI (default to spi header pins)
 #ifndef NRF_PORT_CSN
 #define NRF_PORT_CSN			HW_SPI_PORT_NSS
diff --git a/hwconf/hw_60.c b/hwconf/hw_60.c
index 23cf51ba..eb3fb008 100644
--- a/hwconf/hw_60.c
+++ b/hwconf/hw_60.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2012-2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2012-2020 Benjamin Vedder	benjamin@vedder.se
 
 	This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License as published by
@@ -28,7 +28,7 @@
 
 // Variables
 static volatile bool i2c_running = false;
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 static mutex_t shutdown_mutex;
 static float bt_diff = 0.0;
 #endif
@@ -40,13 +40,13 @@ static const I2CConfig i2cfg = {
 		STD_DUTY_CYCLE
 };
 
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 static void terminal_shutdown_now(int argc, const char **argv);
 static void terminal_button_test(int argc, const char **argv);
 #endif
 
 void hw_init_gpio(void) {
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 	chMtxObjectInit(&shutdown_mutex);
 #endif
 
@@ -110,6 +110,14 @@ void hw_init_gpio(void) {
 	palSetPadMode(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2, PAL_MODE_INPUT_PULLUP);
 	palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
 
+	// Phase filters
+#ifdef HW60_IS_MK5
+	palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	PHASE_FILTER_OFF();
+#endif
+
 	// Fault pin
 	palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);
 
@@ -126,13 +134,13 @@ void hw_init_gpio(void) {
 	palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
 	palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
 	palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
-#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4)
+#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4) && !defined(HW60_IS_MK5)
 	palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
 #endif
 
 	drv8301_init();
 
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 	terminal_register_command_callback(
 		"shutdown",
 		"Shutdown VESC now.",
@@ -275,7 +283,7 @@ void hw_try_restore_i2c(void) {
 	}
 }
 
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 bool hw_sample_shutdown_button(void) {
 	chMtxLock(&shutdown_mutex);
 
diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h
index 8c418af8..06965976 100644
--- a/hwconf/hw_60.h
+++ b/hwconf/hw_60.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2020 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -24,6 +24,8 @@
 #define HW_NAME					"60_MK3"
 #elif defined(HW60_IS_MK4)
 #define HW_NAME					"60_MK4"
+#elif defined(HW60_IS_MK5)
+#define HW_NAME					"60_MK5"
 #else
 #define HW_NAME					"60"
 #endif
@@ -32,7 +34,7 @@
 #define HW_HAS_DRV8301
 #define HW_HAS_3_SHUNTS
 #define HW_HAS_PHASE_SHUNTS
-#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4)
+#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4) && !defined(HW60_IS_MK5)
 #define HW_HAS_PERMANENT_NRF
 #endif
 
@@ -56,7 +58,14 @@
 #define CURRENT_FILTER_ON()		palSetPad(GPIOD, 2)
 #define CURRENT_FILTER_OFF()	palClearPad(GPIOD, 2)
 
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#ifdef HW60_IS_MK5
+#define PHASE_FILTER_GPIO		GPIOC
+#define PHASE_FILTER_PIN		13
+#define PHASE_FILTER_ON()		palSetPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
+#define PHASE_FILTER_OFF()		palClearPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
+#endif
+
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 // Shutdown pin
 #define HW_SHUTDOWN_GPIO		GPIOC
 #define HW_SHUTDOWN_PIN			5
@@ -120,7 +129,7 @@
 #define ADC_IND_TEMP_MOS		8
 #define ADC_IND_TEMP_MOTOR		9
 #define ADC_IND_VREFINT			12
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 #define ADC_IND_SHUTDOWN		10
 #endif
 
@@ -182,7 +191,7 @@
 #define HW_UART_RX_PORT			GPIOB
 #define HW_UART_RX_PIN			11
 
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 // Permanent UART Peripheral (for NRF51)
 #define HW_UART_P_BAUD			115200
 #define HW_UART_P_DEV			SD4
@@ -230,7 +239,7 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
-#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4)
+#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4) && !defined(HW60_IS_MK5)
 // NRF pins
 #define NRF_PORT_CSN			GPIOB
 #define NRF_PIN_CSN				12
@@ -255,7 +264,7 @@
 #define HW_SPI_PIN_MISO			6
 
 // SPI for DRV8301
-#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4)
+#if !defined(HW60_IS_MK3) && !defined(HW60_IS_MK4) && !defined(HW60_IS_MK5)
 #define DRV8301_MOSI_GPIO		GPIOB
 #define DRV8301_MOSI_PIN		4
 #define DRV8301_MISO_GPIO		GPIOB
@@ -276,7 +285,7 @@
 #endif
 
 // MPU9250
-#ifndef HW60_IS_MK4
+#if !defined(HW60_IS_MK4) && !defined(HW60_IS_MK5)
 #define MPU9X50_SDA_GPIO		GPIOB
 #define MPU9X50_SDA_PIN			2
 #define MPU9X50_SCL_GPIO		GPIOA
@@ -291,7 +300,7 @@
 #define IMU_ROT_180
 #endif
 
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 // NRF SWD
 #define NRF5x_SWDIO_GPIO		GPIOB
 #define NRF5x_SWDIO_PIN			12
@@ -332,7 +341,7 @@
 #define HW_LIM_TEMP_FET			-40.0, 110.0
 
 // Functions
-#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4)
+#if defined(HW60_IS_MK3) || defined(HW60_IS_MK4) || defined(HW60_IS_MK5)
 bool hw_sample_shutdown_button(void);
 #endif
 
diff --git a/hwconf/hw_75_300.h b/hwconf/hw_75_300.h
index c3526fd5..a73bcbe7 100644
--- a/hwconf/hw_75_300.h
+++ b/hwconf/hw_75_300.h
@@ -51,7 +51,7 @@
 #define LED_RED_ON()			palSetPad(LED_RED_GPIO, LED_RED_PIN)
 #define LED_RED_OFF()			palClearPad(LED_RED_GPIO, LED_RED_PIN)
 
-#ifdef HW75_300_REV_2
+#if defined(HW75_300_REV_2) || defined(HW75_300_REV_3)
 #define PHASE_FILTER_GPIO		GPIOC
 #define PHASE_FILTER_PIN		9
 #else
diff --git a/hwconf/hw_es19.c b/hwconf/hw_es19.c
new file mode 100644
index 00000000..306d3144
--- /dev/null
+++ b/hwconf/hw_es19.c
@@ -0,0 +1,262 @@
+/*
+	Copyright 2018 Benjamin Vedder	benjamin@vedder.se
+
+	This program 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.
+
+    This program 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 "hw.h"
+
+#include "ch.h"
+#include "hal.h"
+#include "stm32f4xx_conf.h"
+#include "utils.h"
+#include <math.h>
+#include "mc_interface.h"
+#include "si8900.h"
+
+// Variables
+static volatile bool i2c_running = false;
+
+// I2C configuration
+static const I2CConfig i2cfg = {
+		OPMODE_I2C,
+		100000,
+		STD_DUTY_CYCLE
+};
+
+void hw_init_gpio(void) {
+	// GPIO clock enable
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC, ENABLE);
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
+
+	// LEDs
+	palSetPadMode(LED_GREEN_GPIO, LED_GREEN_PIN,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(LED_RED_GPIO, LED_RED_PIN,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
+	palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_FLOATING);
+	palSetPadMode(GPIOA, 9, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_FLOATING);
+	palSetPadMode(GPIOA, 10, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_FLOATING);
+
+	palSetPadMode(GPIOB, 13, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_FLOATING);
+	palSetPadMode(GPIOB, 14, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_FLOATING);
+	palSetPadMode(GPIOB, 15, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_FLOATING);
+
+	// Hall sensors
+	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);
+	palSetPadMode(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3, PAL_MODE_INPUT_PULLUP);
+
+	// Phase filters
+	palSetPadMode(PHASE_FILTER_GPIO, PHASE_FILTER_PIN,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	PHASE_FILTER_OFF();
+
+	// Current filter
+	palSetPadMode(GPIOD, 2,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(GPIOD, 3,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	CURRENT_FILTER_OFF();
+
+	si8900_init();
+
+	// ADC Pins
+	palSetPadMode(GPIOA, 0, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOA, 1, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOA, 2, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOA, 3, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOA, 5, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOA, 6, PAL_MODE_INPUT_ANALOG);
+
+	palSetPadMode(GPIOB, 0, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOB, 1, PAL_MODE_INPUT_ANALOG);
+
+	palSetPadMode(GPIOC, 0, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOC, 1, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOC, 2, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOC, 3, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
+	palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
+}
+
+void hw_setup_adc_channels(void) {
+	// ADC1 regular channels
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_5, 3, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_8, 6, ADC_SampleTime_15Cycles);
+
+	// ADC2 regular channels
+	ADC_RegularChannelConfig(ADC2, ADC_Channel_1, 1, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC2, ADC_Channel_6, 3, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC2, ADC_Channel_15, 4, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC2, ADC_Channel_0, 5, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC2, ADC_Channel_9, 6, ADC_SampleTime_15Cycles);
+
+	// ADC3 regular channels
+	ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 1, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC3, ADC_Channel_3, 3, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC3, ADC_Channel_13, 4, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC3, ADC_Channel_1, 5, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC3, ADC_Channel_2, 6, ADC_SampleTime_15Cycles);
+
+	// Injected channels
+	ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 1, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 1, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 1, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 2, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 2, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 2, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC1, ADC_Channel_10, 3, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC2, ADC_Channel_11, 3, ADC_SampleTime_15Cycles);
+	ADC_InjectedChannelConfig(ADC3, ADC_Channel_12, 3, ADC_SampleTime_15Cycles);
+}
+
+void hw_start_i2c(void) {
+	i2cAcquireBus(&HW_I2C_DEV);
+
+	if (!i2c_running) {
+		palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
+				PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
+				PAL_STM32_OTYPE_OPENDRAIN |
+				PAL_STM32_OSPEED_MID1 |
+				PAL_STM32_PUDR_PULLUP);
+		palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
+				PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
+				PAL_STM32_OTYPE_OPENDRAIN |
+				PAL_STM32_OSPEED_MID1 |
+				PAL_STM32_PUDR_PULLUP);
+
+		i2cStart(&HW_I2C_DEV, &i2cfg);
+		i2c_running = true;
+	}
+
+	i2cReleaseBus(&HW_I2C_DEV);
+}
+
+void hw_stop_i2c(void) {
+	i2cAcquireBus(&HW_I2C_DEV);
+
+	if (i2c_running) {
+		palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN, PAL_MODE_INPUT);
+		palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN, PAL_MODE_INPUT);
+
+		i2cStop(&HW_I2C_DEV);
+		i2c_running = false;
+
+	}
+
+	i2cReleaseBus(&HW_I2C_DEV);
+}
+
+/**
+ * Try to restore the i2c bus
+ */
+void hw_try_restore_i2c(void) {
+	if (i2c_running) {
+		i2cAcquireBus(&HW_I2C_DEV);
+
+		palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
+				PAL_STM32_OTYPE_OPENDRAIN |
+				PAL_STM32_OSPEED_MID1 |
+				PAL_STM32_PUDR_PULLUP);
+
+		palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
+				PAL_STM32_OTYPE_OPENDRAIN |
+				PAL_STM32_OSPEED_MID1 |
+				PAL_STM32_PUDR_PULLUP);
+
+		palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
+		palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
+
+		chThdSleep(1);
+
+		for(int i = 0;i < 16;i++) {
+			palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
+			chThdSleep(1);
+			palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
+			chThdSleep(1);
+		}
+
+		// Generate start then stop condition
+		palClearPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
+		chThdSleep(1);
+		palClearPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
+		chThdSleep(1);
+		palSetPad(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN);
+		chThdSleep(1);
+		palSetPad(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN);
+
+		palSetPadMode(HW_I2C_SCL_PORT, HW_I2C_SCL_PIN,
+				PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
+				PAL_STM32_OTYPE_OPENDRAIN |
+				PAL_STM32_OSPEED_MID1 |
+				PAL_STM32_PUDR_PULLUP);
+
+		palSetPadMode(HW_I2C_SDA_PORT, HW_I2C_SDA_PIN,
+				PAL_MODE_ALTERNATE(HW_I2C_GPIO_AF) |
+				PAL_STM32_OTYPE_OPENDRAIN |
+				PAL_STM32_OSPEED_MID1 |
+				PAL_STM32_PUDR_PULLUP);
+
+		HW_I2C_DEV.state = I2C_STOP;
+		i2cStart(&HW_I2C_DEV, &i2cfg);
+
+		i2cReleaseBus(&HW_I2C_DEV);
+	}
+}
+
+float hw_es19_get_temp(void) {
+	float t1 = NTC_TEMP_MOS1();
+	float t2 = NTC_TEMP_MOS2();
+	float t3 = NTC_TEMP_MOS3();
+	float res = 0.0;
+
+	if (t1 > t2 && t1 > t3) {
+		res = t1;
+	} else if (t2 > t1 && t2 > t3) {
+		res = t2;
+	} else {
+		res = t3;
+	}
+
+	return res;
+}
diff --git a/hwconf/hw_es19.h b/hwconf/hw_es19.h
new file mode 100644
index 00000000..e939339b
--- /dev/null
+++ b/hwconf/hw_es19.h
@@ -0,0 +1,284 @@
+/*
+	Copyright 2020 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 HW_ES19_H_
+#define HW_ES19_H_
+
+#define HW_NAME					"ES19"
+
+#include "si8900.h"
+
+// HW properties
+#define HW_HAS_3_SHUNTS
+#define HW_HAS_PHASE_SHUNTS
+//#define HW_HAS_PHASE_FILTERS
+#define HW_HAS_SI8900
+
+// Macros
+#define LED_GREEN_GPIO			GPIOE
+#define LED_GREEN_PIN			8
+#define LED_RED_GPIO			GPIOE
+#define LED_RED_PIN				9
+
+#define LED_GREEN_ON()			palSetPad(LED_GREEN_GPIO, LED_GREEN_PIN)
+#define LED_GREEN_OFF()			palClearPad(LED_GREEN_GPIO, LED_GREEN_PIN)
+#define LED_RED_ON()			palSetPad(LED_RED_GPIO, LED_RED_PIN)
+#define LED_RED_OFF()			palClearPad(LED_RED_GPIO, LED_RED_PIN)
+
+#define PHASE_FILTER_GPIO		GPIOD
+#define PHASE_FILTER_PIN		4
+
+#define PHASE_FILTER_ON()		palSetPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
+#define PHASE_FILTER_OFF()		palClearPad(PHASE_FILTER_GPIO, PHASE_FILTER_PIN)
+
+#define CURRENT_FILTER_ON()		palSetPad(GPIOD, 2);  palSetPad(GPIOD, 3)
+#define CURRENT_FILTER_OFF()	palClearPad(GPIOD, 2); palClearPad(GPIOD, 3)
+
+/*
+ * ADC Vector
+ *
+ * 0  (1):	IN0		SENS1
+ * 1  (2):	IN1		SENS2
+ * 2  (3):	IN2		SENS3
+ * 3  (1):	IN10	CURR1
+ * 4  (2):	IN11	CURR2
+ * 5  (3):	IN12	CURR3
+ * 6  (1):	IN5		ADC_EXT1
+ * 7  (2):	IN6		ADC_EXT2
+ * 8  (3):	IN3		TEMP_MOS
+ * 9  (1):	IN14	TEMP_MOTOR
+ * 10 (2):	IN15	ADC_EXT3
+ * 11 (3):	IN13	AN_IN
+ * 12 (1):	Vrefint
+ * 13 (2):	IN0		SENS1
+ * 14 (3):	IN1		SENS2
+ * 15 (1):  IN8		TEMP_MOS_2
+ * 16 (2):  IN9		TEMP_MOS_3
+ * 17 (3):  IN3		SENS3
+ */
+
+#define HW_ADC_CHANNELS			18
+#define HW_ADC_INJ_CHANNELS		3
+#define HW_ADC_NBR_CONV			6
+
+// ADC Indexes
+#define ADC_IND_SENS1			0
+#define ADC_IND_SENS2			1
+#define ADC_IND_SENS3			2
+#define ADC_IND_CURR1			3
+#define ADC_IND_CURR2			4
+#define ADC_IND_CURR3			5
+#define ADC_IND_VIN_SENS		11
+#define ADC_IND_EXT				6
+#define ADC_IND_EXT2			7
+#define ADC_IND_EXT3			10
+#ifdef HW75_300_VEDDER_FIRST_PCB
+#define ADC_IND_TEMP_MOS		8
+#define ADC_IND_TEMP_MOS_2		8
+#define ADC_IND_TEMP_MOS_3		8
+#else
+#define ADC_IND_TEMP_MOS		8
+#define ADC_IND_TEMP_MOS_2		15
+#define ADC_IND_TEMP_MOS_3		16
+#endif
+#define ADC_IND_TEMP_MOTOR		9
+#define ADC_IND_VREFINT			12
+
+// ADC macros and settings
+
+// Component parameters (can be overridden)
+#ifndef V_REG
+#define V_REG					3.3
+#endif
+#ifndef VIN_R1
+#define VIN_R1					(499e3 * 6.0)
+#endif
+#ifndef VIN_R2
+#define VIN_R2					0.94737e3 // Take the 18k single-ended input impedance of the AMC1301 into account. TODO: Double-check this
+#endif
+#ifndef CURRENT_AMP_GAIN
+#define CURRENT_AMP_GAIN		(1.5 / 1000.0)
+#endif
+#ifndef CURRENT_SHUNT_RES
+#define CURRENT_SHUNT_RES		1.0
+#endif
+
+// Voltage on ADC channel
+#define ADC_VOLTS(ch)			((float)ADC_Value[ch] / 4096.0 * V_REG)
+#define ADC_VOLTS_PH_FACTOR		((1.0 / 8.2) * (10.0 / 15.0)) // AMC1301 gain + diff amp gain
+#define ADC_VOLTS_INPUT_FACTOR	ADC_VOLTS_PH_FACTOR
+
+// Input voltage
+#define GET_INPUT_VOLTAGE()		((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_INPUT_FACTOR)
+
+// NTC Termistors
+#define NTC_RES(adc_val)		((4095.0 * 10000.0) / adc_val - 10000.0)
+#define NTC_RES_REL(adc_val)	(10000.0 / (1.0 / adc_val - 1.0))
+#define NTC_TEMP(adc_ind)		hw_es19_get_temp()
+
+#define NTC_RES_MOTOR(adc_val)	(10000.0 / ((4095.0 / (float)adc_val) - 1.0)) // Motor temp sensor on low side
+#define NTC_TEMP_MOTOR(beta)	(1.0 / ((logf(NTC_RES_MOTOR(ADC_Value[ADC_IND_TEMP_MOTOR]) / 10000.0) / beta) + (1.0 / 298.15)) - 273.15)
+
+#define NTC_TEMP_MOS1()			(1.0 / ((logf(NTC_RES_REL(si8900_get_val_rel(0)) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#define NTC_TEMP_MOS2()			(1.0 / ((logf(NTC_RES_REL(si8900_get_val_rel(1)) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#define NTC_TEMP_MOS3()			(1.0 / ((logf(NTC_RES_REL(si8900_get_val_rel(2)) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+
+// Double samples in beginning and end for positive current measurement.
+// Useful when the shunt sense traces have noise that causes offset.
+#ifndef CURR1_DOUBLE_SAMPLE
+#define CURR1_DOUBLE_SAMPLE		0
+#endif
+#ifndef CURR2_DOUBLE_SAMPLE
+#define CURR2_DOUBLE_SAMPLE		0
+#endif
+#ifndef CURR3_DOUBLE_SAMPLE
+#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
+#define HW_UART_TX_PORT			GPIOB
+#define HW_UART_TX_PIN			10
+#define HW_UART_RX_PORT			GPIOB
+#define HW_UART_RX_PIN			11
+
+// UART for SI8900
+#define HW_SI8900_DEV			SD4
+#define HW_SI8900_GPIO_AF		GPIO_AF_UART4
+#define HW_SI8900_TX_PORT		GPIOC
+#define HW_SI8900_TX_PIN		10
+#define HW_SI8900_RX_PORT		GPIOC
+#define HW_SI8900_RX_PIN		11
+
+// ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
+#define HW_ICU_TIMER			TIM9
+#define HW_ICU_TIM_CLK_EN()		RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE)
+#define HW_ICU_DEV				ICUD9
+#define HW_ICU_CHANNEL			ICU_CHANNEL_1
+#define HW_ICU_GPIO_AF			GPIO_AF_TIM9
+#define HW_ICU_GPIO				GPIOE
+#define HW_ICU_PIN				5
+
+// I2C Peripheral
+#define HW_I2C_DEV				I2CD2
+#define HW_I2C_GPIO_AF			GPIO_AF_I2C2
+#define HW_I2C_SCL_PORT			GPIOB
+#define HW_I2C_SCL_PIN			10
+#define HW_I2C_SDA_PORT			GPIOB
+#define HW_I2C_SDA_PIN			11
+
+// Hall/encoder pins
+#define HW_HALL_ENC_GPIO1		GPIOC
+#define HW_HALL_ENC_PIN1		6
+#define HW_HALL_ENC_GPIO2		GPIOC
+#define HW_HALL_ENC_PIN2		7
+#define HW_HALL_ENC_GPIO3		GPIOC
+#define HW_HALL_ENC_PIN3		8
+#define HW_ENC_TIM				TIM3
+#define HW_ENC_TIM_AF			GPIO_AF_TIM3
+#define HW_ENC_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
+#define HW_ENC_EXTI_PORTSRC		EXTI_PortSourceGPIOC
+#define HW_ENC_EXTI_PINSRC		EXTI_PinSource8
+#define HW_ENC_EXTI_CH			EXTI9_5_IRQn
+#define HW_ENC_EXTI_LINE		EXTI_Line8
+#define HW_ENC_EXTI_ISR_VEC		EXTI9_5_IRQHandler
+#define HW_ENC_TIM_ISR_CH		TIM3_IRQn
+#define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
+
+// SPI pins
+#define HW_SPI_DEV				SPID1
+#define HW_SPI_GPIO_AF			GPIO_AF_SPI1
+#define HW_SPI_PORT_NSS			GPIOA
+#define HW_SPI_PIN_NSS			4
+#define HW_SPI_PORT_SCK			GPIOA
+#define HW_SPI_PIN_SCK			5
+#define HW_SPI_PORT_MOSI		GPIOA
+#define HW_SPI_PIN_MOSI			7
+#define HW_SPI_PORT_MISO		GPIOA
+#define HW_SPI_PIN_MISO			6
+
+// BMI160
+#define BMI160_SDA_GPIO			GPIOE
+#define BMI160_SDA_PIN			14
+#define BMI160_SCL_GPIO			GPIOE
+#define BMI160_SCL_PIN			15
+#define IMU_FLIP
+
+// Measurement macros
+#define ADC_V_L1				ADC_Value[ADC_IND_SENS1]
+#define ADC_V_L2				ADC_Value[ADC_IND_SENS2]
+#define ADC_V_L3				ADC_Value[ADC_IND_SENS3]
+#define ADC_V_ZERO				(ADC_Value[ADC_IND_VIN_SENS] / 2)
+
+// Macros
+#define READ_HALL1()			palReadPad(HW_HALL_ENC_GPIO1, HW_HALL_ENC_PIN1)
+#define READ_HALL2()			palReadPad(HW_HALL_ENC_GPIO2, HW_HALL_ENC_PIN2)
+#define READ_HALL3()			palReadPad(HW_HALL_ENC_GPIO3, HW_HALL_ENC_PIN3)
+
+// Override dead time. See the stm32f4 reference manual for calculating this value.
+#define HW_DEAD_TIME_NSEC		400.0
+
+// Default setting overrides
+#ifndef MCCONF_L_MIN_VOLTAGE
+#define MCCONF_L_MIN_VOLTAGE			0.0		// Minimum input voltage
+#endif
+#ifndef MCCONF_L_MAX_VOLTAGE
+#define MCCONF_L_MAX_VOLTAGE			600.0	// Maximum input voltage
+#endif
+#ifndef MCCONF_DEFAULT_MOTOR_TYPE
+#define MCCONF_DEFAULT_MOTOR_TYPE		MOTOR_TYPE_FOC
+#endif
+#ifndef MCCONF_FOC_F_SW
+#define MCCONF_FOC_F_SW					30000.0
+#endif
+#ifndef MCCONF_L_MAX_ABS_CURRENT
+#define MCCONF_L_MAX_ABS_CURRENT		700.0	// The maximum absolute current above which a fault is generated
+#endif
+#ifndef MCCONF_FOC_SAMPLE_V0_V7
+#define MCCONF_FOC_SAMPLE_V0_V7			false	// Run control loop in both v0 and v7 (requires phase shunts)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MAX
+#define MCCONF_L_IN_CURRENT_MAX			250.0	// Input current limit in Amperes (Upper)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MIN
+#define MCCONF_L_IN_CURRENT_MIN			-200.0	// Input current limit in Amperes (Lower)
+#endif
+
+// Setting limits
+#define HW_LIM_CURRENT			-600.0, 600.0
+#define HW_LIM_CURRENT_IN		-600.0, 600.0
+#define HW_LIM_CURRENT_ABS		0.0, 480.0
+#define HW_LIM_VIN				0.0, 620.0
+#define HW_LIM_ERPM				-200e3, 200e3
+#define HW_LIM_DUTY_MIN			0.0, 0.1
+#define HW_LIM_DUTY_MAX			0.0, 0.99
+#define HW_LIM_TEMP_FET			-40.0, 110.0
+
+// HW-specific functions
+float hw_es19_get_temp(void);
+
+#endif /* HW_ES19_H_ */
diff --git a/hwconf/hwconf.mk b/hwconf/hwconf.mk
index 648f182e..0d384c25 100644
--- a/hwconf/hwconf.mk
+++ b/hwconf/hwconf.mk
@@ -2,6 +2,7 @@ HWSRC =	hwconf/hw.c \
 	hwconf/drv8301.c \
 	hwconf/drv8305.c \
 	hwconf/drv8320s.c \
-	hwconf/drv8323s.c
+	hwconf/drv8323s.c \
+	hwconf/si8900.c
 
 HWINC = hwconf
diff --git a/hwconf/si8900.c b/hwconf/si8900.c
new file mode 100644
index 00000000..cecb4253
--- /dev/null
+++ b/hwconf/si8900.c
@@ -0,0 +1,143 @@
+/*
+	Copyright 2020 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 "si8900.h"
+#include "conf_general.h"
+#include "terminal.h"
+#include "commands.h"
+
+#ifdef HW_HAS_SI8900
+
+// Private variables
+static THD_FUNCTION(si_read_thread, arg);
+static THD_WORKING_AREA(si_read_thread_wa, 512);
+static volatile float m_voltages[3];
+
+// Private functions
+static void terminal_read(int argc, const char **argv);
+
+static SerialConfig uart_cfg = {
+		115200,
+		0,
+		USART_CR2_LINEN,
+		0
+};
+
+void si8900_init(void) {
+	sdStart(&HW_SI8900_DEV, &uart_cfg);
+	palSetPadMode(HW_SI8900_TX_PORT, HW_SI8900_TX_PIN, PAL_MODE_ALTERNATE(HW_SI8900_GPIO_AF) |
+			PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP);
+	palSetPadMode(HW_SI8900_RX_PORT, HW_SI8900_RX_PIN, PAL_MODE_ALTERNATE(HW_SI8900_GPIO_AF) |
+			PAL_STM32_OSPEED_HIGHEST | PAL_STM32_PUDR_PULLUP);
+	chThdCreateStatic(si_read_thread_wa, sizeof(si_read_thread_wa), NORMALPRIO, si_read_thread, NULL);
+
+	terminal_register_command_callback(
+			"si8900_read",
+			"Read and print ADC values for 10 seconds.",
+			0,
+			terminal_read);
+}
+
+float si8900_get_voltage(int channel) {
+	float res = -1.0;
+
+	if (channel >= 0 && channel < 3) {
+		res = m_voltages[channel];
+	}
+
+	return res;
+}
+
+float si8900_get_val_rel(int channel) {
+	return si8900_get_voltage(channel) / 3.3;
+}
+
+static void terminal_read(int argc, const char **argv) {
+	(void)argc;
+	(void)argv;
+
+	for (int i = 0;i < 100;i++) {
+		commands_printf("[IN0 IN1 IN2] = [%.3f %.3f %.3f]",
+				(double)si8900_get_voltage(0),
+				(double)si8900_get_voltage(1),
+				(double)si8900_get_voltage(2));
+		chThdSleepMilliseconds(100);
+	}
+
+	commands_printf("Done\n");
+}
+
+static THD_FUNCTION(si_read_thread, arg) {
+	(void)arg;
+	chRegSetThreadName("SI8900");
+
+	while(!chThdShouldTerminateX()) {
+		uint8_t txb[1];
+		size_t tx_len = 1;
+		uint8_t rxb[3];
+		size_t rx_len = 3;
+
+		// Baud rate adjustment
+		txb[0] = 0xAA;
+		tx_len = 1;
+		for (int i = 0;i < 4;i++) {
+			sdWriteTimeout(&HW_SI8900_DEV, txb, tx_len, MS2ST(10));
+			rx_len = 1;
+			size_t res = sdReadTimeout(&HW_SI8900_DEV, rxb, rx_len, 5);
+			if (res == rx_len && rxb[0] == 0x55) {
+				break;
+			}
+		}
+
+		for (int i = 0;i < 3;i++) {
+			if (i == 0) {
+				txb[0] = SI8900_CNFG_0 | SI8900_CNFG_0_PGA | SI8900_CNFG_0_MODE;
+			} else if (i == 1) {
+				txb[0] = SI8900_CNFG_0 | SI8900_CNFG_0_PGA | SI8900_CNFG_0_MODE | SI8900_CNFG_0_MX0;
+			} else {
+				txb[0] = SI8900_CNFG_0 | SI8900_CNFG_0_PGA | SI8900_CNFG_0_MODE | SI8900_CNFG_0_MX1;
+			}
+
+			tx_len = 1;
+
+			for (int j = 0;j < 4;j++) {
+				sdWriteTimeout(&HW_SI8900_DEV, txb, tx_len, MS2ST(10));
+				rx_len = 1;
+				size_t res = sdReadTimeout(&HW_SI8900_DEV, rxb, rx_len, 5);
+				if (res == rx_len && rxb[0] == txb[0]) {
+					break;
+				}
+			}
+
+			rx_len = 2;
+			size_t res = sdReadTimeout(&HW_SI8900_DEV, rxb, rx_len, MS2ST(10));
+			if (res == rx_len) {
+				m_voltages[i] = (float)((((uint16_t)rxb[0] & 0b00001111) << 6) |
+						(((uint16_t)rxb[1] >> 1) & 0b00111111)) / 1023.0 * 3.3;
+			}
+		}
+
+		chThdSleepMilliseconds(20);
+		while(sdGetTimeout(&HW_SI8900_DEV, TIME_IMMEDIATE) != MSG_TIMEOUT){
+			chThdSleepMilliseconds(1);
+		};
+	}
+}
+
+#endif
diff --git a/hwconf/si8900.h b/hwconf/si8900.h
new file mode 100644
index 00000000..f53d4ad4
--- /dev/null
+++ b/hwconf/si8900.h
@@ -0,0 +1,39 @@
+/*
+	Copyright 2020 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 SI8900_H_
+#define SI8900_H_
+
+#include "ch.h"
+#include "hal.h"
+
+// Functions
+void si8900_init(void);
+float si8900_get_voltage(int channel);
+float si8900_get_val_rel(int channel);
+
+// Commands
+#define SI8900_CNFG_0			0xC0
+#define SI8900_CNFG_0_PGA		(1 << 0)
+#define SI8900_CNFG_0_MODE		(1 << 1)
+#define SI8900_CNFG_0_VREF		(1 << 3)
+#define SI8900_CNFG_0_MX0		(1 << 4)
+#define SI8900_CNFG_0_MX1		(1 << 5)
+
+#endif /* SI8900_H_ */
diff --git a/mc_interface.c b/mc_interface.c
index 8daf5251..246af9ca 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -2258,10 +2258,10 @@ static THD_FUNCTION(sample_send_thread, arg) {
 			buffer[index++] = COMM_SAMPLE_PRINT;
 			buffer_append_float32_auto(buffer, (float)m_curr0_samples[ind_samp] * FAC_CURRENT, &index);
 			buffer_append_float32_auto(buffer, (float)m_curr1_samples[ind_samp] * FAC_CURRENT, &index);
-			buffer_append_float32_auto(buffer, ((float)m_ph1_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2), &index);
-			buffer_append_float32_auto(buffer, ((float)m_ph2_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2), &index);
-			buffer_append_float32_auto(buffer, ((float)m_ph3_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2), &index);
-			buffer_append_float32_auto(buffer, ((float)m_vzero_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2), &index);
+			buffer_append_float32_auto(buffer, ((float)m_ph1_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR, &index);
+			buffer_append_float32_auto(buffer, ((float)m_ph2_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR, &index);
+			buffer_append_float32_auto(buffer, ((float)m_ph3_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR, &index);
+			buffer_append_float32_auto(buffer, ((float)m_vzero_samples[ind_samp] / 4096.0 * V_REG) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_INPUT_FACTOR, &index);
 			buffer_append_float32_auto(buffer, (float)m_curr_fir_samples[ind_samp] / (8.0 / FAC_CURRENT), &index);
 			buffer_append_float32_auto(buffer, (float)m_f_sw_samples[ind_samp] * 10.0, &index);
 			buffer[index++] = m_status_samples[ind_samp];
diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
index f2c0b697..b27fa08e 100644
--- a/mcconf/mcconf_default.h
+++ b/mcconf/mcconf_default.h
@@ -279,19 +279,22 @@
 #define MCCONF_FOC_DUTY_DOWNRAMP_KI		200.0	// PI controller for duty control when decreasing the duty
 #endif
 #ifndef MCCONF_FOC_OPENLOOP_RPM
-#define MCCONF_FOC_OPENLOOP_RPM			400.0	// Openloop RPM (sensorless low speed or when finding index pulse)
+#define MCCONF_FOC_OPENLOOP_RPM			1500.0	// Openloop RPM (sensorless low speed or when finding index pulse)
+#endif
+#ifndef MCCONF_FOC_OPENLOOP_RPM_LOW
+#define MCCONF_FOC_OPENLOOP_RPM_LOW		0.0		// Fraction of OPENLOOP_RPM at minimum motor current
 #endif
 #ifndef MCCONF_FOC_SL_OPENLOOP_HYST
 #define MCCONF_FOC_SL_OPENLOOP_HYST		0.1		// Time below min RPM to activate openloop (s)
 #endif
 #ifndef MCCONF_FOC_SL_OPENLOOP_TIME
-#define MCCONF_FOC_SL_OPENLOOP_TIME		0.1		// Time to remain in openloop (s)
+#define MCCONF_FOC_SL_OPENLOOP_TIME		0.05	// Time to remain in openloop after ramping (s)
 #endif
-#ifndef MCCONF_FOC_SL_D_CURRENT_DUTY
-#define MCCONF_FOC_SL_D_CURRENT_DUTY	0.0		// Inject d-axis current below this duty cycle in sensorless more
+#ifndef MCCONF_FOC_SL_OPENLOOP_T_LOCK
+#define MCCONF_FOC_SL_OPENLOOP_T_LOCK	0.0		// Time to lock motor in beginning of open loop sequence
 #endif
-#ifndef MCCONF_FOC_SL_D_CURRENT_FACTOR
-#define MCCONF_FOC_SL_D_CURRENT_FACTOR	0.0		// Maximum q-axis current factor
+#ifndef MCCONF_FOC_SL_OPENLOOP_T_RAMP
+#define MCCONF_FOC_SL_OPENLOOP_T_RAMP	0.1		// Time to ramp up motor to openloop speed
 #endif
 #ifndef MCCONF_FOC_HALL_TAB_0
 #define MCCONF_FOC_HALL_TAB_0			255
@@ -444,6 +447,9 @@
 #ifndef MCCONF_M_PTC_MOTOR_COEFF
 #define MCCONF_M_PTC_MOTOR_COEFF		0.61 // %/K coefficient for motor PTC sensor
 #endif
+#ifndef MCCONF_M_HALL_EXTRA_SAMPLES
+#define MCCONF_M_HALL_EXTRA_SAMPLES		1 // Extra samples for filtering when reading hall sensors
+#endif
 
 // Setup Info
 #ifndef MCCONF_SI_MOTOR_POLES
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index 7ae6ff4f..aeadc7c9 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -2031,7 +2031,7 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
 			motor->m_phase_now_override = (float)j * M_PI / 180.0;
 			chThdSleepMilliseconds(5);
 
-			int hall = utils_read_hall(motor != &m_motor_1);
+			int hall = utils_read_hall(motor != &m_motor_1, motor->m_conf->m_hall_extra_samples);
 			float s, c;
 			sincosf(motor->m_phase_now_override, &s, &c);
 			sin_hall[hall] += s;
@@ -2046,7 +2046,7 @@ bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table) {
 			motor->m_phase_now_override = (float)j * M_PI / 180.0;
 			chThdSleepMilliseconds(5);
 
-			int hall = utils_read_hall(motor != &m_motor_1);
+			int hall = utils_read_hall(motor != &m_motor_1, motor->m_conf->m_hall_extra_samples);
 			float s, c;
 			sincosf(motor->m_phase_now_override, &s, &c);
 			sin_hall[hall] += s;
@@ -2506,17 +2506,8 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 				motor_now->m_motor_state.phase = motor_now->m_phase_now_observer;
 			}
 
-			// Inject D axis current at low speed to make the observer track
-			// better. This does not seem to be necessary with dead time
-			// compensation.
-			// Note: this is done at high rate prevent noise.
 			if (!motor_now->m_phase_override) {
-				if (duty_abs < conf_now->foc_sl_d_current_duty) {
-					id_set_tmp = utils_map(duty_abs, 0.0, conf_now->foc_sl_d_current_duty,
-										   fabsf(motor_now->m_motor_state.iq_target) * conf_now->foc_sl_d_current_factor, 0.0);
-				} else {
-					id_set_tmp = 0.0;
-				}
+				id_set_tmp = 0.0;
 			}
 			break;
 
@@ -2612,35 +2603,35 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 #ifdef HW_HAS_3_SHUNTS
 		float Va, Vb, Vc;
 		if (is_second_motor) {
-			Va = ADC_VOLTS(ADC_IND_SENS4) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vb = ADC_VOLTS(ADC_IND_SENS5) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vc = ADC_VOLTS(ADC_IND_SENS6) * ((VIN_R1 + VIN_R2) / VIN_R2);
+			Va = ADC_VOLTS(ADC_IND_SENS4) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vb = ADC_VOLTS(ADC_IND_SENS5) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vc = ADC_VOLTS(ADC_IND_SENS6) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
 		} else {
-			Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vb = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vc = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
+			Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vb = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vc = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
 		}
 #else
 		float Va, Vb, Vc;
 		if (is_second_motor) {
-			Va = ADC_VOLTS(ADC_IND_SENS4) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vb = ADC_VOLTS(ADC_IND_SENS6) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vc = ADC_VOLTS(ADC_IND_SENS5) * ((VIN_R1 + VIN_R2) / VIN_R2);
+			Va = ADC_VOLTS(ADC_IND_SENS4) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vb = ADC_VOLTS(ADC_IND_SENS6) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vc = ADC_VOLTS(ADC_IND_SENS5) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
 		} else {
-			Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
-			Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);
+			Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+			Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
 		}
 #endif
 #else
 #ifdef HW_HAS_3_SHUNTS
-		float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
-		float Vb = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);
-		float Vc = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
+		float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+		float Vb = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+		float Vc = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
 #else
-		float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2);
-		float Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2);
-		float Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2);
+		float Va = ADC_VOLTS(ADC_IND_SENS1) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+		float Vb = ADC_VOLTS(ADC_IND_SENS3) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
+		float Vc = ADC_VOLTS(ADC_IND_SENS2) * ((VIN_R1 + VIN_R2) / VIN_R2) * ADC_VOLTS_PH_FACTOR;
 #endif
 #endif
 
@@ -2821,43 +2812,99 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 // Private functions
 
 static void timer_update(volatile motor_all_state_t *motor, float dt) {
-	float openloop_rpm = utils_map(fabsf(motor->m_motor_state.iq_target),
-								   0.0, motor->m_conf->l_current_max,
-								   0.0, motor->m_conf->foc_openloop_rpm);
+	float t_lock = motor->m_conf->foc_sl_openloop_time_lock;
+	float t_ramp = motor->m_conf->foc_sl_openloop_time_ramp;
+	float t_const = motor->m_conf->foc_sl_openloop_time;
 
-	utils_truncate_number_abs(&openloop_rpm, motor->m_conf->foc_openloop_rpm);
+	// Use this to study the openloop timers under experiment plot
+#if 0
+	{
+		static bool plot_started = false;
+		static int plot_div = 0;
+		static float plot_int = 0.0;
+		static int get_fw_version_cnt = 0;
+
+		if (commands_get_fw_version_sent_cnt() != get_fw_version_cnt) {
+			get_fw_version_cnt = commands_get_fw_version_sent_cnt();
+			plot_started = false;
+		}
+
+		plot_div++;
+		if (plot_div >= 10) {
+			plot_div = 0;
+			if (!plot_started) {
+				plot_started = true;
+				commands_init_plot("Time", "Val");
+				commands_plot_add_graph("m_min_rpm_timer");
+				commands_plot_add_graph("m_min_rpm_hyst_timer");
+			}
+
+			commands_plot_set_graph(0);
+			commands_send_plot_points(plot_int, motor->m_min_rpm_timer);
+			commands_plot_set_graph(1);
+			commands_send_plot_points(plot_int, motor->m_min_rpm_hyst_timer);
+			plot_int++;
+		}
+	}
+#endif
+
+	float openloop_rpm_max = utils_map(fabsf(motor->m_motor_state.iq_filter),
+			0.0, motor->m_conf->l_current_max,
+			motor->m_conf->foc_openloop_rpm_low * motor->m_conf->foc_openloop_rpm,
+			motor->m_conf->foc_openloop_rpm);
+
+	utils_truncate_number_abs(&openloop_rpm_max, motor->m_conf->foc_openloop_rpm);
+
+	float openloop_rpm = openloop_rpm_max;
+	if (motor->m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) {
+		float time_fwd = t_lock + t_ramp + t_const - motor->m_min_rpm_timer;
+		if (time_fwd < t_lock) {
+			openloop_rpm = 0.0;
+		} else if (time_fwd < (t_lock + t_ramp)) {
+			openloop_rpm = utils_map(time_fwd, t_lock,
+					t_lock + t_ramp, 0.0, openloop_rpm);
+		}
+	}
+
+	utils_truncate_number_abs(&openloop_rpm, openloop_rpm_max);
 
-	const float min_rads = (openloop_rpm * 2.0 * M_PI) / 60.0;
 	float add_min_speed = 0.0;
 	if (motor->m_motor_state.duty_now > 0.0) {
-		add_min_speed = min_rads * dt;
+		add_min_speed = ((openloop_rpm * 2.0 * M_PI) / 60.0) * dt;
 	} else {
-		add_min_speed = -min_rads * dt;
+		add_min_speed = -((openloop_rpm * 2.0 * M_PI) / 60.0) * dt;
 	}
 
 	// Open loop encoder angle for when the index is not found
 	motor->m_phase_now_encoder_no_index += add_min_speed;
 	utils_norm_angle_rad((float*)&motor->m_phase_now_encoder_no_index);
 
-	// Output a minimum speed from the observer
-	if (fabsf(motor->m_pll_speed) < min_rads) {
+	if (fabsf(motor->m_pll_speed) < ((openloop_rpm_max * 2.0 * M_PI) / 60.0) &&
+			motor->m_min_rpm_hyst_timer < motor->m_conf->foc_sl_openloop_hyst) {
 		motor->m_min_rpm_hyst_timer += dt;
 	} else if (motor->m_min_rpm_hyst_timer > 0.0) {
 		motor->m_min_rpm_hyst_timer -= dt;
 	}
 
 	// Don't use this in brake mode.
-	if (motor->m_control_mode == CONTROL_MODE_CURRENT_BRAKE || fabsf(motor->m_motor_state.duty_now) < 0.001) {
+	if (motor->m_control_mode == CONTROL_MODE_CURRENT_BRAKE ||
+			(motor->m_state == MC_STATE_RUNNING && fabsf(motor->m_motor_state.duty_now) < 0.001)) {
 		motor->m_min_rpm_hyst_timer = 0.0;
+		motor->m_min_rpm_timer = 0.0;
 		motor->m_phase_observer_override = false;
 	}
 
 	bool started_now = false;
-	if (motor->m_min_rpm_hyst_timer > motor->m_conf->foc_sl_openloop_hyst && motor->m_min_rpm_timer <= 0.0001) {
-		motor->m_min_rpm_timer = motor->m_conf->foc_sl_openloop_time;
+	if (motor->m_min_rpm_hyst_timer >= motor->m_conf->foc_sl_openloop_hyst &&
+			motor->m_min_rpm_timer <= 0.0001) {
+		motor->m_min_rpm_timer = t_lock + t_ramp + t_const;
 		started_now = true;
 	}
 
+	if (motor->m_state != MC_STATE_RUNNING) {
+		motor->m_min_rpm_timer = 0.0;
+	}
+
 	if (motor->m_min_rpm_timer > 0.0) {
 		motor->m_phase_now_observer_override += add_min_speed;
 
@@ -2928,7 +2975,6 @@ static void input_current_offset_measurement(void) {
 #endif
 }
 
-
 static THD_FUNCTION(timer_thread, arg) {
 	(void)arg;
 
@@ -3903,7 +3949,8 @@ static float correct_hall(float angle, float dt, volatile motor_all_state_t *mot
 		}
 	}
 
-	int ang_hall_int = conf_now->foc_hall_table[utils_read_hall(motor != &m_motor_1)];
+	int ang_hall_int = conf_now->foc_hall_table[utils_read_hall(
+			motor != &m_motor_1, conf_now->m_hall_extra_samples)];
 
 	// Only override the observer if the hall sensor value is valid.
 	if (ang_hall_int < 201) {
@@ -3943,7 +3990,8 @@ static float correct_hall(float angle, float dt, volatile motor_all_state_t *mot
 
 		motor->m_ang_hall_int_prev = ang_hall_int;
 
-		if (((60.0 / (2.0 * M_PI)) * ((M_PI / 3.0) / motor->m_hall_dt_diff_now)) < 100) {
+		if (((60.0 / (2.0 * M_PI)) * ((M_PI / 3.0) /
+				fmaxf(fabsf(motor->m_hall_dt_diff_now), fabsf(motor->m_hall_dt_diff_last)))) < 100) {
 			// Don't interpolate on very low speed, just use the closest hall sensor. The reason is that we might
 			// get stuck at 60 degrees off if a direction change happens between two steps.
 			motor->m_ang_hall = ang_hall_now;
diff --git a/terminal.c b/terminal.c
index f2f8a732..f5b0a144 100644
--- a/terminal.c
+++ b/terminal.c
@@ -261,6 +261,41 @@ void terminal_process_string(char *str) {
 				commands_printf("Current            : %.2f", (double)msg->current);
 				commands_printf("Duty               : %.2f\n", (double)msg->duty);
 			}
+
+			io_board_adc_values *io_adc = comm_can_get_io_board_adc_1_4_index(i);
+			if (io_adc->id >= 0 && UTILS_AGE_S(io_adc->rx_time) < 1.0) {
+				commands_printf("IO Board ADC 1_4");
+				commands_printf("ID                 : %i", io_adc->id);
+				commands_printf("RX Time            : %i", io_adc->rx_time);
+				commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(io_adc->rx_time) * 1000.0));
+				commands_printf("ADC                : %.2f %.2f %.2f %.2f\n",
+						(double)io_adc->adc_voltages[0], (double)io_adc->adc_voltages[1],
+						(double)io_adc->adc_voltages[2], (double)io_adc->adc_voltages[3]);
+			}
+
+			io_adc = comm_can_get_io_board_adc_5_8_index(i);
+			if (io_adc->id >= 0 && UTILS_AGE_S(io_adc->rx_time) < 1.0) {
+				commands_printf("IO Board ADC 5_8");
+				commands_printf("ID                 : %i", io_adc->id);
+				commands_printf("RX Time            : %i", io_adc->rx_time);
+				commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(io_adc->rx_time) * 1000.0));
+				commands_printf("ADC                : %.2f %.2f %.2f %.2f\n",
+						(double)io_adc->adc_voltages[0], (double)io_adc->adc_voltages[1],
+						(double)io_adc->adc_voltages[2], (double)io_adc->adc_voltages[3]);
+			}
+
+			io_board_digial_inputs *io_in = comm_can_get_io_board_digital_in_index(i);
+			if (io_in->id >= 0 && UTILS_AGE_S(io_in->rx_time) < 1.0) {
+				commands_printf("IO Board Inputs");
+				commands_printf("ID                 : %i", io_in->id);
+				commands_printf("RX Time            : %i", io_in->rx_time);
+				commands_printf("Age (milliseconds) : %.2f", (double)(UTILS_AGE_S(io_in->rx_time) * 1000.0));
+				commands_printf("IN                 : %llu %llu %llu %llu %llu %llu %llu %llu\n",
+						(io_in->inputs >> 0) & 1, (io_in->inputs >> 1) & 1,
+						(io_in->inputs >> 2) & 1, (io_in->inputs >> 3) & 1,
+						(io_in->inputs >> 4) & 1, (io_in->inputs >> 5) & 1,
+						(io_in->inputs >> 6) & 1, (io_in->inputs >> 7) & 1);
+			}
 		}
 	} else if (strcmp(argv[0], "foc_encoder_detect") == 0) {
 		if (argc == 2) {
@@ -706,8 +741,9 @@ void terminal_process_string(char *str) {
 	} else if (strcmp(argv[0], "can_scan") == 0) {
 		bool found = false;
 		for (int i = 0;i < 254;i++) {
-			if (comm_can_ping(i)) {
-				commands_printf("Found VESC with ID: %d", i);
+			HW_TYPE hw_type;
+			if (comm_can_ping(i, &hw_type)) {
+				commands_printf("Found %s with ID: %d", utils_hw_type_to_string(hw_type), i);
 				found = true;
 			}
 		}
@@ -869,13 +905,13 @@ void terminal_process_string(char *str) {
 				}
 
 				bool is_second_motor = mc_interface_get_motor_thread() == 2;
-				int hall_last = utils_read_hall(is_second_motor);
+				int hall_last = utils_read_hall(is_second_motor, mcconf->m_hall_extra_samples);
 				float transitions[7] = {0.0};
 				int states[8] = {-1, -1, -1, -1, -1, -1, -1, -1};
 				int transition_index = 0;
 
 				for (int i = 0;i < 720;i++) {
-					int hall = utils_read_hall(is_second_motor);
+					int hall = utils_read_hall(is_second_motor, mcconf->m_hall_extra_samples);
 					if (hall_last != hall) {
 						if (transition_index < 7) {
 							transitions[transition_index++] = phase;
@@ -958,6 +994,40 @@ void terminal_process_string(char *str) {
 		} else {
 			commands_printf("This command requires one argument.\n");
 		}
+	} else if (strcmp(argv[0], "io_board_set_output") == 0) {
+		if (argc == 4) {
+			int id = -1;
+			int channel = -1;
+			int state = -1;
+
+			sscanf(argv[1], "%d", &id);
+			sscanf(argv[2], "%d", &channel);
+			sscanf(argv[3], "%d", &state);
+
+			if (id >= 0 && channel >= 0 && state >= 0) {
+				comm_can_io_board_set_output_digital(id, channel, state);
+				commands_printf("OK\n");
+			} else {
+				commands_printf("Invalid arguments\n");
+			}
+		}
+	} else if (strcmp(argv[0], "io_board_set_output_pwm") == 0) {
+		if (argc == 4) {
+			int id = -1;
+			int channel = -1;
+			float duty = -1.0;
+
+			sscanf(argv[1], "%d", &id);
+			sscanf(argv[2], "%d", &channel);
+			sscanf(argv[3], "%f", &duty);
+
+			if (id >= 0 && channel >= 0 && duty >= 0.0 && duty <= 1.0) {
+				comm_can_io_board_set_output_pwm(id, channel, duty);
+				commands_printf("OK\n");
+			} else {
+				commands_printf("Invalid arguments\n");
+			}
+		}
 	}
 
 	// The help command
@@ -1085,6 +1155,12 @@ void terminal_process_string(char *str) {
 		commands_printf("hall_analyze [current]");
 		commands_printf("  Rotate motor in open loop and analyze hall sensors.");
 
+		commands_printf("io_board_set_output [id] [ch] [state]");
+		commands_printf("  Set digital output of IO board.");
+
+		commands_printf("io_board_set_output_pwm [id] [ch] [duty]");
+		commands_printf("  Set pwm output of IO board.");
+
 		for (int i = 0;i < callback_write;i++) {
 			if (callbacks[i].cbf == 0) {
 				continue;
diff --git a/utils.c b/utils.c
index 9fae2273..e04f5351 100644
--- a/utils.c
+++ b/utils.c
@@ -760,36 +760,39 @@ uint8_t utils_second_motor_id(void) {
 #endif
 }
 
-int utils_read_hall(bool is_second_motor) {
-	int h1, h2, h3;
+/**
+ * Read hall sensors
+ *
+ * @param is_second_motor
+ * Use hall sensor port for second motor on dual motor hardware.
+ *
+ * @param samples
+ * The number of samples to read and filter over.
+ *
+ * @return
+ * The state of the three hall sensors.
+ */
+int utils_read_hall(bool is_second_motor, int samples) {
+	samples = 1 + 2 * samples;
+
+	int h1 = 0, h2 = 0, h3 = 0;
+	int tres = samples / 2;
 
 	if (is_second_motor) {
-		h1 = READ_HALL1_2();
-		h2 = READ_HALL2_2();
-		h3 = READ_HALL3_2();
-
-		h1 += READ_HALL1_2();
-		h2 += READ_HALL2_2();
-		h3 += READ_HALL3_2();
-
-		h1 += READ_HALL1_2();
-		h2 += READ_HALL2_2();
-		h3 += READ_HALL3_2();
+		while (samples--) {
+			h1 += READ_HALL1_2();
+			h2 += READ_HALL2_2();
+			h3 += READ_HALL3_2();
+		}
 	} else {
-		h1 = READ_HALL1();
-		h2 = READ_HALL2();
-		h3 = READ_HALL3();
-
-		h1 += READ_HALL1();
-		h2 += READ_HALL2();
-		h3 += READ_HALL3();
-
-		h1 += READ_HALL1();
-		h2 += READ_HALL2();
-		h3 += READ_HALL3();
+		while (samples--) {
+			h1 += READ_HALL1();
+			h2 += READ_HALL2();
+			h3 += READ_HALL3();
+		}
 	}
 
-	return (h1 > 1) | ((h2 > 1) << 1) | ((h3 > 1) << 2);
+	return (h1 > tres) | ((h2 > tres) << 1) | ((h3 > tres) << 2);
 }
 
 // A mapping of a samsung 30q cell for % remaining capacity vs. voltage from
@@ -822,6 +825,15 @@ uint16_t utils_median_filter_uint16_run(uint16_t *buffer,
 	return buffer_sorted[filter_len / 2];
 }
 
+const char* utils_hw_type_to_string(HW_TYPE hw) {
+	switch (hw) {
+	case HW_TYPE_VESC: return "HW_TYPE_VESC"; break;
+	case HW_TYPE_VESC_BMS: return "HW_TYPE_VESC_BMS"; break;
+	case HW_TYPE_CUSTOM_MODULE: return "HW_TYPE_CUSTOM_MODULE"; break;
+	default: return "FAULT_HARDWARE"; break;
+	}
+}
+
 const float utils_tab_sin_32_1[] = {
 	0.000000, 0.195090, 0.382683, 0.555570, 0.707107, 0.831470, 0.923880, 0.980785,
 	1.000000, 0.980785, 0.923880, 0.831470, 0.707107, 0.555570, 0.382683, 0.195090,
diff --git a/utils.h b/utils.h
index 89d655ff..62aa793a 100644
--- a/utils.h
+++ b/utils.h
@@ -22,6 +22,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include "datatypes.h"
 
 void utils_step_towards(float *value, float goal, float step);
 float utils_calc_ratio(float low, float high, float val);
@@ -60,10 +61,11 @@ void utils_fft8_bin0(float *real_in, float *real, float *imag);
 void utils_fft8_bin1(float *real_in, float *real, float *imag);
 void utils_fft8_bin2(float *real_in, float *real, float *imag);
 uint8_t utils_second_motor_id(void);
-int utils_read_hall(bool is_second_motor);
+int utils_read_hall(bool is_second_motor, int samples);
 float utils_batt_liion_norm_v_to_capacity(float norm_v);
 uint16_t utils_median_filter_uint16_run(uint16_t *buffer,
 		unsigned int *buffer_index, unsigned int filter_len, uint16_t sample);
+const char* utils_hw_type_to_string(HW_TYPE hw);
 
 // Return the sign of the argument. -1 if negative, 1 if zero or positive.
 #define SIGN(x)				((x < 0) ? -1 : 1)