diff --git a/CHANGELOG b/CHANGELOG
index 54ecb8c43..9403d532c 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -1,3 +1,68 @@
+=== FW 3.48 ===
+* Added pairing flag to appconf.
+* Decreased CAN TX timeout.
+
+=== FW 3.47 ===
+* Current percentage limits.
+* Mcconf_temp based on current scale instead of absolute current.
+* Removed battery current from mcconf_temp.
+* Added current scale parameter.
+* Different braking behavior: prefer cogging over locking the brakes.
+
+=== FW 3.46 ===
+* DC motor RPM measurement and RPM control when using encoder.
+* Support for configurable current low pass filter.
+* Much better recovery when failing to decode packets.
+* Run all detect functions in separate thread.
+* Fixed bug introduced when adding support for dual UARTs.
+* Added support for reverse state on NRF remote.
+* Support to disable app output for a specified time.
+
+=== FW 3.45 ===
+* Default CAN ID from UUID, and hook to define it in hwconf.
+* CAN ping support.
+* Simultaneous firmware update over CAN-bus.
+* Fully automated motor detection, based on maximum motor power losses.
+* Sensor autodetection and configuration support.
+* Softer encoder detection.
+* Better NRF_EXT support.
+* New more reliable flux linkage measurement.
+* Simpler to add hardware versions to build system. 
+* More DAS hardware support.
+* DRV8323s support.
+* Initial UAVCAN support.
+* Moved from uart to serial driver to avoid DMA conflicts.
+* Support for permanent UART.
+
+=== FW 3.44 ===
+* NRF_EXT commands support.
+ - Use NRF51822 with ESB remotes.
+* Different radio channel for NRF pairing.
+
+=== FW 3.43 ===
+* Added battery ah to setup info.
+* Changed tacho values in COMM_GET_VALUES_SETUP to meters.
+* Added battery wh COMM_GET_VALUES_SETUP.
+* Better remaining battery capacity calculation.
+
+=== FW 3.42 ===
+* Added setup info parameters:
+ - Motor Poles
+ - Gear Ratio
+ - Wheel Diameter
+ - Battery Type
+ - Battery Cells
+* Added more CAN status messages.
+* Updated speed PID to start properly when braking is disabled.
+* Added COMM_GET_VALUES_SETUP.
+* Added COMM_SET_MCCONF_TEMP.
+* Added COMM_SET_MCCONF_TEMP_SETUP.
+* Added COMM_GET_VALUES_SELECTIVE.
+* Added COMM_GET_VALUES_SETUP_SELECTIVE.
+
+=== FW 3.41 ===
+* First general purpose DC output implementation.
+
 === FW 3.40 ===
 * Added motor controller ID to COMM_GET_VALUES.
 
diff --git a/CONTRIBUTING b/CONTRIBUTING
index 6de6954e1..6f987cac5 100644
--- a/CONTRIBUTING
+++ b/CONTRIBUTING
@@ -71,13 +71,32 @@ and
 
 * If you write function comments, write them Doxygen-style.
 
-* There is no limit on line length, so long lines in general should not be wrapped.
+* Line lengths should be kept below 90 characters if possible, but that is not a strict requirement if braking the line looks ugly.
+
+* Source files should end with a new line.
+
+* Avoid more than one conscutive empty line.
+
 
 === Make sure that all hardware versions and configuration variations work ===
 
 When making updates it is easy to break things for different configurations. In order to make sure that the firmware at least builds for different hardwares and configurations it is a good idea to run the build_all/rebuild_all script and ensure that is finished without warnings and/or errors.
 
 
+=== Other guidelines ===
+
+* Use single precition floating point operations, as the FPU in the STM32F4 is 32 bits only. Double precision operations can take up to 50 times (!) longer.
+ - float instead if double
+ - Use the math library functions ending with f (sinf, cosf, powf, fabsf etc.)
+
+* Make sure that the code compiles without warnings.
+
+* Avoid dynamic memory allocation if possible, so that the RAM usage is known at compile time.
+
+* If the code crashed randomly, use the ChibiOS state checker:
+http://www.chibios.org/dokuwiki/doku.php?id=chibios:articles:state_checker
+
+
 === Be patient, and don't take criticism personally ===
 
 Be prepared to have to rework your contribution several times before it is considered acceptable. Once code is in, it's difficult to get it reworked for better quality, so it's important that this is done before the code is even accepted. Don't take it personally; instead appreciate that it is this peer review that makes the code great in the end!
@@ -85,4 +104,3 @@ Be prepared to have to rework your contribution several times before it is consi
 
 
 Thanks for reading!
-
diff --git a/ChibiOS_3.0.2/os/hal/src/usb.c b/ChibiOS_3.0.2/os/hal/src/usb.c
index c563d5fea..76fbeb4aa 100644
--- a/ChibiOS_3.0.2/os/hal/src/usb.c
+++ b/ChibiOS_3.0.2/os/hal/src/usb.c
@@ -757,6 +757,7 @@ void _usb_ep0in(USBDriver *usbp, usbep_t ep) {
       usbp->ep0state = USB_EP0_WAITING_TX0;
       return;
     }
+    /* Falls through. */
     /* Falls into, it is intentional.*/
   case USB_EP0_WAITING_TX0:
     /* Transmit phase over, receiving the zero sized status packet.*/
@@ -782,7 +783,8 @@ void _usb_ep0in(USBDriver *usbp, usbep_t ep) {
   case USB_EP0_RX:
     /* All the above are invalid states in the IN phase.*/
     osalDbgAssert(false, "EP0 state machine error");
-    /* Falling through is intentional.*/
+    /* Falls through. */
+    /* Falling into is intentional.*/
   case USB_EP0_ERROR:
     /* Error response, the state machine goes into an error state, the low
        level layer will have to reset it to USB_EP0_WAITING_SETUP after
@@ -842,7 +844,8 @@ void _usb_ep0out(USBDriver *usbp, usbep_t ep) {
   case USB_EP0_SENDING_STS:
     /* All the above are invalid states in the IN phase.*/
     osalDbgAssert(false, "EP0 state machine error");
-    /* Falling through is intentional.*/
+    /* Falls through. */
+    /* Falling into is intentional.*/
   case USB_EP0_ERROR:
     /* Error response, the state machine goes into an error state, the low
        level layer will have to reset it to USB_EP0_WAITING_SETUP after
diff --git a/ChibiOS_3.0.2/os/rt/src/chschd.c b/ChibiOS_3.0.2/os/rt/src/chschd.c
index e53384b3d..b75651d85 100644
--- a/ChibiOS_3.0.2/os/rt/src/chschd.c
+++ b/ChibiOS_3.0.2/os/rt/src/chschd.c
@@ -288,9 +288,11 @@ static void wakeup(void *p) {
 #if CH_CFG_USE_SEMAPHORES == TRUE
   case CH_STATE_WTSEM:
     chSemFastSignalI(tp->p_u.wtsemp);
+    /* Falls through. */
     /* Falls into, intentional. */
 #endif
 #if (CH_CFG_USE_CONDVARS == TRUE) && (CH_CFG_USE_CONDVARS_TIMEOUT == TRUE)
+    /* Falls through. */
   case CH_STATE_WTCOND:
 #endif
   case CH_STATE_QUEUED:
diff --git a/Makefile b/Makefile
index f8b952622..66e04176e 100644
--- a/Makefile
+++ b/Makefile
@@ -107,6 +107,7 @@ include $(CHIBIOS)/os/rt/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk
 include hwconf/hwconf.mk
 include applications/applications.mk
 include nrf/nrf.mk
+include libcanard/canard.mk
 
 # Define linker script file here
 LDSCRIPT= ld_eeprom_emu.ld
@@ -147,9 +148,11 @@ CSRC = $(STARTUPSRC) \
        flash_helper.c \
        mc_interface.c \
        mcpwm_foc.c \
+       gpdrive.c \
        $(HWSRC) \
        $(APPSRC) \
-       $(NRFSRC)
+       $(NRFSRC) \
+       $(CANARDSRC)
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global
 # setting.
@@ -186,7 +189,8 @@ INCDIR = $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
          appconf \
          $(HWINC) \
          $(APPINC) \
-         $(NRFINC)
+         $(NRFINC) \
+         $(CANARDINC)
 
 #
 # Project, sources and paths
diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h
index 69b8e90a4..d6b337378 100644
--- a/appconf/appconf_default.h
+++ b/appconf/appconf_default.h
@@ -22,7 +22,7 @@
 
 // Default app configuration
 #ifndef APPCONF_CONTROLLER_ID
-#define APPCONF_CONTROLLER_ID				0
+#define APPCONF_CONTROLLER_ID				-1 // Controller id. -1 means it should be calculated from UUID.
 #endif
 #ifndef APPCONF_TIMEOUT_MSEC
 #define APPCONF_TIMEOUT_MSEC				1000
@@ -31,14 +31,23 @@
 #define APPCONF_TIMEOUT_BRAKE_CURRENT		0.0
 #endif
 #ifndef APPCONF_SEND_CAN_STATUS
-#define APPCONF_SEND_CAN_STATUS				false
+#define APPCONF_SEND_CAN_STATUS				CAN_STATUS_DISABLED
+#endif
+#ifndef APPCONF_UAVCAN_ENABLE
+#define APPCONF_UAVCAN_ENABLE				false
+#endif
+#ifndef APPCONF_UAVCAN_ESC_INDEX
+#define APPCONF_UAVCAN_ESC_INDEX			0
 #endif
 #ifndef APPCONF_SEND_CAN_STATUS_RATE_HZ
-#define APPCONF_SEND_CAN_STATUS_RATE_HZ		100
+#define APPCONF_SEND_CAN_STATUS_RATE_HZ		50
 #endif
 #ifndef APPCONF_CAN_BAUD_RATE
 #define APPCONF_CAN_BAUD_RATE				CAN_BAUD_500K
 #endif
+#ifndef APPCONF_PAIRING_DONE
+#define APPCONF_PAIRING_DONE				false
+#endif
 
 // The default app is UART in case the UART port is used for
 // firmware updates.
@@ -87,7 +96,7 @@
 #define APPCONF_PPM_RAMP_TIME_NEG			0.1
 #endif
 #ifndef APPCONF_PPM_MULTI_ESC
-#define APPCONF_PPM_MULTI_ESC				false
+#define APPCONF_PPM_MULTI_ESC				true
 #endif
 #ifndef APPCONF_PPM_TC
 #define APPCONF_PPM_TC						false
@@ -116,7 +125,7 @@
 #define APPCONF_ADC_VOLTAGE2_START			0.9
 #endif
 #ifndef APPCONF_ADC_VOLTAGE2_END
-#define APPCONF_ADC_VOLTAGE2_END				3.0
+#define APPCONF_ADC_VOLTAGE2_END			3.0
 #endif
 #ifndef APPCONF_ADC_USE_FILTER
 #define APPCONF_ADC_USE_FILTER				true
@@ -152,7 +161,7 @@
 #define APPCONF_ADC_RAMP_TIME_NEG			0.1
 #endif
 #ifndef APPCONF_ADC_MULTI_ESC
-#define APPCONF_ADC_MULTI_ESC				false
+#define APPCONF_ADC_MULTI_ESC				true
 #endif
 #ifndef APPCONF_ADC_TC
 #define APPCONF_ADC_TC						false
@@ -195,7 +204,7 @@
 #define APPCONF_CHUK_THROTTLE_EXP_MODE		THR_EXP_POLY
 #endif
 #ifndef APPCONF_CHUK_MULTI_ESC
-#define APPCONF_CHUK_MULTI_ESC				false
+#define APPCONF_CHUK_MULTI_ESC				true
 #endif
 #ifndef APPCONF_CHUK_TC
 #define APPCONF_CHUK_TC						false
diff --git a/applications/app.c b/applications/app.c
index 3b09fa617..7aa8c3eca 100644
--- a/applications/app.c
+++ b/applications/app.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -27,6 +27,12 @@
 
 // Private variables
 static app_configuration appconf;
+static virtual_timer_t output_vt;
+static bool output_vt_init_done = false;
+static volatile bool output_disabled_now = false;
+
+// Private functions
+static void output_vt_cb(void *arg);
 
 const app_configuration* app_get_configuration(void) {
 	return &appconf;
@@ -117,3 +123,38 @@ void app_set_configuration(app_configuration *conf) {
 
 	rfhelp_update_conf(&appconf.app_nrf_conf);
 }
+
+/**
+ * Disable output on apps
+ *
+ * @param time_ms
+ * The amount of time to disable output in ms
+ * 0: Enable output now
+ * -1: Disable forever
+ * >0: Amount of milliseconds to disable output
+ */
+void app_disable_output(int time_ms) {
+	if (!output_vt_init_done) {
+		chVTObjectInit(&output_vt);
+		output_vt_init_done = true;
+	}
+
+	if (time_ms == 0) {
+		output_disabled_now = false;
+	} else if (time_ms == -1) {
+		output_disabled_now = true;
+		chVTReset(&output_vt);
+	} else {
+		output_disabled_now = true;
+		chVTSet(&output_vt, MS2ST(time_ms), output_vt_cb, 0);
+	}
+}
+
+bool app_is_output_disabled(void) {
+	return output_disabled_now;
+}
+
+static void output_vt_cb(void *arg) {
+	(void)arg;
+	output_disabled_now = false;
+}
diff --git a/applications/app.h b/applications/app.h
index 66729d7b5..106c8e998 100644
--- a/applications/app.h
+++ b/applications/app.h
@@ -25,6 +25,8 @@
 // Functions
 const app_configuration* app_get_configuration(void);
 void app_set_configuration(app_configuration *conf);
+void app_disable_output(int time_ms);
+bool app_is_output_disabled(void);
 
 // Standard apps
 void app_ppm_start(void);
@@ -41,8 +43,10 @@ float app_adc_get_decoded_level2(void);
 float app_adc_get_voltage2(void);
 
 void app_uartcomm_start(void);
+void app_uartcomm_start_permanent(void);
 void app_uartcomm_stop(void);
 void app_uartcomm_configure(uint32_t baudrate);
+void app_uartcomm_send_packet(unsigned char *data, unsigned int len);
 
 void app_nunchuk_start(void);
 void app_nunchuk_stop(void);
diff --git a/applications/app_adc.c b/applications/app_adc.c
index 03a12c8b9..219665a2e 100644
--- a/applications/app_adc.c
+++ b/applications/app_adc.c
@@ -244,6 +244,12 @@ static THD_FUNCTION(adc_thread, arg) {
 			}
 		}
 
+		// All pins and buttons are still decoded for debugging, even
+		// when output is disabled.
+		if (app_is_output_disabled()) {
+			continue;
+		}
+
 		switch (config.ctrl_type) {
 		case ADC_CTRL_TYPE_CURRENT_REV_CENTER:
 		case ADC_CTRL_TYPE_CURRENT_NOREV_BRAKE_CENTER:
diff --git a/applications/app_nunchuk.c b/applications/app_nunchuk.c
index 7fe0fd20d..e6e15ebcf 100644
--- a/applications/app_nunchuk.c
+++ b/applications/app_nunchuk.c
@@ -181,6 +181,8 @@ static THD_FUNCTION(chuk_thread, arg) {
 				chuck_d_tmp.acc_z = (rxbuf[4] << 2) | ((rxbuf[5] >> 6) & 3);
 				chuck_d_tmp.bt_z = !((rxbuf[5] >> 0) & 1);
 				chuck_d_tmp.bt_c = !((rxbuf[5] >> 1) & 1);
+				chuck_d_tmp.rev_has_state = false;
+				chuck_d_tmp.is_rev = false;
 
 				app_nunchuk_update_output(&chuck_d_tmp);
 			}
@@ -216,27 +218,38 @@ static THD_FUNCTION(output_thread, arg) {
 			continue;
 		}
 
+		if (app_is_output_disabled()) {
+			continue;
+		}
+
 		const volatile mc_configuration *mcconf = mc_interface_get_configuration();
 		static bool is_reverse = false;
 		static bool was_z = false;
 		const float current_now = mc_interface_get_tot_current_directional_filtered();
 		static float prev_current = 0.0;
-		const float max_current_diff = mcconf->l_current_max * 0.2;
+		const float max_current_diff = mcconf->l_current_max * mcconf->l_current_max_scale * 0.2;
 
 		if (chuck_d.bt_c && chuck_d.bt_z) {
 			led_external_set_state(LED_EXT_BATT);
 			continue;
 		}
 
-		if (chuck_d.bt_z && !was_z && config.ctrl_type == CHUK_CTRL_TYPE_CURRENT &&
-				fabsf(current_now) < max_current_diff) {
-			if (is_reverse) {
-				is_reverse = false;
-			} else {
-				is_reverse = true;
+		if (fabsf(current_now) < max_current_diff) {
+			if (chuck_d.rev_has_state) {
+				is_reverse = chuck_d.is_rev;
+			} else if (chuck_d.bt_z && !was_z) {
+				if (is_reverse) {
+					is_reverse = false;
+				} else {
+					is_reverse = true;
+				}
 			}
 		}
 
+		if (config.ctrl_type == CHUK_CTRL_TYPE_CURRENT_NOREV) {
+			is_reverse = false;
+		}
+
 		was_z = chuck_d.bt_z;
 
 		led_external_set_reversed(is_reverse);
@@ -391,7 +404,8 @@ static THD_FUNCTION(output_thread, arg) {
 		}
 
 		// Apply ramping
-		const float current_range = mcconf->l_current_max + fabsf(mcconf->l_current_min);
+		const float current_range = mcconf->l_current_max * mcconf->l_current_max_scale +
+				fabsf(mcconf->l_current_min) * mcconf->l_current_min_scale;
 		const float ramp_time = fabsf(current) > fabsf(prev_current) ? config.ramp_time_pos : config.ramp_time_neg;
 
 		if (ramp_time > 0.01) {
diff --git a/applications/app_ppm.c b/applications/app_ppm.c
index beaa4f25d..0850c79d4 100644
--- a/applications/app_ppm.c
+++ b/applications/app_ppm.c
@@ -167,6 +167,12 @@ static THD_FUNCTION(ppm_thread, arg) {
 			break;
 		}
 
+		// All pins and buttons are still decoded for debugging, even
+		// when output is disabled.
+		if (app_is_output_disabled()) {
+			continue;
+		}
+
 		if (timeout_has_timeout() || servodec_get_time_since_update() > timeout_get_timeout_msec() ||
 				mc_interface_get_fault() != FAULT_CODE_NONE) {
 			pulses_without_power = 0;
diff --git a/applications/app_uartcomm.c b/applications/app_uartcomm.c
index 2c067eec0..fbb2d0bf5 100644
--- a/applications/app_uartcomm.c
+++ b/applications/app_uartcomm.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -29,143 +29,132 @@
 // Settings
 #define BAUDRATE					115200
 #define PACKET_HANDLER				1
-#define SERIAL_RX_BUFFER_SIZE		1024
+#define PACKET_HANDLER_P			2
 
 // Threads
 static THD_FUNCTION(packet_process_thread, arg);
 static THD_WORKING_AREA(packet_process_thread_wa, 4096);
-static thread_t *process_tp = 0;
 
 // Variables
-static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
-static int serial_rx_read_pos = 0;
-static int serial_rx_write_pos = 0;
-static volatile bool is_running = false;
+static volatile bool thread_is_running = false;
+static volatile bool uart_is_running = false;
+static mutex_t send_mutex;
+static bool send_mutex_init_done = false;
 
 // Private functions
 static void process_packet(unsigned char *data, unsigned int len);
-static void send_packet_wrapper(unsigned char *data, unsigned int len);
 static void send_packet(unsigned char *data, unsigned int len);
 
-/*
- * This callback is invoked when a transmission buffer has been completely
- * read by the driver.
- */
-static void txend1(UARTDriver *uartp) {
-	(void)uartp;
-}
-
-/*
- * This callback is invoked when a transmission has physically completed.
- */
-static void txend2(UARTDriver *uartp) {
-	(void)uartp;
-}
-
-/*
- * This callback is invoked on a receive error, the errors mask is passed
- * as parameter.
- */
-static void rxerr(UARTDriver *uartp, uartflags_t e) {
-	(void)uartp;
-	(void)e;
-}
-
-/*
- * This callback is invoked when a character is received but the application
- * was not ready to receive it, the character is passed as parameter.
- */
-static void rxchar(UARTDriver *uartp, uint16_t c) {
-	(void)uartp;
-	serial_rx_buffer[serial_rx_write_pos++] = c;
-
-	if (serial_rx_write_pos == SERIAL_RX_BUFFER_SIZE) {
-		serial_rx_write_pos = 0;
-	}
-
-	chSysLockFromISR();
-	chEvtSignalI(process_tp, (eventmask_t) 1);
-	chSysUnlockFromISR();
-}
-
-/*
- * This callback is invoked when a receive buffer has been completely written.
- */
-static void rxend(UARTDriver *uartp) {
-	(void)uartp;
-}
-
-/*
- * UART driver configuration structure.
- */
-static UARTConfig uart_cfg = {
-		txend1,
-		txend2,
-		rxend,
-		rxchar,
-		rxerr,
+static SerialConfig uart_cfg = {
 		BAUDRATE,
 		0,
 		USART_CR2_LINEN,
 		0
 };
 
+#ifdef HW_UART_P_DEV
+static volatile bool from_p_uart = false;
+static volatile bool uart_p_is_running = false;
+static SerialConfig uart_p_cfg = {
+		HW_UART_P_BAUD,
+		0,
+		USART_CR2_LINEN,
+		0
+};
+#endif
+
 static void process_packet(unsigned char *data, unsigned int len) {
-	commands_set_send_func(send_packet_wrapper);
+	commands_set_send_func(app_uartcomm_send_packet);
 	commands_process_packet(data, len);
 }
 
-static void send_packet_wrapper(unsigned char *data, unsigned int len) {
-	packet_send_packet(data, len, PACKET_HANDLER);
-}
-
 static void send_packet(unsigned char *data, unsigned int len) {
-	// Wait for the previous transmission to finish.
-	while (HW_UART_DEV.txstate == UART_TX_ACTIVE) {
-		chThdSleep(1);
+#ifdef HW_UART_P_DEV
+	if (from_p_uart) {
+		if (uart_p_is_running) {
+			sdWrite(&HW_UART_P_DEV, data, len);
+		}
+	} else {
+		if (uart_is_running) {
+			sdWrite(&HW_UART_DEV, data, len);
+		}
 	}
-
-	// Copy this data to a new buffer in case the provided one is re-used
-	// after this function returns.
-	static uint8_t buffer[PACKET_MAX_PL_LEN + 5];
-	memcpy(buffer, data, len);
-
-	uartStartSend(&HW_UART_DEV, len, buffer);
+#else
+	if (uart_is_running) {
+		sdWrite(&HW_UART_DEV, data, len);
+	}
+#endif
 }
 
 void app_uartcomm_start(void) {
 	packet_init(send_packet, process_packet, PACKET_HANDLER);
-	serial_rx_read_pos = 0;
-	serial_rx_write_pos = 0;
 
-	if (!is_running) {
+	if (!thread_is_running) {
 		chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa),
 				NORMALPRIO, packet_process_thread, NULL);
-		is_running = true;
+		thread_is_running = true;
 	}
 
-	uartStart(&HW_UART_DEV, &uart_cfg);
+	sdStart(&HW_UART_DEV, &uart_cfg);
 	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_GPIO_AF) |
 			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);
+
+	uart_is_running = true;
+}
+
+void app_uartcomm_start_permanent(void) {
+#ifdef HW_UART_P_DEV
+	packet_init(send_packet, process_packet, PACKET_HANDLER);
+	packet_init(send_packet, process_packet, PACKET_HANDLER_P);
+
+	if (!thread_is_running) {
+		chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa),
+				NORMALPRIO, packet_process_thread, NULL);
+		thread_is_running = true;
+	}
+
+	sdStart(&HW_UART_P_DEV, &uart_p_cfg);
+	palSetPadMode(HW_UART_P_TX_PORT, HW_UART_P_TX_PIN, PAL_MODE_ALTERNATE(HW_UART_P_GPIO_AF) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_PULLUP);
+	palSetPadMode(HW_UART_P_RX_PORT, HW_UART_P_RX_PIN, PAL_MODE_ALTERNATE(HW_UART_P_GPIO_AF) |
+			PAL_STM32_OSPEED_HIGHEST |
+			PAL_STM32_PUDR_PULLUP);
+
+	uart_p_is_running = true;
+#endif
 }
 
 void app_uartcomm_stop(void) {
-	uartStop(&HW_UART_DEV);
+	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.
 }
 
+void app_uartcomm_send_packet(unsigned char *data, unsigned int len) {
+	if (!send_mutex_init_done) {
+		chMtxObjectInit(&send_mutex);
+		send_mutex_init_done = true;
+	}
+
+	chMtxLock(&send_mutex);
+	packet_send_packet(data, len, PACKET_HANDLER);
+	chMtxUnlock(&send_mutex);
+}
+
 void app_uartcomm_configure(uint32_t baudrate) {
 	uart_cfg.speed = baudrate;
 
-	if (is_running) {
-		uartStart(&HW_UART_DEV, &uart_cfg);
+	if (thread_is_running) {
+		sdStart(&HW_UART_DEV, &uart_cfg);
+		uart_is_running = true;
 	}
 }
 
@@ -174,17 +163,38 @@ static THD_FUNCTION(packet_process_thread, arg) {
 
 	chRegSetThreadName("uartcomm process");
 
-	process_tp = chThdGetSelfX();
+	event_listener_t el;
+	chEvtRegisterMaskWithFlags(&HW_UART_DEV.event, &el, EVENT_MASK(0), CHN_INPUT_AVAILABLE);
 
-	for(;;) {
-		chEvtWaitAny((eventmask_t) 1);
+#ifdef HW_UART_P_DEV
+	event_listener_t elp;
+	chEvtRegisterMaskWithFlags(&HW_UART_P_DEV.event, &elp, EVENT_MASK(0), CHN_INPUT_AVAILABLE);
+#endif
 
-		while (serial_rx_read_pos != serial_rx_write_pos) {
-			packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], PACKET_HANDLER);
+	for(;;) {
+		chEvtWaitAny(ALL_EVENTS);
+
+		bool rx = true;
+		while (rx) {
+			rx = false;
+
+			msg_t res = sdGetTimeout(&HW_UART_DEV, TIME_IMMEDIATE);
+			if (res != MSG_TIMEOUT) {
+#ifdef HW_UART_P_DEV
+				from_p_uart = false;
+#endif
+				packet_process_byte(res, PACKET_HANDLER);
+				rx = true;
+			}
 
-			if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
-				serial_rx_read_pos = 0;
+#ifdef HW_UART_P_DEV
+			res = sdGetTimeout(&HW_UART_P_DEV, TIME_IMMEDIATE);
+			if (res != MSG_TIMEOUT) {
+				from_p_uart = true;
+				packet_process_byte(res, PACKET_HANDLER_P);
+				rx = true;
 			}
+#endif
 		}
 	}
 }
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 6d2239732..628b6e837 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 149d3033e..042d7b596 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 4f7c6144a..204af7fa8 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 77f439e0c..f7af72506 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 fd9259f64..efc22f746 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 db3576f07..b3acde385 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 adebfba2a..b3f2e0584 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 7880cde08..5250110af 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 a4fb34890..eee63b26b 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 4d9af487e..7edc4fc8b 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 27a16352b..d8e6295c1 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 8a4a4ec43..fc5c36d21 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 897912c33..5ab34065b 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 da2773f88..509a60d20 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 baa1d0c85..c8966a307 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 9ed30aabd..478187a42 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 2d69d4b3e..568fc560e 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 ccce204bc..d308f6cd6 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 1b841311e..1785555ba 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 338634b55..c9300d7ae 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 fd78d6fd8..5dccaf548 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 4b64bd4a6..3dced123f 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 6b85bc60c..2ff56da5d 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/DAS_RS/VESC_default.bin b/build_all/DAS_RS/VESC_default.bin
index 126f2fb75..8e3183e7f 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/rebuild_all b/build_all/rebuild_all
index 481c45087..50ff8a690 100755
--- a/build_all/rebuild_all
+++ b/build_all/rebuild_all
@@ -12,42 +12,42 @@ rm $COPYDIR/*
 # default
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_46"
+make -j8 build_args='-DHW_SOURCE=\"hw_46.c\" -DHW_HEADER=\"hw_46.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
 
 # 33k
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DVIN_R1=33000.0 -DHW_VERSION_46"
+make -j8 build_args='-DVIN_R1=33000.0 -DHW_SOURCE=\"hw_46.c\" -DHW_HEADER=\"hw_46.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_33k.bin
 
 # 0005ohm
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DCURRENT_SHUNT_RES=0.0005 -DHW_VERSION_46"
+make -j8 build_args='-DCURRENT_SHUNT_RES=0.0005 -DHW_SOURCE=\"hw_46.c\" -DHW_HEADER=\"hw_46.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_0005ohm.bin
 
 # ws2811
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DWS2811_ENABLE=1 -DHW_VERSION_46"
+make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_46.c\" -DHW_HEADER=\"hw_46.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
 
 # ws2811_33k
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DWS2811_ENABLE=1 -DVIN_R1=33000.0 -DHW_VERSION_46"
+make -j8 build_args='-DWS2811_ENABLE=1 -DVIN_R1=33000.0 -DHW_SOURCE=\"hw_46.c\" -DHW_HEADER=\"hw_46.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811_33k.bin
 
 # servoout
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DSERVO_OUT_ENABLE=1 -DHW_VERSION_46"
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_46.c\" -DHW_HEADER=\"hw_46.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
 
@@ -59,28 +59,28 @@ rm $COPYDIR/*
 # default
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_48"
+make -j8 build_args='-DHW_SOURCE=\"hw_48.c\" -DHW_HEADER=\"hw_48.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
 
 # 0005ohm
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DCURRENT_SHUNT_RES=0.0005 -DHW_VERSION_48"
+make -j8 build_args='-DCURRENT_SHUNT_RES=0.0005 -DHW_SOURCE=\"hw_48.c\" -DHW_HEADER=\"hw_48.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_0005ohm.bin
 
 # ws2811
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DWS2811_ENABLE=1 -DHW_VERSION_48"
+make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_48.c\" -DHW_HEADER=\"hw_48.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
 
 # servoout
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DSERVO_OUT_ENABLE=1 -DHW_VERSION_48"
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_48.c\" -DHW_HEADER=\"hw_48.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
 
@@ -92,35 +92,35 @@ rm $COPYDIR/*
 # default
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_410"
+make -j8 build_args='-DHW_SOURCE=\"hw_410.c\" -DHW_HEADER=\"hw_410.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
 
 # default with HW limits disables
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_410 -DDISABLE_HW_LIMITS"
+make -j8 build_args='-DDISABLE_HW_LIMITS -DHW_SOURCE=\"hw_410.c\" -DHW_HEADER=\"hw_410.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
 
 # 0005ohm
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DCURRENT_SHUNT_RES=0.0005 -DHW_VERSION_410"
+make -j8 build_args='-DCURRENT_SHUNT_RES=0.0005 -DHW_SOURCE=\"hw_410.c\" -DHW_HEADER=\"hw_410.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_0005ohm.bin
 
 # ws2811
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DWS2811_ENABLE=1 -DHW_VERSION_410"
+make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_410.c\" -DHW_HEADER=\"hw_410.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
 
 # servoout
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DSERVO_OUT_ENABLE=1 -DHW_VERSION_410"
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_410.c\" -DHW_HEADER=\"hw_410.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
 
@@ -132,28 +132,28 @@ rm $COPYDIR/*
 # default
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_60"
+make -j8 build_args='-DHW_SOURCE=\"hw_60.c\" -DHW_HEADER=\"hw_60.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
 
 # default with HW limits disables
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_60 -DDISABLE_HW_LIMITS"
+make -j8 build_args='-DDISABLE_HW_LIMITS -DHW_SOURCE=\"hw_60.c\" -DHW_HEADER=\"hw_60.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
 
 # ws2811
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DWS2811_ENABLE=1 -DHW_VERSION_60"
+make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_60.c\" -DHW_HEADER=\"hw_60.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
 
 # servoout
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DSERVO_OUT_ENABLE=1 -DHW_VERSION_60"
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_60.c\" -DHW_HEADER=\"hw_60.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
 
@@ -170,7 +170,7 @@ rm $COPYDIR/*
 # default
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_DAS_RS"
+make -j8 build_args='-DHW_SOURCE=\"hw_das_rs.c\" -DHW_HEADER=\"hw_das_rs.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
 
@@ -187,28 +187,28 @@ rm $COPYDIR/*
 # default
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_75_300"
+make -j8 build_args='-DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default.bin
 
 # default with HW limits disables
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DHW_VERSION_75_300 -DDISABLE_HW_LIMITS"
+make -j8 build_args='-DDISABLE_HW_LIMITS -DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_default_no_hw_limits.bin
 
 # ws2811
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DWS2811_ENABLE=1 -DHW_VERSION_75_300"
+make -j8 build_args='-DWS2811_ENABLE=1 -DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_ws2811.bin
 
 # servoout
 cd $FWPATH
 touch conf_general.h
-make -j8 build_args="-DSERVO_OUT_ENABLE=1 -DHW_VERSION_75_300"
+make -j8 build_args='-DSERVO_OUT_ENABLE=1 -DHW_SOURCE=\"hw_75_300.c\" -DHW_HEADER=\"hw_75_300.h\"'
 cd $DIR
 cp $FWPATH/build/BLDC_4_ChibiOS.bin $COPYDIR/VESC_servoout.bin
 
diff --git a/comm_can.c b/comm_can.c
index e7f24c536..ba4965f19 100644
--- a/comm_can.c
+++ b/comm_can.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -31,12 +31,14 @@
 #include "app.h"
 #include "crc.h"
 #include "packet.h"
+#include "hw.h"
+#include "canard_driver.h"
 
 // Settings
-#define CANDx			CAND1
 #define RX_FRAMES_SIZE	100
 #define RX_BUFFER_SIZE	PACKET_MAX_PL_LEN
 
+#if CAN_ENABLE
 // Threads
 static THD_WORKING_AREA(cancom_read_thread_wa, 512);
 static THD_WORKING_AREA(cancom_process_thread_wa, 4096);
@@ -45,15 +47,24 @@ static THD_FUNCTION(cancom_read_thread, arg);
 static THD_FUNCTION(cancom_status_thread, arg);
 static THD_FUNCTION(cancom_process_thread, arg);
 
-// Variables
-static can_status_msg stat_msgs[CAN_STATUS_MSGS_TO_STORE];
 static mutex_t can_mtx;
+static mutex_t can_rx_mtx;
 static uint8_t rx_buffer[RX_BUFFER_SIZE];
 static unsigned int rx_buffer_last_id;
 static CANRxFrame rx_frames[RX_FRAMES_SIZE];
 static int rx_frame_read;
 static int rx_frame_write;
-static thread_t *process_tp;
+static thread_t *process_tp = 0;
+static thread_t *ping_tp = 0;
+#endif
+
+// Variables
+static can_status_msg stat_msgs[CAN_STATUS_MSGS_TO_STORE];
+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 unsigned int detect_all_foc_res_index = 0;
+static int8_t detect_all_foc_res[50];
 
 /*
  * 500KBaud, automatic wakeup, automatic recover
@@ -67,7 +78,9 @@ static CANConfig cancfg = {
 };
 
 // Private functions
+#if CAN_ENABLE
 static void send_packet_wrapper(unsigned char *data, unsigned int len);
+#endif
 static void set_timing(int brp, int ts1, int ts2);
 
 // Function pointers
@@ -76,23 +89,30 @@ static void(*sid_callback)(uint32_t id, uint8_t *data, uint8_t len) = 0;
 void comm_can_init(void) {
 	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
 		stat_msgs[i].id = -1;
+		stat_msgs_2[i].id = -1;
+		stat_msgs_3[i].id = -1;
+		stat_msgs_4[i].id = -1;
 	}
 
+#if CAN_ENABLE
 	rx_frame_read = 0;
 	rx_frame_write = 0;
 
 	chMtxObjectInit(&can_mtx);
+	chMtxObjectInit(&can_rx_mtx);
 
-	palSetPadMode(GPIOB, 8,
-			PAL_MODE_ALTERNATE(GPIO_AF_CAN1) |
+	palSetPadMode(HW_CANH_PORT, HW_CANH_PIN,
+			PAL_MODE_ALTERNATE(HW_CAN_GPIO_AF) |
 			PAL_STM32_OTYPE_PUSHPULL |
 			PAL_STM32_OSPEED_MID1);
-	palSetPadMode(GPIOB, 9,
-			PAL_MODE_ALTERNATE(GPIO_AF_CAN1) |
+	palSetPadMode(HW_CANL_PORT, HW_CANL_PIN,
+			PAL_MODE_ALTERNATE(HW_CAN_GPIO_AF) |
 			PAL_STM32_OTYPE_PUSHPULL |
 			PAL_STM32_OSPEED_MID1);
 
-	canStart(&CANDx, &cancfg);
+	canStart(&HW_CAN_DEV, &cancfg);
+
+	canard_driver_init();
 
 	chThdCreateStatic(cancom_read_thread_wa, sizeof(cancom_read_thread_wa), NORMALPRIO + 1,
 			cancom_read_thread, NULL);
@@ -100,6 +120,7 @@ void comm_can_init(void) {
 			cancom_status_thread, NULL);
 	chThdCreateStatic(cancom_process_thread_wa, sizeof(cancom_process_thread_wa), NORMALPRIO,
 			cancom_process_thread, NULL);
+#endif
 }
 
 void comm_can_set_baud(CAN_BAUD baud) {
@@ -112,232 +133,7 @@ void comm_can_set_baud(CAN_BAUD baud) {
 	}
 }
 
-static THD_FUNCTION(cancom_read_thread, arg) {
-	(void)arg;
-	chRegSetThreadName("CAN");
-
-	event_listener_t el;
-	CANRxFrame rxmsg;
-
-	chEvtRegister(&CANDx.rxfull_event, &el, 0);
-
-	while(!chThdShouldTerminateX()) {
-		if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
-			continue;
-		}
-
-		msg_t result = canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
-
-		while (result == MSG_OK) {
-			rx_frames[rx_frame_write++] = rxmsg;
-			if (rx_frame_write == RX_FRAMES_SIZE) {
-				rx_frame_write = 0;
-			}
-
-			chEvtSignal(process_tp, (eventmask_t) 1);
-
-			result = canReceive(&CANDx, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
-		}
-	}
-
-	chEvtUnregister(&CANDx.rxfull_event, &el);
-}
-
-static THD_FUNCTION(cancom_process_thread, arg) {
-	(void)arg;
-
-	chRegSetThreadName("Cancom process");
-	process_tp = chThdGetSelfX();
-
-	int32_t ind = 0;
-	unsigned int rxbuf_len;
-	unsigned int rxbuf_ind;
-	uint8_t crc_low;
-	uint8_t crc_high;
-	bool commands_send;
-
-	for(;;) {
-		chEvtWaitAny((eventmask_t) 1);
-
-		while (rx_frame_read != rx_frame_write) {
-			CANRxFrame rxmsg = rx_frames[rx_frame_read++];
-
-			if (rxmsg.IDE == CAN_IDE_EXT) {
-				uint8_t id = rxmsg.EID & 0xFF;
-				CAN_PACKET_ID cmd = rxmsg.EID >> 8;
-				can_status_msg *stat_tmp;
-
-				if (id == 255 || id == app_get_configuration()->controller_id) {
-					switch (cmd) {
-					case CAN_PACKET_SET_DUTY:
-						ind = 0;
-						mc_interface_set_duty(buffer_get_float32(rxmsg.data8, 1e5, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_CURRENT:
-						ind = 0;
-						mc_interface_set_current(buffer_get_float32(rxmsg.data8, 1e3, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_CURRENT_BRAKE:
-						ind = 0;
-						mc_interface_set_brake_current(buffer_get_float32(rxmsg.data8, 1e3, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_RPM:
-						ind = 0;
-						mc_interface_set_pid_speed(buffer_get_float32(rxmsg.data8, 1e0, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_POS:
-						ind = 0;
-						mc_interface_set_pid_pos(buffer_get_float32(rxmsg.data8, 1e6, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_FILL_RX_BUFFER:
-						memcpy(rx_buffer + rxmsg.data8[0], rxmsg.data8 + 1, rxmsg.DLC - 1);
-						break;
-
-					case CAN_PACKET_FILL_RX_BUFFER_LONG:
-						rxbuf_ind = (unsigned int)rxmsg.data8[0] << 8;
-						rxbuf_ind |= rxmsg.data8[1];
-						if (rxbuf_ind < RX_BUFFER_SIZE) {
-							memcpy(rx_buffer + rxbuf_ind, rxmsg.data8 + 2, rxmsg.DLC - 2);
-						}
-						break;
-
-					case CAN_PACKET_PROCESS_RX_BUFFER:
-						ind = 0;
-						rx_buffer_last_id = rxmsg.data8[ind++];
-						commands_send = rxmsg.data8[ind++];
-						rxbuf_len = (unsigned int)rxmsg.data8[ind++] << 8;
-						rxbuf_len |= (unsigned int)rxmsg.data8[ind++];
-
-						if (rxbuf_len > RX_BUFFER_SIZE) {
-							break;
-						}
-
-						crc_high = rxmsg.data8[ind++];
-						crc_low = rxmsg.data8[ind++];
-
-						if (crc16(rx_buffer, rxbuf_len)
-								== ((unsigned short) crc_high << 8
-										| (unsigned short) crc_low)) {
-
-							if (commands_send) {
-								commands_send_packet(rx_buffer, rxbuf_len);
-							} else {
-								commands_set_send_func(send_packet_wrapper);
-								commands_process_packet(rx_buffer, rxbuf_len);
-							}
-						}
-						break;
-
-					case CAN_PACKET_PROCESS_SHORT_BUFFER:
-						ind = 0;
-						rx_buffer_last_id = rxmsg.data8[ind++];
-						commands_send = rxmsg.data8[ind++];
-
-						if (commands_send) {
-							commands_send_packet(rxmsg.data8 + ind, rxmsg.DLC - ind);
-						} else {
-							commands_set_send_func(send_packet_wrapper);
-							commands_process_packet(rxmsg.data8 + ind, rxmsg.DLC - ind);
-						}
-						break;
-
-					case CAN_PACKET_SET_CURRENT_REL:
-						ind = 0;
-						mc_interface_set_current_rel(buffer_get_float32(rxmsg.data8, 1e5, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_CURRENT_BRAKE_REL:
-						ind = 0;
-						mc_interface_set_brake_current_rel(buffer_get_float32(rxmsg.data8, 1e5, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_CURRENT_HANDBRAKE:
-						ind = 0;
-						mc_interface_set_handbrake(buffer_get_float32(rxmsg.data8, 1e3, &ind));
-						timeout_reset();
-						break;
-
-					case CAN_PACKET_SET_CURRENT_HANDBRAKE_REL:
-						ind = 0;
-						mc_interface_set_handbrake_rel(buffer_get_float32(rxmsg.data8, 1e5, &ind));
-						timeout_reset();
-						break;
-
-					default:
-						break;
-					}
-				}
-
-				switch (cmd) {
-				case CAN_PACKET_STATUS:
-					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
-						stat_tmp = &stat_msgs[i];
-						if (stat_tmp->id == id || stat_tmp->id == -1) {
-							ind = 0;
-							stat_tmp->id = id;
-							stat_tmp->rx_time = chVTGetSystemTime();
-							stat_tmp->rpm = (float)buffer_get_int32(rxmsg.data8, &ind);
-							stat_tmp->current = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
-							stat_tmp->duty = (float)buffer_get_int16(rxmsg.data8, &ind) / 1000.0;
-							break;
-						}
-					}
-					break;
-
-				default:
-					break;
-				}
-			} else {
-				if (sid_callback) {
-					sid_callback(rxmsg.SID, rxmsg.data8, rxmsg.DLC);
-				}
-			}
-
-			if (rx_frame_read == RX_FRAMES_SIZE) {
-				rx_frame_read = 0;
-			}
-		}
-	}
-}
-
-static THD_FUNCTION(cancom_status_thread, arg) {
-	(void)arg;
-	chRegSetThreadName("CAN status");
-
-	for(;;) {
-		if (app_get_configuration()->send_can_status) {
-			// Send status message
-			int32_t send_index = 0;
-			uint8_t buffer[8];
-			buffer_append_int32(buffer, (int32_t)mc_interface_get_rpm(), &send_index);
-			buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current() * 10.0), &send_index);
-			buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1000.0), &send_index);
-			comm_can_transmit_eid(app_get_configuration()->controller_id |
-					((uint32_t)CAN_PACKET_STATUS << 8), buffer, send_index);
-		}
-
-		systime_t sleep_time = CH_CFG_ST_FREQUENCY / app_get_configuration()->send_can_status_rate_hz;
-		if (sleep_time == 0) {
-			sleep_time = 1;
-		}
-
-		chThdSleep(sleep_time);
-	}
-}
-
-void comm_can_transmit_eid(uint32_t id, uint8_t *data, uint8_t len) {
+void comm_can_transmit_eid(uint32_t id, const uint8_t *data, uint8_t len) {
 	if (len > 8) {
 		len = 8;
 	}
@@ -351,9 +147,8 @@ void comm_can_transmit_eid(uint32_t id, uint8_t *data, uint8_t len) {
 	memcpy(txmsg.data8, data, len);
 
 	chMtxLock(&can_mtx);
-	canTransmit(&CANDx, CAN_ANY_MAILBOX, &txmsg, MS2ST(20));
+	canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5));
 	chMtxUnlock(&can_mtx);
-
 #else
 	(void)id;
 	(void)data;
@@ -375,9 +170,8 @@ void comm_can_transmit_sid(uint32_t id, uint8_t *data, uint8_t len) {
 	memcpy(txmsg.data8, data, len);
 
 	chMtxLock(&can_mtx);
-	canTransmit(&CANDx, CAN_ANY_MAILBOX, &txmsg, MS2ST(20));
+	canTransmit(&HW_CAN_DEV, CAN_ANY_MAILBOX, &txmsg, MS2ST(5));
 	chMtxUnlock(&can_mtx);
-
 #else
 	(void)id;
 	(void)data;
@@ -410,10 +204,12 @@ void comm_can_set_sid_rx_callback(void (*p_func)(uint32_t id, uint8_t *data, uin
  * The payload length.
  *
  * @param send
- * If true, this packet will be passed to the send function of commands.
- * Otherwise, it will be passed to the process function.
+ * 0: Packet goes to commands_process_packet of receiver
+ * 1: Packet goes to commands_send_packet of receiver
+ * 2: Packet goes to commands_process and send function is set to null
+ *    so that no reply is sent back.
  */
-void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len, bool send) {
+void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len, uint8_t send) {
 	uint8_t send_buffer[8];
 
 	if (len <= 6) {
@@ -586,45 +382,740 @@ void comm_can_set_handbrake_rel(uint8_t controller_id, float current_rel) {
 }
 
 /**
- * Get status message by index.
+ * Check if a VESC on the CAN-bus responds.
  *
- * @param index
- * Index in the array
+ * @param controller_id
+ * The ID of the VESC.
  *
  * @return
- * The message or 0 for an invalid index.
+ * True for success, false otherwise.
  */
-can_status_msg *comm_can_get_status_msg_index(int index) {
-	if (index < CAN_STATUS_MSGS_TO_STORE) {
-		return &stat_msgs[index];
-	} else {
-		return 0;
-	}
+bool comm_can_ping(uint8_t controller_id) {
+	ping_tp = chThdGetSelfX();
+	chEvtGetAndClearEvents(ALL_EVENTS);
+
+	uint8_t buffer[1];
+	buffer[0] = app_get_configuration()->controller_id;
+	comm_can_transmit_eid(controller_id |
+			((uint32_t)CAN_PACKET_PING << 8), buffer, 1);
+
+	int ret = chEvtWaitAnyTimeout(1 << 29, MS2ST(10));
+	ping_tp = 0;
+	return ret != 0;
 }
 
 /**
- * Get status message by id.
+ * Detect and apply FOC settings.
  *
- * @param id
- * Id of the controller that sent the status message.
+ * @param controller_id
+ * The ID of the VESC.
  *
- * @return
- * The message or 0 for an invalid id.
+ * @param activate_status_msgs
+ * Activate CAN status messages on the target VESC on success.
+ *
+ * @param max_power_loss
+ * Maximum accepted power losses.
  */
-can_status_msg *comm_can_get_status_msg_id(int id) {
-	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
-		if (stat_msgs[i].id == id) {
-			return &stat_msgs[i];
-		}
-	}
-
-	return 0;
+void comm_can_detect_apply_all_foc(uint8_t controller_id, bool activate_status_msgs, float max_power_loss) {
+	int32_t send_index = 0;
+	uint8_t buffer[6];
+	buffer[send_index++] = app_get_configuration()->controller_id;
+	buffer[send_index++] = activate_status_msgs;
+	buffer_append_float32(buffer, max_power_loss, 1e3, &send_index);
+	comm_can_transmit_eid(controller_id |
+			((uint32_t)CAN_PACKET_DETECT_APPLY_ALL_FOC << 8), buffer, send_index);
 }
 
-static void send_packet_wrapper(unsigned char *data, unsigned int len) {
-	comm_can_send_buffer(rx_buffer_last_id, data, len, true);
+/**
+ * Update current limits on VESC on CAN-bus.
+ *
+ * @param controller_id
+ * ID of the VESC.
+ *
+ * @param store
+ * Store parameters in emulated EEPROM (FLASH).
+ *
+ * @param min
+ * Minimum current (negative value).
+ *
+ * @param max
+ * Maximum current.
+ */
+void comm_can_conf_current_limits(uint8_t controller_id,
+		bool store, float min, float max) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+	buffer_append_float32(buffer, min, 1e3, &send_index);
+	buffer_append_float32(buffer, max, 1e3, &send_index);
+	comm_can_transmit_eid(controller_id |
+			((uint32_t)(store ? CAN_PACKET_CONF_STORE_CURRENT_LIMITS :
+					CAN_PACKET_CONF_CURRENT_LIMITS) << 8), buffer, send_index);
 }
 
+/**
+ * Update input current limits on VESC on CAN-bus.
+ *
+ * @param controller_id
+ * ID of the VESC.
+ *
+ * @param store
+ * Store parameters in emulated EEPROM (FLASH).
+ *
+ * @param min
+ * Minimum current (negative value).
+ *
+ * @param max
+ * Maximum current.
+ */
+void comm_can_conf_current_limits_in(uint8_t controller_id,
+		bool store, float min, float max) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+	buffer_append_float32(buffer, min, 1e3, &send_index);
+	buffer_append_float32(buffer, max, 1e3, &send_index);
+	comm_can_transmit_eid(controller_id |
+			((uint32_t)(store ? CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN :
+					CAN_PACKET_CONF_CURRENT_LIMITS_IN) << 8), buffer, send_index);
+}
+
+/**
+ * Update FOC ERPM settings on VESC on CAN-bus.
+ *
+ * @param controller_id
+ * ID of the VESC.
+ *
+ * @param store
+ * Store parameters in emulated EEPROM (FLASH).
+ *
+ * @param foc_openloop_rpm
+ * Run in openloop below this ERPM in sensorless mode.
+ *
+ * @param foc_sl_erpm
+ * Use sensors below this ERPM in sensored mode.
+ */
+void comm_can_conf_foc_erpms(uint8_t controller_id,
+		bool store, float foc_openloop_rpm, float foc_sl_erpm) {
+	int32_t send_index = 0;
+	uint8_t buffer[8];
+	buffer_append_float32(buffer, foc_openloop_rpm, 1e3, &send_index);
+	buffer_append_float32(buffer, foc_sl_erpm, 1e3, &send_index);
+	comm_can_transmit_eid(controller_id |
+			((uint32_t)(store ? CAN_PACKET_CONF_STORE_FOC_ERPMS :
+					CAN_PACKET_CONF_FOC_ERPMS) << 8), buffer, send_index);
+}
+
+int comm_can_detect_all_foc_res(unsigned int index) {
+	if (index < detect_all_foc_res_index) {
+		return detect_all_foc_res[detect_all_foc_res_index];
+	} else {
+		return -999;
+	}
+}
+
+int comm_can_detect_all_foc_res_size(void) {
+	return detect_all_foc_res_index;
+}
+
+void comm_can_detect_all_foc_res_clear(void) {
+	detect_all_foc_res_index = 0;
+}
+
+/**
+ * Get status message by index.
+ *
+ * @param index
+ * Index in the array
+ *
+ * @return
+ * The message or 0 for an invalid index.
+ */
+can_status_msg *comm_can_get_status_msg_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &stat_msgs[index];
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Get status message by id.
+ *
+ * @param id
+ * Id of the controller that sent the status message.
+ *
+ * @return
+ * The message or 0 for an invalid id.
+ */
+can_status_msg *comm_can_get_status_msg_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (stat_msgs[i].id == id) {
+			return &stat_msgs[i];
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Get status message 2 by index.
+ *
+ * @param index
+ * Index in the array
+ *
+ * @return
+ * The message or 0 for an invalid index.
+ */
+can_status_msg_2 *comm_can_get_status_msg_2_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &stat_msgs_2[index];
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Get status message 2 by id.
+ *
+ * @param id
+ * Id of the controller that sent the status message.
+ *
+ * @return
+ * The message or 0 for an invalid id.
+ */
+can_status_msg_2 *comm_can_get_status_msg_2_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (stat_msgs_2[i].id == id) {
+			return &stat_msgs_2[i];
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Get status message 3 by index.
+ *
+ * @param index
+ * Index in the array
+ *
+ * @return
+ * The message or 0 for an invalid index.
+ */
+can_status_msg_3 *comm_can_get_status_msg_3_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &stat_msgs_3[index];
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Get status message 3 by id.
+ *
+ * @param id
+ * Id of the controller that sent the status message.
+ *
+ * @return
+ * The message or 0 for an invalid id.
+ */
+can_status_msg_3 *comm_can_get_status_msg_3_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (stat_msgs_3[i].id == id) {
+			return &stat_msgs_3[i];
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * Get status message 4 by index.
+ *
+ * @param index
+ * Index in the array
+ *
+ * @return
+ * The message or 0 for an invalid index.
+ */
+can_status_msg_4 *comm_can_get_status_msg_4_index(int index) {
+	if (index < CAN_STATUS_MSGS_TO_STORE) {
+		return &stat_msgs_4[index];
+	} else {
+		return 0;
+	}
+}
+
+/**
+ * Get status message 4 by id.
+ *
+ * @param id
+ * Id of the controller that sent the status message.
+ *
+ * @return
+ * The message or 0 for an invalid id.
+ */
+can_status_msg_4 *comm_can_get_status_msg_4_id(int id) {
+	for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+		if (stat_msgs_4[i].id == id) {
+			return &stat_msgs_4[i];
+		}
+	}
+
+	return 0;
+}
+
+CANRxFrame *comm_can_get_rx_frame(void) {
+	chMtxLock(&can_rx_mtx);
+	if (rx_frame_read != rx_frame_write) {
+		CANRxFrame *res = &rx_frames[rx_frame_read++];
+
+		if (rx_frame_read == RX_FRAMES_SIZE) {
+			rx_frame_read = 0;
+		}
+
+		chMtxUnlock(&can_rx_mtx);
+		return res;
+	} else {
+		chMtxUnlock(&can_rx_mtx);
+		return 0;
+	}
+}
+
+#if CAN_ENABLE
+static THD_FUNCTION(cancom_read_thread, arg) {
+	(void)arg;
+	chRegSetThreadName("CAN read");
+
+	event_listener_t el;
+	CANRxFrame rxmsg;
+
+	chEvtRegister(&HW_CAN_DEV.rxfull_event, &el, 0);
+
+	while(!chThdShouldTerminateX()) {
+		if (chEvtWaitAnyTimeout(ALL_EVENTS, MS2ST(10)) == 0) {
+			continue;
+		}
+
+		msg_t result = canReceive(&HW_CAN_DEV, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
+
+		while (result == MSG_OK) {
+
+			chMtxLock(&can_rx_mtx);
+			rx_frames[rx_frame_write++] = rxmsg;
+			if (rx_frame_write == RX_FRAMES_SIZE) {
+				rx_frame_write = 0;
+			}
+			chMtxUnlock(&can_rx_mtx);
+
+			chEvtSignal(process_tp, (eventmask_t) 1);
+
+			result = canReceive(&HW_CAN_DEV, CAN_ANY_MAILBOX, &rxmsg, TIME_IMMEDIATE);
+		}
+	}
+
+	chEvtUnregister(&HW_CAN_DEV.rxfull_event, &el);
+}
+
+static THD_FUNCTION(cancom_process_thread, arg) {
+	(void)arg;
+
+	chRegSetThreadName("CAN process");
+	process_tp = chThdGetSelfX();
+
+	int32_t ind = 0;
+	unsigned int rxbuf_len;
+	unsigned int rxbuf_ind;
+	uint8_t crc_low;
+	uint8_t crc_high;
+	uint8_t commands_send;
+
+	for(;;) {
+		chEvtWaitAny((eventmask_t)1);
+
+		if (app_get_configuration()->uavcan_enable) {
+			continue;
+		}
+
+		CANRxFrame *rxmsg_tmp;
+		while ((rxmsg_tmp = comm_can_get_rx_frame()) != 0) {
+			CANRxFrame rxmsg = *rxmsg_tmp;
+
+			if (rxmsg.IDE == CAN_IDE_EXT) {
+				uint8_t id = rxmsg.EID & 0xFF;
+				CAN_PACKET_ID cmd = rxmsg.EID >> 8;
+				can_status_msg *stat_tmp;
+				can_status_msg_2 *stat_tmp_2;
+				can_status_msg_3 *stat_tmp_3;
+				can_status_msg_4 *stat_tmp_4;
+
+				if (id == 255 || id == app_get_configuration()->controller_id) {
+					switch (cmd) {
+					case CAN_PACKET_SET_DUTY:
+						ind = 0;
+						mc_interface_set_duty(buffer_get_float32(rxmsg.data8, 1e5, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_CURRENT:
+						ind = 0;
+						mc_interface_set_current(buffer_get_float32(rxmsg.data8, 1e3, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_CURRENT_BRAKE:
+						ind = 0;
+						mc_interface_set_brake_current(buffer_get_float32(rxmsg.data8, 1e3, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_RPM:
+						ind = 0;
+						mc_interface_set_pid_speed(buffer_get_float32(rxmsg.data8, 1e0, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_POS:
+						ind = 0;
+						mc_interface_set_pid_pos(buffer_get_float32(rxmsg.data8, 1e6, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_FILL_RX_BUFFER:
+						memcpy(rx_buffer + rxmsg.data8[0], rxmsg.data8 + 1, rxmsg.DLC - 1);
+						break;
+
+					case CAN_PACKET_FILL_RX_BUFFER_LONG:
+						rxbuf_ind = (unsigned int)rxmsg.data8[0] << 8;
+						rxbuf_ind |= rxmsg.data8[1];
+						if (rxbuf_ind < RX_BUFFER_SIZE) {
+							memcpy(rx_buffer + rxbuf_ind, rxmsg.data8 + 2, rxmsg.DLC - 2);
+						}
+						break;
+
+					case CAN_PACKET_PROCESS_RX_BUFFER:
+						ind = 0;
+						rx_buffer_last_id = rxmsg.data8[ind++];
+						commands_send = rxmsg.data8[ind++];
+						rxbuf_len = (unsigned int)rxmsg.data8[ind++] << 8;
+						rxbuf_len |= (unsigned int)rxmsg.data8[ind++];
+
+						if (rxbuf_len > RX_BUFFER_SIZE) {
+							break;
+						}
+
+						crc_high = rxmsg.data8[ind++];
+						crc_low = rxmsg.data8[ind++];
+
+						if (crc16(rx_buffer, rxbuf_len)
+								== ((unsigned short) crc_high << 8
+										| (unsigned short) crc_low)) {
+							switch (commands_send) {
+								case 0:
+									commands_set_send_func(send_packet_wrapper);
+									commands_process_packet(rx_buffer, rxbuf_len);
+									break;
+								case 1:
+									commands_send_packet(rx_buffer, rxbuf_len);
+									break;
+								case 2:
+									commands_set_send_func(0);
+									commands_process_packet(rx_buffer, rxbuf_len);
+									break;
+								default:
+									break;
+							}
+						}
+						break;
+
+					case CAN_PACKET_PROCESS_SHORT_BUFFER:
+						ind = 0;
+						rx_buffer_last_id = rxmsg.data8[ind++];
+						commands_send = rxmsg.data8[ind++];
+
+						switch (commands_send) {
+						case 0:
+							commands_set_send_func(send_packet_wrapper);
+							commands_process_packet(rxmsg.data8 + ind, rxmsg.DLC - ind);
+							break;
+						case 1:
+							commands_send_packet(rxmsg.data8 + ind, rxmsg.DLC - ind);
+							break;
+						case 2:
+							commands_set_send_func(0);
+							commands_process_packet(rxmsg.data8 + ind, rxmsg.DLC - ind);
+							break;
+						default:
+							break;
+						}
+						break;
+
+					case CAN_PACKET_SET_CURRENT_REL:
+						ind = 0;
+						mc_interface_set_current_rel(buffer_get_float32(rxmsg.data8, 1e5, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_CURRENT_BRAKE_REL:
+						ind = 0;
+						mc_interface_set_brake_current_rel(buffer_get_float32(rxmsg.data8, 1e5, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_CURRENT_HANDBRAKE:
+						ind = 0;
+						mc_interface_set_handbrake(buffer_get_float32(rxmsg.data8, 1e3, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_SET_CURRENT_HANDBRAKE_REL:
+						ind = 0;
+						mc_interface_set_handbrake_rel(buffer_get_float32(rxmsg.data8, 1e5, &ind));
+						timeout_reset();
+						break;
+
+					case CAN_PACKET_PING: {
+						uint8_t buffer[1];
+						buffer[0] = app_get_configuration()->controller_id;
+						comm_can_transmit_eid(rxmsg.data8[0] |
+								((uint32_t)CAN_PACKET_PONG << 8), buffer, 1);
+					} break;
+
+					case CAN_PACKET_PONG:
+						// rxmsg.data8[0]; // Sender ID
+						if (ping_tp) {
+							chEvtSignal(ping_tp, 1 << 29);
+						}
+						break;
+
+					case CAN_PACKET_DETECT_APPLY_ALL_FOC: {
+						ind = 1;
+						bool activate_status = rxmsg.data8[ind++];
+						float max_power_loss = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						int res = conf_general_detect_apply_all_foc(max_power_loss, true, false);
+						if (res >= 0 && activate_status) {
+							app_configuration appconf = *app_get_configuration();
+
+							if (appconf.send_can_status != CAN_STATUS_1_2_3_4) {
+								appconf.send_can_status = CAN_STATUS_1_2_3_4;
+								conf_general_store_app_configuration(&appconf);
+								app_set_configuration(&appconf);
+							}
+						}
+
+						int8_t buffer[1];
+						buffer[0] = res;
+						comm_can_transmit_eid(rxmsg.data8[0] |
+								((uint32_t)CAN_PACKET_DETECT_APPLY_ALL_FOC_RES << 8), (uint8_t*)buffer, 1);
+					} break;
+
+					case CAN_PACKET_DETECT_APPLY_ALL_FOC_RES: {
+						detect_all_foc_res[detect_all_foc_res_index++] = (int8_t)rxmsg.data8[0];
+						detect_all_foc_res_index %= sizeof(detect_all_foc_res);
+					} break;
+
+					case CAN_PACKET_CONF_CURRENT_LIMITS:
+					case CAN_PACKET_CONF_STORE_CURRENT_LIMITS: {
+						ind = 0;
+						float min = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						float max = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						mc_configuration mcconf = *mc_interface_get_configuration();
+
+						if (mcconf.l_current_min != min || mcconf.l_current_max != max) {
+							mcconf.l_current_min = min;
+							mcconf.l_current_max = max;
+
+							if (cmd == CAN_PACKET_CONF_STORE_CURRENT_LIMITS) {
+								conf_general_store_mc_configuration(&mcconf);
+							}
+
+							mc_interface_set_configuration(&mcconf);
+						}
+					} break;
+
+					case CAN_PACKET_CONF_CURRENT_LIMITS_IN:
+					case CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN: {
+						ind = 0;
+						float min = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						float max = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						mc_configuration mcconf = *mc_interface_get_configuration();
+
+						if (mcconf.l_in_current_min != min || mcconf.l_in_current_max != max) {
+							mcconf.l_in_current_min = min;
+							mcconf.l_in_current_max = max;
+
+							if (cmd == CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN) {
+								conf_general_store_mc_configuration(&mcconf);
+							}
+
+							mc_interface_set_configuration(&mcconf);
+						}
+					} break;
+
+					case CAN_PACKET_CONF_FOC_ERPMS:
+					case CAN_PACKET_CONF_STORE_FOC_ERPMS: {
+						ind = 0;
+						float foc_openloop_rpm = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						float foc_sl_erpm = buffer_get_float32(rxmsg.data8, 1e3, &ind);
+						mc_configuration mcconf = *mc_interface_get_configuration();
+
+						if (mcconf.foc_openloop_rpm != foc_openloop_rpm ||
+								mcconf.foc_sl_erpm != foc_sl_erpm) {
+							mcconf.foc_openloop_rpm = foc_openloop_rpm;
+							mcconf.foc_sl_erpm = foc_sl_erpm;
+
+							if (cmd == CAN_PACKET_CONF_STORE_FOC_ERPMS) {
+								conf_general_store_mc_configuration(&mcconf);
+							}
+
+							mc_interface_set_configuration(&mcconf);
+						}
+					} break;
+
+					default:
+						break;
+					}
+				}
+
+				switch (cmd) {
+				case CAN_PACKET_STATUS:
+					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+						stat_tmp = &stat_msgs[i];
+						if (stat_tmp->id == id || stat_tmp->id == -1) {
+							ind = 0;
+							stat_tmp->id = id;
+							stat_tmp->rx_time = chVTGetSystemTime();
+							stat_tmp->rpm = (float)buffer_get_int32(rxmsg.data8, &ind);
+							stat_tmp->current = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
+							stat_tmp->duty = (float)buffer_get_int16(rxmsg.data8, &ind) / 1000.0;
+							break;
+						}
+					}
+					break;
+
+				case CAN_PACKET_STATUS_2:
+					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+						stat_tmp_2 = &stat_msgs_2[i];
+						if (stat_tmp_2->id == id || stat_tmp_2->id == -1) {
+							ind = 0;
+							stat_tmp_2->id = id;
+							stat_tmp_2->rx_time = chVTGetSystemTime();
+							stat_tmp_2->amp_hours = (float)buffer_get_int32(rxmsg.data8, &ind) / 1e4;
+							stat_tmp_2->amp_hours_charged = (float)buffer_get_int32(rxmsg.data8, &ind) / 1e4;
+							break;
+						}
+					}
+					break;
+
+				case CAN_PACKET_STATUS_3:
+					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+						stat_tmp_3 = &stat_msgs_3[i];
+						if (stat_tmp_3->id == id || stat_tmp_3->id == -1) {
+							ind = 0;
+							stat_tmp_3->id = id;
+							stat_tmp_3->rx_time = chVTGetSystemTime();
+							stat_tmp_3->watt_hours = (float)buffer_get_int32(rxmsg.data8, &ind) / 1e4;
+							stat_tmp_3->watt_hours_charged = (float)buffer_get_int32(rxmsg.data8, &ind) / 1e4;
+							break;
+						}
+					}
+					break;
+
+				case CAN_PACKET_STATUS_4:
+					for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+						stat_tmp_4 = &stat_msgs_4[i];
+						if (stat_tmp_4->id == id || stat_tmp_4->id == -1) {
+							ind = 0;
+							stat_tmp_4->id = id;
+							stat_tmp_4->rx_time = chVTGetSystemTime();
+							stat_tmp_4->temp_fet = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
+							stat_tmp_4->temp_motor = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
+							stat_tmp_4->current_in = (float)buffer_get_int16(rxmsg.data8, &ind) / 10.0;
+							stat_tmp_4->pid_pos_now = (float)buffer_get_int16(rxmsg.data8, &ind) / 50.0;
+							break;
+						}
+					}
+					break;
+
+				default:
+					break;
+				}
+			} else {
+				if (sid_callback) {
+					sid_callback(rxmsg.SID, rxmsg.data8, rxmsg.DLC);
+				}
+			}
+		}
+	}
+}
+
+static THD_FUNCTION(cancom_status_thread, arg) {
+	(void)arg;
+	chRegSetThreadName("CAN status");
+
+	for(;;) {
+		const app_configuration *conf = app_get_configuration();
+
+		if (!conf->uavcan_enable) {
+			if (conf->send_can_status == CAN_STATUS_1 ||
+					conf->send_can_status == CAN_STATUS_1_2 ||
+					conf->send_can_status == CAN_STATUS_1_2_3 ||
+					conf->send_can_status == CAN_STATUS_1_2_3_4) {
+				int32_t send_index = 0;
+				uint8_t buffer[8];
+				buffer_append_int32(buffer, (int32_t)mc_interface_get_rpm(), &send_index);
+				buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_filtered() * 1e1), &send_index);
+				buffer_append_int16(buffer, (int16_t)(mc_interface_get_duty_cycle_now() * 1e3), &send_index);
+				comm_can_transmit_eid(conf->controller_id |
+						((uint32_t)CAN_PACKET_STATUS << 8), buffer, send_index);
+			}
+
+			if (conf->send_can_status == CAN_STATUS_1_2 ||
+					conf->send_can_status == CAN_STATUS_1_2_3||
+					conf->send_can_status == CAN_STATUS_1_2_3_4) {
+				int32_t send_index = 0;
+				uint8_t buffer[8];
+				buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours(false) * 1e4), &send_index);
+				buffer_append_int32(buffer, (int32_t)(mc_interface_get_amp_hours_charged(false) * 1e4), &send_index);
+				comm_can_transmit_eid(conf->controller_id |
+						((uint32_t)CAN_PACKET_STATUS_2 << 8), buffer, send_index);
+			}
+
+			if (conf->send_can_status == CAN_STATUS_1_2_3 ||
+					conf->send_can_status == CAN_STATUS_1_2_3_4) {
+				int32_t send_index = 0;
+				uint8_t buffer[8];
+				buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours(false) * 1e4), &send_index);
+				buffer_append_int32(buffer, (int32_t)(mc_interface_get_watt_hours_charged(false) * 1e4), &send_index);
+				comm_can_transmit_eid(conf->controller_id |
+						((uint32_t)CAN_PACKET_STATUS_3 << 8), buffer, send_index);
+			}
+
+			if (conf->send_can_status == CAN_STATUS_1_2_3_4) {
+				int32_t send_index = 0;
+				uint8_t buffer[8];
+				buffer_append_int16(buffer, (int16_t)(mc_interface_temp_fet_filtered() * 1e1), &send_index);
+				buffer_append_int16(buffer, (int16_t)(mc_interface_temp_motor_filtered() * 1e1), &send_index);
+				buffer_append_int16(buffer, (int16_t)(mc_interface_get_tot_current_in_filtered() * 1e1), &send_index);
+				buffer_append_int16(buffer, (int16_t)(mc_interface_get_pid_pos_now() * 50.0), &send_index);
+				comm_can_transmit_eid(conf->controller_id |
+						((uint32_t)CAN_PACKET_STATUS_4 << 8), buffer, send_index);
+			}
+		}
+
+		systime_t sleep_time = CH_CFG_ST_FREQUENCY / conf->send_can_status_rate_hz;
+		if (sleep_time == 0) {
+			sleep_time = 1;
+		}
+
+		chThdSleep(sleep_time);
+	}
+}
+
+static void send_packet_wrapper(unsigned char *data, unsigned int len) {
+	comm_can_send_buffer(rx_buffer_last_id, data, len, 1);
+}
+#endif
+
 static void set_timing(int brp, int ts1, int ts2) {
 	brp &= 0b1111111111;
 	ts1 &= 0b1111;
@@ -633,6 +1124,6 @@ static void set_timing(int brp, int ts1, int ts2) {
 	cancfg.btr = CAN_BTR_SJW(3) | CAN_BTR_TS2(ts2) |
 		CAN_BTR_TS1(ts1) | CAN_BTR_BRP(brp);
 
-	canStop(&CANDx);
-	canStart(&CANDx, &cancfg);
+	canStop(&HW_CAN_DEV);
+	canStart(&HW_CAN_DEV, &cancfg);
 }
diff --git a/comm_can.h b/comm_can.h
index b196a2532..ab7f92014 100644
--- a/comm_can.h
+++ b/comm_can.h
@@ -21,6 +21,7 @@
 #define COMM_CAN_H_
 
 #include "conf_general.h"
+#include "hal.h"
 
 // Settings
 #define CAN_STATUS_MSG_INT_MS		1
@@ -29,10 +30,10 @@
 // Functions
 void comm_can_init(void);
 void comm_can_set_baud(CAN_BAUD baud);
-void comm_can_transmit_eid(uint32_t id, uint8_t *data, uint8_t len);
+void comm_can_transmit_eid(uint32_t id, const uint8_t *data, uint8_t len);
 void comm_can_transmit_sid(uint32_t id, uint8_t *data, uint8_t len);
 void comm_can_set_sid_rx_callback(void (*p_func)(uint32_t id, uint8_t *data, uint8_t len));
-void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len, bool send);
+void comm_can_send_buffer(uint8_t controller_id, uint8_t *data, unsigned int len, uint8_t send);
 void comm_can_set_duty(uint8_t controller_id, float duty);
 void comm_can_set_current(uint8_t controller_id, float current);
 void comm_can_set_current_brake(uint8_t controller_id, float current);
@@ -40,7 +41,25 @@ 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);
+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);
+void comm_can_conf_current_limits_in(uint8_t controller_id,
+		bool store, float min, float max);
+void comm_can_conf_foc_erpms(uint8_t controller_id,
+		bool store, float foc_openloop_rpm, float foc_sl_erpm);
+int comm_can_detect_all_foc_res(unsigned int index);
+int comm_can_detect_all_foc_res_size(void);
+void comm_can_detect_all_foc_res_clear(void);
 can_status_msg *comm_can_get_status_msg_index(int index);
 can_status_msg *comm_can_get_status_msg_id(int id);
+can_status_msg_2 *comm_can_get_status_msg_2_index(int index);
+can_status_msg_2 *comm_can_get_status_msg_2_id(int id);
+can_status_msg_3 *comm_can_get_status_msg_3_index(int index);
+can_status_msg_3 *comm_can_get_status_msg_3_id(int id);
+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);
+CANRxFrame *comm_can_get_rx_frame(void);
 
 #endif /* COMM_CAN_H_ */
diff --git a/comm_usb.c b/comm_usb.c
index be73492e1..18a041ed5 100644
--- a/comm_usb.c
+++ b/comm_usb.c
@@ -32,15 +32,14 @@
 static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
 static int serial_rx_read_pos = 0;
 static int serial_rx_write_pos = 0;
-static THD_WORKING_AREA(serial_read_thread_wa, 512);
+static THD_WORKING_AREA(serial_read_thread_wa, 256);
 static THD_WORKING_AREA(serial_process_thread_wa, 4096);
 static mutex_t send_mutex;
 static thread_t *process_tp;
 
 // Private functions
 static void process_packet(unsigned char *data, unsigned int len);
-static void send_packet(unsigned char *buffer, unsigned int len);
-static void send_packet_wrapper(unsigned char *data, unsigned int len);
+static void send_packet_raw(unsigned char *buffer, unsigned int len);
 
 static THD_FUNCTION(serial_read_thread, arg) {
 	(void)arg;
@@ -93,23 +92,17 @@ static THD_FUNCTION(serial_process_thread, arg) {
 }
 
 static void process_packet(unsigned char *data, unsigned int len) {
-	commands_set_send_func(send_packet_wrapper);
+	commands_set_send_func(comm_usb_send_packet);
 	commands_process_packet(data, len);
 }
 
-static void send_packet_wrapper(unsigned char *data, unsigned int len) {
-	chMtxLock(&send_mutex);
-	packet_send_packet(data, len, PACKET_HANDLER);
-	chMtxUnlock(&send_mutex);
-}
-
-static void send_packet(unsigned char *buffer, unsigned int len) {
+static void send_packet_raw(unsigned char *buffer, unsigned int len) {
 	chSequentialStreamWrite(&SDU1, buffer, len);
 }
 
 void comm_usb_init(void) {
 	comm_usb_serial_init();
-	packet_init(send_packet, process_packet, PACKET_HANDLER);
+	packet_init(send_packet_raw, process_packet, PACKET_HANDLER);
 
 	chMtxObjectInit(&send_mutex);
 
@@ -117,3 +110,9 @@ void comm_usb_init(void) {
 	chThdCreateStatic(serial_read_thread_wa, sizeof(serial_read_thread_wa), NORMALPRIO, serial_read_thread, NULL);
 	chThdCreateStatic(serial_process_thread_wa, sizeof(serial_process_thread_wa), NORMALPRIO, serial_process_thread, NULL);
 }
+
+void comm_usb_send_packet(unsigned char *data, unsigned int len) {
+	chMtxLock(&send_mutex);
+	packet_send_packet(data, len, PACKET_HANDLER);
+	chMtxUnlock(&send_mutex);
+}
diff --git a/comm_usb.h b/comm_usb.h
index 2062e9184..5ecf69544 100644
--- a/comm_usb.h
+++ b/comm_usb.h
@@ -24,5 +24,6 @@
 
 // Functions
 void comm_usb_init(void);
+void comm_usb_send_packet(unsigned char *data, unsigned int len);
 
 #endif /* COMM_USB_H_ */
diff --git a/commands.c b/commands.c
index b14b8bf83..46a85ab92 100644
--- a/commands.c
+++ b/commands.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -38,6 +38,7 @@
 #include "packet.h"
 #include "encoder.h"
 #include "nrf_driver.h"
+#include "gpdrive.h"
 
 #include <math.h>
 #include <string.h>
@@ -51,16 +52,13 @@ static thread_t *detect_tp;
 
 // Private variables
 static uint8_t send_buffer[PACKET_MAX_PL_LEN];
-static float detect_cycle_int_limit;
-static float detect_coupling_k;
-static float detect_current;
-static float detect_min_rpm;
-static float detect_low_duty;
-static int8_t detect_hall_table[8];
-static int detect_hall_res;
-static void(*send_func)(unsigned char *data, unsigned int len) = 0;
-static void(*send_func_last)(unsigned char *data, unsigned int len) = 0;
-static void(*appdata_func)(unsigned char *data, unsigned int len) = 0;
+static uint8_t detect_thread_cmd_buffer[50];
+static volatile bool is_detecting = false;
+static void(* volatile send_func)(unsigned char *data, unsigned int len) = 0;
+static void(* volatile send_func_detect)(unsigned char *data, unsigned int len) = 0;
+static void(* volatile send_func_last)(unsigned char *data, unsigned int len) = 0;
+static void(* volatile appdata_func)(unsigned char *data, unsigned int len) = 0;
+static void(* volatile send_func_nrf)(unsigned char *data, unsigned int len) = 0;
 static disp_pos_mode display_position_mode;
 
 void commands_init(void) {
@@ -74,6 +72,7 @@ void commands_init(void) {
  * A pointer to the packet sending function.
  */
 void commands_set_send_func(void(*func)(unsigned char *data, unsigned int len)) {
+	send_func_last = send_func;
 	send_func = func;
 }
 
@@ -92,6 +91,26 @@ void commands_send_packet(unsigned char *data, unsigned int len) {
 	}
 }
 
+/**
+ * Send a packet using the set NRF51 send function. The NRF51 send function
+ * is set when the COMM_EXT_NRF_PRESENT and COMM_EXT_NRF_ESB_RX_DATA commands
+ * are received, at which point the previous send function is restored. The
+ * intention behind that is to make the NRF51-related communication only with
+ * the interface that has an NRF51, and prevent the NRF51 communication from
+ * interfering with other communication.
+ *
+ * @param data
+ * The packet data.
+ *
+ * @param len
+ * The data length.
+ */
+void commands_send_packet_nrf(unsigned char *data, unsigned int len) {
+	if (send_func_nrf) {
+		send_func_nrf(data, len);
+	}
+}
+
 /**
  * Process a received buffer with commands and data.
  *
@@ -108,11 +127,8 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 
 	COMM_PACKET_ID packet_id;
 	int32_t ind = 0;
-	static mc_configuration mcconf, mcconf_old; // Static to save some stack space
-	app_configuration appconf;
-	uint16_t flash_res;
-	uint32_t new_app_offset;
-	chuck_data chuck_d_tmp;
+	static mc_configuration mcconf; // Static to save some stack space
+	static app_configuration appconf;
 
 	packet_id = data[0];
 	data++;
@@ -125,65 +141,149 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		send_buffer[ind++] = FW_VERSION_MAJOR;
 		send_buffer[ind++] = FW_VERSION_MINOR;
 
-#ifdef HW_NAME
 		strcpy((char*)(send_buffer + ind), HW_NAME);
 		ind += strlen(HW_NAME) + 1;
 
 		memcpy(send_buffer + ind, STM32_UUID_8, 12);
 		ind += 12;
-#endif
+
+		send_buffer[ind++] = app_get_configuration()->pairing_done;
 
 		commands_send_packet(send_buffer, ind);
 		break;
 
+	case COMM_JUMP_TO_BOOTLOADER_ALL_CAN:
+		data[-1] = COMM_JUMP_TO_BOOTLOADER;
+		comm_can_send_buffer(255, data - 1, len + 1, 2);
+		chThdSleepMilliseconds(100);
+		/* Falls through. */
+		/* no break */
 	case COMM_JUMP_TO_BOOTLOADER:
 		flash_helper_jump_to_bootloader();
 		break;
 
-	case COMM_ERASE_NEW_APP:
+	case COMM_ERASE_NEW_APP_ALL_CAN:
+		if (nrf_driver_ext_nrf_running()) {
+			nrf_driver_pause(6000);
+		}
+
+		data[-1] = COMM_ERASE_NEW_APP;
+		comm_can_send_buffer(255, data - 1, len + 1, 2);
+		chThdSleepMilliseconds(1500);
+		/* Falls through. */
+		/* no break */
+	case COMM_ERASE_NEW_APP: {
 		ind = 0;
-		flash_res = flash_helper_erase_new_app(buffer_get_uint32(data, &ind));
+
+		if (nrf_driver_ext_nrf_running()) {
+			nrf_driver_pause(6000);
+		}
+		uint16_t flash_res = flash_helper_erase_new_app(buffer_get_uint32(data, &ind));
 
 		ind = 0;
 		send_buffer[ind++] = COMM_ERASE_NEW_APP;
 		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
 		commands_send_packet(send_buffer, ind);
-		break;
+	} break;
+
+	case COMM_WRITE_NEW_APP_DATA_ALL_CAN:
+		if (nrf_driver_ext_nrf_running()) {
+			nrf_driver_pause(2000);
+		}
 
-	case COMM_WRITE_NEW_APP_DATA:
+		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: {
 		ind = 0;
-		new_app_offset = buffer_get_uint32(data, &ind);
-		flash_res = flash_helper_write_new_app_data(new_app_offset, data + ind, len - ind);
+		uint32_t new_app_offset = buffer_get_uint32(data, &ind);
+
+		if (nrf_driver_ext_nrf_running()) {
+			nrf_driver_pause(2000);
+		}
+		uint16_t flash_res = flash_helper_write_new_app_data(new_app_offset, data + ind, len - ind);
 
 		ind = 0;
 		send_buffer[ind++] = COMM_WRITE_NEW_APP_DATA;
 		send_buffer[ind++] = flash_res == FLASH_COMPLETE ? 1 : 0;
 		commands_send_packet(send_buffer, ind);
-		break;
+	} break;
 
 	case COMM_GET_VALUES:
+	case COMM_GET_VALUES_SELECTIVE: {
 		ind = 0;
-		send_buffer[ind++] = COMM_GET_VALUES;
-		buffer_append_float16(send_buffer, mc_interface_temp_fet_filtered(), 1e1, &ind);
-		buffer_append_float16(send_buffer, mc_interface_temp_motor_filtered(), 1e1, &ind);
-		buffer_append_float32(send_buffer, mc_interface_read_reset_avg_motor_current(), 1e2, &ind);
-		buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
-		buffer_append_float32(send_buffer, mc_interface_read_reset_avg_id(), 1e2, &ind);
-		buffer_append_float32(send_buffer, mc_interface_read_reset_avg_iq(), 1e2, &ind);
-		buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
-		buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
-		buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
-		buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
-		buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
-		buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
-		buffer_append_float32(send_buffer, mc_interface_get_watt_hours_charged(false), 1e4, &ind);
-		buffer_append_int32(send_buffer, mc_interface_get_tachometer_value(false), &ind);
-		buffer_append_int32(send_buffer, mc_interface_get_tachometer_abs_value(false), &ind);
-		send_buffer[ind++] = mc_interface_get_fault();
-		buffer_append_float32(send_buffer, mc_interface_get_pid_pos_now(), 1e6, &ind);
-		send_buffer[ind++] = app_get_configuration()->controller_id;
+		send_buffer[ind++] = packet_id;
+
+		uint32_t mask = 0xFFFFFFFF;
+		if (packet_id == COMM_GET_VALUES_SELECTIVE) {
+			int32_t ind2 = 0;
+			mask = buffer_get_uint32(data, &ind2);
+			buffer_append_uint32(send_buffer, mask, &ind);
+		}
+
+		if (mask & ((uint32_t)1 << 0)) {
+			buffer_append_float16(send_buffer, mc_interface_temp_fet_filtered(), 1e1, &ind);
+		}
+		if (mask & ((uint32_t)1 << 1)) {
+			buffer_append_float16(send_buffer, mc_interface_temp_motor_filtered(), 1e1, &ind);
+		}
+		if (mask & ((uint32_t)1 << 2)) {
+			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_motor_current(), 1e2, &ind);
+		}
+		if (mask & ((uint32_t)1 << 3)) {
+			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_input_current(), 1e2, &ind);
+		}
+		if (mask & ((uint32_t)1 << 4)) {
+			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_id(), 1e2, &ind);
+		}
+		if (mask & ((uint32_t)1 << 5)) {
+			buffer_append_float32(send_buffer, mc_interface_read_reset_avg_iq(), 1e2, &ind);
+		}
+		if (mask & ((uint32_t)1 << 6)) {
+			buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 7)) {
+			buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
+		}
+		if (mask & ((uint32_t)1 << 8)) {
+			buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
+		}
+		if (mask & ((uint32_t)1 << 9)) {
+			buffer_append_float32(send_buffer, mc_interface_get_amp_hours(false), 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 10)) {
+			buffer_append_float32(send_buffer, mc_interface_get_amp_hours_charged(false), 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 11)) {
+			buffer_append_float32(send_buffer, mc_interface_get_watt_hours(false), 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 12)) {
+			buffer_append_float32(send_buffer, mc_interface_get_watt_hours_charged(false), 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 13)) {
+			buffer_append_int32(send_buffer, mc_interface_get_tachometer_value(false), &ind);
+		}
+		if (mask && ((uint32_t)1 << 14)) {
+			buffer_append_int32(send_buffer, mc_interface_get_tachometer_abs_value(false), &ind);
+		}
+		if (mask & ((uint32_t)1 << 15)) {
+			send_buffer[ind++] = mc_interface_get_fault();
+		}
+		if (mask & ((uint32_t)1 << 16)) {
+			buffer_append_float32(send_buffer, mc_interface_get_pid_pos_now(), 1e6, &ind);
+		}
+		if (mask & ((uint32_t)1 << 17)) {
+			send_buffer[ind++] = app_get_configuration()->controller_id;
+		}
+		if (mask & ((uint32_t)1 << 18)) {
+			buffer_append_float16(send_buffer, NTC_TEMP_MOS1(), 1e1, &ind);
+			buffer_append_float16(send_buffer, NTC_TEMP_MOS2(), 1e1, &ind);
+			buffer_append_float16(send_buffer, NTC_TEMP_MOS3(), 1e1, &ind);
+		}
+
 		commands_send_packet(send_buffer, ind);
-		break;
+	} break;
 
 	case COMM_SET_DUTY:
 		ind = 0;
@@ -278,13 +378,18 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.l_max_duty = buffer_get_float32_auto(data, &ind);
 		mcconf.l_watt_max = buffer_get_float32_auto(data, &ind);
 		mcconf.l_watt_min = buffer_get_float32_auto(data, &ind);
+		mcconf.l_current_max_scale = buffer_get_float32_auto(data, &ind);
+		mcconf.l_current_min_scale = buffer_get_float32_auto(data, &ind);
+
+		utils_truncate_number(&mcconf.l_current_max_scale , 0.0, 1.0);
+		utils_truncate_number(&mcconf.l_current_min_scale , 0.0, 1.0);
 
-		mcconf.lo_current_max = mcconf.l_current_max;
-		mcconf.lo_current_min = mcconf.l_current_min;
+		mcconf.lo_current_max = mcconf.l_current_max * mcconf.l_current_max_scale;
+		mcconf.lo_current_min = mcconf.l_current_min * mcconf.l_current_min_scale;
 		mcconf.lo_in_current_max = mcconf.l_in_current_max;
 		mcconf.lo_in_current_min = mcconf.l_in_current_min;
-		mcconf.lo_current_motor_max_now = mcconf.l_current_max;
-		mcconf.lo_current_motor_min_now = mcconf.l_current_min;
+		mcconf.lo_current_motor_max_now = mcconf.lo_current_max;
+		mcconf.lo_current_motor_min_now = mcconf.lo_current_min;
 
 		mcconf.sl_min_erpm = buffer_get_float32_auto(data, &ind);
 		mcconf.sl_min_erpm_cycle_int_limit = buffer_get_float32_auto(data, &ind);
@@ -330,6 +435,12 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.foc_temp_comp_base_temp = buffer_get_float32_auto(data, &ind);
 		mcconf.foc_current_filter_const = buffer_get_float32_auto(data, &ind);
 
+		mcconf.gpd_buffer_notify_left = buffer_get_int16(data, &ind);
+		mcconf.gpd_buffer_interpol = buffer_get_int16(data, &ind);
+		mcconf.gpd_current_filter_const = buffer_get_float32_auto(data, &ind);
+		mcconf.gpd_current_kp = buffer_get_float32_auto(data, &ind);
+		mcconf.gpd_current_ki = buffer_get_float32_auto(data, &ind);
+
 		mcconf.s_pid_kp = buffer_get_float32_auto(data, &ind);
 		mcconf.s_pid_ki = buffer_get_float32_auto(data, &ind);
 		mcconf.s_pid_kd = buffer_get_float32_auto(data, &ind);
@@ -362,39 +473,14 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		mcconf.m_ntc_motor_beta = buffer_get_float32_auto(data, &ind);
 		mcconf.m_out_aux_mode = data[ind++];
 
-		// Apply limits if they are defined
-#ifndef DISABLE_HW_LIMITS
-#ifdef HW_LIM_CURRENT
-		utils_truncate_number(&mcconf.l_current_max, HW_LIM_CURRENT);
-		utils_truncate_number(&mcconf.l_current_min, HW_LIM_CURRENT);
-#endif
-#ifdef HW_LIM_CURRENT_IN
-		utils_truncate_number(&mcconf.l_in_current_max, HW_LIM_CURRENT_IN);
-		utils_truncate_number(&mcconf.l_in_current_min, HW_LIM_CURRENT);
-#endif
-#ifdef HW_LIM_CURRENT_ABS
-		utils_truncate_number(&mcconf.l_abs_current_max, HW_LIM_CURRENT_ABS);
-#endif
-#ifdef HW_LIM_VIN
-		utils_truncate_number(&mcconf.l_max_vin, HW_LIM_VIN);
-		utils_truncate_number(&mcconf.l_min_vin, HW_LIM_VIN);
-#endif
-#ifdef HW_LIM_ERPM
-		utils_truncate_number(&mcconf.l_max_erpm, HW_LIM_ERPM);
-		utils_truncate_number(&mcconf.l_min_erpm, HW_LIM_ERPM);
-#endif
-#ifdef HW_LIM_DUTY_MIN
-		utils_truncate_number(&mcconf.l_min_duty, HW_LIM_DUTY_MIN);
-#endif
-#ifdef HW_LIM_DUTY_MAX
-		utils_truncate_number(&mcconf.l_max_duty, HW_LIM_DUTY_MAX);
-#endif
-#ifdef HW_LIM_TEMP_FET
-		utils_truncate_number(&mcconf.l_temp_fet_start, HW_LIM_TEMP_FET);
-		utils_truncate_number(&mcconf.l_temp_fet_end, HW_LIM_TEMP_FET);
-#endif
-#endif
+		mcconf.si_motor_poles = data[ind++];
+		mcconf.si_gear_ratio = buffer_get_float32_auto(data, &ind);
+		mcconf.si_wheel_diameter = buffer_get_float32_auto(data, &ind);
+		mcconf.si_battery_type = data[ind++];
+		mcconf.si_battery_cells = data[ind++];
+		mcconf.si_battery_ah = buffer_get_float32_auto(data, &ind);
 
+		commands_apply_mcconf_hw_limits(&mcconf);
 		conf_general_store_mc_configuration(&mcconf);
 		mc_interface_set_configuration(&mcconf);
 		chThdSleepMilliseconds(200);
@@ -412,116 +498,7 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 			conf_general_get_default_mc_configuration(&mcconf);
 		}
 
-		ind = 0;
-		send_buffer[ind++] = packet_id;
-
-		send_buffer[ind++] = mcconf.pwm_mode;
-		send_buffer[ind++] = mcconf.comm_mode;
-		send_buffer[ind++] = mcconf.motor_type;
-		send_buffer[ind++] = mcconf.sensor_mode;
-
-		buffer_append_float32_auto(send_buffer, mcconf.l_current_max, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_current_min, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_in_current_max, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_in_current_min, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_abs_current_max, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_min_erpm, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_max_erpm, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_erpm_start, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_max_erpm_fbrake, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_max_erpm_fbrake_cc, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_min_vin, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_max_vin, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_battery_cut_start, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_battery_cut_end, &ind);
-		send_buffer[ind++] = mcconf.l_slow_abs_current;
-		buffer_append_float32_auto(send_buffer, mcconf.l_temp_fet_start, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_temp_fet_end, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_temp_motor_start, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_temp_motor_end, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_temp_accel_dec, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_min_duty, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_max_duty, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_watt_max, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.l_watt_min, &ind);
-
-		buffer_append_float32_auto(send_buffer, mcconf.sl_min_erpm, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.sl_min_erpm_cycle_int_limit, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.sl_max_fullbreak_current_dir_change, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.sl_cycle_int_limit, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.sl_phase_advance_at_br, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.sl_cycle_int_rpm_br, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.sl_bemf_coupling_k, &ind);
-
-		memcpy(send_buffer + ind, mcconf.hall_table, 8);
-		ind += 8;
-		buffer_append_float32_auto(send_buffer, mcconf.hall_sl_erpm, &ind);
-
-		buffer_append_float32_auto(send_buffer, mcconf.foc_current_kp, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_current_ki, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_f_sw, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_dt_us, &ind);
-		send_buffer[ind++] = mcconf.foc_encoder_inverted;
-		buffer_append_float32_auto(send_buffer, mcconf.foc_encoder_offset, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_encoder_ratio, &ind);
-		send_buffer[ind++] = mcconf.foc_sensor_mode;
-		buffer_append_float32_auto(send_buffer, mcconf.foc_pll_kp, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_pll_ki, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_motor_l, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_motor_r, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_motor_flux_linkage, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_observer_gain, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_observer_gain_slow, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_duty_dowmramp_kp, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_duty_dowmramp_ki, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_openloop_rpm, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_sl_openloop_hyst, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_sl_openloop_time, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_sl_d_current_duty, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_sl_d_current_factor, &ind);
-		memcpy(send_buffer + ind, mcconf.foc_hall_table, 8);
-		ind += 8;
-		buffer_append_float32_auto(send_buffer, mcconf.foc_sl_erpm, &ind);
-		send_buffer[ind++] = mcconf.foc_sample_v0_v7;
-		send_buffer[ind++] = mcconf.foc_sample_high_current;
-		buffer_append_float32_auto(send_buffer, mcconf.foc_sat_comp, &ind);
-		send_buffer[ind++] = mcconf.foc_temp_comp;
-		buffer_append_float32_auto(send_buffer, mcconf.foc_temp_comp_base_temp, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.foc_current_filter_const, &ind);
-
-		buffer_append_float32_auto(send_buffer, mcconf.s_pid_kp, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.s_pid_ki, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.s_pid_kd, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.s_pid_kd_filter, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.s_pid_min_erpm, &ind);
-		send_buffer[ind++] = mcconf.s_pid_allow_braking;
-
-		buffer_append_float32_auto(send_buffer, mcconf.p_pid_kp, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.p_pid_ki, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.p_pid_kd, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.p_pid_kd_filter, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.p_pid_ang_div, &ind);
-
-		buffer_append_float32_auto(send_buffer, mcconf.cc_startup_boost_duty, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.cc_min_current, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.cc_gain, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.cc_ramp_step_max, &ind);
-
-		buffer_append_int32(send_buffer, mcconf.m_fault_stop_time_ms, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.m_duty_ramp_step, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.m_current_backoff_gain, &ind);
-		buffer_append_uint32(send_buffer, mcconf.m_encoder_counts, &ind);
-		send_buffer[ind++] = mcconf.m_sensor_port_mode;
-		send_buffer[ind++] = mcconf.m_invert_direction;
-		send_buffer[ind++] = mcconf.m_drv8301_oc_mode;
-		send_buffer[ind++] = mcconf.m_drv8301_oc_adj;
-		buffer_append_float32_auto(send_buffer, mcconf.m_bldc_f_sw_min, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.m_bldc_f_sw_max, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.m_dc_f_sw, &ind);
-		buffer_append_float32_auto(send_buffer, mcconf.m_ntc_motor_beta, &ind);
-		send_buffer[ind++] = mcconf.m_out_aux_mode;
-
-		commands_send_packet(send_buffer, ind);
+		commands_send_mcconf(packet_id, &mcconf);
 		break;
 
 	case COMM_SET_APPCONF:
@@ -534,6 +511,10 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		appconf.send_can_status = data[ind++];
 		appconf.send_can_status_rate_hz = buffer_get_uint16(data, &ind);
 		appconf.can_baud_rate = data[ind++];
+		appconf.pairing_done = data[ind++];
+
+		appconf.uavcan_enable = data[ind++];
+		appconf.uavcan_esc_index = data[ind++];
 
 		appconf.app_to_use = data[ind++];
 
@@ -640,157 +621,19 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		break;
 
 	case COMM_DETECT_MOTOR_PARAM:
-		ind = 0;
-		detect_current = buffer_get_float32(data, 1e3, &ind);
-		detect_min_rpm = buffer_get_float32(data, 1e3, &ind);
-		detect_low_duty = buffer_get_float32(data, 1e3, &ind);
-
-		send_func_last = send_func;
-
-		chEvtSignal(detect_tp, (eventmask_t) 1);
-		break;
-
-	case COMM_DETECT_MOTOR_R_L: {
-		mcconf = *mc_interface_get_configuration();
-		mcconf_old = mcconf;
-
-		send_func_last = send_func;
-
-		mcconf.motor_type = MOTOR_TYPE_FOC;
-		mc_interface_set_configuration(&mcconf);
-
-		float r = 0.0;
-		float l = 0.0;
-		bool res = mcpwm_foc_measure_res_ind(&r, &l);
-		mc_interface_set_configuration(&mcconf_old);
-
-		if (!res) {
-			r = 0.0;
-			l = 0.0;
+	case COMM_DETECT_MOTOR_R_L:
+	case COMM_DETECT_MOTOR_FLUX_LINKAGE:
+	case COMM_DETECT_ENCODER:
+	case COMM_DETECT_HALL_FOC:
+	case COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP:
+	case COMM_DETECT_APPLY_ALL_FOC:
+		if (!is_detecting && len < sizeof(detect_thread_cmd_buffer)) {
+			memcpy(detect_thread_cmd_buffer, data - 1, len + 1);
+			is_detecting = true;
+			send_func_detect = send_func;
+			chEvtSignal(detect_tp, (eventmask_t)1);
 		}
-
-		ind = 0;
-		send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
-		buffer_append_float32(send_buffer, r, 1e6, &ind);
-		buffer_append_float32(send_buffer, l, 1e3, &ind);
-		if (send_func_last) {
-			send_func_last(send_buffer, ind);
-		} else {
-			commands_send_packet(send_buffer, ind);
-		}
-	}
-	break;
-
-	case COMM_DETECT_MOTOR_FLUX_LINKAGE: {
-		ind = 0;
-		float current = buffer_get_float32(data, 1e3, &ind);
-		float min_rpm = buffer_get_float32(data, 1e3, &ind);
-		float duty = buffer_get_float32(data, 1e3, &ind);
-		float resistance = buffer_get_float32(data, 1e6, &ind);
-
-		send_func_last = send_func;
-
-		float linkage;
-		bool res = conf_general_measure_flux_linkage(current, duty, min_rpm, resistance, &linkage);
-
-		if (!res) {
-			linkage = 0.0;
-		}
-
-		ind = 0;
-		send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE;
-		buffer_append_float32(send_buffer, linkage, 1e7, &ind);
-		if (send_func_last) {
-			send_func_last(send_buffer, ind);
-		} else {
-			commands_send_packet(send_buffer, ind);
-		}
-	}
-	break;
-
-	case COMM_DETECT_ENCODER: {
-		if (encoder_is_configured()) {
-			mcconf = *mc_interface_get_configuration();
-			mcconf_old = mcconf;
-
-			send_func_last = send_func;
-
-			ind = 0;
-			float current = buffer_get_float32(data, 1e3, &ind);
-
-			mcconf.motor_type = MOTOR_TYPE_FOC;
-			mcconf.foc_f_sw = 10000.0;
-			mcconf.foc_current_kp = 0.01;
-			mcconf.foc_current_ki = 10.0;
-			mc_interface_set_configuration(&mcconf);
-
-			float offset = 0.0;
-			float ratio = 0.0;
-			bool inverted = false;
-			mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
-			mc_interface_set_configuration(&mcconf_old);
-
-			ind = 0;
-			send_buffer[ind++] = COMM_DETECT_ENCODER;
-			buffer_append_float32(send_buffer, offset, 1e6, &ind);
-			buffer_append_float32(send_buffer, ratio, 1e6, &ind);
-			send_buffer[ind++] = inverted;
-			if (send_func_last) {
-				send_func_last(send_buffer, ind);
-			} else {
-				commands_send_packet(send_buffer, ind);
-			}
-		} else {
-			ind = 0;
-			send_buffer[ind++] = COMM_DETECT_ENCODER;
-			buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
-			buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
-			send_buffer[ind++] = false;
-			commands_send_packet(send_buffer, ind);
-		}
-	}
-	break;
-
-	case COMM_DETECT_HALL_FOC: {
-		mcconf = *mc_interface_get_configuration();
-
-		if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
-			mcconf_old = mcconf;
-			ind = 0;
-			float current = buffer_get_float32(data, 1e3, &ind);
-
-			send_func_last = send_func;
-
-			mcconf.motor_type = MOTOR_TYPE_FOC;
-			mcconf.foc_f_sw = 10000.0;
-			mcconf.foc_current_kp = 0.01;
-			mcconf.foc_current_ki = 10.0;
-			mc_interface_set_configuration(&mcconf);
-
-			uint8_t hall_tab[8];
-			bool res = mcpwm_foc_hall_detect(current, hall_tab);
-			mc_interface_set_configuration(&mcconf_old);
-
-			ind = 0;
-			send_buffer[ind++] = COMM_DETECT_HALL_FOC;
-			memcpy(send_buffer + ind, hall_tab, 8);
-			ind += 8;
-			send_buffer[ind++] = res ? 0 : 1;
-
-			if (send_func_last) {
-				send_func_last(send_buffer, ind);
-			} else {
-				commands_send_packet(send_buffer, ind);
-			}
-		} else {
-			ind = 0;
-			send_buffer[ind++] = COMM_DETECT_HALL_FOC;
-			memset(send_buffer, 255, 8);
-			ind += 8;
-			send_buffer[ind++] = 0;
-		}
-	}
-	break;
+		break;
 
 	case COMM_REBOOT:
 		// Lock the system and enter an infinite loop. The watchdog will reboot.
@@ -828,10 +671,12 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		break;
 
 	case COMM_FORWARD_CAN:
-		comm_can_send_buffer(data[0], data + 1, len - 1, false);
+		comm_can_send_buffer(data[0], data + 1, len - 1, 0);
 		break;
 
-	case COMM_SET_CHUCK_DATA:
+	case COMM_SET_CHUCK_DATA: {
+		chuck_data chuck_d_tmp;
+
 		ind = 0;
 		chuck_d_tmp.js_x = data[ind++];
 		chuck_d_tmp.js_y = data[ind++];
@@ -840,8 +685,16 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		chuck_d_tmp.acc_x = buffer_get_int16(data, &ind);
 		chuck_d_tmp.acc_y = buffer_get_int16(data, &ind);
 		chuck_d_tmp.acc_z = buffer_get_int16(data, &ind);
+
+		if (len >= (unsigned int)ind + 2) {
+			chuck_d_tmp.rev_has_state = data[ind++];
+			chuck_d_tmp.is_rev = data[ind++];
+		} else {
+			chuck_d_tmp.rev_has_state = false;
+			chuck_d_tmp.is_rev = false;
+		}
 		app_nunchuk_update_output(&chuck_d_tmp);
-		break;
+	} break;
 
 	case COMM_CUSTOM_APP_DATA:
 		if (appdata_func) {
@@ -859,6 +712,320 @@ void commands_process_packet(unsigned char *data, unsigned int len) {
 		commands_send_packet(send_buffer, ind);
 		break;
 
+	case COMM_GPD_SET_FSW:
+		timeout_reset();
+		ind = 0;
+		gpdrive_set_switching_frequency((float)buffer_get_int32(data, &ind));
+		break;
+
+	case COMM_GPD_BUFFER_SIZE_LEFT:
+		ind = 0;
+		send_buffer[ind++] = COMM_GPD_BUFFER_SIZE_LEFT;
+		buffer_append_int32(send_buffer, gpdrive_buffer_size_left(), &ind);
+		commands_send_packet(send_buffer, ind);
+		break;
+
+	case COMM_GPD_FILL_BUFFER:
+		timeout_reset();
+		ind = 0;
+		while (ind < (int)len) {
+			gpdrive_add_buffer_sample(buffer_get_float32_auto(data, &ind));
+		}
+		break;
+
+	case COMM_GPD_OUTPUT_SAMPLE:
+		timeout_reset();
+		ind = 0;
+		gpdrive_add_buffer_sample(buffer_get_float32_auto(data, &ind));
+		break;
+
+	case COMM_GPD_SET_MODE:
+		timeout_reset();
+		ind = 0;
+		gpdrive_set_mode(data[ind++]);
+		break;
+
+	case COMM_GPD_FILL_BUFFER_INT8:
+		timeout_reset();
+		ind = 0;
+		while (ind < (int)len) {
+			gpdrive_add_buffer_sample_int((int8_t)data[ind++]);
+		}
+		break;
+
+	case COMM_GPD_FILL_BUFFER_INT16:
+		timeout_reset();
+		ind = 0;
+		while (ind < (int)len) {
+			gpdrive_add_buffer_sample_int(buffer_get_int16(data, &ind));
+		}
+		break;
+
+	case COMM_GPD_SET_BUFFER_INT_SCALE:
+		ind = 0;
+		gpdrive_set_buffer_int_scale(buffer_get_float32_auto(data, &ind));
+		break;
+
+	case COMM_GET_VALUES_SETUP:
+	case COMM_GET_VALUES_SETUP_SELECTIVE: {
+		float ah_tot = 0.0;
+		float ah_charge_tot = 0.0;
+		float wh_tot = 0.0;
+		float wh_charge_tot = 0.0;
+		float current_tot = 0.0;
+		float current_in_tot = 0.0;
+		uint8_t num_vescs = 1;
+
+		ah_tot += mc_interface_get_amp_hours(false);
+		ah_charge_tot += mc_interface_get_amp_hours_charged(false);
+		wh_tot += mc_interface_get_watt_hours(false);
+		wh_charge_tot += mc_interface_get_watt_hours_charged(false);
+		current_tot += mc_interface_get_tot_current_filtered();
+		current_in_tot += mc_interface_get_tot_current_in_filtered();
+
+		for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+			can_status_msg *msg = comm_can_get_status_msg_index(i);
+			if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
+				current_tot += msg->current;
+				num_vescs++;
+			}
+
+			can_status_msg_2 *msg2 = comm_can_get_status_msg_2_index(i);
+			if (msg2->id >= 0 && UTILS_AGE_S(msg2->rx_time) < 0.1) {
+				ah_tot += msg2->amp_hours;
+				ah_charge_tot += msg2->amp_hours_charged;
+			}
+
+			can_status_msg_3 *msg3 = comm_can_get_status_msg_3_index(i);
+			if (msg3->id >= 0 && UTILS_AGE_S(msg3->rx_time) < 0.1) {
+				wh_tot += msg3->watt_hours;
+				wh_charge_tot += msg3->watt_hours_charged;
+			}
+
+			can_status_msg_4 *msg4 = comm_can_get_status_msg_4_index(i);
+			if (msg4->id >= 0 && UTILS_AGE_S(msg4->rx_time) < 0.1) {
+				current_in_tot += msg4->current_in;
+			}
+		}
+
+		float wh_batt_left = 0.0;
+		float battery_level = mc_interface_get_battery_level(&wh_batt_left);
+
+		ind = 0;
+		send_buffer[ind++] = packet_id;
+
+		uint32_t mask = 0xFFFFFFFF;
+		if (packet_id == COMM_GET_VALUES_SETUP_SELECTIVE) {
+			int32_t ind2 = 0;
+			mask = buffer_get_uint32(data, &ind2);
+			buffer_append_uint32(send_buffer, mask, &ind);
+		}
+
+		if (mask & ((uint32_t)1 << 0)) {
+			buffer_append_float16(send_buffer, mc_interface_temp_fet_filtered(), 1e1, &ind);
+		}
+		if (mask & ((uint32_t)1 << 1)) {
+			buffer_append_float16(send_buffer, mc_interface_temp_motor_filtered(), 1e1, &ind);
+		}
+		if (mask & ((uint32_t)1 << 2)) {
+			buffer_append_float32(send_buffer, current_tot, 1e2, &ind);
+		}
+		if (mask & ((uint32_t)1 << 3)) {
+			buffer_append_float32(send_buffer, current_in_tot, 1e2, &ind);
+		}
+		if (mask & ((uint32_t)1 << 4)) {
+			buffer_append_float16(send_buffer, mc_interface_get_duty_cycle_now(), 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 5)) {
+			buffer_append_float32(send_buffer, mc_interface_get_rpm(), 1e0, &ind);
+		}
+		if (mask & ((uint32_t)1 << 6)) {
+			buffer_append_float32(send_buffer, mc_interface_get_speed(), 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 7)) {
+			buffer_append_float16(send_buffer, GET_INPUT_VOLTAGE(), 1e1, &ind);
+		}
+		if (mask & ((uint32_t)1 << 8)) {
+			buffer_append_float16(send_buffer, battery_level, 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 9)) {
+			buffer_append_float32(send_buffer, ah_tot, 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 10)) {
+			buffer_append_float32(send_buffer, ah_charge_tot, 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 11)) {
+			buffer_append_float32(send_buffer, wh_tot, 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 12)) {
+			buffer_append_float32(send_buffer, wh_charge_tot, 1e4, &ind);
+		}
+		if (mask & ((uint32_t)1 << 13)) {
+			buffer_append_float32(send_buffer, mc_interface_get_distance(), 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 14)) {
+			buffer_append_float32(send_buffer, mc_interface_get_distance_abs(), 1e3, &ind);
+		}
+		if (mask & ((uint32_t)1 << 15)) {
+			buffer_append_float32(send_buffer, mc_interface_get_pid_pos_now(), 1e6, &ind);
+		}
+		if (mask & ((uint32_t)1 << 16)) {
+			send_buffer[ind++] = mc_interface_get_fault();
+		}
+		if (mask & ((uint32_t)1 << 17)) {
+			send_buffer[ind++] = app_get_configuration()->controller_id;
+		}
+		if (mask & ((uint32_t)1 << 18)) {
+			send_buffer[ind++] = num_vescs;
+		}
+		if (mask & ((uint32_t)1 << 19)) {
+			buffer_append_float32(send_buffer, wh_batt_left, 1e3, &ind);
+		}
+
+		commands_send_packet(send_buffer, ind);
+	} break;
+
+	case COMM_SET_MCCONF_TEMP:
+	case COMM_SET_MCCONF_TEMP_SETUP: {
+		mcconf = *mc_interface_get_configuration();
+
+		ind = 0;
+		bool store = data[ind++];
+		bool forward_can = data[ind++];
+		bool ack = data[ind++];
+		bool divide_by_controllers = data[ind++];
+
+		float controller_num = 1.0;
+
+		if (divide_by_controllers) {
+			for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+				can_status_msg *msg = comm_can_get_status_msg_index(i);
+				if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
+					controller_num += 1.0;
+				}
+			}
+		}
+
+		mcconf.l_current_min_scale = buffer_get_float32_auto(data, &ind);
+		mcconf.l_current_max_scale = buffer_get_float32_auto(data, &ind);
+
+		if (packet_id == COMM_SET_MCCONF_TEMP_SETUP) {
+			const float fact = ((mcconf.si_motor_poles / 2.0) * 60.0 *
+					mcconf.si_gear_ratio) / (mcconf.si_wheel_diameter * M_PI);
+
+			mcconf.l_min_erpm = buffer_get_float32_auto(data, &ind) * fact;
+			mcconf.l_max_erpm = buffer_get_float32_auto(data, &ind) * fact;
+
+			// Write computed RPM back and change forwarded packet id to
+			// COMM_SET_MCCONF_TEMP. This way only the master has to be
+			// aware of the setup information.
+			ind -= 8;
+			buffer_append_float32_auto(data, mcconf.l_min_erpm, &ind);
+			buffer_append_float32_auto(data, mcconf.l_max_erpm, &ind);
+		} else {
+			mcconf.l_min_erpm = buffer_get_float32_auto(data, &ind);
+			mcconf.l_max_erpm = buffer_get_float32_auto(data, &ind);
+		}
+
+		mcconf.l_min_duty = buffer_get_float32_auto(data, &ind);
+		mcconf.l_max_duty = buffer_get_float32_auto(data, &ind);
+		mcconf.l_watt_min = buffer_get_float32_auto(data, &ind) / controller_num;
+		mcconf.l_watt_max = buffer_get_float32_auto(data, &ind) / controller_num;
+
+		// Write divided data back to the buffer, as the other controllers have no way to tell
+		// how many controllers are on the bus and thus need pre-divided data.
+		// We set divide by controllers to false before forwarding.
+		ind -= 8;
+		buffer_append_float32_auto(data, mcconf.l_watt_min, &ind);
+		buffer_append_float32_auto(data, mcconf.l_watt_max, &ind);
+
+		mcconf.lo_current_min = mcconf.l_current_min * mcconf.l_current_min_scale;
+		mcconf.lo_current_max = mcconf.l_current_max * mcconf.l_current_max_scale;
+		mcconf.lo_current_motor_min_now = mcconf.lo_current_min;
+		mcconf.lo_current_motor_max_now = mcconf.lo_current_max;
+
+		commands_apply_mcconf_hw_limits(&mcconf);
+
+		if (store) {
+			conf_general_store_mc_configuration(&mcconf);
+		}
+
+		mc_interface_set_configuration(&mcconf);
+
+		if (forward_can) {
+			data[-1] = COMM_SET_MCCONF_TEMP;
+			data[1] = 0; // No more forward
+			data[2] = 0; // No ack
+			data[3] = 0; // No dividing, see comment above
+
+			// TODO: Maybe broadcast on CAN-bus?
+			for (int i = 0;i < CAN_STATUS_MSGS_TO_STORE;i++) {
+				can_status_msg *msg = comm_can_get_status_msg_index(i);
+				if (msg->id >= 0 && UTILS_AGE_S(msg->rx_time) < 0.1) {
+					comm_can_send_buffer(msg->id, data - 1, len + 1, 0);
+				}
+			}
+		}
+
+		if (ack) {
+			ind = 0;
+			send_buffer[ind++] = packet_id;
+			commands_send_packet(send_buffer, ind);
+		}
+	} break;
+
+	case COMM_EXT_NRF_PRESENT: {
+		send_func_nrf = send_func;
+		send_func = send_func_last;
+
+		if (!conf_general_permanent_nrf_found) {
+			nrf_driver_init_ext_nrf();
+			if (!nrf_driver_is_pairing()) {
+				const app_configuration *appconf_ptr = app_get_configuration();
+				send_buffer[0] = COMM_EXT_NRF_ESB_SET_CH_ADDR;
+				send_buffer[1] = appconf_ptr->app_nrf_conf.channel;
+				send_buffer[2] = appconf_ptr->app_nrf_conf.address[0];
+				send_buffer[3] = appconf_ptr->app_nrf_conf.address[1];
+				send_buffer[4] = appconf_ptr->app_nrf_conf.address[2];
+				commands_send_packet_nrf(send_buffer, 5);
+			}
+		}
+	} break;
+
+	case COMM_EXT_NRF_ESB_RX_DATA: {
+		send_func_nrf = send_func;
+		send_func = send_func_last;
+		nrf_driver_process_packet(data, len);
+	} break;
+
+	case COMM_PING_CAN:
+		ind = 0;
+		// Send buffer cannot be used, as it might be altered while waiting
+		// for pings.
+		uint8_t buffer[256];
+		buffer[ind++] = COMM_PING_CAN;
+
+		for (uint8_t i = 0;i < 255;i++) {
+			if (comm_can_ping(i)) {
+				buffer[ind++] = i;
+			}
+		}
+
+		commands_send_packet(buffer, ind);
+		break;
+
+	case COMM_APP_DISABLE_OUTPUT: {
+		ind = 0;
+		bool fwd_can = data[ind++];
+		int time = buffer_get_int32(data, &ind);
+		app_disable_output(time);
+
+		if (fwd_can) {
+			data[0] = 0; // Don't continue forwarding
+			comm_can_send_buffer(255, data - 1, len + 1, 0);
+		}
+	} break;
+
 	default:
 		break;
 	}
@@ -924,6 +1091,140 @@ void commands_send_app_data(unsigned char *data, unsigned int len) {
 	commands_send_packet(send_buffer, index);
 }
 
+void commands_send_gpd_buffer_notify(void) {
+	int32_t index = 0;
+	send_buffer[index++] = COMM_GPD_BUFFER_NOTIFY;
+	commands_send_packet(send_buffer, index);
+}
+
+void commands_send_mcconf(COMM_PACKET_ID packet_id, mc_configuration *mcconf) {
+	int32_t ind = 0;
+	send_buffer[ind++] = packet_id;
+
+	send_buffer[ind++] = mcconf->pwm_mode;
+	send_buffer[ind++] = mcconf->comm_mode;
+	send_buffer[ind++] = mcconf->motor_type;
+	send_buffer[ind++] = mcconf->sensor_mode;
+
+	buffer_append_float32_auto(send_buffer, mcconf->l_current_max, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_current_min, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_in_current_max, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_in_current_min, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_abs_current_max, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_min_erpm, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_max_erpm, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_erpm_start, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_max_erpm_fbrake, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_max_erpm_fbrake_cc, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_min_vin, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_max_vin, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_battery_cut_start, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_battery_cut_end, &ind);
+	send_buffer[ind++] = mcconf->l_slow_abs_current;
+	buffer_append_float32_auto(send_buffer, mcconf->l_temp_fet_start, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_temp_fet_end, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_temp_motor_start, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_temp_motor_end, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_temp_accel_dec, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_min_duty, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_max_duty, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_watt_max, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_watt_min, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_current_max_scale, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->l_current_min_scale, &ind);
+
+	buffer_append_float32_auto(send_buffer, mcconf->sl_min_erpm, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->sl_min_erpm_cycle_int_limit, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->sl_max_fullbreak_current_dir_change, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->sl_cycle_int_limit, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->sl_phase_advance_at_br, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->sl_cycle_int_rpm_br, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->sl_bemf_coupling_k, &ind);
+
+	memcpy(send_buffer + ind, mcconf->hall_table, 8);
+	ind += 8;
+	buffer_append_float32_auto(send_buffer, mcconf->hall_sl_erpm, &ind);
+
+	buffer_append_float32_auto(send_buffer, mcconf->foc_current_kp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_current_ki, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_f_sw, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_dt_us, &ind);
+	send_buffer[ind++] = mcconf->foc_encoder_inverted;
+	buffer_append_float32_auto(send_buffer, mcconf->foc_encoder_offset, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_encoder_ratio, &ind);
+	send_buffer[ind++] = mcconf->foc_sensor_mode;
+	buffer_append_float32_auto(send_buffer, mcconf->foc_pll_kp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_pll_ki, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_motor_l, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_motor_r, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_motor_flux_linkage, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_observer_gain, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_observer_gain_slow, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_duty_dowmramp_kp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_duty_dowmramp_ki, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_openloop_rpm, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_sl_openloop_hyst, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_sl_openloop_time, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_sl_d_current_duty, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_sl_d_current_factor, &ind);
+	memcpy(send_buffer + ind, mcconf->foc_hall_table, 8);
+	ind += 8;
+	buffer_append_float32_auto(send_buffer, mcconf->foc_sl_erpm, &ind);
+	send_buffer[ind++] = mcconf->foc_sample_v0_v7;
+	send_buffer[ind++] = mcconf->foc_sample_high_current;
+	buffer_append_float32_auto(send_buffer, mcconf->foc_sat_comp, &ind);
+	send_buffer[ind++] = mcconf->foc_temp_comp;
+	buffer_append_float32_auto(send_buffer, mcconf->foc_temp_comp_base_temp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->foc_current_filter_const, &ind);
+
+	buffer_append_int16(send_buffer, mcconf->gpd_buffer_notify_left, &ind);
+	buffer_append_int16(send_buffer, mcconf->gpd_buffer_interpol, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->gpd_current_filter_const, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->gpd_current_kp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->gpd_current_ki, &ind);
+
+	buffer_append_float32_auto(send_buffer, mcconf->s_pid_kp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->s_pid_ki, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->s_pid_kd, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->s_pid_kd_filter, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->s_pid_min_erpm, &ind);
+	send_buffer[ind++] = mcconf->s_pid_allow_braking;
+
+	buffer_append_float32_auto(send_buffer, mcconf->p_pid_kp, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->p_pid_ki, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->p_pid_kd, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->p_pid_kd_filter, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->p_pid_ang_div, &ind);
+
+	buffer_append_float32_auto(send_buffer, mcconf->cc_startup_boost_duty, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->cc_min_current, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->cc_gain, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->cc_ramp_step_max, &ind);
+
+	buffer_append_int32(send_buffer, mcconf->m_fault_stop_time_ms, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->m_duty_ramp_step, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->m_current_backoff_gain, &ind);
+	buffer_append_uint32(send_buffer, mcconf->m_encoder_counts, &ind);
+	send_buffer[ind++] = mcconf->m_sensor_port_mode;
+	send_buffer[ind++] = mcconf->m_invert_direction;
+	send_buffer[ind++] = mcconf->m_drv8301_oc_mode;
+	send_buffer[ind++] = mcconf->m_drv8301_oc_adj;
+	buffer_append_float32_auto(send_buffer, mcconf->m_bldc_f_sw_min, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->m_bldc_f_sw_max, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->m_dc_f_sw, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->m_ntc_motor_beta, &ind);
+	send_buffer[ind++] = mcconf->m_out_aux_mode;
+
+	send_buffer[ind++] = mcconf->si_motor_poles;
+	buffer_append_float32_auto(send_buffer, mcconf->si_gear_ratio, &ind);
+	buffer_append_float32_auto(send_buffer, mcconf->si_wheel_diameter, &ind);
+	send_buffer[ind++] = mcconf->si_battery_type;
+	send_buffer[ind++] = mcconf->si_battery_cells;
+	buffer_append_float32_auto(send_buffer, mcconf->si_battery_ah, &ind);
+
+	commands_send_packet(send_buffer, ind);
+}
+
 void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf) {
 	int32_t ind = 0;
 	send_buffer[ind++] = packet_id;
@@ -933,6 +1234,10 @@ void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf)
 	send_buffer[ind++] = appconf->send_can_status;
 	buffer_append_uint16(send_buffer, appconf->send_can_status_rate_hz, &ind);
 	send_buffer[ind++] = appconf->can_baud_rate;
+	send_buffer[ind++] = appconf->pairing_done;
+
+	send_buffer[ind++] = appconf->uavcan_enable;
+	send_buffer[ind++] = appconf->uavcan_esc_index;
 
 	send_buffer[ind++] = appconf->app_to_use;
 
@@ -1003,6 +1308,43 @@ void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf)
 	commands_send_packet(send_buffer, ind);
 }
 
+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);
+
+#ifndef DISABLE_HW_LIMITS
+#ifdef HW_LIM_CURRENT
+	utils_truncate_number(&mcconf->l_current_max, HW_LIM_CURRENT);
+	utils_truncate_number(&mcconf->l_current_min, HW_LIM_CURRENT);
+#endif
+#ifdef HW_LIM_CURRENT_IN
+	utils_truncate_number(&mcconf->l_in_current_max, HW_LIM_CURRENT_IN);
+	utils_truncate_number(&mcconf->l_in_current_min, HW_LIM_CURRENT);
+#endif
+#ifdef HW_LIM_CURRENT_ABS
+	utils_truncate_number(&mcconf->l_abs_current_max, HW_LIM_CURRENT_ABS);
+#endif
+#ifdef HW_LIM_VIN
+	utils_truncate_number(&mcconf->l_max_vin, HW_LIM_VIN);
+	utils_truncate_number(&mcconf->l_min_vin, HW_LIM_VIN);
+#endif
+#ifdef HW_LIM_ERPM
+	utils_truncate_number(&mcconf->l_max_erpm, HW_LIM_ERPM);
+	utils_truncate_number(&mcconf->l_min_erpm, HW_LIM_ERPM);
+#endif
+#ifdef HW_LIM_DUTY_MIN
+	utils_truncate_number(&mcconf->l_min_duty, HW_LIM_DUTY_MIN);
+#endif
+#ifdef HW_LIM_DUTY_MAX
+	utils_truncate_number(&mcconf->l_max_duty, HW_LIM_DUTY_MAX);
+#endif
+#ifdef HW_LIM_TEMP_FET
+	utils_truncate_number(&mcconf->l_temp_fet_start, HW_LIM_TEMP_FET);
+	utils_truncate_number(&mcconf->l_temp_fet_end, HW_LIM_TEMP_FET);
+#endif
+#endif
+}
+
 static THD_FUNCTION(detect_thread, arg) {
 	(void)arg;
 
@@ -1013,25 +1355,222 @@ static THD_FUNCTION(detect_thread, arg) {
 	for(;;) {
 		chEvtWaitAny((eventmask_t) 1);
 
-		if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
-				detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k,
-				detect_hall_table, &detect_hall_res)) {
-			detect_cycle_int_limit = 0.0;
-			detect_coupling_k = 0.0;
-		}
+		uint8_t *data = detect_thread_cmd_buffer;
 
+		COMM_PACKET_ID packet_id;
 		int32_t ind = 0;
-		send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
-		buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
-		buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
-		memcpy(send_buffer + ind, detect_hall_table, 8);
-		ind += 8;
-		send_buffer[ind++] = detect_hall_res;
+		static mc_configuration mcconf, mcconf_old;
+		static uint8_t send_buffer[50];
 
-		if (send_func_last) {
-			send_func_last(send_buffer, ind);
-		} else {
-			commands_send_packet(send_buffer, ind);
+		packet_id = data[0];
+		data++;
+
+		switch (packet_id) {
+		case COMM_DETECT_MOTOR_PARAM: {
+			ind = 0;
+			float detect_current = buffer_get_float32(data, 1e3, &ind);
+			float detect_min_rpm = buffer_get_float32(data, 1e3, &ind);
+			float detect_low_duty = buffer_get_float32(data, 1e3, &ind);
+			float detect_cycle_int_limit;
+			float detect_coupling_k;
+			int8_t detect_hall_table[8];
+			int detect_hall_res;
+
+			if (!conf_general_detect_motor_param(detect_current, detect_min_rpm,
+					detect_low_duty, &detect_cycle_int_limit, &detect_coupling_k,
+					detect_hall_table, &detect_hall_res)) {
+				detect_cycle_int_limit = 0.0;
+				detect_coupling_k = 0.0;
+			}
+
+			int32_t ind = 0;
+			send_buffer[ind++] = COMM_DETECT_MOTOR_PARAM;
+			buffer_append_int32(send_buffer, (int32_t)(detect_cycle_int_limit * 1000.0), &ind);
+			buffer_append_int32(send_buffer, (int32_t)(detect_coupling_k * 1000.0), &ind);
+			memcpy(send_buffer + ind, detect_hall_table, 8);
+			ind += 8;
+			send_buffer[ind++] = detect_hall_res;
+
+			if (send_func_detect) {
+				send_func_detect(send_buffer, ind);
+			}
+		} break;
+
+		case COMM_DETECT_MOTOR_R_L: {
+			mcconf = *mc_interface_get_configuration();
+			mcconf_old = mcconf;
+
+			mcconf.motor_type = MOTOR_TYPE_FOC;
+			mc_interface_set_configuration(&mcconf);
+
+			float r = 0.0;
+			float l = 0.0;
+			bool res = mcpwm_foc_measure_res_ind(&r, &l);
+			mc_interface_set_configuration(&mcconf_old);
+
+			if (!res) {
+				r = 0.0;
+				l = 0.0;
+			}
+
+			ind = 0;
+			send_buffer[ind++] = COMM_DETECT_MOTOR_R_L;
+			buffer_append_float32(send_buffer, r, 1e6, &ind);
+			buffer_append_float32(send_buffer, l, 1e3, &ind);
+			if (send_func_detect) {
+				send_func_detect(send_buffer, ind);
+			}
+		} break;
+
+		case COMM_DETECT_MOTOR_FLUX_LINKAGE: {
+			ind = 0;
+			float current = buffer_get_float32(data, 1e3, &ind);
+			float min_rpm = buffer_get_float32(data, 1e3, &ind);
+			float duty = buffer_get_float32(data, 1e3, &ind);
+			float resistance = buffer_get_float32(data, 1e6, &ind);
+
+			float linkage;
+			bool res = conf_general_measure_flux_linkage(current, duty, min_rpm, resistance, &linkage);
+
+			if (!res) {
+				linkage = 0.0;
+			}
+
+			ind = 0;
+			send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE;
+			buffer_append_float32(send_buffer, linkage, 1e7, &ind);
+			if (send_func_detect) {
+				send_func_detect(send_buffer, ind);
+			}
+		} break;
+
+		case COMM_DETECT_ENCODER: {
+			if (encoder_is_configured()) {
+				mcconf = *mc_interface_get_configuration();
+				mcconf_old = mcconf;
+
+				ind = 0;
+				float current = buffer_get_float32(data, 1e3, &ind);
+
+				mcconf.motor_type = MOTOR_TYPE_FOC;
+				mcconf.foc_f_sw = 10000.0;
+				mcconf.foc_current_kp = 0.01;
+				mcconf.foc_current_ki = 10.0;
+				mc_interface_set_configuration(&mcconf);
+
+				float offset = 0.0;
+				float ratio = 0.0;
+				bool inverted = false;
+				mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
+				mc_interface_set_configuration(&mcconf_old);
+
+				ind = 0;
+				send_buffer[ind++] = COMM_DETECT_ENCODER;
+				buffer_append_float32(send_buffer, offset, 1e6, &ind);
+				buffer_append_float32(send_buffer, ratio, 1e6, &ind);
+				send_buffer[ind++] = inverted;
+
+				if (send_func_detect) {
+					send_func_detect(send_buffer, ind);
+				}
+			} else {
+				ind = 0;
+				send_buffer[ind++] = COMM_DETECT_ENCODER;
+				buffer_append_float32(send_buffer, 1001.0, 1e6, &ind);
+				buffer_append_float32(send_buffer, 0.0, 1e6, &ind);
+				send_buffer[ind++] = false;
+
+				if (send_func_detect) {
+					send_func_detect(send_buffer, ind);
+				}
+			}
+		} break;
+
+		case COMM_DETECT_HALL_FOC: {
+			mcconf = *mc_interface_get_configuration();
+
+			if (mcconf.m_sensor_port_mode == SENSOR_PORT_MODE_HALL) {
+				mcconf_old = mcconf;
+				ind = 0;
+				float current = buffer_get_float32(data, 1e3, &ind);
+
+				mcconf.motor_type = MOTOR_TYPE_FOC;
+				mcconf.foc_f_sw = 10000.0;
+				mcconf.foc_current_kp = 0.01;
+				mcconf.foc_current_ki = 10.0;
+				mc_interface_set_configuration(&mcconf);
+
+				uint8_t hall_tab[8];
+				bool res = mcpwm_foc_hall_detect(current, hall_tab);
+				mc_interface_set_configuration(&mcconf_old);
+
+				ind = 0;
+				send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+				memcpy(send_buffer + ind, hall_tab, 8);
+				ind += 8;
+				send_buffer[ind++] = res ? 0 : 1;
+
+				if (send_func_detect) {
+					send_func_detect(send_buffer, ind);
+				}
+			} else {
+				ind = 0;
+				send_buffer[ind++] = COMM_DETECT_HALL_FOC;
+				memset(send_buffer, 255, 8);
+				ind += 8;
+				send_buffer[ind++] = 0;
+				if (send_func_detect) {
+					send_func_detect(send_buffer, ind);
+				}
+			}
+		} break;
+
+		case COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP: {
+			ind = 0;
+			float current = buffer_get_float32(data, 1e3, &ind);
+			float erpm_per_sec = buffer_get_float32(data, 1e3, &ind);
+			float duty = buffer_get_float32(data, 1e3, &ind);
+			float resistance = buffer_get_float32(data, 1e6, &ind);
+
+			float linkage;
+			bool res = conf_general_measure_flux_linkage_openloop(current, duty, erpm_per_sec, resistance, &linkage);
+
+			if (!res) {
+				linkage = 0.0;
+			}
+
+			ind = 0;
+			send_buffer[ind++] = COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP;
+			buffer_append_float32(send_buffer, linkage, 1e7, &ind);
+			if (send_func_detect) {
+				send_func_detect(send_buffer, ind);
+			}
+		} break;
+
+		case COMM_DETECT_APPLY_ALL_FOC: {
+			ind = 0;
+			bool detect_can = data[ind++];
+			float max_power_loss = buffer_get_float32(data, 1e3, &ind);
+			float min_current_in = buffer_get_float32(data, 1e3, &ind);
+			float max_current_in = buffer_get_float32(data, 1e3, &ind);
+			float openloop_rpm = buffer_get_float32(data, 1e3, &ind);
+			float sl_erpm = buffer_get_float32(data, 1e3, &ind);
+
+			int res = conf_general_detect_apply_all_foc_can(detect_can, max_power_loss,
+					min_current_in, max_current_in, openloop_rpm, sl_erpm);
+
+			ind = 0;
+			send_buffer[ind++] = COMM_DETECT_APPLY_ALL_FOC;
+			buffer_append_int16(send_buffer, res, &ind);
+			if (send_func_detect) {
+				send_func_detect(send_buffer, ind);
+			}
+		} break;
+
+		default:
+			break;
 		}
+
+		is_detecting = false;
 	}
 }
diff --git a/commands.h b/commands.h
index 448eaaa41..d252b14cb 100644
--- a/commands.h
+++ b/commands.h
@@ -26,6 +26,7 @@
 void commands_init(void);
 void commands_set_send_func(void(*func)(unsigned char *data, unsigned int len));
 void commands_send_packet(unsigned char *data, unsigned int len);
+void commands_send_packet_nrf(unsigned char *data, unsigned int len);
 void commands_process_packet(unsigned char *data, unsigned int len);
 void commands_printf(const char* format, ...);
 void commands_send_rotor_pos(float rotor_pos);
@@ -33,6 +34,9 @@ void commands_send_experiment_samples(float *samples, int len);
 disp_pos_mode commands_get_disp_pos_mode(void);
 void commands_set_app_data_handler(void(*func)(unsigned char *data, unsigned int len));
 void commands_send_app_data(unsigned char *data, unsigned int len);
+void commands_send_gpd_buffer_notify(void);
+void commands_send_mcconf(COMM_PACKET_ID packet_id, mc_configuration *mcconf);
 void commands_send_appconf(COMM_PACKET_ID packet_id, app_configuration *appconf);
+void commands_apply_mcconf_hw_limits(mc_configuration *mcconf);
 
 #endif /* COMMANDS_H_ */
diff --git a/conf_general.c b/conf_general.c
index 4385646a4..f6dac4103 100644
--- a/conf_general.c
+++ b/conf_general.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 - 2017 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -21,28 +21,21 @@
 #include "ch.h"
 #include "eeprom.h"
 #include "mcpwm.h"
+#include "mcpwm_foc.h"
 #include "mc_interface.h"
 #include "hw.h"
 #include "utils.h"
 #include "stm32f4xx_conf.h"
 #include "timeout.h"
+#include "commands.h"
+#include "encoder.h"
+#include "comm_can.h"
+#include "app.h"
 
 #include <string.h>
 #include <math.h>
 
-// User defined default motor configuration file
-#ifdef MCCONF_DEFAULT_USER
-#include MCCONF_DEFAULT_USER
-#endif
-
-// User defined default app configuration file
-#ifdef APPCONF_DEFAULT_USER
-#include APPCONF_DEFAULT_USER
-#endif
-
-// Default configuration parameters that can be overridden
-#include "mcconf_default.h"
-#include "appconf_default.h"
+#include "conf_mc_app_default.h"
 
 // EEPROM settings
 #define EEPROM_BASE_MCCONF		1000
@@ -82,12 +75,16 @@ void conf_general_init(void) {
  */
 void conf_general_get_default_app_configuration(app_configuration *conf) {
 	memset(conf, 0, sizeof(app_configuration));
-	conf->controller_id = APPCONF_CONTROLLER_ID;
+	conf->controller_id = HW_DEFAULT_ID;
 	conf->timeout_msec = APPCONF_TIMEOUT_MSEC;
 	conf->timeout_brake_current = APPCONF_TIMEOUT_BRAKE_CURRENT;
 	conf->send_can_status = APPCONF_SEND_CAN_STATUS;
 	conf->send_can_status_rate_hz = APPCONF_SEND_CAN_STATUS_RATE_HZ;
 	conf->can_baud_rate = APPCONF_CAN_BAUD_RATE;
+	conf->pairing_done = APPCONF_PAIRING_DONE;
+
+	conf->uavcan_enable = APPCONF_UAVCAN_ENABLE;
+	conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
 
 	conf->app_to_use = APPCONF_APP_TO_USE;
 
@@ -194,13 +191,15 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->l_max_duty = MCCONF_L_MAX_DUTY;
 	conf->l_watt_max = MCCONF_L_WATT_MAX;
 	conf->l_watt_min = MCCONF_L_WATT_MIN;
+	conf->l_current_max_scale = MCCONF_L_CURRENT_MAX_SCALE;
+	conf->l_current_min_scale = MCCONF_L_CURRENT_MIN_SCALE;
 
-	conf->lo_current_max = conf->l_current_max;
-	conf->lo_current_min = conf->l_current_min;
+	conf->lo_current_max = conf->l_current_max * conf->l_current_max_scale;
+	conf->lo_current_min = conf->l_current_min * conf->l_current_min_scale;
 	conf->lo_in_current_max = conf->l_in_current_max;
 	conf->lo_in_current_min = conf->l_in_current_min;
-	conf->lo_current_motor_max_now = conf->l_current_max;
-	conf->lo_current_motor_min_now = conf->l_current_min;
+	conf->lo_current_motor_max_now = conf->lo_current_max;
+	conf->lo_current_motor_min_now = conf->lo_current_min;
 
 	conf->sl_min_erpm = MCCONF_SL_MIN_RPM;
 	conf->sl_max_fullbreak_current_dir_change = MCCONF_SL_MAX_FB_CURR_DIR_CHANGE;
@@ -258,6 +257,12 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->foc_temp_comp_base_temp = MCCONF_FOC_TEMP_COMP_BASE_TEMP;
 	conf->foc_current_filter_const = MCCONF_FOC_CURRENT_FILTER_CONST;
 
+	conf->gpd_buffer_notify_left = MCCONF_GPD_BUFFER_NOTIFY_LEFT;
+	conf->gpd_buffer_interpol = MCCONF_GPD_BUFFER_INTERPOL;
+	conf->gpd_current_filter_const = MCCONF_GPD_CURRENT_FILTER_CONST;
+	conf->gpd_current_kp = MCCONF_GPD_CURRENT_KP;
+	conf->gpd_current_ki = MCCONF_GPD_CURRENT_KI;
+
 	conf->s_pid_kp = MCCONF_S_PID_KP;
 	conf->s_pid_ki = MCCONF_S_PID_KI;
 	conf->s_pid_kd = MCCONF_S_PID_KD;
@@ -289,6 +294,13 @@ void conf_general_get_default_mc_configuration(mc_configuration *conf) {
 	conf->m_dc_f_sw = MCCONF_M_DC_F_SW;
 	conf->m_ntc_motor_beta = MCCONF_M_NTC_MOTOR_BETA;
 	conf->m_out_aux_mode = MCCONF_M_OUT_AUX_MODE;
+
+	conf->si_motor_poles = MCCONF_SI_MOTOR_POLES;
+	conf->si_gear_ratio = MCCONF_SI_GEAR_RATIO;
+	conf->si_wheel_diameter = MCCONF_SI_WHEEL_DIAMETER;
+	conf->si_battery_type = MCCONF_SI_BATTERY_TYPE;
+	conf->si_battery_cells = MCCONF_SI_BATTERY_CELLS;
+	conf->si_battery_ah = MCCONF_SI_BATTERY_AH;
 }
 
 /**
@@ -666,9 +678,13 @@ bool conf_general_measure_flux_linkage(float current, float duty,
 		chThdSleepMilliseconds(10);
 	}
 
+	if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+		mc_interface_set_configuration(&mcconf_old);
+		return false;
+	}
+
 	// Wait one second for things to get ready after
-	// the fault disapears. (will fry things otherwise...)
-	// TODO: Add FAULT_INIT_NOT_DONE
+	// the fault disapears.
 	chThdSleepMilliseconds(1000);
 
 	// Disable timeout
@@ -782,3 +798,563 @@ bool conf_general_measure_flux_linkage(float current, float duty,
 
 	return true;
 }
+
+bool conf_general_measure_flux_linkage_openloop(float current, float duty,
+		float erpm_per_sec, float res, float *linkage) {
+	bool result = false;
+
+	mcconf = *mc_interface_get_configuration();
+	mcconf_old = mcconf;
+
+	mcconf.motor_type = MOTOR_TYPE_FOC;
+	mcconf.foc_sensor_mode = FOC_SENSOR_MODE_SENSORLESS;
+	mcconf.foc_current_kp = 0.0005;
+	mcconf.foc_current_ki = 1.0;
+	mc_interface_set_configuration(&mcconf);
+
+	// Wait maximum 5s for fault code to disappear
+	for (int i = 0;i < 500;i++) {
+		if (mc_interface_get_fault() == FAULT_CODE_NONE) {
+			break;
+		}
+		chThdSleepMilliseconds(10);
+	}
+
+	if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+		mc_interface_set_configuration(&mcconf_old);
+		return false;
+	}
+
+	// Wait one second for things to get ready after
+	// the fault disapears.
+	chThdSleepMilliseconds(1000);
+
+	// Disable timeout
+	systime_t tout = timeout_get_timeout_msec();
+	float tout_c = timeout_get_brake_current();
+	timeout_reset();
+	timeout_configure(60000, 0.0);
+
+	mc_interface_lock();
+
+	int cnt = 0;
+	float rpm_now = 0;
+
+	// Start by locking the motor
+	mcpwm_foc_set_openloop(current, rpm_now);
+
+	float duty_still = 0;
+	float samples = 0;
+	for (int i = 0;i < 1000;i++) {
+		duty_still += fabsf(mc_interface_get_duty_cycle_now());
+		samples += 1.0;
+		chThdSleepMilliseconds(1);
+	}
+
+	duty_still /= samples;
+	float duty_max = 0.0;
+	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;
+
+		chThdSleepMilliseconds(1);
+		cnt++;
+
+		float duty_now = fabsf(mc_interface_get_duty_cycle_now());
+
+		if (duty_now > duty_max) {
+			duty_max = duty_now;
+		}
+
+		if (cnt >= max_time) {
+			*linkage = -1.0;
+			break;
+		}
+
+		if (cnt > 4000 && duty_now < (duty_max * 0.7)) {
+			cnt = max_time;
+			*linkage = -2.0;
+			break;
+		}
+
+		if (cnt > 4000 && duty < duty_still * 1.1) {
+			cnt = max_time;
+			*linkage = -3.0;
+			break;
+		}
+
+		if (rpm_now >= 20000) {
+			break;
+		}
+	}
+
+	chThdSleepMilliseconds(1000);
+
+	if (cnt < max_time) {
+		float vq_avg = 0.0;
+		float vd_avg = 0.0;
+		float iq_avg = 0.0;
+		float id_avg = 0.0;
+		float samples = 0.0;
+
+		for (int i = 0;i < 1000;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();
+			samples += 1.0;
+			chThdSleepMilliseconds(1);
+		}
+
+		vq_avg /= samples;
+		vd_avg /= samples;
+		iq_avg /= samples;
+		id_avg /= samples;
+
+		*linkage = (sqrtf(SQ(vq_avg) + SQ(vd_avg)) - res *
+				sqrtf(SQ(iq_avg) + SQ(id_avg))) / (rpm_now * ((2.0 * M_PI) / 60.0));
+
+		result = true;
+	}
+
+	timeout_configure(tout, tout_c);
+	mc_interface_unlock();
+	mc_interface_release_motor();
+	mc_interface_set_configuration(&mcconf_old);
+	return result;
+}
+
+/**
+ * Automatically detect sensors and apply settings in FOC mode.
+ *
+ * @param current
+ * Current to use for detection.
+ *
+ * @param store_mcconf_on_success
+ * Store motor configuration in emulated EEPROM if the detection succeeds.
+ *
+ * @param send_mcconf_on_success
+ * Send motor configuration if the detection succeeds.
+ *
+ * @return
+ * 2: AS5147 detected successfully
+ * 1: Hall sensors detected successfully
+ * 0: No sensors detected and sensorless mode applied successfully
+ * -1: Detection failed
+ */
+int conf_general_autodetect_apply_sensors_foc(float current,
+		bool store_mcconf_on_success, bool send_mcconf_on_success) {
+	int result = -1;
+
+	mcconf = *mc_interface_get_configuration();
+	mcconf_old = mcconf;
+
+	mcconf.motor_type = MOTOR_TYPE_FOC;
+	mcconf.foc_sensor_mode = FOC_SENSOR_MODE_SENSORLESS;
+	mcconf.foc_current_kp = 0.0005;
+	mcconf.foc_current_ki = 1.0;
+	mc_interface_set_configuration(&mcconf);
+
+	// Wait maximum 5s for fault code to disappear
+	for (int i = 0;i < 500;i++) {
+		if (mc_interface_get_fault() == FAULT_CODE_NONE) {
+			break;
+		}
+		chThdSleepMilliseconds(10);
+	}
+
+	if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+		mc_interface_set_configuration(&mcconf_old);
+		return -1;
+	}
+
+	// Wait one second for things to get ready after
+	// the fault disapears.
+	chThdSleepMilliseconds(1000);
+
+	// Disable timeout
+	systime_t tout = timeout_get_timeout_msec();
+	float tout_c = timeout_get_brake_current();
+	timeout_reset();
+	timeout_configure(60000, 0.0);
+
+	mc_interface_lock();
+
+	// Hall sensors
+	mcconf.m_sensor_port_mode = SENSOR_PORT_MODE_HALL;
+	mc_interface_set_configuration(&mcconf);
+
+	uint8_t hall_table[8];
+	bool res = mcpwm_foc_hall_detect(current, hall_table);
+
+	// Disable timeout and lock, as hall detection will undo the lock
+	timeout_reset();
+	timeout_configure(60000, 0.0);
+	mc_interface_lock();
+
+	if (res) {
+		mcconf_old.m_sensor_port_mode = SENSOR_PORT_MODE_HALL;
+		mcconf_old.foc_sensor_mode = FOC_SENSOR_MODE_HALL;
+		for (int i = 0;i < 8;i++) {
+			mcconf_old.foc_hall_table[i] = hall_table[i];
+		}
+
+		result = 1;
+	}
+
+	// AS5047 encoder
+	if (!res) {
+		mcconf.m_sensor_port_mode = SENSOR_PORT_MODE_AS5047_SPI;
+		mc_interface_set_configuration(&mcconf);
+
+		mcpwm_foc_set_openloop_phase(current, 0.0);
+		chThdSleepMilliseconds(1000);
+
+		float phase_start = encoder_read_deg();
+		float phase_mid = 0.0;
+		float phase_end = 0.0;
+
+		for (int i = 0;i < 180.0;i++) {
+			mcpwm_foc_set_openloop_phase(current, i);
+			chThdSleepMilliseconds(5);
+
+			if (i == 90) {
+				phase_mid = encoder_read_deg();
+			}
+		}
+
+		phase_end = encoder_read_deg();
+		float diff = fabsf(utils_angle_difference(phase_start, phase_end));
+		float diff_mid = fabsf(utils_angle_difference(phase_mid, phase_end));
+
+		if (diff > 2.0 && (diff_mid - diff / 2.0) < (diff / 4)) {
+			float offset, ratio;
+			bool inverted;
+			mcpwm_foc_encoder_detect(current, false, &offset, &ratio, &inverted);
+			mcconf_old.m_sensor_port_mode = SENSOR_PORT_MODE_AS5047_SPI;
+			mcconf_old.foc_sensor_mode = FOC_SENSOR_MODE_ENCODER;
+			mcconf_old.foc_encoder_offset = offset;
+			mcconf_old.foc_encoder_ratio = ratio;
+			mcconf_old.foc_encoder_inverted = inverted;
+
+			res = true;
+			result = 2;
+		}
+	}
+
+	// Sensorless
+	if (!res) {
+		mcconf_old.foc_sensor_mode = FOC_SENSOR_MODE_SENSORLESS;
+		result = 0;
+		res = true;
+	}
+
+	timeout_configure(tout, tout_c);
+	mc_interface_unlock();
+	mc_interface_release_motor();
+	mc_interface_set_configuration(&mcconf_old);
+
+	// On success store the mc configuration, also send it to VESC Tool.
+	if (res) {
+		if (store_mcconf_on_success) {
+			conf_general_store_mc_configuration(&mcconf_old);
+		}
+
+		if (send_mcconf_on_success) {
+			commands_send_mcconf(COMM_GET_MCCONF, &mcconf_old);
+		}
+	}
+
+	return result;
+}
+
+/**
+ * Detect and apply all parameters, current limits and sensors.
+ *
+ * @param max_power_loss
+ * The maximum power loss to derive current limits, as well as detection currents, from.
+ *
+ * @param store_mcconf_on_success
+ * Store motor configuration in emulated EEPROM if the detection succeeds.
+ *
+ * @param send_mcconf_on_success
+ * Send motor configuration if the detection succeeds.
+ *
+ * @return
+ * >=0: Success, see conf_general_autodetect_apply_sensors_foc codes
+ * -10: Flux linkage detection failed
+ *  -x: see conf_general_autodetect_apply_sensors_foc faults
+ */
+int conf_general_detect_apply_all_foc(float max_power_loss,
+		bool store_mcconf_on_success, bool send_mcconf_on_success) {
+	int result = -1;
+
+	mcconf = *mc_interface_get_configuration();
+	mcconf_old = mcconf;
+
+	mcconf.motor_type = MOTOR_TYPE_FOC;
+	mcconf.foc_sensor_mode = FOC_SENSOR_MODE_SENSORLESS;
+	mcconf.foc_current_kp = 0.0005;
+	mcconf.foc_current_ki = 1.0;
+	mcconf.l_current_max = MCCONF_L_CURRENT_MAX;
+	mcconf.l_current_min = MCCONF_L_CURRENT_MIN;
+	mcconf.l_current_max_scale = MCCONF_L_CURRENT_MAX_SCALE;
+	mcconf.l_current_min_scale = MCCONF_L_CURRENT_MIN_SCALE;
+	mcconf.l_watt_max = MCCONF_L_WATT_MAX;
+	mcconf.l_watt_min = MCCONF_L_WATT_MIN;
+	mcconf.l_max_erpm = MCCONF_L_RPM_MAX;
+	mcconf.l_min_erpm = MCCONF_L_RPM_MIN;
+	mc_interface_set_configuration(&mcconf);
+
+	// Wait maximum 5s for fault code to disappear
+	for (int i = 0;i < 500;i++) {
+		if (mc_interface_get_fault() == FAULT_CODE_NONE) {
+			break;
+		}
+		chThdSleepMilliseconds(10);
+	}
+
+	if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+		mc_interface_set_configuration(&mcconf_old);
+		return -1;
+	}
+
+	// Wait one second for things to get ready after
+	// the fault disappears.
+	chThdSleepMilliseconds(1000);
+
+	// Disable timeout
+	systime_t tout = timeout_get_timeout_msec();
+	float tout_c = timeout_get_brake_current();
+	timeout_reset();
+	timeout_configure(60000, 0.0);
+
+	mc_interface_lock();
+
+	float current_start = mcconf.l_current_max / 50;
+	utils_truncate_number_abs(&current_start, mcconf.cc_min_current * 1.1);
+
+	float i_last = 0.0;
+	for (float i = current_start;i < mcconf.l_current_max;i *= 1.5) {
+		float res_tmp = mcpwm_foc_measure_resistance(i, 5);
+		i_last = i;
+
+		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+			timeout_configure(tout, tout_c);
+			mc_interface_unlock();
+			mc_interface_release_motor();
+			mc_interface_set_configuration(&mcconf_old);
+			return -11;
+		}
+
+		if ((i * i * res_tmp) >= (max_power_loss / 3.0)) {
+			break;
+		}
+	}
+
+	float r = mcpwm_foc_measure_resistance(i_last, 100);
+	float l = mcpwm_foc_measure_inductance_current(i_last, 100, 0) * 1e-6;
+	float i_max = sqrtf(max_power_loss / r);
+	utils_truncate_number(&i_max, HW_LIM_CURRENT);
+
+	float lambda = 0.0;
+	int res = conf_general_measure_flux_linkage_openloop(i_max / 2.5, 0.3, 1800, r, &lambda);
+
+	float old_r = mcconf_old.foc_motor_r;
+	float old_l = mcconf_old.foc_motor_l;
+	float old_flux_linkage = mcconf_old.foc_motor_flux_linkage;
+	float old_kp = mcconf_old.foc_current_kp;
+	float old_ki = mcconf_old.foc_current_ki;
+	float old_observer_gain = mcconf_old.foc_observer_gain;
+	bool old_temp_comp = mcconf_old.foc_temp_comp;
+	float old_temp_comp_base_temp = mcconf_old.foc_temp_comp_base_temp;
+
+	if (res) {
+		mcconf_old.l_current_max = i_max;
+		mcconf_old.l_current_min = -i_max;
+
+		float tc = 1000.0;
+		float bw = 1.0 / (tc * 1e-6);
+		float kp = l * bw;
+		float ki = r * bw;
+		float gain = 0.001 / (lambda * lambda);
+
+		mcconf_old.foc_motor_r = r;
+		mcconf_old.foc_motor_l = l;
+		mcconf_old.foc_motor_flux_linkage = lambda;
+		mcconf_old.foc_current_kp = kp;
+		mcconf_old.foc_current_ki = ki;
+		mcconf_old.foc_observer_gain = gain * 1e6;
+
+		// Temperature compensation
+		if (mc_interface_temp_motor_filtered() > 0.0) {
+			mcconf_old.foc_temp_comp = true;
+			mcconf_old.foc_temp_comp_base_temp = mc_interface_temp_motor_filtered();
+		} else {
+			mcconf_old.foc_temp_comp = false;
+		}
+	} else {
+		result = -10;
+	}
+
+	timeout_configure(tout, tout_c);
+	mc_interface_unlock();
+	mc_interface_release_motor();
+	mc_interface_set_configuration(&mcconf_old);
+
+	// Restore initial settings on sensor detection failure
+	if (res) {
+		// Wait for motor to stop
+		chThdSleepMilliseconds(100);
+		for (int i = 0;i < 1000;i++) {
+			if (fabsf(mc_interface_get_rpm()) > 100.0) {
+				chThdSleepMilliseconds(10);
+			} else {
+				break;
+			}
+		}
+
+		// This will also store the settings to emulated eeprom and send them to vesc tool
+		result = conf_general_autodetect_apply_sensors_foc(i_max / 3.0,
+				store_mcconf_on_success, send_mcconf_on_success);
+	} else {
+		mcconf_old.foc_motor_r = old_r;
+		mcconf_old.foc_motor_l = old_l;
+		mcconf_old.foc_motor_flux_linkage = old_flux_linkage;
+		mcconf_old.foc_current_kp = old_kp;
+		mcconf_old.foc_current_ki = old_ki;
+		mcconf_old.foc_observer_gain = old_observer_gain;
+		mcconf_old.foc_temp_comp = old_temp_comp;
+		mcconf_old.foc_temp_comp_base_temp = old_temp_comp_base_temp;
+		mc_interface_set_configuration(&mcconf_old);
+	}
+
+	return result;
+}
+
+/**
+ * Same as conf_general_detect_apply_all_foc, but also start detection in VESCs found on the CAN-bus.
+ *
+ * @param detect_can
+ * Run detection on VESCs found on the CAN-bus as well. Setting this to false makes
+ * this function behave like conf_general_detect_apply_all_foc, with the convenience
+ * of also applying the settings.
+ *
+ * @param max_power_loss
+ * The maximum power loss to derive current limits, as well as detection currents, from.
+ *
+ * @param min_current_in
+ * Minimum input current (negative value). 0 means leave it unchanged.
+ *
+ * @param max_current_in
+ * MAximum input current. 0 means leave it unchanged.
+ *
+ * @param openloop_rpm
+ * FOC openloop ERPM in sensorless mode. 0 means leave it unchanged.
+ *
+ * @param sl_erpm
+ * FOC ERPM above which sensorless should be used in sensored modes. 0 means leave it unchanged.
+ *
+ * @return
+ * Same as conf_general_detect_apply_all_foc, and
+ * -50: CAN detection timed out
+ * -51: CAN detection failed
+ */
+int conf_general_detect_apply_all_foc_can(bool detect_can, float max_power_loss,
+		float min_current_in, float max_current_in, float openloop_rpm, float sl_erpm) {
+	app_configuration appconf = *app_get_configuration();
+	uint8_t id_new = appconf.controller_id;
+
+	mcconf = *mc_interface_get_configuration();
+
+	if (fabsf(min_current_in) > 0.001) {
+		mcconf.l_in_current_min = min_current_in;
+	} else {
+		mcconf.l_in_current_min = MCCONF_L_IN_CURRENT_MIN;
+	}
+
+	if (fabsf(max_current_in) > 0.001) {
+		mcconf.l_in_current_max = max_current_in;
+	} else {
+		mcconf.l_in_current_max = MCCONF_L_IN_CURRENT_MAX;
+	}
+
+	if (fabsf(openloop_rpm) > 0.001) {
+		mcconf.foc_openloop_rpm = openloop_rpm;
+	} else {
+		mcconf.foc_openloop_rpm = MCCONF_FOC_OPENLOOP_RPM;
+	}
+
+	if (fabsf(sl_erpm) > 0.001) {
+		mcconf.foc_sl_erpm = sl_erpm;
+	} else {
+		mcconf.foc_sl_erpm = MCCONF_FOC_SL_ERPM;
+	}
+
+	mc_interface_set_configuration(&mcconf);
+	int can_devs = 0;
+	comm_can_detect_all_foc_res_clear();
+
+	if (detect_can) {
+		for (int i = 0;i < 255;i++) {
+			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 (i == id_new) {
+					id_new++;
+				}
+
+				if (id_new == 255) {
+					id_new = 0;
+				}
+			}
+		}
+	}
+
+	int res = conf_general_detect_apply_all_foc(max_power_loss, false, false);
+
+	// Wait for all VESCs on the CAN-bus to finish detection
+	int timeout = true;
+	for (int i = 0;i < 4000;i++) {
+		if (comm_can_detect_all_foc_res_size() >= can_devs) {
+			timeout = false;
+			break;
+		}
+		chThdSleepMilliseconds(10);
+	}
+
+	if (timeout) {
+		res = -50;
+	} else {
+		for (int i = 0;i < can_devs;i++) {
+			if (comm_can_detect_all_foc_res(i) < 0) {
+				res = -51;
+			}
+		}
+	}
+
+	// Store and send settings
+	if (res >= 0) {
+		if (appconf.controller_id != id_new || appconf.send_can_status != CAN_STATUS_1_2_3_4) {
+			appconf.controller_id = id_new;
+			appconf.send_can_status = CAN_STATUS_1_2_3_4;
+			conf_general_store_app_configuration(&appconf);
+			app_set_configuration(&appconf);
+			commands_send_appconf(COMM_GET_APPCONF, &appconf);
+			chThdSleepMilliseconds(1000);
+		}
+
+		mcconf = *mc_interface_get_configuration();
+		conf_general_store_mc_configuration(&mcconf);
+		commands_send_mcconf(COMM_GET_MCCONF, &mcconf);
+		chThdSleepMilliseconds(1000);
+	}
+
+	return res;
+}
+
diff --git a/conf_general.h b/conf_general.h
index 201f6f419..642a1e384 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2017 - 2018 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2017 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -22,7 +22,7 @@
 
 // Firmware version
 #define FW_VERSION_MAJOR		3
-#define FW_VERSION_MINOR		40
+#define FW_VERSION_MINOR		48
 
 #include "datatypes.h"
 
@@ -48,30 +48,71 @@
 //#define HW75_300_VEDDER_FIRST_PCB
 
 /*
- * Select only one hardware version
+ * Select only one hardware version, if it is not passed
+ * as an argument.
  */
-#if !defined(HW_VERSION_40) && !defined(HW_VERSION_45) && !defined(HW_VERSION_46) && \
-	!defined(HW_VERSION_48) && !defined(HW_VERSION_49) && !defined(HW_VERSION_410) && \
-	!defined(HW_VERSION_60) && !defined(HW_VERSION_R2) && !defined(HW_VERSION_VICTOR_R1A) && \
-	!defined(HW_VERSION_DAS_RS) && !defined(HW_VERSION_PALTA) && !defined(HW_VERSION_RH) && \
-	!defined(HW_VERSION_TP) && !defined(HW_VERSION_75_300) && !defined(HW_VERSION_MINI4) && \
-	!defined(HW_VERSION_DAS_MINI)
-//#define HW_VERSION_40
-//#define HW_VERSION_45
-//#define HW_VERSION_46 // Also for 4.7
-//#define HW_VERSION_48
-//#define HW_VERSION_49
-//#define HW_VERSION_410 // Also for 4.11 and 4.12
-#define HW_VERSION_60
-//#define HW_VERSION_R2
-//#define HW_VERSION_VICTOR_R1A
-//#define HW_VERSION_DAS_RS
-//#define HW_VERSION_PALTA
-//#define HW_VERSION_RH
-//#define HW_VERSION_TP
-//#define HW_VERSION_75_300
-//#define HW_VERSION_MINI4
-//#define HW_VERSION_DAS_MINI
+#if !defined(HW_SOURCE) && !defined(HW_HEADER)
+//#define HW_SOURCE "hw_40.c"
+//#define HW_HEADER "hw_40.h"
+
+//#define HW_SOURCE "hw_45.c"
+//#define HW_HEADER "hw_45.h"
+
+//#define HW_SOURCE "hw_46.c" // Also for 4.7
+//#define HW_HEADER "hw_46.h" // Also for 4.7
+
+//#define HW_SOURCE "hw_48.c"
+//#define HW_HEADER "hw_48.h"
+
+//#define HW_SOURCE "hw_49.c"
+//#define HW_HEADER "hw_49.h"
+
+//#define HW_SOURCE "hw_410.c" // Also for 4.11 and 4.12
+//#define HW_HEADER "hw_410.h" // Also for 4.11 and 4.12
+
+#define HW_SOURCE "hw_60.c"
+#define HW_HEADER "hw_60.h"
+
+//#define HW_SOURCE "hw_r2.c"
+//#define HW_HEADER "hw_r2.h"
+
+//#define HW_SOURCE "hw_victor_r1a.c"
+//#define HW_HEADER "hw_victor_r1a.h"
+
+//#define HW_SOURCE "hw_das_rs.c"
+//#define HW_HEADER "hw_das_rs.h"
+
+//#define HW_SOURCE "hw_palta.c"
+//#define HW_HEADER "hw_palta.h"
+
+//#define HW_SOURCE "hw_rh.c"
+//#define HW_HEADER "hw_rh.h"
+
+//#define HW_SOURCE "hw_tp.c"
+//#define HW_HEADER "hw_tp.h"
+
+//#define HW_SOURCE "hw_75_300.c"
+//#define HW_HEADER "hw_75_300.h"
+
+//#define HW_SOURCE "hw_mini4.c"
+//#define HW_HEADER "hw_mini4.h"
+
+//#define HW_SOURCE "hw_das_mini.c"
+//#define HW_HEADER "hw_das_mini.h"
+
+//#define HW_SOURCE "hw_uavc_qcube.c"
+//#define HW_HEADER "hw_uavc_qcube.h"
+
+//#define HW_SOURCE "hw_uavc_basic.c"
+//#define HW_HEADER "hw_uavc_basic.h"
+#endif
+
+#ifndef HW_SOURCE
+#error "No hardware source file set"
+#endif
+
+#ifndef HW_HEADER
+#error "No hardware header file set"
 #endif
 
 /*
@@ -177,5 +218,13 @@ bool conf_general_detect_motor_param(float current, float min_rpm, float low_dut
 		float *int_limit, float *bemf_coupling_k, int8_t *hall_table, int *hall_res);
 bool conf_general_measure_flux_linkage(float current, float duty,
 		float min_erpm, float res, float *linkage);
+bool conf_general_measure_flux_linkage_openloop(float current, float duty,
+		float erpm_per_sec, float res, float *linkage);
+int conf_general_autodetect_apply_sensors_foc(float current,
+		bool store_mcconf_on_success, bool send_mcconf_on_success);
+int conf_general_detect_apply_all_foc(float max_power_loss,
+		bool store_mcconf_on_success, bool send_mcconf_on_success);
+int conf_general_detect_apply_all_foc_can(bool detect_can, float max_power_loss,
+		float min_current_in, float max_current_in, float openloop_rpm, float sl_erpm);
 
 #endif /* CONF_GENERAL_H_ */
diff --git a/conf_mc_app_default.h b/conf_mc_app_default.h
new file mode 100644
index 000000000..1b3c00617
--- /dev/null
+++ b/conf_mc_app_default.h
@@ -0,0 +1,39 @@
+/*
+	Copyright 2019 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 CONF_MC_APP_DEFAULT_H_
+#define CONF_MC_APP_DEFAULT_H_
+
+#include "conf_general.h"
+
+// User defined default motor configuration file
+#ifdef MCCONF_DEFAULT_USER
+#include MCCONF_DEFAULT_USER
+#endif
+
+// User defined default app configuration file
+#ifdef APPCONF_DEFAULT_USER
+#include APPCONF_DEFAULT_USER
+#endif
+
+// Default configuration parameters that can be overridden
+#include "mcconf_default.h"
+#include "appconf_default.h"
+
+#endif /* CONF_MC_APP_DEFAULT_H_ */
diff --git a/datatypes.h b/datatypes.h
index 73df149b2..b203ef609 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -63,10 +63,19 @@ typedef enum {
 	OUT_AUX_MODE_ON_AFTER_10S
 } out_aux_mode;
 
+// General purpose drive output mode
+typedef enum {
+	GPD_OUTPUT_MODE_NONE = 0,
+	GPD_OUTPUT_MODE_MODULATION,
+	GPD_OUTPUT_MODE_VOLTAGE,
+	GPD_OUTPUT_MODE_CURRENT
+} gpd_output_mode;
+
 typedef enum {
 	MOTOR_TYPE_BLDC = 0,
 	MOTOR_TYPE_DC,
-	MOTOR_TYPE_FOC
+	MOTOR_TYPE_FOC,
+	MOTOR_TYPE_GPD
 } mc_motor_type;
 
 typedef enum {
@@ -87,6 +96,7 @@ typedef enum {
 	CONTROL_MODE_POS,
 	CONTROL_MODE_HANDBRAKE,
 	CONTROL_MODE_OPENLOOP,
+	CONTROL_MODE_OPENLOOP_PHASE,
 	CONTROL_MODE_NONE
 } mc_control_mode;
 
@@ -103,7 +113,8 @@ typedef enum {
 typedef enum {
 	SENSOR_PORT_MODE_HALL = 0,
 	SENSOR_PORT_MODE_ABI,
-	SENSOR_PORT_MODE_AS5047_SPI
+	SENSOR_PORT_MODE_AS5047_SPI,
+	SENSOR_PORT_MODE_AD2S1205
 } sensor_port_mode;
 
 typedef struct {
@@ -141,6 +152,12 @@ typedef enum {
 	CAN_BAUD_1M
 } CAN_BAUD;
 
+typedef enum {
+	BATTERY_TYPE_LIION_3_0__4_2,
+	BATTERY_TYPE_LIIRON_2_6__3_6,
+	BATTERY_TYPE_LEAD_ACID
+} BATTERY_TYPE;
+
 typedef struct {
 	// Switching and drive
 	mc_pwm_mode pwm_mode;
@@ -172,6 +189,8 @@ typedef struct {
 	float l_max_duty;
 	float l_watt_max;
 	float l_watt_min;
+	float l_current_max_scale;
+	float l_current_min_scale;
 	// Overridden limits (Computed during runtime)
 	float lo_current_max;
 	float lo_current_min;
@@ -221,6 +240,12 @@ typedef struct {
 	bool foc_temp_comp;
 	float foc_temp_comp_base_temp;
 	float foc_current_filter_const;
+	// GPDrive
+	int gpd_buffer_notify_left;
+	int gpd_buffer_interpol;
+	float gpd_current_filter_const;
+	float gpd_current_kp;
+	float gpd_current_ki;
 	// Speed PID
 	float s_pid_kp;
 	float s_pid_ki;
@@ -253,6 +278,13 @@ typedef struct {
 	float m_dc_f_sw;
 	float m_ntc_motor_beta;
 	out_aux_mode m_out_aux_mode;
+	// Setup info
+	uint8_t si_motor_poles;
+	float si_gear_ratio;
+	float si_wheel_diameter;
+	BATTERY_TYPE si_battery_type;
+	int si_battery_cells;
+	float si_battery_ah;
 } mc_configuration;
 
 // Applications to use
@@ -427,14 +459,28 @@ typedef struct {
 	bool send_crc_ack;
 } nrf_config;
 
+// CAN status modes
+typedef enum {
+	CAN_STATUS_DISABLED = 0,
+	CAN_STATUS_1,
+	CAN_STATUS_1_2,
+	CAN_STATUS_1_2_3,
+	CAN_STATUS_1_2_3_4
+} CAN_STATUS_MODE;
+
 typedef struct {
 	// Settings
 	uint8_t controller_id;
 	uint32_t timeout_msec;
 	float timeout_brake_current;
-	bool send_can_status;
+	CAN_STATUS_MODE send_can_status;
 	uint32_t send_can_status_rate_hz;
 	CAN_BAUD can_baud_rate;
+	bool pairing_done;
+
+	// UAVCAN
+	bool uavcan_enable;
+	uint8_t uavcan_esc_index;
 
 	// Application to use
 	app_use app_to_use;
@@ -494,7 +540,33 @@ typedef enum {
 	COMM_FORWARD_CAN,
 	COMM_SET_CHUCK_DATA,
 	COMM_CUSTOM_APP_DATA,
-	COMM_NRF_START_PAIRING
+	COMM_NRF_START_PAIRING,
+	COMM_GPD_SET_FSW,
+	COMM_GPD_BUFFER_NOTIFY,
+	COMM_GPD_BUFFER_SIZE_LEFT,
+	COMM_GPD_FILL_BUFFER,
+	COMM_GPD_OUTPUT_SAMPLE,
+	COMM_GPD_SET_MODE,
+	COMM_GPD_FILL_BUFFER_INT8,
+	COMM_GPD_FILL_BUFFER_INT16,
+	COMM_GPD_SET_BUFFER_INT_SCALE,
+	COMM_GET_VALUES_SETUP,
+	COMM_SET_MCCONF_TEMP,
+	COMM_SET_MCCONF_TEMP_SETUP,
+	COMM_GET_VALUES_SELECTIVE,
+	COMM_GET_VALUES_SETUP_SELECTIVE,
+	COMM_EXT_NRF_PRESENT,
+	COMM_EXT_NRF_ESB_SET_CH_ADDR,
+	COMM_EXT_NRF_ESB_SEND_DATA,
+	COMM_EXT_NRF_ESB_RX_DATA,
+	COMM_EXT_NRF_SET_ENABLED,
+	COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP,
+	COMM_DETECT_APPLY_ALL_FOC,
+	COMM_JUMP_TO_BOOTLOADER_ALL_CAN,
+	COMM_ERASE_NEW_APP_ALL_CAN,
+	COMM_WRITE_NEW_APP_DATA_ALL_CAN,
+	COMM_PING_CAN,
+	COMM_APP_DISABLE_OUTPUT
 } COMM_PACKET_ID;
 
 // CAN commands
@@ -512,7 +584,20 @@ typedef enum {
 	CAN_PACKET_SET_CURRENT_REL,
 	CAN_PACKET_SET_CURRENT_BRAKE_REL,
 	CAN_PACKET_SET_CURRENT_HANDBRAKE,
-	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL
+	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL,
+	CAN_PACKET_STATUS_2,
+	CAN_PACKET_STATUS_3,
+	CAN_PACKET_STATUS_4,
+	CAN_PACKET_PING,
+	CAN_PACKET_PONG,
+	CAN_PACKET_DETECT_APPLY_ALL_FOC,
+	CAN_PACKET_DETECT_APPLY_ALL_FOC_RES,
+	CAN_PACKET_CONF_CURRENT_LIMITS,
+	CAN_PACKET_CONF_STORE_CURRENT_LIMITS,
+	CAN_PACKET_CONF_CURRENT_LIMITS_IN,
+	CAN_PACKET_CONF_STORE_CURRENT_LIMITS_IN,
+	CAN_PACKET_CONF_FOC_ERPMS,
+	CAN_PACKET_CONF_STORE_FOC_ERPMS
 } CAN_PACKET_ID;
 
 // Logged fault data
@@ -553,6 +638,8 @@ typedef struct {
 	int acc_z;
 	bool bt_c;
 	bool bt_z;
+	bool rev_has_state;
+	bool is_rev;
 } chuck_data;
 
 typedef struct {
@@ -563,12 +650,37 @@ typedef struct {
 	float duty;
 } can_status_msg;
 
+typedef struct {
+	int id;
+	systime_t rx_time;
+	float amp_hours;
+	float amp_hours_charged;
+} can_status_msg_2;
+
+typedef struct {
+	int id;
+	systime_t rx_time;
+	float watt_hours;
+	float watt_hours_charged;
+} can_status_msg_3;
+
+typedef struct {
+	int id;
+	systime_t rx_time;
+	float temp_fet;
+	float temp_motor;
+	float current_in;
+	float pid_pos_now;
+} can_status_msg_4;
+
 typedef struct {
 	uint8_t js_x;
 	uint8_t js_y;
 	bool bt_c;
 	bool bt_z;
 	bool bt_push;
+	bool rev_has_state;
+	bool is_rev;
 	float vbat;
 } mote_state;
 
diff --git a/encoder.c b/encoder.c
index c87209a4e..2f5f33353 100644
--- a/encoder.c
+++ b/encoder.c
@@ -28,6 +28,9 @@
 #define AS5047P_READ_ANGLECOM		(0x3FFF | 0x4000 | 0x8000) // This is just ones
 #define AS5047_SAMPLE_RATE_HZ		20000
 
+#define AD2S1205_SAMPLE_RATE_HZ		20000		//25MHz max spi clk
+
+
 #if AS5047_USE_HW_SPI_PINS
 #ifdef HW_SPI_DEV
 #define SPI_SW_MISO_GPIO			HW_SPI_PORT_MISO
@@ -62,7 +65,8 @@
 typedef enum {
 	ENCODER_MODE_NONE = 0,
 	ENCODER_MODE_ABI,
-	ENCODER_MODE_AS5047P_SPI
+	ENCODER_MODE_AS5047P_SPI,
+	RESOLVER_MODE_AD2S1205
 } encoder_mode;
 
 // Private variables
@@ -177,10 +181,63 @@ void encoder_init_as5047p_spi(void) {
 	index_found = true;
 }
 
+void encoder_init_ad2s1205_spi(void) {
+	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+
+	palSetPadMode(SPI_SW_MISO_GPIO, SPI_SW_MISO_PIN, PAL_MODE_INPUT);
+	palSetPadMode(SPI_SW_SCK_GPIO, SPI_SW_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(SPI_SW_CS_GPIO, SPI_SW_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+	// Set MOSI to 1
+#if AS5047_USE_HW_SPI_PINS
+	palSetPadMode(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(SPI_SW_MOSI_GPIO, SPI_SW_MOSI_PIN);
+#endif
+
+	// TODO: Choose pins on comm port when these are not defined
+#if defined(AD2S1205_SAMPLE_GPIO) && defined(AD2S1205_RDVEL_GPIO)
+	palSetPadMode(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+
+	palSetPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);	// Prepare for a falling edge SAMPLE assertion
+	palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN);		// Will always read position
+#endif
+
+
+	// Enable timer clock
+	HW_ENC_TIM_CLK_EN();
+
+	// Time Base configuration
+	TIM_TimeBaseStructure.TIM_Prescaler = 0;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+	TIM_TimeBaseStructure.TIM_Period = ((168000000 / 2 / AD2S1205_SAMPLE_RATE_HZ) - 1);
+	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+	TIM_TimeBaseInit(HW_ENC_TIM, &TIM_TimeBaseStructure);
+
+	// Enable overflow interrupt
+	TIM_ITConfig(HW_ENC_TIM, TIM_IT_Update, ENABLE);
+
+	// Enable timer
+	TIM_Cmd(HW_ENC_TIM, ENABLE);
+
+	nvicEnableVector(HW_ENC_TIM_ISR_CH, 6);
+
+	mode = RESOLVER_MODE_AD2S1205;
+	index_found = true;
+}
+
+
 bool encoder_is_configured(void) {
 	return mode != ENCODER_MODE_NONE;
 }
 
+/**
+ * Read angle from configured encoder.
+ *
+ * @return
+ * The current encoder angle in degrees.
+ */
 float encoder_read_deg(void) {
 	static float angle = 0.0;
 
@@ -190,6 +247,7 @@ float encoder_read_deg(void) {
 		break;
 
 	case ENCODER_MODE_AS5047P_SPI:
+	case RESOLVER_MODE_AD2S1205:
 		angle = last_enc_angle;
 		break;
 
@@ -241,12 +299,39 @@ void encoder_reset(void) {
 void encoder_tim_isr(void) {
 	uint16_t pos;
 
-	spi_begin();
-	spi_transfer(&pos, 0, 1);
-	spi_end();
+	if(mode == ENCODER_MODE_AS5047P_SPI) {
+		spi_begin();
+		spi_transfer(&pos, 0, 1);
+		spi_end();
+
+		pos &= 0x3FFF;
+		last_enc_angle = ((float)pos * 360.0) / 16384.0;
+	}
+
+	if(mode == RESOLVER_MODE_AD2S1205) {
+		// SAMPLE signal should have been be asserted in sync with ADC sampling
+#ifdef AD2S1205_RDVEL_GPIO
+		palSetPad(AD2S1205_RDVEL_GPIO, AD2S1205_RDVEL_PIN);	// Always read position
+#endif
 
-	pos &= 0x3FFF;
-	last_enc_angle = ((float)pos * 360.0) / 16384.0;
+		spi_begin(); // CS uses the same mcu pin as AS5047
+
+		spi_transfer(&pos, 0, 1);
+		spi_end();
+
+		uint16_t RDVEL = pos & 0x08; // 1 means a position read
+		uint16_t DOS = pos & 0x04;
+		uint16_t LOT = pos & 0x02;
+	//	uint16_t parity = pos & 0x01; // 16 bit frame should have odd parity
+
+		pos &= 0xFFF0;
+		pos = pos >> 4;
+		pos &= 0x0FFF; // check if needed
+
+		if((RDVEL != 0) && (DOS != 0) && (LOT != 0)) {
+			last_enc_angle = ((float)pos * 360.0) / 4096.0;
+		}
+	}
 }
 
 /**
diff --git a/encoder.h b/encoder.h
index f6b11066e..3ea6c19ab 100644
--- a/encoder.h
+++ b/encoder.h
@@ -26,6 +26,7 @@
 void encoder_deinit(void);
 void encoder_init_abi(uint32_t counts);
 void encoder_init_as5047p_spi(void);
+void encoder_init_ad2s1205_spi(void);
 bool encoder_is_configured(void);
 float encoder_read_deg(void);
 void encoder_reset(void);
diff --git a/flash_helper.c b/flash_helper.c
index 4216c2407..51be09eac 100644
--- a/flash_helper.c
+++ b/flash_helper.c
@@ -141,7 +141,7 @@ void flash_helper_jump_to_bootloader(void) {
 	usbDisconnectBus(&USBD1);
 	usbStop(&USBD1);
 
-	uartStop(&HW_UART_DEV);
+	sdStop(&HW_UART_DEV);
 	palSetPadMode(HW_UART_TX_PORT, HW_UART_TX_PIN, PAL_MODE_INPUT);
 	palSetPadMode(HW_UART_RX_PORT, HW_UART_RX_PIN, PAL_MODE_INPUT);
 
diff --git a/gpdrive.c b/gpdrive.c
new file mode 100644
index 000000000..ca3292e52
--- /dev/null
+++ b/gpdrive.c
@@ -0,0 +1,654 @@
+/*
+	Copyright 2018 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 "gpdrive.h"
+#include "ch.h"
+#include "hal.h"
+#include "stm32f4xx_conf.h"
+#include "digital_filter.h"
+#include "utils.h"
+#include "ledpwm.h"
+#include "terminal.h"
+#include "commands.h"
+#include "timeout.h"
+#include "mc_interface.h"
+#include <math.h>
+#include <string.h>
+#include <stdlib.h>
+
+// Settings
+#define SAMPLE_BUFFER_SIZE			2048
+
+// Private types
+typedef struct {
+	float current_set;
+	float voltage_now;
+	float voltage_int;
+} cc_state;
+
+typedef struct {
+	float buffer[SAMPLE_BUFFER_SIZE];
+	int read;
+	int write;
+} sample_buffer;
+
+// Private variables
+static volatile mc_configuration *m_conf;
+static volatile float m_fsw_now;
+static volatile float m_mod_now;
+static volatile float m_current_now;
+static volatile float m_current_now_filtered;
+static volatile bool m_init_done = false;
+static volatile gpd_output_mode m_output_mode;
+static volatile sample_buffer m_sample_buffer;
+static volatile float m_buffer_int_scale;
+static volatile bool m_is_running;
+static volatile float m_output_now;
+static volatile bool m_dccal_done = false;
+static volatile int m_curr_samples;
+static volatile int m_curr0_sum;
+static volatile int m_curr1_sum;
+static volatile int m_curr0_offset;
+static volatile int m_curr1_offset;
+#ifdef HW_HAS_3_SHUNTS
+static volatile int m_curr2_sum;
+static volatile int m_curr2_offset;
+#endif
+static volatile float m_last_adc_isr_duration;
+static volatile cc_state m_current_state;
+
+// Private functions
+static void stop_pwm_hw(void);
+static void adc_int_handler(void *p, uint32_t flags);
+static void set_modulation(float mod);
+static void do_dc_cal(void);
+
+// Threads
+static THD_WORKING_AREA(timer_thread_wa, 2048);
+static THD_FUNCTION(timer_thread, arg);
+static volatile bool timer_thd_stop;
+
+void gpdrive_init(volatile mc_configuration *configuration) {
+	utils_sys_lock_cnt();
+
+	m_init_done = false;
+
+	// Restore timers
+	TIM_DeInit(TIM1);
+	TIM_DeInit(TIM8);
+	TIM1->CNT = 0;
+	TIM8->CNT = 0;
+
+	// Disable channel 2 pins
+	palSetPadMode(GPIOA, 9, PAL_MODE_OUTPUT_PUSHPULL);
+	palClearPad(GPIOA, 9);
+	palSetPadMode(GPIOB, 14, PAL_MODE_OUTPUT_PUSHPULL);
+	palClearPad(GPIOB, 14);
+
+	m_conf = configuration;
+	m_fsw_now = 40000;
+	m_mod_now = 0.0;
+	m_current_now = 0.0;
+	m_current_now_filtered = 0.0;
+	m_output_mode = GPD_OUTPUT_MODE_NONE;
+	memset((void*)&m_sample_buffer, 0, sizeof(m_sample_buffer));
+	m_buffer_int_scale = 1.0 / 128.0;
+	m_is_running = false;
+	m_output_now = 0.0;
+	m_curr0_sum = 0;
+	m_curr1_sum = 0;
+	m_curr_samples = 0;
+	m_curr0_offset = 0;
+	m_curr1_offset = 0;
+	m_dccal_done = false;
+#ifdef HW_HAS_3_SHUNTS
+	m_curr2_sum = 0;
+	m_curr2_offset = 0;
+#endif
+	m_last_adc_isr_duration = 0;
+	memset((void*)&m_current_state, 0, sizeof(m_current_state));
+
+	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
+	TIM_OCInitTypeDef  TIM_OCInitStructure;
+	TIM_BDTRInitTypeDef TIM_BDTRInitStructure;
+
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
+
+	TIM_TimeBaseStructure.TIM_Prescaler = 0;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+	TIM_TimeBaseStructure.TIM_Period = SYSTEM_CORE_CLOCK / (int)m_fsw_now;
+	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
+	TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
+
+	TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
+	TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
+	TIM_OCInitStructure.TIM_OutputNState = TIM_OutputNState_Enable;
+	TIM_OCInitStructure.TIM_Pulse = TIM1->ARR / 2;
+	TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
+	TIM_OCInitStructure.TIM_OCNPolarity = TIM_OCNPolarity_High;
+	TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
+	TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCNIdleState_Set;
+
+	TIM_OC1Init(TIM1, &TIM_OCInitStructure);
+	TIM_OC2Init(TIM1, &TIM_OCInitStructure);
+	TIM_OC3Init(TIM1, &TIM_OCInitStructure);
+	TIM_OC4Init(TIM1, &TIM_OCInitStructure);
+
+	TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable);
+	TIM_OC2PreloadConfig(TIM1, TIM_OCPreload_Enable);
+	TIM_OC3PreloadConfig(TIM1, TIM_OCPreload_Enable);
+	TIM_OC4PreloadConfig(TIM1, TIM_OCPreload_Enable);
+
+	TIM_BDTRInitStructure.TIM_OSSRState = TIM_OSSRState_Enable;
+	TIM_BDTRInitStructure.TIM_OSSIState = TIM_OSSIState_Enable;
+	TIM_BDTRInitStructure.TIM_LOCKLevel = TIM_LOCKLevel_OFF;
+	TIM_BDTRInitStructure.TIM_DeadTime = HW_DEAD_TIME_VALUE;
+	TIM_BDTRInitStructure.TIM_Break = TIM_Break_Disable;
+	TIM_BDTRInitStructure.TIM_BreakPolarity = TIM_BreakPolarity_High;
+	TIM_BDTRInitStructure.TIM_AutomaticOutput = TIM_AutomaticOutput_Disable;
+
+	TIM_BDTRConfig(TIM1, &TIM_BDTRInitStructure);
+	TIM_CCPreloadControl(TIM1, ENABLE);
+	TIM_ARRPreloadConfig(TIM1, ENABLE);
+
+	ADC_CommonInitTypeDef ADC_CommonInitStructure;
+	DMA_InitTypeDef DMA_InitStructure;
+	ADC_InitTypeDef ADC_InitStructure;
+
+	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA2 | RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOC, ENABLE);
+	RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1 | RCC_APB2Periph_ADC2 | RCC_APB2Periph_ADC3, ENABLE);
+
+	dmaStreamAllocate(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)),
+			3,
+			(stm32_dmaisr_t)adc_int_handler,
+			(void *)0);
+
+	DMA_InitStructure.DMA_Channel = DMA_Channel_0;
+	DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&ADC_Value;
+	DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC->CDR;
+	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
+	DMA_InitStructure.DMA_BufferSize = HW_ADC_CHANNELS;
+	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
+	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
+	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
+	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
+	DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
+	DMA_InitStructure.DMA_Priority = DMA_Priority_High;
+	DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
+	DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
+	DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
+	DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
+	DMA_Init(DMA2_Stream4, &DMA_InitStructure);
+
+	DMA_Cmd(DMA2_Stream4, ENABLE);
+	DMA_ITConfig(DMA2_Stream4, DMA_IT_TC, ENABLE);
+
+	// Note that the ADC is running at 42MHz, which is higher than the
+	// specified 36MHz in the data sheet, but it works.
+	ADC_CommonInitStructure.ADC_Mode = ADC_TripleMode_RegSimult;
+	ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div2;
+	ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_1;
+	ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_5Cycles;
+	ADC_CommonInit(&ADC_CommonInitStructure);
+
+	ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b;
+	ADC_InitStructure.ADC_ScanConvMode = ENABLE;
+	ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
+	ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_Falling;
+	ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC2;
+	ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
+	ADC_InitStructure.ADC_NbrOfConversion = HW_ADC_NBR_CONV;
+
+	ADC_Init(ADC1, &ADC_InitStructure);
+	ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None;
+	ADC_InitStructure.ADC_ExternalTrigConv = 0;
+	ADC_Init(ADC2, &ADC_InitStructure);
+	ADC_Init(ADC3, &ADC_InitStructure);
+
+	ADC_TempSensorVrefintCmd(ENABLE);
+
+	ADC_MultiModeDMARequestAfterLastTransferCmd(ENABLE);
+
+	hw_setup_adc_channels();
+
+	ADC_Cmd(ADC1, ENABLE);
+	ADC_Cmd(ADC2, ENABLE);
+	ADC_Cmd(ADC3, ENABLE);
+
+	TIM_Cmd(TIM1, ENABLE);
+	TIM_CtrlPWMOutputs(TIM1, ENABLE);
+
+	// Always sample ADC in the beginning of the PWM cycle
+	TIM1->CCR2 = 200;
+
+	utils_sys_unlock_cnt();
+
+	ENABLE_GATE();
+	DCCAL_OFF();
+	do_dc_cal();
+
+	// Various time measurements
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM12, ENABLE);
+	TIM_TimeBaseStructure.TIM_Period = 0xFFFFFFFF;
+	TIM_TimeBaseStructure.TIM_Prescaler = (uint16_t)(((SYSTEM_CORE_CLOCK / 2) / 10000000) - 1);
+	TIM_TimeBaseStructure.TIM_ClockDivision = 0;
+	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
+	TIM_TimeBaseInit(TIM12, &TIM_TimeBaseStructure);
+	TIM_Cmd(TIM12, ENABLE);
+
+	// Start threads
+	timer_thd_stop = false;
+	chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
+
+	// WWDG
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_WWDG, ENABLE);
+	WWDG_SetPrescaler(WWDG_Prescaler_1);
+	WWDG_SetWindowValue(255);
+	WWDG_Enable(100);
+
+	stop_pwm_hw();
+
+	m_init_done = true;
+}
+
+void gpdrive_deinit(void) {
+	if (!m_init_done) {
+		return;
+	}
+
+	m_init_done = false;
+
+	WWDG_DeInit();
+
+	timer_thd_stop = true;
+
+	while (timer_thd_stop) {
+		chThdSleepMilliseconds(1);
+	}
+
+	TIM_DeInit(TIM1);
+	TIM_DeInit(TIM8);
+	TIM_DeInit(TIM12);
+	ADC_DeInit();
+	DMA_DeInit(DMA2_Stream4);
+	nvicDisableVector(ADC_IRQn);
+	dmaStreamRelease(STM32_DMA_STREAM(STM32_DMA_STREAM_ID(2, 4)));
+
+	// Restore pins
+	palSetPadMode(GPIOA, 9, 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);
+}
+
+bool gpdrive_init_done(void) {
+	return m_init_done;
+}
+
+bool gpdrive_is_dccal_done(void) {
+	return m_dccal_done;
+}
+
+float gpdrive_get_switching_frequency_now(void) {
+	return m_fsw_now;
+}
+
+void gpdrive_set_configuration(volatile mc_configuration *configuration) {
+	// Stop everything first to be safe
+	m_output_mode = GPD_OUTPUT_MODE_NONE;
+	stop_pwm_hw();
+
+	utils_sys_lock_cnt();
+	m_conf = configuration;
+	utils_sys_unlock_cnt();
+}
+
+void gpdrive_output_sample(float sample) {
+	m_output_now = sample;
+
+	switch (m_output_mode) {
+	case GPD_OUTPUT_MODE_MODULATION:
+		set_modulation(sample);
+		break;
+
+	case GPD_OUTPUT_MODE_VOLTAGE:
+		set_modulation(sample / GET_INPUT_VOLTAGE());
+		break;
+
+	case GPD_OUTPUT_MODE_CURRENT:
+		m_current_state.current_set = sample;
+		break;
+
+	default:
+		break;
+	}
+}
+
+void gpdrive_fill_buffer(float *samples, int sample_num) {
+	for (int i = 0;i < sample_num;i++) {
+		m_sample_buffer.buffer[m_sample_buffer.write++] = samples[i];
+		m_sample_buffer.write %= SAMPLE_BUFFER_SIZE;
+	}
+}
+
+void gpdrive_add_buffer_sample(float sample) {
+	m_sample_buffer.buffer[m_sample_buffer.write++] = sample;
+	m_sample_buffer.write %= SAMPLE_BUFFER_SIZE;
+}
+
+void gpdrive_add_buffer_sample_int(int sample) {
+	m_sample_buffer.buffer[m_sample_buffer.write++] = (float)sample * m_buffer_int_scale;
+	m_sample_buffer.write %= SAMPLE_BUFFER_SIZE;
+}
+
+void gpdrive_set_buffer_int_scale(float scale) {
+	m_buffer_int_scale = scale;
+}
+
+void gpdrive_set_switching_frequency(float freq) {
+	m_fsw_now = freq;
+	TIM1->ARR = SYSTEM_CORE_CLOCK / (int)m_fsw_now;
+	set_modulation(m_mod_now);
+}
+
+int gpdrive_buffer_size_left(void) {
+	return (m_sample_buffer.write > m_sample_buffer.read)
+                ? m_sample_buffer.write - m_sample_buffer.read
+                : SAMPLE_BUFFER_SIZE - m_sample_buffer.read +m_sample_buffer.write;
+}
+
+void gpdrive_set_mode(gpd_output_mode mode) {
+	m_output_mode = mode;
+
+	if (m_output_mode == GPD_OUTPUT_MODE_NONE) {
+		stop_pwm_hw();
+	}
+}
+
+float gpdrive_get_current(void) {
+	return m_current_now;
+}
+
+float gpdrive_get_current_filtered(void) {
+	return m_current_now_filtered;
+}
+
+float gpdrive_get_modulation(void) {
+	return m_mod_now;
+}
+
+float gpdrive_get_last_adc_isr_duration(void) {
+	return m_last_adc_isr_duration;
+}
+
+// Private functions
+
+static void set_modulation(float mod) {
+	utils_truncate_number_abs(&mod, m_conf->l_max_duty);
+	m_mod_now = mod;
+
+	if (m_output_mode == GPD_OUTPUT_MODE_NONE || mc_interface_get_fault() != FAULT_CODE_NONE) {
+		return;
+	}
+
+	if (m_conf->pwm_mode == PWM_MODE_BIPOLAR) {
+		uint32_t duty = (uint32_t) (((float)TIM1->ARR / 2.0) * mod + ((float)TIM1->ARR / 2.0));
+		TIM1->CCR1 = duty;
+		TIM1->CCR3 = duty;
+
+		// +
+		TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
+		TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
+		TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
+
+		// -
+		TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM2);
+		TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
+		TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
+	} else {
+		uint32_t duty = (uint32_t)((float)TIM1->ARR * fabsf(mod));
+		TIM1->CCR1 = duty;
+		TIM1->CCR3 = duty;
+
+		if (mod >= 0) {
+			// +
+			TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_PWM1);
+			TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
+			TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
+
+			// -
+			TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_Inactive);
+			TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
+			TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
+		} else {
+			// +
+			TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_OCMode_PWM1);
+			TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
+			TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Enable);
+
+			// -
+			TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_OCMode_Inactive);
+			TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
+			TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Enable);
+		}
+	}
+
+	TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
+
+	m_is_running = true;
+}
+
+static void stop_pwm_hw(void) {
+	m_is_running = false;
+	m_sample_buffer.write = 0;
+	m_sample_buffer.read = 0;
+
+#ifdef HW_HAS_DRV8313
+	DISABLE_BR();
+#endif
+
+	TIM_SelectOCxM(TIM1, TIM_Channel_1, TIM_ForcedAction_InActive);
+	TIM_CCxCmd(TIM1, TIM_Channel_1, TIM_CCx_Enable);
+	TIM_CCxNCmd(TIM1, TIM_Channel_1, TIM_CCxN_Disable);
+
+	TIM_SelectOCxM(TIM1, TIM_Channel_3, TIM_ForcedAction_InActive);
+	TIM_CCxCmd(TIM1, TIM_Channel_3, TIM_CCx_Enable);
+	TIM_CCxNCmd(TIM1, TIM_Channel_3, TIM_CCxN_Disable);
+
+	TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
+}
+
+static void do_dc_cal(void) {
+	DCCAL_ON();
+
+	// Wait max 5 seconds
+	int cnt = 0;
+	while(IS_DRV_FAULT()){
+		chThdSleepMilliseconds(1);
+		cnt++;
+		if (cnt > 5000) {
+			break;
+		}
+	};
+
+	chThdSleepMilliseconds(1000);
+	m_curr0_sum = 0;
+	m_curr1_sum = 0;
+#ifdef HW_HAS_3_SHUNTS
+	m_curr2_sum = 0;
+#endif
+	m_curr_samples = 0;
+	while(m_curr_samples < 4000) {};
+	m_curr0_offset = m_curr0_sum / m_curr_samples;
+	m_curr1_offset = m_curr1_sum / m_curr_samples;
+#ifdef HW_HAS_3_SHUNTS
+	m_curr2_offset = m_curr2_sum / m_curr_samples;
+#endif
+	DCCAL_OFF();
+	m_dccal_done = true;
+}
+
+static void adc_int_handler(void *p, uint32_t flags) {
+	(void)p;
+	(void)flags;
+
+	TIM12->CNT = 0;
+
+	// Reset the watchdog
+	WWDG_SetCounter(100);
+
+	int curr0 = GET_CURRENT1();
+	int curr1 = GET_CURRENT2();
+
+#ifdef HW_HAS_3_SHUNTS
+	int curr2 = GET_CURRENT3();
+#endif
+
+	m_curr0_sum += curr0;
+	m_curr1_sum += curr1;
+#ifdef HW_HAS_3_SHUNTS
+	m_curr2_sum += curr2;
+#endif
+
+	curr0 -= m_curr0_offset;
+	curr1 -= m_curr1_offset;
+#ifdef HW_HAS_3_SHUNTS
+	curr2 -= m_curr2_offset;
+#endif
+
+	m_curr_samples++;
+
+	// Update current
+#ifdef HW_HAS_3_SHUNTS
+	float i1 = -(float)curr2;
+#else
+	float i1 = -(float)curr1;
+#endif
+	float i2 = (float)curr0;
+
+	m_current_now = utils_max_abs(i1, i2) * FAC_CURRENT;
+	UTILS_LP_FAST(m_current_now_filtered, m_current_now, m_conf->gpd_current_filter_const);
+
+	// Check for most critical faults here, as doing it in mc_interface can be too slow
+	// for high switching frequencies.
+
+	const float input_voltage = GET_INPUT_VOLTAGE();
+
+	static int wrong_voltage_iterations = 0;
+	if (input_voltage < m_conf->l_min_vin ||
+			input_voltage > m_conf->l_max_vin) {
+		wrong_voltage_iterations++;
+
+		if ((wrong_voltage_iterations >= 8)) {
+			mc_interface_fault_stop(input_voltage < m_conf->l_min_vin ?
+					FAULT_CODE_UNDER_VOLTAGE : FAULT_CODE_OVER_VOLTAGE);
+		}
+	} else {
+		wrong_voltage_iterations = 0;
+	}
+
+	if (m_conf->l_slow_abs_current) {
+		if (fabsf(m_current_now) > m_conf->l_abs_current_max) {
+			mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
+		}
+	} else {
+		if (fabsf(m_current_now_filtered) > m_conf->l_abs_current_max) {
+			mc_interface_fault_stop(FAULT_CODE_ABS_OVER_CURRENT);
+		}
+	}
+
+	// Buffer handling
+	static bool buffer_was_empty = true;
+	static int interpol = 0;
+	static float buffer_last = 0.0;
+	static float buffer_next = 0.0;
+
+	interpol++;
+
+	if (interpol > m_conf->gpd_buffer_interpol) {
+		interpol = 0;
+		if (m_sample_buffer.read != m_sample_buffer.write) {
+			buffer_last = buffer_next;
+			buffer_next = m_sample_buffer.buffer[m_sample_buffer.read++];
+			m_sample_buffer.read %= SAMPLE_BUFFER_SIZE;
+
+			m_output_now = buffer_last;
+			m_is_running = true;
+
+			buffer_was_empty = false;
+		} else {
+			if (!buffer_was_empty) {
+				stop_pwm_hw();
+			}
+
+			buffer_was_empty = true;
+		}
+	} else if (!buffer_was_empty) {
+		m_output_now = utils_map((float)interpol,
+				0.0, (float)m_conf->gpd_buffer_interpol + 1.0,
+			buffer_last, buffer_next);
+		m_is_running = true;
+	}
+
+	if (m_is_running) {
+		gpdrive_output_sample(m_output_now);
+
+		if (m_output_mode == GPD_OUTPUT_MODE_CURRENT) {
+			float v_in = GET_INPUT_VOLTAGE();
+			float err = m_current_state.current_set - m_current_now_filtered;
+			m_current_state.voltage_now = m_current_state.voltage_int + err * m_conf->gpd_current_kp;
+			m_current_state.voltage_int += err * m_conf->gpd_current_ki * (1.0 / m_fsw_now);
+			utils_truncate_number_abs((float*)&m_current_state.voltage_int, v_in);
+			set_modulation(m_current_state.voltage_now / v_in);
+		}
+	}
+
+	ledpwm_update_pwm();
+
+	m_last_adc_isr_duration = (float)TIM12->CNT / 10000000.0;
+}
+
+static THD_FUNCTION(timer_thread, arg) {
+	(void)arg;
+
+	chRegSetThreadName("gpdrive timer");
+
+	for(;;) {
+		if (timer_thd_stop) {
+			timer_thd_stop = false;
+			return;
+		}
+
+		static bool buffer_empty_before = true;
+		if (gpdrive_buffer_size_left() > 0 &&
+				gpdrive_buffer_size_left() < m_conf->gpd_buffer_notify_left) {
+			if (!buffer_empty_before) {
+				commands_send_gpd_buffer_notify();
+			}
+			buffer_empty_before = true;
+		} else {
+			buffer_empty_before = false;
+		}
+
+		chThdSleepMilliseconds(1);
+	}
+}
diff --git a/gpdrive.h b/gpdrive.h
new file mode 100644
index 000000000..3723bc272
--- /dev/null
+++ b/gpdrive.h
@@ -0,0 +1,45 @@
+/*
+	Copyright 2018 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 GPDRIVE_H_
+#define GPDRIVE_H_
+
+#include "conf_general.h"
+
+// Functions
+void gpdrive_init(volatile mc_configuration *configuration);
+void gpdrive_deinit(void);
+bool gpdrive_init_done(void);
+bool gpdrive_is_dccal_done(void);
+float gpdrive_get_switching_frequency_now(void);
+void gpdrive_set_configuration(volatile mc_configuration *configuration);
+void gpdrive_output_sample(float sample);
+void gpdrive_fill_buffer(float *samples, int sample_num);
+void gpdrive_add_buffer_sample(float sample);
+void gpdrive_add_buffer_sample_int(int sample);
+void gpdrive_set_buffer_int_scale(float scale);
+void gpdrive_set_switching_frequency(float freq);
+int gpdrive_buffer_size_left(void);
+void gpdrive_set_mode(gpd_output_mode mode);
+float gpdrive_get_current(void);
+float gpdrive_get_current_filtered(void);
+float gpdrive_get_modulation(void);
+float gpdrive_get_last_adc_isr_duration(void);
+
+#endif /* GPDRIVE_H_ */
diff --git a/halconf.h b/halconf.h
index 2e5e5ab21..acb08b2b4 100644
--- a/halconf.h
+++ b/halconf.h
@@ -132,7 +132,7 @@
  * @brief   Enables the SERIAL subsystem.
  */
 #if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__)
-#define HAL_USE_SERIAL              FALSE
+#define HAL_USE_SERIAL              TRUE
 #endif
 
 /**
@@ -153,7 +153,7 @@
  * @brief   Enables the UART subsystem.
  */
 #if !defined(HAL_USE_UART) || defined(__DOXYGEN__)
-#define HAL_USE_UART                TRUE
+#define HAL_USE_UART                FALSE
 #endif
 
 /**
@@ -280,7 +280,7 @@
  *          default configuration.
  */
 #if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__)
-#define SERIAL_DEFAULT_BITRATE      38400
+#define SERIAL_DEFAULT_BITRATE      115200
 #endif
 
 /**
@@ -291,7 +291,7 @@
  *          buffers.
  */
 #if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__)
-#define SERIAL_BUFFERS_SIZE         16
+#define SERIAL_BUFFERS_SIZE         128
 #endif
 
 /*===========================================================================*/
diff --git a/hwconf/drv8320.h b/hwconf/drv8320.h
deleted file mode 100644
index 86b40105f..000000000
--- a/hwconf/drv8320.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
-	Copyright 2017 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/>.
-    */
-
-#ifndef HWCONF_DRV8320_H_
-#define HWCONF_DRV8320_H_
-
-#include "datatypes.h"
-
-// Functions
-void drv8320_init(void);
-void drv8320_set_oc_adj(int val);
-void drv8320_set_oc_mode(drv8301_oc_mode mode);
-unsigned long drv8320_read_faults(void);
-void drv8320_reset_faults(void);
-char* drv8320_faults_to_string(unsigned long faults);
-unsigned int drv8320_read_reg(int reg);
-void drv8320_write_reg(int reg, int data);
-
-// Defines
-#define DRV8320_FAULT_FET_LC_OC		(1 << 0)
-#define DRV8320_FAULT_FET_HC_OC		(1 << 1)
-#define DRV8320_FAULT_FET_LB_OC		(1 << 2)
-#define DRV8320_FAULT_FET_HB_OC		(1 << 3)
-#define DRV8320_FAULT_FET_LA_OC		(1 << 4)
-#define DRV8320_FAULT_FET_HA_OC		(1 << 5)
-#define DRV8320_FAULT_OTSD			(1 << 6)
-#define DRV8320_FAULT_UVLO			(1 << 7)
-#define DRV8320_FAULT_GDF   		(1 << 8)
-#define DRV8320_FAULT_VDS_OCP		(1 << 9)
-#define DRV8320_FAULT_FAULT			(1 << 10)
-
-#define DRV8320_FAULT_VGS_LC        (1 << 16)
-#define DRV8320_FAULT_VGS_HC        (1 << 17)
-#define DRV8320_FAULT_VGS_LB        (1 << 18)
-#define DRV8320_FAULT_VGS_HB        (1 << 19)
-#define DRV8320_FAULT_VGS_LA        (1 << 20)
-#define DRV8320_FAULT_VGS_HA        (1 << 21)
-#define DRV8320_FAULT_CPUV          (1 << 22)
-#define DRV8320_FAULT_OTW           (1 << 23)
-#define DRV8320_FAULT_SC_OC         (1 << 24)
-#define DRV8320_FAULT_SB_OC         (1 << 25)
-#define DRV8320_FAULT_SA_OC         (1 << 26)
-
-#endif /* HWCONF_DRV8320_H_ */
\ No newline at end of file
diff --git a/hwconf/drv8320.c b/hwconf/drv8320s.c
similarity index 69%
rename from hwconf/drv8320.c
rename to hwconf/drv8320s.c
index f4156c34d..24c23858e 100644
--- a/hwconf/drv8320.c
+++ b/hwconf/drv8320s.c
@@ -18,9 +18,9 @@
  */
 
 #include "hw.h"
-#ifdef HW_HAS_DRV8320
+#ifdef HW_HAS_DRV8320S
 
-#include "drv8320.h"
+#include "drv8320s.h"
 #include "ch.h"
 #include "hal.h"
 #include "stm32f4xx_conf.h"
@@ -45,53 +45,53 @@ static void terminal_reset_faults(int argc, const char **argv);
 // Private variables
 static char m_fault_print_buffer[120];
 
-void drv8320_init(void) {
-	// DRV8320 SPI
-	palSetPadMode(DRV8320_MISO_GPIO, DRV8320_MISO_PIN, PAL_MODE_INPUT_PULLUP);
-	palSetPadMode(DRV8320_SCK_GPIO, DRV8320_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(DRV8320_CS_GPIO, DRV8320_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPadMode(DRV8320_MOSI_GPIO, DRV8320_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
-	palSetPad(DRV8320_MOSI_GPIO, DRV8320_MOSI_PIN);
+void drv8320s_init(void) {
+	// DRV8320S SPI
+	palSetPadMode(DRV8320S_MISO_GPIO, DRV8320S_MISO_PIN, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(DRV8320S_SCK_GPIO, DRV8320S_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(DRV8320S_CS_GPIO, DRV8320S_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(DRV8320S_MOSI_GPIO, DRV8320S_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(DRV8320S_MOSI_GPIO, DRV8320S_MOSI_PIN);
 
 	chThdSleepMilliseconds(100);
 
 	// Disable OC
-	drv8320_write_reg(5, 0x04C0);
-	drv8320_write_reg(5, 0x04C0);
+	drv8320s_write_reg(5, 0x04C0);
+	drv8320s_write_reg(5, 0x04C0);
 
 	terminal_register_command_callback(
-			"drv8320_read_reg",
-			"Read a register from the DRV8320 and print it.",
+			"drv8320s_read_reg",
+			"Read a register from the DRV8320S and print it.",
 			"[reg]",
 			terminal_read_reg);
 
 	terminal_register_command_callback(
-			"drv8320_write_reg",
-			"Write to a DRV8320 register.",
+			"drv8320s_write_reg",
+			"Write to a DRV8320S register.",
 			"[reg] [hexvalue]",
 			terminal_write_reg);
 
 	terminal_register_command_callback(
-			"drv8320_set_oc_adj",
-			"Set the DRV8320 OC ADJ register.",
+			"drv8320s_set_oc_adj",
+			"Set the DRV8320S OC ADJ register.",
 			"[value]",
 			terminal_set_oc_adj);
 
 	terminal_register_command_callback(
-			"drv8320_print_faults",
-			"Print all current DRV8320 faults.",
+			"drv8320s_print_faults",
+			"Print all current DRV8320S faults.",
 			0,
 			terminal_print_faults);
 
 	terminal_register_command_callback(
-			"drv8320_reset_faults",
-			"Reset all latched DRV8320 faults.",
+			"drv8320s_reset_faults",
+			"Reset all latched DRV8320S faults.",
 			0,
 			terminal_reset_faults);
 }
 
 /**
- * Set the threshold of the over current protection of the DRV8320. It works by measuring
+ * Set the threshold of the over current protection of the DRV8320S. It works by measuring
  * the voltage drop across drain-source of the MOSFETs and activates when it is higher than
  * a set value. Notice that this current limit is not very accurate.
  *
@@ -99,36 +99,36 @@ void drv8320_init(void) {
  * The value to use. Range [0 15]. A lower value corresponds to a lower current limit. See
  * the drv8320 datasheet for how to convert these values to currents.
  */
-void drv8320_set_oc_adj(int val) {
+void drv8320s_set_oc_adj(int val) {
 	// Match with the drv8301 levels
 	val >>= 1;
-	int reg = drv8320_read_reg(5);
+	int reg = drv8320s_read_reg(5);
 	reg &= 0xFFF0;
 	reg |= (val & 0xF);
-	drv8320_write_reg(5, reg);
+	drv8320s_write_reg(5, reg);
 }
 
 /**
- * Set the over current protection mode of the DRV8320.
+ * Set the over current protection mode of the DRV8320S.
  *
  * @param mode
  * The over current protection mode.
  */
-void drv8320_set_oc_mode(drv8301_oc_mode mode) {
+void drv8320s_set_oc_mode(drv8301_oc_mode mode) {
 	// Match with the drv8301 modes
 	if (mode == DRV8301_OC_LATCH_SHUTDOWN) {
 		mode = DRV8301_OC_LIMIT;
 	} else if (mode == DRV8301_OC_LIMIT) {
 		mode = DRV8301_OC_LATCH_SHUTDOWN;
 	}
-	int reg = drv8320_read_reg(5);
+	int reg = drv8320s_read_reg(5);
 	reg &= 0xFF3F;
 	reg |= (mode & 0x03) << 6;
-	drv8320_write_reg(5, reg);
+	drv8320s_write_reg(5, reg);
 }
 
 /**
- * Read the fault codes of the DRV8320.
+ * Read the fault codes of the DRV8320S.
  *
  * @return
  * The fault codes, where the bits represent the following:
@@ -146,112 +146,112 @@ void drv8320_set_oc_mode(drv8301_oc_mode mode) {
  * b11: GVDD_OV
  *
  */
-unsigned long drv8320_read_faults(void) {
-	unsigned long r0 = drv8320_read_reg(0);
-	int r1 = drv8320_read_reg(1);
+unsigned long drv8320s_read_faults(void) {
+	unsigned long r0 = drv8320s_read_reg(0);
+	int r1 = drv8320s_read_reg(1);
 	return r0 | (r1 << 16);
 }
 
 /**
  * Reset all latched faults.
  */
-void drv8320_reset_faults(void) {
-	int reg = drv8320_read_reg(2);
+void drv8320s_reset_faults(void) {
+	int reg = drv8320s_read_reg(2);
 	reg |= 1;
-	drv8320_write_reg(2, reg);
+	drv8320s_write_reg(2, reg);
 }
 
-char* drv8320_faults_to_string(unsigned long faults) {
+char* drv8320s_faults_to_string(unsigned long faults) {
 	if (faults == 0) {
-		strcpy(m_fault_print_buffer, "No DRV8320 faults");
+		strcpy(m_fault_print_buffer, "No DRV8320S faults");
 	} else {
 		strcpy(m_fault_print_buffer, "|");
 
-		if (faults & DRV8320_FAULT_FET_LC_OC) {
+		if (faults & DRV8320S_FAULT_FET_LC_OC) {
 			strcat(m_fault_print_buffer, " FETLC_OC |");
 		}
 
-		if (faults & DRV8320_FAULT_FET_HC_OC) {
+		if (faults & DRV8320S_FAULT_FET_HC_OC) {
 			strcat(m_fault_print_buffer, " FETHC_OC |");
 		}
 
-		if (faults & DRV8320_FAULT_FET_LB_OC) {
+		if (faults & DRV8320S_FAULT_FET_LB_OC) {
 			strcat(m_fault_print_buffer, " FETLB_OC |");
 		}
 
-		if (faults & DRV8320_FAULT_FET_HB_OC) {
+		if (faults & DRV8320S_FAULT_FET_HB_OC) {
 			strcat(m_fault_print_buffer, " FETHB_OC |");
 		}
 
-		if (faults & DRV8320_FAULT_FET_LA_OC) {
+		if (faults & DRV8320S_FAULT_FET_LA_OC) {
 			strcat(m_fault_print_buffer, " FETLA_OC |");
 		}
 
-		if (faults & DRV8320_FAULT_FET_HA_OC) {
+		if (faults & DRV8320S_FAULT_FET_HA_OC) {
 			strcat(m_fault_print_buffer, " FETHA_OC |");
 		}
 
-		if (faults & DRV8320_FAULT_OTSD) {
+		if (faults & DRV8320S_FAULT_OTSD) {
 			strcat(m_fault_print_buffer, " OTSD |");
 		}
 
-		if (faults & DRV8320_FAULT_UVLO) {
+		if (faults & DRV8320S_FAULT_UVLO) {
 			strcat(m_fault_print_buffer, " UVLO |");
 		}
 
-		if (faults & DRV8320_FAULT_GDF) {
+		if (faults & DRV8320S_FAULT_GDF) {
 			strcat(m_fault_print_buffer, " GDF |");
 		}
 
-		if (faults & DRV8320_FAULT_VDS_OCP) {
+		if (faults & DRV8320S_FAULT_VDS_OCP) {
 			strcat(m_fault_print_buffer, " VDS OCP |");
 		}
 
-		if (faults & DRV8320_FAULT_FAULT) {
+		if (faults & DRV8320S_FAULT_FAULT) {
 			strcat(m_fault_print_buffer, " FAULT |");
 		}
 
-		if (faults & DRV8320_FAULT_VGS_LC) {
+		if (faults & DRV8320S_FAULT_VGS_LC) {
 			strcat(m_fault_print_buffer, " FETLC VGS |");
 		}
 
-		if (faults & DRV8320_FAULT_VGS_HC) {
+		if (faults & DRV8320S_FAULT_VGS_HC) {
 			strcat(m_fault_print_buffer, " FETHC VGS |");
 		}
 
-		if (faults & DRV8320_FAULT_VGS_LB) {
+		if (faults & DRV8320S_FAULT_VGS_LB) {
 			strcat(m_fault_print_buffer, " FETLB VGS |");
 		}
 
-		if (faults & DRV8320_FAULT_VGS_HB) {
+		if (faults & DRV8320S_FAULT_VGS_HB) {
 			strcat(m_fault_print_buffer, " FETHB VGS |");
 		}
 
-		if (faults & DRV8320_FAULT_VGS_LA) {
+		if (faults & DRV8320S_FAULT_VGS_LA) {
 			strcat(m_fault_print_buffer, " FETLA VGS |");
 		}
 
-		if (faults & DRV8320_FAULT_VGS_HA) {
+		if (faults & DRV8320S_FAULT_VGS_HA) {
 			strcat(m_fault_print_buffer, " FETHA VGS |");
 		}
 
-		if (faults & DRV8320_FAULT_CPUV) {
+		if (faults & DRV8320S_FAULT_CPUV) {
 			strcat(m_fault_print_buffer, " CPU V |");
 		}
 
-		if (faults & DRV8320_FAULT_OTW) {
+		if (faults & DRV8320S_FAULT_OTW) {
 			strcat(m_fault_print_buffer, " OTW |");
 		}
 
-		if (faults & DRV8320_FAULT_SC_OC) {
+		if (faults & DRV8320S_FAULT_SC_OC) {
 			strcat(m_fault_print_buffer, " AMP C OC |");
 		}
 
-		if (faults & DRV8320_FAULT_SB_OC) {
+		if (faults & DRV8320S_FAULT_SB_OC) {
 			strcat(m_fault_print_buffer, " AMP B OC |");
 		}
 
-		if (faults & DRV8320_FAULT_SA_OC) {
+		if (faults & DRV8320S_FAULT_SA_OC) {
 			strcat(m_fault_print_buffer, " AMP A OC |");
 		}
 	}
@@ -259,7 +259,7 @@ char* drv8320_faults_to_string(unsigned long faults) {
 	return m_fault_print_buffer;
 }
 
-unsigned int drv8320_read_reg(int reg) {
+unsigned int drv8320s_read_reg(int reg) {
 	uint16_t out = 0;
 	out |= (1 << 15);
 	out |= (reg & 0x0F) << 11;
@@ -278,7 +278,7 @@ unsigned int drv8320_read_reg(int reg) {
 	return res;
 }
 
-void drv8320_write_reg(int reg, int data) {
+void drv8320s_write_reg(int reg, int data) {
 	uint16_t out = 0;
 	out |= (reg & 0x0F) << 11;
 	out |= data & 0x7FF;
@@ -301,20 +301,20 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
 		uint16_t receive = 0;
 
 		for (int bit = 0;bit < 16;bit++) {
-			palWritePad(DRV8320_MOSI_GPIO, DRV8320_MOSI_PIN, send >> 15);
+			palWritePad(DRV8320S_MOSI_GPIO, DRV8320S_MOSI_PIN, send >> 15);
 			send <<= 1;
 
-			palSetPad(DRV8320_SCK_GPIO, DRV8320_SCK_PIN);
+			palSetPad(DRV8320S_SCK_GPIO, DRV8320S_SCK_PIN);
 			spi_delay();
 
-			palClearPad(DRV8320_SCK_GPIO, DRV8320_SCK_PIN);
+			palClearPad(DRV8320S_SCK_GPIO, DRV8320S_SCK_PIN);
 
 			int r1, r2, r3;
-			r1 = palReadPad(DRV8320_MISO_GPIO, DRV8320_MISO_PIN);
+			r1 = palReadPad(DRV8320S_MISO_GPIO, DRV8320S_MISO_PIN);
 			__NOP();
-			r2 = palReadPad(DRV8320_MISO_GPIO, DRV8320_MISO_PIN);
+			r2 = palReadPad(DRV8320S_MISO_GPIO, DRV8320S_MISO_PIN);
 			__NOP();
-			r3 = palReadPad(DRV8320_MISO_GPIO, DRV8320_MISO_PIN);
+			r3 = palReadPad(DRV8320S_MISO_GPIO, DRV8320S_MISO_PIN);
 
 			receive <<= 1;
 			if (utils_middle_of_3_int(r1, r2, r3)) {
@@ -331,11 +331,11 @@ static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length)
 }
 
 static void spi_begin(void) {
-	palClearPad(DRV8320_CS_GPIO, DRV8320_CS_PIN);
+	palClearPad(DRV8320S_CS_GPIO, DRV8320S_CS_PIN);
 }
 
 static void spi_end(void) {
-	palSetPad(DRV8320_CS_GPIO, DRV8320_CS_PIN);
+	palSetPad(DRV8320S_CS_GPIO, DRV8320S_CS_PIN);
 }
 
 static void spi_delay(void) {
@@ -350,7 +350,7 @@ static void terminal_read_reg(int argc, const char **argv) {
 		sscanf(argv[1], "%d", &reg);
 
 		if (reg >= 0) {
-			unsigned int res = drv8320_read_reg(reg);
+			unsigned int res = drv8320s_read_reg(reg);
 			char bl[9];
 			char bh[9];
 
@@ -374,8 +374,8 @@ static void terminal_write_reg(int argc, const char **argv) {
 		sscanf(argv[2], "%x", &val);
 
 		if (reg >= 0 && val >= 0) {
-			drv8320_write_reg(reg, val);
-			unsigned int res = drv8320_read_reg(reg);
+			drv8320s_write_reg(reg, val);
+			unsigned int res = drv8320s_read_reg(reg);
 			char bl[9];
 			char bh[9];
 
@@ -397,8 +397,8 @@ static void terminal_set_oc_adj(int argc, const char **argv) {
 		sscanf(argv[1], "%d", &val);
 
 		if (val >= 0 && val < 32) {
-			drv8320_set_oc_adj(val);
-			unsigned int res = drv8320_read_reg(5);
+			drv8320s_set_oc_adj(val);
+			unsigned int res = drv8320s_read_reg(5);
 			char bl[9];
 			char bh[9];
 
@@ -417,13 +417,13 @@ static void terminal_set_oc_adj(int argc, const char **argv) {
 static void terminal_print_faults(int argc, const char **argv) {
 	(void)argc;
 	(void)argv;
-	commands_printf(drv8320_faults_to_string(drv8320_read_faults()));
+	commands_printf(drv8320s_faults_to_string(drv8320s_read_faults()));
 }
 
 static void terminal_reset_faults(int argc, const char **argv) {
 	(void)argc;
 	(void)argv;
-	drv8320_reset_faults();
+	drv8320s_reset_faults();
 }
 
 #endif
diff --git a/hwconf/drv8320s.h b/hwconf/drv8320s.h
new file mode 100644
index 000000000..71a154fde
--- /dev/null
+++ b/hwconf/drv8320s.h
@@ -0,0 +1,58 @@
+/*
+	Copyright 2017 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/>.
+    */
+
+#ifndef HWCONF_DRV8320S_H_
+#define HWCONF_DRV8320S_H_
+
+#include "datatypes.h"
+
+// Functions
+void drv8320s_init(void);
+void drv8320s_set_oc_adj(int val);
+void drv8320s_set_oc_mode(drv8301_oc_mode mode);
+unsigned long drv8320s_read_faults(void);
+void drv8320s_reset_faults(void);
+char* drv8320s_faults_to_string(unsigned long faults);
+unsigned int drv8320s_read_reg(int reg);
+void drv8320s_write_reg(int reg, int data);
+
+// Defines
+#define DRV8320S_FAULT_FET_LC_OC		(1 << 0)
+#define DRV8320S_FAULT_FET_HC_OC		(1 << 1)
+#define DRV8320S_FAULT_FET_LB_OC		(1 << 2)
+#define DRV8320S_FAULT_FET_HB_OC		(1 << 3)
+#define DRV8320S_FAULT_FET_LA_OC		(1 << 4)
+#define DRV8320S_FAULT_FET_HA_OC		(1 << 5)
+#define DRV8320S_FAULT_OTSD			(1 << 6)
+#define DRV8320S_FAULT_UVLO			(1 << 7)
+#define DRV8320S_FAULT_GDF   		(1 << 8)
+#define DRV8320S_FAULT_VDS_OCP		(1 << 9)
+#define DRV8320S_FAULT_FAULT			(1 << 10)
+
+#define DRV8320S_FAULT_VGS_LC        (1 << 16)
+#define DRV8320S_FAULT_VGS_HC        (1 << 17)
+#define DRV8320S_FAULT_VGS_LB        (1 << 18)
+#define DRV8320S_FAULT_VGS_HB        (1 << 19)
+#define DRV8320S_FAULT_VGS_LA        (1 << 20)
+#define DRV8320S_FAULT_VGS_HA        (1 << 21)
+#define DRV8320S_FAULT_CPUV          (1 << 22)
+#define DRV8320S_FAULT_OTW           (1 << 23)
+#define DRV8320S_FAULT_SC_OC         (1 << 24)
+#define DRV8320S_FAULT_SB_OC         (1 << 25)
+#define DRV8320S_FAULT_SA_OC         (1 << 26)
+
+#endif /* HWCONF_DRV8320S_H_ */
\ No newline at end of file
diff --git a/hwconf/drv8323s.c b/hwconf/drv8323s.c
new file mode 100644
index 000000000..15900420a
--- /dev/null
+++ b/hwconf/drv8323s.c
@@ -0,0 +1,475 @@
+/*
+	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+
+	This file is part of the VESC firmware.
+
+	The VESC firmware is free software: you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    The VESC firmware is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You should have received a copy of the GNU General Public License
+    along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "hw.h"
+#ifdef HW_HAS_DRV8323S
+
+#include "drv8323s.h"
+#include "ch.h"
+#include "hal.h"
+#include "stm32f4xx_conf.h"
+#include "utils.h"
+#include "terminal.h"
+#include "commands.h"
+#include <string.h>
+#include <stdio.h>
+
+// Private functions
+static uint16_t spi_exchange(uint16_t x);
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length);
+static void spi_begin(void);
+static void spi_end(void);
+static void spi_delay(void);
+static void terminal_read_reg(int argc, const char **argv);
+static void terminal_write_reg(int argc, const char **argv);
+static void terminal_set_oc_adj(int argc, const char **argv);
+static void terminal_print_faults(int argc, const char **argv);
+static void terminal_reset_faults(int argc, const char **argv);
+
+// Private variables
+static char m_fault_print_buffer[120];
+
+void drv8323s_init(void) {
+	// DRV8323S SPI
+	palSetPadMode(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN, PAL_MODE_INPUT_PULLUP);
+	palSetPadMode(DRV8323S_SCK_GPIO, DRV8323S_SCK_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(DRV8323S_CS_GPIO, DRV8323S_CS_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(DRV8323S_MOSI_GPIO, DRV8323S_MOSI_PIN, PAL_MODE_OUTPUT_PUSHPULL | PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(DRV8323S_MOSI_GPIO, DRV8323S_MOSI_PIN);
+
+	chThdSleepMilliseconds(100);
+
+	// Disable OC
+	drv8323s_write_reg(5, 0x04C0);
+
+	// Set shunt amp gain
+	drv8323s_set_current_amp_gain(CURRENT_AMP_GAIN);
+
+	terminal_register_command_callback(
+		"drv8323s_read_reg",
+		"Read a register from the DRV8323S and print it.",
+		"[reg]",
+		terminal_read_reg);
+
+	terminal_register_command_callback(
+		"drv8323s_write_reg",
+		"Write to a DRV8323S register.",
+		"[reg] [hexvalue]",
+		terminal_write_reg);
+
+	terminal_register_command_callback(
+		"drv8323s_set_oc_adj",
+		"Set the DRV8323S OC ADJ register.",
+		"[value]",
+		terminal_set_oc_adj);
+
+	terminal_register_command_callback(
+		"drv8323s_print_faults",
+		"Print all current DRV8323S faults.",
+		0,
+		terminal_print_faults);
+
+	terminal_register_command_callback(
+		"drv8323s_reset_faults",
+		"Reset all latched DRV8323S faults.",
+		0,
+		terminal_reset_faults);
+}
+
+/**
+ * Set the threshold of the over current protection of the DRV8323S. It works by measuring
+ * the voltage drop across drain-source of the MOSFETs and activates when it is higher than
+ * a set value. Notice that this current limit is not very accurate.
+ *
+ * @param val
+ * The value to use. Range [0 15]. A lower value corresponds to a lower current limit. See
+ * the drv8323s datasheet for how to convert these values to currents.
+ */
+void drv8323s_set_oc_adj(int val) {
+	// Match with the drv8301 levels
+	val >>= 1;
+	int reg = drv8323s_read_reg(5);
+	reg &= 0xFFF0;
+	reg |= (val & 0xF);
+	drv8323s_write_reg(5, reg);
+}
+
+/**
+ * Set the gain of the shunt amps DRV8323S.
+ *
+ * @param mode
+ */
+void drv8323s_set_oc_mode(drv8301_oc_mode mode) {
+	// Match with the drv8301 modes
+	if (mode == DRV8301_OC_LATCH_SHUTDOWN) {
+		mode = DRV8301_OC_LIMIT;
+	} else if (mode == DRV8301_OC_LIMIT) {
+		mode = DRV8301_OC_LATCH_SHUTDOWN;
+	}
+	int reg = drv8323s_read_reg(5);
+	reg &= 0xFF3F;
+	reg |= (mode & 0x03) << 6;
+	drv8323s_write_reg(5, reg);
+}
+
+/**
+ * Set the gain of the DRV8323S.
+ *
+ * @param gain
+ * The gain of the shunt amps.
+ */
+void drv8323s_set_current_amp_gain(int gain) {
+	int reg = drv8323s_read_reg(6);
+	reg &= ~(0x03 << 6);
+
+	switch(gain) {
+	case 5:
+		reg |= (0 & 0x03) << 6;
+		break;
+	case 10:
+		reg |= (1 & 0x03) << 6;
+		break;
+	case 20:
+		reg |= (2 & 0x03) << 6;
+		break;
+	case 40:
+		reg |= (3 & 0x03) << 6;
+		break;
+	default:
+		//gain not supported
+		break;
+	}
+
+	drv8323s_write_reg(6, reg);
+}
+
+void drv8323s_dccal_on(void)
+{
+	int reg = drv8323s_read_reg(6);
+	reg |= (1 << 2);
+	drv8323s_write_reg(6, reg);
+}
+
+void drv8323s_dccal_off(void)
+{
+	int reg = drv8323s_read_reg(6);
+	reg &= ~(1 << 2);
+	drv8323s_write_reg(6, reg);
+}
+
+/**
+ * Read the fault codes of the DRV8323S.
+ *
+ * @return
+ * The fault codes, where the bits represent the following:
+ * b0: FETLC_OC
+ * b1: FETHC_OC
+ * b2: FETLB_OC
+ * b3: FETHB_OC
+ * b4: FETLA_OC
+ * b5: FETHA_OC
+ * b6: OTW
+ * b7: OTSD
+ * b8: PVDD_UV
+ * b9: GVDD_UV
+ * b10: FAULT
+ * b11: GVDD_OV
+ *
+ */
+unsigned long drv8323s_read_faults(void) {
+	unsigned long r0 = drv8323s_read_reg(0);
+	int r1 = drv8323s_read_reg(1);
+	return r0 | (r1 << 16);
+}
+
+/**
+ * Reset all latched faults.
+ */
+void drv8323s_reset_faults(void) {
+	int reg = drv8323s_read_reg(2);
+	reg |= 1;
+	drv8323s_write_reg(2, reg);
+}
+
+char* drv8323s_faults_to_string(unsigned long faults) {
+	if (faults == 0) {
+		strcpy(m_fault_print_buffer, "No DRV8323S faults");
+	} else {
+		strcpy(m_fault_print_buffer, "|");
+
+		if (faults & DRV8323S_FAULT_FET_LC_OC) {
+			strcat(m_fault_print_buffer, " FETLC_OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_FET_HC_OC) {
+			strcat(m_fault_print_buffer, " FETHC_OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_FET_LB_OC) {
+			strcat(m_fault_print_buffer, " FETLB_OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_FET_HB_OC) {
+			strcat(m_fault_print_buffer, " FETHB_OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_FET_LA_OC) {
+			strcat(m_fault_print_buffer, " FETLA_OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_FET_HA_OC) {
+			strcat(m_fault_print_buffer, " FETHA_OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_OTSD) {
+			strcat(m_fault_print_buffer, " OTSD |");
+		}
+
+		if (faults & DRV8323S_FAULT_UVLO) {
+			strcat(m_fault_print_buffer, " UVLO |");
+		}
+
+		if (faults & DRV8323S_FAULT_GDF) {
+			strcat(m_fault_print_buffer, " GDF |");
+		}
+
+		if (faults & DRV8323S_FAULT_VDS_OCP) {
+			strcat(m_fault_print_buffer, " VDS OCP |");
+		}
+
+		if (faults & DRV8323S_FAULT_FAULT) {
+			strcat(m_fault_print_buffer, " FAULT |");
+		}
+
+		if (faults & DRV8323S_FAULT_VGS_LC) {
+			strcat(m_fault_print_buffer, " FETLC VGS |");
+		}
+
+		if (faults & DRV8323S_FAULT_VGS_HC) {
+			strcat(m_fault_print_buffer, " FETHC VGS |");
+		}
+
+		if (faults & DRV8323S_FAULT_VGS_LB) {
+			strcat(m_fault_print_buffer, " FETLB VGS |");
+		}
+
+		if (faults & DRV8323S_FAULT_VGS_HB) {
+			strcat(m_fault_print_buffer, " FETHB VGS |");
+		}
+
+		if (faults & DRV8323S_FAULT_VGS_LA) {
+			strcat(m_fault_print_buffer, " FETLA VGS |");
+		}
+
+		if (faults & DRV8323S_FAULT_VGS_HA) {
+			strcat(m_fault_print_buffer, " FETHA VGS |");
+		}
+
+		if (faults & DRV8323S_FAULT_CPUV) {
+			strcat(m_fault_print_buffer, " CPU V |");
+		}
+
+		if (faults & DRV8323S_FAULT_OTW) {
+			strcat(m_fault_print_buffer, " OTW |");
+		}
+
+		if (faults & DRV8323S_FAULT_SC_OC) {
+			strcat(m_fault_print_buffer, " AMP C OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_SB_OC) {
+			strcat(m_fault_print_buffer, " AMP B OC |");
+		}
+
+		if (faults & DRV8323S_FAULT_SA_OC) {
+			strcat(m_fault_print_buffer, " AMP A OC |");
+		}
+	}
+
+	return m_fault_print_buffer;
+}
+
+unsigned int drv8323s_read_reg(int reg) {
+	uint16_t out = 0;
+	out |= (1 << 15);
+	out |= (reg & 0x0F) << 11;
+	out |= 0x807F;
+
+	if (reg != 0) {
+		spi_begin();
+		spi_exchange(out);
+		spi_end();
+	}
+
+	spi_begin();
+	uint16_t res = spi_exchange(out);
+	spi_end();
+
+	return res;
+}
+
+void drv8323s_write_reg(int reg, int data) {
+	uint16_t out = 0;
+	out |= (reg & 0x0F) << 11;
+	out |= data & 0x7FF;
+
+	spi_begin();
+	spi_exchange(out);
+	spi_end();
+}
+
+// Software SPI
+static uint16_t spi_exchange(uint16_t x) {
+	uint16_t rx;
+	spi_transfer(&rx, &x, 1);
+	return rx;
+}
+
+static void spi_transfer(uint16_t *in_buf, const uint16_t *out_buf, int length) {
+	for (int i = 0;i < length;i++) {
+		uint16_t send = out_buf ? out_buf[i] : 0xFFFF;
+		uint16_t receive = 0;
+
+		for (int bit = 0;bit < 16;bit++) {
+			palWritePad(DRV8323S_MOSI_GPIO, DRV8323S_MOSI_PIN, send >> 15);
+			send <<= 1;
+
+			palSetPad(DRV8323S_SCK_GPIO, DRV8323S_SCK_PIN);
+			spi_delay();
+
+			palClearPad(DRV8323S_SCK_GPIO, DRV8323S_SCK_PIN);
+
+			int r1, r2, r3;
+			r1 = palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+			__NOP();
+			r2 = palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+			__NOP();
+			r3 = palReadPad(DRV8323S_MISO_GPIO, DRV8323S_MISO_PIN);
+
+			receive <<= 1;
+			if (utils_middle_of_3_int(r1, r2, r3)) {
+				receive |= 1;
+			}
+
+			spi_delay();
+		}
+
+		if (in_buf) {
+			in_buf[i] = receive;
+		}
+	}
+}
+
+static void spi_begin(void) {
+	palClearPad(DRV8323S_CS_GPIO, DRV8323S_CS_PIN);
+}
+
+static void spi_end(void) {
+	palSetPad(DRV8323S_CS_GPIO, DRV8323S_CS_PIN);
+}
+
+static void spi_delay(void) {
+	for (volatile int i = 0;i < 10;i++) {
+		__NOP();
+	}
+}
+
+static void terminal_read_reg(int argc, const char **argv) {
+	if (argc == 2) {
+		int reg = -1;
+		sscanf(argv[1], "%d", &reg);
+
+		if (reg >= 0) {
+			unsigned int res = drv8323s_read_reg(reg);
+			char bl[9];
+			char bh[9];
+
+			utils_byte_to_binary((res >> 8) & 0xFF, bh);
+			utils_byte_to_binary(res & 0xFF, bl);
+
+			commands_printf("Reg 0x%02x: %s %s (0x%04x)\n", reg, bh, bl, res);
+		} else {
+			commands_printf("Invalid argument(s).\n");
+		}
+	} else {
+		commands_printf("This command requires one argument.\n");
+	}
+}
+
+static void terminal_write_reg(int argc, const char **argv) {
+	if (argc == 3) {
+		int reg = -1;
+		int val = -1;
+		sscanf(argv[1], "%d", &reg);
+		sscanf(argv[2], "%x", &val);
+
+		if (reg >= 0 && val >= 0) {
+			drv8323s_write_reg(reg, val);
+			unsigned int res = drv8323s_read_reg(reg);
+			char bl[9];
+			char bh[9];
+
+			utils_byte_to_binary((res >> 8) & 0xFF, bh);
+			utils_byte_to_binary(res & 0xFF, bl);
+
+			commands_printf("New reg value 0x%02x: %s %s (0x%04x)\n", reg, bh, bl, res);
+		} else {
+			commands_printf("Invalid argument(s).\n");
+		}
+	} else {
+		commands_printf("This command requires two arguments.\n");
+	}
+}
+
+static void terminal_set_oc_adj(int argc, const char **argv) {
+	if (argc == 2) {
+		int val = -1;
+		sscanf(argv[1], "%d", &val);
+
+		if (val >= 0 && val < 32) {
+			drv8323s_set_oc_adj(val);
+			unsigned int res = drv8323s_read_reg(5);
+			char bl[9];
+			char bh[9];
+
+			utils_byte_to_binary((res >> 8) & 0xFF, bh);
+			utils_byte_to_binary(res & 0xFF, bl);
+
+			commands_printf("New reg value 0x%02x: %s %s (0x%04x)\n", 2, bh, bl, res);
+		} else {
+			commands_printf("Invalid argument(s).\n");
+		}
+	} else {
+		commands_printf("This command requires one argument.\n");
+	}
+}
+
+static void terminal_print_faults(int argc, const char **argv) {
+	(void)argc;
+	(void)argv;
+	commands_printf(drv8323s_faults_to_string(drv8323s_read_faults()));
+}
+
+static void terminal_reset_faults(int argc, const char **argv) {
+	(void)argc;
+	(void)argv;
+	drv8323s_reset_faults();
+}
+
+#endif
diff --git a/hwconf/drv8323s.h b/hwconf/drv8323s.h
new file mode 100644
index 000000000..427d91930
--- /dev/null
+++ b/hwconf/drv8323s.h
@@ -0,0 +1,61 @@
+/*
+	Copyright 2017 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/>.
+    */
+
+#ifndef HWCONF_DRV8323S_H_
+#define HWCONF_DRV8323S_H_
+
+#include "datatypes.h"
+
+// Functions
+void drv8323s_init(void);
+void drv8323s_set_oc_adj(int val);
+void drv8323s_set_oc_mode(drv8301_oc_mode mode);
+void drv8323s_set_current_amp_gain(int gain);
+void drv8323s_dccal_on(void);
+void drv8323s_dccal_off(void);
+unsigned long drv8323s_read_faults(void);
+void drv8323s_reset_faults(void);
+char* drv8323s_faults_to_string(unsigned long faults);
+unsigned int drv8323s_read_reg(int reg);
+void drv8323s_write_reg(int reg, int data);
+
+// Defines
+#define DRV8323S_FAULT_FET_LC_OC		(1 << 0)
+#define DRV8323S_FAULT_FET_HC_OC		(1 << 1)
+#define DRV8323S_FAULT_FET_LB_OC		(1 << 2)
+#define DRV8323S_FAULT_FET_HB_OC		(1 << 3)
+#define DRV8323S_FAULT_FET_LA_OC		(1 << 4)
+#define DRV8323S_FAULT_FET_HA_OC		(1 << 5)
+#define DRV8323S_FAULT_OTSD			    (1 << 6)
+#define DRV8323S_FAULT_UVLO			    (1 << 7)
+#define DRV8323S_FAULT_GDF   		    (1 << 8)
+#define DRV8323S_FAULT_VDS_OCP		    (1 << 9)
+#define DRV8323S_FAULT_FAULT			(1 << 10)
+
+#define DRV8323S_FAULT_VGS_LC        (1 << 16)
+#define DRV8323S_FAULT_VGS_HC        (1 << 17)
+#define DRV8323S_FAULT_VGS_LB        (1 << 18)
+#define DRV8323S_FAULT_VGS_HB        (1 << 19)
+#define DRV8323S_FAULT_VGS_LA        (1 << 20)
+#define DRV8323S_FAULT_VGS_HA        (1 << 21)
+#define DRV8323S_FAULT_CPUV          (1 << 22)
+#define DRV8323S_FAULT_OTW           (1 << 23)
+#define DRV8323S_FAULT_SC_OC         (1 << 24)
+#define DRV8323S_FAULT_SB_OC         (1 << 25)
+#define DRV8323S_FAULT_SA_OC         (1 << 26)
+
+#endif /* HWCONF_DRV8323S_H_ */
\ No newline at end of file
diff --git a/hwconf/hw.c b/hwconf/hw.c
new file mode 100644
index 000000000..4decaecfa
--- /dev/null
+++ b/hwconf/hw.c
@@ -0,0 +1,26 @@
+/*
+	Copyright 2019 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 "conf_general.h"
+#include "utils.h"
+#include HW_SOURCE
+
+uint8_t hw_id_from_uuid(void) {
+	return utils_crc32c(STM32_UUID_8, 12) & 0x7F;
+}
diff --git a/hwconf/hw.h b/hwconf/hw.h
index 5c0a98d9a..34db88366 100644
--- a/hwconf/hw.h
+++ b/hwconf/hw.h
@@ -1,12 +1,14 @@
 /*
-	Copyright 2012-2017 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2012 - 2019 Benjamin Vedder	benjamin@vedder.se
 
-	This program is free software: you can redistribute it and/or modify
+	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.
 
-    This program is distributed in the hope that it will be useful,
+    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.
@@ -15,56 +17,20 @@
     along with this program.  If not, see <http://www.gnu.org/licenses/>.
     */
 
-/*
- * hw.h
- *
- *  Created on: 12 apr 2014
- *      Author: benjamin
- */
-
 #ifndef HW_H_
 #define HW_H_
 
 #include "conf_general.h"
 #include "stm32f4xx_conf.h"
 
-#ifdef HW_VERSION_40
-#include "hw_40.h"
-#elif defined HW_VERSION_45
-#include "hw_45.h"
-#elif defined HW_VERSION_46
-#include "hw_46.h"
-#elif defined HW_VERSION_48
-#include "hw_48.h"
-#elif defined HW_VERSION_49
-#include "hw_49.h"
-#elif defined HW_VERSION_410
-#include "hw_410.h"
-#elif defined HW_VERSION_60
-#include "hw_60.h"
-#elif defined HW_VERSION_R2
-#include "hw_r2.h"
-#elif defined HW_VERSION_VICTOR_R1A
-#include "hw_victor_r1a.h"
-#elif defined HW_VERSION_DAS_RS
-#include "hw_das_rs.h"
-#elif defined HW_VERSION_PALTA
-#include "hw_palta.h"
-#elif defined HW_VERSION_RH
-#include "hw_rh.h"
-#elif defined HW_VERSION_TP
-#include "hw_tp.h"
-#elif defined HW_VERSION_75_300
-#include "hw_75_300.h"
-#elif defined HW_VERSION_MINI4
-#include "hw_mini4.h"
-#elif defined HW_VERSION_DAS_MINI
-#include "hw_das_mini.h"
-#else
-#error "No hardware version defined"
+#include HW_HEADER
+
+#ifndef HW_NAME
+#error "No hardware name set"
 #endif
 
-// Default empty macros in case there is no hardware support
+// Default macros in case there is no hardware support or no need to change them.
+
 #ifndef ENABLE_GATE
 #define ENABLE_GATE()
 #endif
@@ -80,6 +46,7 @@
 #ifndef IS_DRV_FAULT
 #define IS_DRV_FAULT()			0
 #endif
+
 #ifndef AUX_ON
 #define AUX_ON()
 #endif
@@ -87,11 +54,124 @@
 #define AUX_OFF()
 #endif
 
+#ifndef PHASE_FILTER_ON
+#define PHASE_FILTER_ON()
+#endif
+#ifndef PHASE_FILTER_OFF
+#define PHASE_FILTER_OFF()
+#endif
+
+#ifndef CURRENT_FILTER_ON
+#define CURRENT_FILTER_ON()
+#endif
+#ifndef CURRENT_FILTER_OFF
+#define CURRENT_FILTER_OFF()
+#endif
+
+// Individual MOSFET temperature sensors. Override if available.
+#ifndef NTC_TEMP_MOS1
+#define NTC_TEMP_MOS1()		0.0
+#endif
+#ifndef NTC_TEMP_MOS2
+#define NTC_TEMP_MOS2()		0.0
+#endif
+#ifndef NTC_TEMP_MOS3
+#define NTC_TEMP_MOS3()		0.0
+#endif
+
+// Current ADC macros. Override them for custom current measurement functions.
+#ifndef GET_CURRENT1
+#ifdef INVERTED_SHUNT_POLARITY
+#define GET_CURRENT1()		(4095 - ADC_Value[ADC_IND_CURR1])
+#else
+#define GET_CURRENT1()		ADC_Value[ADC_IND_CURR1]
+#endif
+#endif
+#ifndef GET_CURRENT2
+#ifdef INVERTED_SHUNT_POLARITY
+#define GET_CURRENT2()		(4095 - ADC_Value[ADC_IND_CURR2])
+#else
+#define GET_CURRENT2()		ADC_Value[ADC_IND_CURR2]
+#endif
+#endif
+#ifndef GET_CURRENT3
+#ifdef INVERTED_SHUNT_POLARITY
+#define GET_CURRENT3()		(4095 - ADC_Value[ADC_IND_CURR3])
+#else
+#define GET_CURRENT3()		ADC_Value[ADC_IND_CURR3]
+#endif
+#endif
+
+// NRF SW SPI (default to spi header pins)
+#ifndef NRF_PORT_CSN
+#define NRF_PORT_CSN			HW_SPI_PORT_NSS
+#endif
+#ifndef NRF_PIN_CSN
+#define NRF_PIN_CSN				HW_SPI_PIN_NSS
+#endif
+#ifndef NRF_PORT_SCK
+#define NRF_PORT_SCK			HW_SPI_PORT_SCK
+#endif
+#ifndef NRF_PIN_SCK
+#define NRF_PIN_SCK				HW_SPI_PIN_SCK
+#endif
+#ifndef NRF_PORT_MOSI
+#define NRF_PORT_MOSI			HW_SPI_PORT_MOSI
+#endif
+#ifndef NRF_PIN_MOSI
+#define NRF_PIN_MOSI			HW_SPI_PIN_MOSI
+#endif
+#ifndef NRF_PORT_MISO
+#define NRF_PORT_MISO			HW_SPI_PORT_MISO
+#endif
+#ifndef NRF_PIN_MISO
+#define NRF_PIN_MISO			HW_SPI_PIN_MISO
+#endif
+
+// CAN device and port (default CAN1)
+#ifndef HW_CANH_PORT
+#define HW_CANH_PORT			GPIOB
+#endif
+#ifndef HW_CANH_PIN
+#define HW_CANH_PIN				8
+#endif
+#ifndef HW_CANL_PORT
+#define HW_CANL_PORT			GPIOB
+#endif
+#ifndef HW_CANL_PIN
+#define HW_CANL_PIN				9
+#endif
+#ifndef HW_CAN_GPIO_AF
+#define HW_CAN_GPIO_AF			GPIO_AF_CAN1
+#endif
+#ifndef HW_CAN_DEV
+#define HW_CAN_DEV				CAND1
+#endif
+
+// Hook to call when trying to initialize the permanent NRF failed. Can be
+// used to e.g. reconfigure pins.
+#ifndef HW_PERMANENT_NRF_FAILED_HOOK
+#define HW_PERMANENT_NRF_FAILED_HOOK()
+#endif
+
+// Default ID
+#ifndef HW_DEFAULT_ID
+#define HW_DEFAULT_ID			(APPCONF_CONTROLLER_ID >= 0 ? APPCONF_CONTROLLER_ID : hw_id_from_uuid())
+#endif
+
+#ifndef HW_LIM_CURRENT
+#define HW_LIM_CURRENT			-100.0, 100.0
+#endif
+#ifndef HW_LIM_CURRENT_ABS
+#define HW_LIM_CURRENT_ABS		0.0, 140.0
+#endif
+
 // Functions
 void hw_init_gpio(void);
 void hw_setup_adc_channels(void);
 void hw_start_i2c(void);
 void hw_stop_i2c(void);
 void hw_try_restore_i2c(void);
+uint8_t hw_id_from_uuid(void);
 
 #endif /* HW_H_ */
diff --git a/hwconf/hw_40.c b/hwconf/hw_40.c
index a06963667..37671e09e 100644
--- a/hwconf/hw_40.c
+++ b/hwconf/hw_40.c
@@ -23,7 +23,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_40
 
 #include "ch.h"
 #include "hal.h"
@@ -231,5 +230,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_40.h b/hwconf/hw_40.h
index 856caf47c..bd0f50a42 100644
--- a/hwconf/hw_40.h
+++ b/hwconf/hw_40.h
@@ -113,7 +113,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
diff --git a/hwconf/hw_410.c b/hwconf/hw_410.c
index 53a18788c..2637fa89f 100644
--- a/hwconf/hw_410.c
+++ b/hwconf/hw_410.c
@@ -16,7 +16,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_410
 
 #include "ch.h"
 #include "hal.h"
@@ -223,5 +222,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_410.h b/hwconf/hw_410.h
index 232e89307..ac40e963c 100644
--- a/hwconf/hw_410.h
+++ b/hwconf/hw_410.h
@@ -111,7 +111,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
@@ -153,16 +153,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM4_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM4_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOA
-#define NRF_PIN_CSN				4
-#define NRF_PORT_SCK			GPIOA
-#define NRF_PIN_SCK				5
-#define NRF_PORT_MOSI			GPIOA
-#define NRF_PIN_MOSI			7
-#define NRF_PORT_MISO			GPIOA
-#define NRF_PIN_MISO			6
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
diff --git a/hwconf/hw_45.c b/hwconf/hw_45.c
index 41c4f669e..17f4a7243 100644
--- a/hwconf/hw_45.c
+++ b/hwconf/hw_45.c
@@ -23,7 +23,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_45
 
 #include "ch.h"
 #include "hal.h"
@@ -32,7 +31,8 @@
 
 // Threads
 THD_FUNCTION(temp_thread, arg);
-static THD_WORKING_AREA(temp_thread_wa, 1024);
+static THD_WORKING_AREA(temp_thread_wa, 512);
+static bool temp_thread_running = false;
 
 // Variables
 static volatile bool i2c_running = false;
@@ -144,7 +144,10 @@ void hw_setup_adc_channels(void) {
 	ADC_InjectedChannelConfig(ADC2, ADC_Channel_6, 2, ADC_SampleTime_15Cycles);
 
 	// Setup i2c temperature sensor here
-	chThdCreateStatic(temp_thread_wa, sizeof(temp_thread_wa), NORMALPRIO, temp_thread, NULL);
+	if (!temp_thread_running) {
+		chThdCreateStatic(temp_thread_wa, sizeof(temp_thread_wa), NORMALPRIO, temp_thread, NULL);
+		temp_thread_running = true;
+	}
 }
 
 void hw_start_i2c(void) {
@@ -280,5 +283,3 @@ THD_FUNCTION(temp_thread, arg) {
 float hw45_get_temp(void) {
 	return temp_now;
 }
-
-#endif
diff --git a/hwconf/hw_45.h b/hwconf/hw_45.h
index 3094b77b5..cfb4068da 100644
--- a/hwconf/hw_45.h
+++ b/hwconf/hw_45.h
@@ -116,7 +116,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
diff --git a/hwconf/hw_46.c b/hwconf/hw_46.c
index ac397f243..7bf8b1c03 100644
--- a/hwconf/hw_46.c
+++ b/hwconf/hw_46.c
@@ -23,7 +23,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_46
 
 #include "ch.h"
 #include "hal.h"
@@ -231,5 +230,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_46.h b/hwconf/hw_46.h
index 87278602c..b027b2128 100644
--- a/hwconf/hw_46.h
+++ b/hwconf/hw_46.h
@@ -117,7 +117,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
diff --git a/hwconf/hw_48.c b/hwconf/hw_48.c
index 28ca8c25b..3b975328e 100644
--- a/hwconf/hw_48.c
+++ b/hwconf/hw_48.c
@@ -16,7 +16,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_48
 
 #include "ch.h"
 #include "hal.h"
@@ -223,5 +222,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_48.h b/hwconf/hw_48.h
index 83db3e751..e0054812a 100644
--- a/hwconf/hw_48.h
+++ b/hwconf/hw_48.h
@@ -111,7 +111,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
diff --git a/hwconf/hw_49.c b/hwconf/hw_49.c
index ef4982da1..13584f3fd 100644
--- a/hwconf/hw_49.c
+++ b/hwconf/hw_49.c
@@ -16,7 +16,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_49
 
 #include "ch.h"
 #include "hal.h"
@@ -224,5 +223,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_49.h b/hwconf/hw_49.h
index a0c9c2728..c760233d5 100644
--- a/hwconf/hw_49.h
+++ b/hwconf/hw_49.h
@@ -111,7 +111,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
@@ -153,16 +153,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM4_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM4_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOB
-#define NRF_PIN_CSN				11
-#define NRF_PORT_SCK			GPIOA
-#define NRF_PIN_SCK				5
-#define NRF_PORT_MOSI			GPIOA
-#define NRF_PIN_MOSI			7
-#define NRF_PORT_MISO			GPIOA
-#define NRF_PIN_MISO			6
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
diff --git a/hwconf/hw_60.c b/hwconf/hw_60.c
index 2e048dc8e..82d23c6f9 100644
--- a/hwconf/hw_60.c
+++ b/hwconf/hw_60.c
@@ -16,7 +16,6 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_60
 
 #include "ch.h"
 #include "hal.h"
@@ -62,6 +61,13 @@ void hw_init_gpio(void) {
 
 	ENABLE_GATE();
 
+	// Current filter
+	palSetPadMode(GPIOD, 2,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	CURRENT_FILTER_OFF();
+
 	// GPIOA Configuration: Channel 1 to 3 as alternate function push-pull
 	palSetPadMode(GPIOA, 8, PAL_MODE_ALTERNATE(GPIO_AF_TIM1) |
 			PAL_STM32_OSPEED_HIGHEST |
@@ -236,5 +242,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_60.h b/hwconf/hw_60.h
index d251bd9df..55029da34 100644
--- a/hwconf/hw_60.h
+++ b/hwconf/hw_60.h
@@ -45,6 +45,19 @@
 #define LED_RED_ON()			palSetPad(GPIOB, 1)
 #define LED_RED_OFF()			palClearPad(GPIOB, 1)
 
+#define CURRENT_FILTER_ON()		palSetPad(GPIOD, 2)
+#define CURRENT_FILTER_OFF()	palClearPad(GPIOD, 2)
+
+// Switch on current filter if a permanent
+// NRF24 cannot be found, as the later
+// HW60 has changed one of the permanent NRF
+// pins to the current filter activation pin.
+#define HW_PERMANENT_NRF_FAILED_HOOK() \
+			palSetPadMode(GPIOD, 2, \
+			PAL_MODE_OUTPUT_PUSHPULL | \
+			PAL_STM32_OSPEED_HIGHEST); \
+			CURRENT_FILTER_ON()
+
 /*
  * ADC Vector
  *
@@ -131,7 +144,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
@@ -139,6 +152,7 @@
 #define HW_UART_RX_PIN			11
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_TIMER			TIM4
 #define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
 #define HW_ICU_DEV				ICUD4
@@ -224,7 +238,7 @@
 #define MCCONF_L_MAX_ABS_CURRENT		150.0	// The maximum absolute current above which a fault is generated
 #endif
 #ifndef MCCONF_FOC_SAMPLE_V0_V7
-#define MCCONF_FOC_SAMPLE_V0_V7			true	// Run control loop in both v0 and v7 (requires phase shunts)
+#define MCCONF_FOC_SAMPLE_V0_V7			false	// Run control loop in both v0 and v7 (requires phase shunts)
 #endif
 
 // Setting limits
diff --git a/hwconf/hw_75_300.c b/hwconf/hw_75_300.c
index 24e7a8ce5..0b1159532 100644
--- a/hwconf/hw_75_300.c
+++ b/hwconf/hw_75_300.c
@@ -16,7 +16,6 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_75_300
 
 #include "ch.h"
 #include "hal.h"
@@ -76,15 +75,19 @@ 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);
 
-	// Fault pin
-	palSetPadMode(GPIOB, 7, 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);
+
+	CURRENT_FILTER_OFF();
+
 	// AUX pin
 	AUX_OFF();
 	palSetPadMode(AUX_GPIO, AUX_PIN,
@@ -99,6 +102,9 @@ void hw_init_gpio(void) {
 	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);
@@ -254,5 +260,3 @@ float hw75_300_get_temp(void) {
 
 	return res;
 }
-
-#endif
diff --git a/hwconf/hw_75_300.h b/hwconf/hw_75_300.h
index 37f7c662b..f361cf835 100644
--- a/hwconf/hw_75_300.h
+++ b/hwconf/hw_75_300.h
@@ -25,7 +25,7 @@
 // HW properties
 #define HW_HAS_3_SHUNTS
 #define HW_HAS_PHASE_SHUNTS
-//#define HW_HAS_PHASE_FILTERS // TODO: Does not work on this HW
+//#define HW_HAS_PHASE_FILTERS
 
 // Macros
 #ifdef HW75_300_VEDDER_FIRST_PCB
@@ -55,6 +55,9 @@
 #define AUX_ON()				palSetPad(AUX_GPIO, AUX_PIN)
 #define AUX_OFF()				palClearPad(AUX_GPIO, AUX_PIN)
 
+#define CURRENT_FILTER_ON()		palSetPad(GPIOD, 2)
+#define CURRENT_FILTER_OFF()	palClearPad(GPIOD, 2)
+
 /*
  * ADC Vector
  *
@@ -138,6 +141,12 @@
 #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)
 #endif
 
+#ifndef HW75_300_VEDDER_FIRST_PCB
+#define NTC_TEMP_MOS1()			(1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#define NTC_TEMP_MOS2()			(1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_2]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#define NTC_TEMP_MOS3()			(1.0 / ((logf(NTC_RES(ADC_Value[ADC_IND_TEMP_MOS_3]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+#endif
+
 // Voltage on ADC channel
 #define ADC_VOLTS(ch)			((float)ADC_Value[ch] / 4096.0 * V_REG)
 
@@ -154,7 +163,7 @@
 #endif
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
@@ -162,6 +171,7 @@
 #define HW_UART_RX_PIN			11
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_TIMER			TIM4
 #define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
 #define HW_ICU_DEV				ICUD4
@@ -196,16 +206,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOA
-#define NRF_PIN_CSN				4
-#define NRF_PORT_SCK			GPIOA
-#define NRF_PIN_SCK				5
-#define NRF_PORT_MOSI			GPIOA
-#define NRF_PIN_MOSI			7
-#define NRF_PORT_MISO			GPIOA
-#define NRF_PIN_MISO			6
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
@@ -243,16 +243,22 @@
 #define MCCONF_FOC_F_SW					30000.0
 #endif
 #ifndef MCCONF_L_MAX_ABS_CURRENT
-#define MCCONF_L_MAX_ABS_CURRENT		150.0	// The maximum absolute current above which a fault is generated
+#define MCCONF_L_MAX_ABS_CURRENT		420.0	// The maximum absolute current above which a fault is generated
 #endif
 #ifndef MCCONF_FOC_SAMPLE_V0_V7
-#define MCCONF_FOC_SAMPLE_V0_V7			true	// Run control loop in both v0 and v7 (requires phase shunts)
+#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			-300.0, 300.0
 #define HW_LIM_CURRENT_IN		-300.0, 300.0
-#define HW_LIM_CURRENT_ABS		0.0, 420.0
+#define HW_LIM_CURRENT_ABS		0.0, 450.0
 #define HW_LIM_VIN				6.0, 72.0
 #define HW_LIM_ERPM				-200e3, 200e3
 #define HW_LIM_DUTY_MIN			0.0, 0.1
diff --git a/hwconf/hw_das_mini.c b/hwconf/hw_das_mini.c
index 2f97d5378..8700863b8 100644
--- a/hwconf/hw_das_mini.c
+++ b/hwconf/hw_das_mini.c
@@ -16,13 +16,12 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_DAS_MINI
 
 #include "ch.h"
 #include "hal.h"
 #include "stm32f4xx_conf.h"
 #include "utils.h"
-#include "drv8320.h"
+#include "drv8320s.h"
 
 // Variables
 static volatile bool i2c_running = false;
@@ -100,7 +99,7 @@ void hw_init_gpio(void) {
 	palSetPadMode(GPIOC, 4, PAL_MODE_INPUT_ANALOG);
 	palSetPadMode(GPIOC, 5, PAL_MODE_INPUT_ANALOG);
 
-	drv8320_init();
+	drv8320s_init();
 }
 
 void hw_setup_adc_channels(void) {
@@ -230,5 +229,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_das_mini.h b/hwconf/hw_das_mini.h
index de183dd27..67a152bb0 100644
--- a/hwconf/hw_das_mini.h
+++ b/hwconf/hw_das_mini.h
@@ -23,7 +23,7 @@
 #define HW_NAME					"DAS_MINI"
 
 // HW properties
-#define HW_HAS_DRV8320
+#define HW_HAS_DRV8320S
 #define HW_HAS_3_SHUNTS
 #define HW_HAS_PHASE_SHUNTS
 
@@ -110,6 +110,8 @@
 // Voltage on ADC channel
 #define ADC_VOLTS(ch)			((float)ADC_Value[ch] / 4096.0 * V_REG)
 
+#define HW_DEAD_TIME_VALUE		20
+
 // 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
@@ -126,7 +128,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
@@ -134,6 +136,7 @@
 #define HW_UART_RX_PIN			11
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_DEV				ICUD4
 #define HW_ICU_CHANNEL			ICU_CHANNEL_1
 #define HW_ICU_GPIO_AF			GPIO_AF_TIM4
@@ -166,16 +169,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOB
-#define NRF_PIN_CSN				12
-#define NRF_PORT_SCK			GPIOB
-#define NRF_PIN_SCK				4
-#define NRF_PORT_MOSI			GPIOB
-#define NRF_PIN_MOSI			3
-#define NRF_PORT_MISO			GPIOD
-#define NRF_PIN_MISO			2
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
@@ -188,15 +181,15 @@
 #define HW_SPI_PORT_MISO		GPIOA
 #define HW_SPI_PIN_MISO			6
 
-// SPI for DRV8320
-#define DRV8320_MOSI_GPIO		GPIOC
-#define DRV8320_MOSI_PIN		12
-#define DRV8320_MISO_GPIO		GPIOC
-#define DRV8320_MISO_PIN		11
-#define DRV8320_SCK_GPIO		GPIOC
-#define DRV8320_SCK_PIN			10
-#define DRV8320_CS_GPIO			GPIOC
-#define DRV8320_CS_PIN			9
+// SPI for DRV8320S
+#define DRV8320S_MOSI_GPIO		GPIOC
+#define DRV8320S_MOSI_PIN		12
+#define DRV8320S_MISO_GPIO		GPIOC
+#define DRV8320S_MISO_PIN		11
+#define DRV8320S_SCK_GPIO		GPIOC
+#define DRV8320S_SCK_PIN		10
+#define DRV8320S_CS_GPIO		GPIOC
+#define DRV8320S_CS_PIN			9
 
 // Measurement macros
 #define ADC_V_L1				ADC_Value[ADC_IND_SENS1]
diff --git a/hwconf/hw_das_rs.c b/hwconf/hw_das_rs.c
index 77b82ae0a..227b3b7ff 100644
--- a/hwconf/hw_das_rs.c
+++ b/hwconf/hw_das_rs.c
@@ -16,7 +16,6 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_DAS_RS
 
 #include "ch.h"
 #include "hal.h"
@@ -220,5 +219,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_das_rs.h b/hwconf/hw_das_rs.h
index a429024a7..107d26d91 100644
--- a/hwconf/hw_das_rs.h
+++ b/hwconf/hw_das_rs.h
@@ -143,7 +143,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
@@ -151,6 +151,7 @@
 #define HW_UART_RX_PIN			11
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_TIMER			TIM4
 #define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
 #define HW_ICU_DEV				ICUD4
@@ -185,16 +186,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOB
-#define NRF_PIN_CSN				12
-#define NRF_PORT_SCK			GPIOB
-#define NRF_PIN_SCK				4
-#define NRF_PORT_MOSI			GPIOB
-#define NRF_PIN_MOSI			3
-#define NRF_PORT_MISO			GPIOD
-#define NRF_PIN_MISO			2
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
diff --git a/hwconf/hw_mini4.c b/hwconf/hw_mini4.c
index a7dfec8e1..cfd3ea110 100644
--- a/hwconf/hw_mini4.c
+++ b/hwconf/hw_mini4.c
@@ -16,7 +16,6 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_MINI4
 
 #include "ch.h"
 #include "hal.h"
@@ -229,5 +228,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_mini4.h b/hwconf/hw_mini4.h
index a71fb385d..48d610db6 100644
--- a/hwconf/hw_mini4.h
+++ b/hwconf/hw_mini4.h
@@ -127,7 +127,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
@@ -135,6 +135,7 @@
 #define HW_UART_RX_PIN			11
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_DEV				ICUD4
 #define HW_ICU_CHANNEL			ICU_CHANNEL_1
 #define HW_ICU_GPIO_AF			GPIO_AF_TIM4
@@ -167,16 +168,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOB
-#define NRF_PIN_CSN				12
-#define NRF_PORT_SCK			GPIOB
-#define NRF_PIN_SCK				4
-#define NRF_PORT_MOSI			GPIOB
-#define NRF_PIN_MOSI			3
-#define NRF_PORT_MISO			GPIOD
-#define NRF_PIN_MISO			2
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
diff --git a/hwconf/hw_palta.c b/hwconf/hw_palta.c
index f0a44f17c..ec41d27e0 100644
--- a/hwconf/hw_palta.c
+++ b/hwconf/hw_palta.c
@@ -16,7 +16,6 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_PALTA
 
 #include "ch.h"
 #include "hal.h"
@@ -261,5 +260,3 @@ static void terminal_cmd_reset_oc(int argc, const char **argv) {
 	commands_printf("Palta OC latch reset done!");
 	commands_printf(" ");
 }
-
-#endif
diff --git a/hwconf/hw_palta.h b/hwconf/hw_palta.h
index 1cc77e9e9..294a3c7b3 100644
--- a/hwconf/hw_palta.h
+++ b/hwconf/hw_palta.h
@@ -86,7 +86,7 @@
 #define V_REG					3.3
 #endif
 #ifndef VIN_R1
-#define VIN_R1					246730.0
+#define VIN_R1					294400.0    //TF RevB = 134.81V/V
 #endif
 #ifndef VIN_R2
 #define VIN_R2					2200.0
@@ -95,7 +95,7 @@
 #define CURRENT_AMP_GAIN		8.0
 #endif
 #ifndef CURRENT_SHUNT_RES
-#define CURRENT_SHUNT_RES		0.0004852941
+#define CURRENT_SHUNT_RES		0.000415 //TF 300A/V
 #endif
 
 // Input voltage
@@ -127,7 +127,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
@@ -135,6 +135,7 @@
 #define HW_UART_RX_PIN			11
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_TIMER			TIM4
 #define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
 #define HW_ICU_DEV				ICUD4
@@ -169,6 +170,12 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
+// Resolver interface pins
+#define AD2S1205_SAMPLE_GPIO	GPIOB
+#define AD2S1205_SAMPLE_PIN		3
+#define AD2S1205_RDVEL_GPIO		GPIOC
+#define AD2S1205_RDVEL_PIN		12
+
 // NRF pins
 #define NRF_PORT_CSN			GPIOB
 #define NRF_PIN_CSN				12
@@ -203,7 +210,7 @@
 #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_VALUE		202
+#define HW_DEAD_TIME_VALUE		181
 
 // Default setting overrides
 #ifndef MCCONF_DEFAULT_MOTOR_TYPE
diff --git a/hwconf/hw_r2.c b/hwconf/hw_r2.c
index 9a29a39c5..c6551d833 100644
--- a/hwconf/hw_r2.c
+++ b/hwconf/hw_r2.c
@@ -23,7 +23,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_R2
 
 #include "ch.h"
 #include "hal.h"
@@ -236,5 +235,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_r2.h b/hwconf/hw_r2.h
index 861969f0f..fc547cea5 100644
--- a/hwconf/hw_r2.h
+++ b/hwconf/hw_r2.h
@@ -127,7 +127,7 @@
 #define HW_SERVO_NUM		2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#define HW_UART_DEV				SD3
 #define HW_UART_GPIO_AF			GPIO_AF_USART3
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			10
diff --git a/hwconf/hw_rh.c b/hwconf/hw_rh.c
index 3ccdf11ff..93afde34b 100644
--- a/hwconf/hw_rh.c
+++ b/hwconf/hw_rh.c
@@ -16,7 +16,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_RH
 
 #include "ch.h"
 #include "hal.h"
@@ -229,5 +228,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_rh.h b/hwconf/hw_rh.h
index 901cc2435..f34d43093 100644
--- a/hwconf/hw_rh.h
+++ b/hwconf/hw_rh.h
@@ -24,6 +24,7 @@
 #define HW_HAS_3_SHUNTS
 #define HW_HAS_PHASE_SHUNTS
 #define HW_HAS_DRV8305
+#define HW_USE_INTERNAL_RC
 
 // Macros
 #define ENABLE_GATE()			palSetPad(GPIOG, 6)
@@ -117,7 +118,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
@@ -125,6 +126,7 @@
 #define HW_UART_RX_PIN			7
 
 // ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
 #define HW_ICU_TIMER			TIM4
 #define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
 #define HW_ICU_DEV				ICUD4
@@ -159,16 +161,6 @@
 #define HW_ENC_TIM_ISR_CH		TIM3_IRQn
 #define HW_ENC_TIM_ISR_VEC		TIM3_IRQHandler
 
-// NRF pins
-#define NRF_PORT_CSN			GPIOA
-#define NRF_PIN_CSN				4
-#define NRF_PORT_SCK			GPIOA
-#define NRF_PIN_SCK				5
-#define NRF_PORT_MOSI			GPIOA
-#define NRF_PIN_MOSI			7
-#define NRF_PORT_MISO			GPIOA
-#define NRF_PIN_MISO			6
-
 // SPI pins
 #define HW_SPI_DEV				SPID1
 #define HW_SPI_GPIO_AF			GPIO_AF_SPI1
diff --git a/hwconf/hw_tp.c b/hwconf/hw_tp.c
index e29a937f3..a3f4cd755 100644
--- a/hwconf/hw_tp.c
+++ b/hwconf/hw_tp.c
@@ -16,7 +16,6 @@
     */
 
 #include "hw.h"
-#ifdef HW_VERSION_TP
 
 #include "ch.h"
 #include "hal.h"
@@ -247,5 +246,3 @@ float hwtp_get_temp(void) {
 
 	return res;
 }
-
-#endif
diff --git a/hwconf/hw_tp.h b/hwconf/hw_tp.h
index 29c96d182..4bcf20e66 100644
--- a/hwconf/hw_tp.h
+++ b/hwconf/hw_tp.h
@@ -126,7 +126,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD3
+#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
diff --git a/hwconf/hw_uavc_basic.c b/hwconf/hw_uavc_basic.c
new file mode 100644
index 000000000..ab5e0c32c
--- /dev/null
+++ b/hwconf/hw_uavc_basic.c
@@ -0,0 +1,243 @@
+/*
+	Copyright 2019 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 "drv8323s.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(GPIOB, 0,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(GPIOB, 1,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	// ENABLE_GATE
+	palSetPadMode(GPIOB, 5,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	// Disable BMI160
+	palSetPadMode(GPIOA, 15,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(GPIOA, 15);
+
+	// Disable DCCAL
+	palSetPadMode(GPIOD, 2,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palClearPad(GPIOD, 2);
+
+	ENABLE_GATE();
+
+	// 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);
+
+	// Fault pin
+	palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);
+
+	// 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(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);
+
+	drv8323s_init();
+}
+
+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);
+
+	// 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);
+
+	// 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);
+
+	// 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);
+	}
+}
diff --git a/hwconf/hw_uavc_basic.h b/hwconf/hw_uavc_basic.h
new file mode 100644
index 000000000..e7c0dc3da
--- /dev/null
+++ b/hwconf/hw_uavc_basic.h
@@ -0,0 +1,250 @@
+/*
+	Copyright 2019 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_UAVC_BASIC_H_
+#define HW_UAVC_BASIC_H_
+
+#include "drv8323s.h"
+
+#define HW_NAME					"UAVC_BASIC"
+
+// HW properties
+#define HW_HAS_DRV8323S
+#define HW_HAS_3_SHUNTS
+
+// Macros
+#define ENABLE_GATE()			palSetPad(GPIOB, 5)
+#define DISABLE_GATE()			palClearPad(GPIOB, 5)
+
+#define IS_DRV_FAULT()			(!palReadPad(GPIOB, 7))
+
+#define LED_GREEN_ON()			palSetPad(GPIOB, 0)
+#define LED_GREEN_OFF()			palClearPad(GPIOB, 0)
+#define LED_RED_ON()			palSetPad(GPIOB, 1)
+#define LED_RED_OFF()			palClearPad(GPIOB, 1)
+
+#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)
+
+/*
+ * ADC Vector
+ *
+ * 0:	IN0		SENS1
+ * 1:	IN1		SENS2
+ * 2:	IN2		SENS3
+ * 3:	IN10	CURR1
+ * 4:	IN11	CURR2
+ * 5:	IN12	CURR3
+ * 6:	IN5		ADC_EXT1
+ * 7:	IN6		ADC_EXT2
+ * 8:	IN3		TEMP_PCB
+ * 9:	IN14	TEMP_MOTOR
+ * 10:	IN15	ADC_EXT3
+ * 11:	IN13	AN_IN
+ * 12:	Vrefint
+ * 13:	IN0		SENS1
+ * 14:	IN1		SENS2
+ */
+
+#define HW_ADC_CHANNELS			15
+#define HW_ADC_INJ_CHANNELS		3
+#define HW_ADC_NBR_CONV			5
+
+// 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_TEMP_MOS		8
+#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					39000.0
+#endif
+#ifndef VIN_R2
+#define VIN_R2					2200.0
+#endif
+#ifndef CURRENT_AMP_GAIN
+#define CURRENT_AMP_GAIN		20.0
+#endif
+#ifndef CURRENT_SHUNT_RES
+#define CURRENT_SHUNT_RES		0.0005
+#endif
+
+// Input voltage
+#define GET_INPUT_VOLTAGE()		((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
+
+// NTC Termistors
+#define NTC_RES(adc_val)		((4095.0 * 10000.0) / adc_val - 10000.0)
+#define NTC_TEMP(adc_ind)		(1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+
+#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)
+
+// Voltage on ADC channel
+#define ADC_VOLTS(ch)			((float)ADC_Value[ch] / 4096.0 * V_REG)
+
+// 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
+
+// Number of servo outputs
+#define HW_SERVO_NUM			1
+
+// 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
+
+// Permanent UART Peripheral (for NRF51)
+#define HW_UART_P_BAUD			115200
+#define HW_UART_P_DEV			SD4
+#define HW_UART_P_GPIO_AF		GPIO_AF_UART4
+#define HW_UART_P_TX_PORT		GPIOC
+#define HW_UART_P_TX_PIN		10
+#define HW_UART_P_RX_PORT		GPIOC
+#define HW_UART_P_RX_PIN		11
+
+// ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
+#define HW_ICU_DEV				ICUD4
+#define HW_ICU_CHANNEL			ICU_CHANNEL_1
+#define HW_ICU_GPIO_AF			GPIO_AF_TIM4
+#define HW_ICU_GPIO				GPIOB
+#define HW_ICU_PIN				6
+
+// 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
+
+// SPI for DRV8323S
+#define DRV8323S_MOSI_GPIO		GPIOC
+#define DRV8323S_MOSI_PIN		12
+#define DRV8323S_MISO_GPIO		GPIOB
+#define DRV8323S_MISO_PIN		4
+#define DRV8323S_SCK_GPIO		GPIOB
+#define DRV8323S_SCK_PIN		3
+#define DRV8323S_CS_GPIO		GPIOC
+#define DRV8323S_CS_PIN			9
+
+// 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)
+
+// Default setting overrides
+#ifndef MCCONF_L_CURRENT_MAX
+#define MCCONF_L_CURRENT_MAX				60.0	// Current limit in Amperes (Upper)
+#endif
+#ifndef MCCONF_L_CURRENT_MIN
+#define MCCONF_L_CURRENT_MIN				-60.0	// Current limit in Amperes (Lower)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MAX
+#define MCCONF_L_IN_CURRENT_MAX				60.0	// Input current limit in Amperes (Upper)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MIN
+#define MCCONF_L_IN_CURRENT_MIN				-60.0	// Input current limit in Amperes (Lower)
+#endif
+#ifndef MCCONF_L_MAX_ABS_CURRENT
+#define MCCONF_L_MAX_ABS_CURRENT			120.0	// The maximum absolute current above which a fault is generated
+#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
+
+// Setting limits
+#define HW_LIM_CURRENT			-60.0, 60.0
+#define HW_LIM_CURRENT_IN		-60.0, 60.0
+#define HW_LIM_CURRENT_ABS		0.0, 140.0
+#define HW_LIM_VIN				6.0, 57.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
+
+#endif /* HW_UAVC_QCUBE_H_ */
diff --git a/hwconf/hw_uavc_qcube.c b/hwconf/hw_uavc_qcube.c
new file mode 100644
index 000000000..f2d5e140b
--- /dev/null
+++ b/hwconf/hw_uavc_qcube.c
@@ -0,0 +1,272 @@
+/*
+	Copyright 2019 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 "drv8323s.h"
+
+// Threads
+THD_FUNCTION(dac_thread, arg);
+static THD_WORKING_AREA(dac_thread_wa, 512);
+static bool dac_thread_running = false;
+
+// 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(GPIOB, 0,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPadMode(GPIOB, 1,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	// ENABLE_GATE
+	palSetPadMode(GPIOB, 5,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+
+	// Disable BMI160
+	palSetPadMode(GPIOA, 15,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palSetPad(GPIOA, 15);
+
+	// Disable DCCAL
+	palSetPadMode(GPIOD, 2,
+			PAL_MODE_OUTPUT_PUSHPULL |
+			PAL_STM32_OSPEED_HIGHEST);
+	palClearPad(GPIOD, 2);
+
+	ENABLE_GATE();
+
+	// 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);
+
+	// Fault pin
+	palSetPadMode(GPIOB, 7, PAL_MODE_INPUT_PULLUP);
+
+	// 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(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);
+
+	drv8323s_init();
+}
+
+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_4, 3, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_14, 4, ADC_SampleTime_15Cycles);
+	ADC_RegularChannelConfig(ADC1, ADC_Channel_Vrefint, 5, 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);
+
+	// 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);
+
+	// 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);
+
+	if (!dac_thread_running) {
+		chThdCreateStatic(dac_thread_wa, sizeof(dac_thread_wa), NORMALPRIO, dac_thread, NULL);
+		dac_thread_running = true;
+	}
+}
+
+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);
+	}
+}
+
+THD_FUNCTION(dac_thread, arg) {
+	(void)arg;
+
+	chRegSetThreadName("DAC");
+
+	RCC_APB1PeriphClockCmd(RCC_APB1Periph_DAC, ENABLE);
+	DAC->CR |= DAC_CR_EN2;
+
+	int i = 0;
+	for(;;) {
+		DAC->DHR12R2 = i++;
+		if (i == 4096) {
+			i = 0;
+		}
+
+		chThdSleepMilliseconds(10);
+	}
+}
diff --git a/hwconf/hw_uavc_qcube.h b/hwconf/hw_uavc_qcube.h
new file mode 100644
index 000000000..ad845b42e
--- /dev/null
+++ b/hwconf/hw_uavc_qcube.h
@@ -0,0 +1,253 @@
+/*
+	Copyright 2019 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_UAVC_QCUBE_H_
+#define HW_UAVC_QCUBE_H_
+
+#include "drv8323s.h"
+
+#define HW_NAME					"UAVC_QCUBE"
+
+// HW properties
+#define HW_HAS_DRV8323S
+#define HW_HAS_3_SHUNTS
+
+// Macros
+#define ENABLE_GATE()			palSetPad(GPIOB, 5)
+#define DISABLE_GATE()			palClearPad(GPIOB, 5)
+
+#define IS_DRV_FAULT()			(!palReadPad(GPIOB, 7))
+
+#define LED_GREEN_ON()			palSetPad(GPIOB, 0)
+#define LED_GREEN_OFF()			palClearPad(GPIOB, 0)
+#define LED_RED_ON()			palSetPad(GPIOB, 1)
+#define LED_RED_OFF()			palClearPad(GPIOB, 1)
+
+#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)
+
+/*
+ * ADC Vector
+ *
+ * 0:	IN0		SENS1
+ * 1:	IN1		SENS2
+ * 2:	IN2		SENS3
+ * 3:	IN10	CURR1
+ * 4:	IN11	CURR2
+ * 5:	IN12	CURR3
+ * 6:	IN5		ADC_EXT1
+ * 7:	IN6		ADC_EXT2
+ * 8:	IN3		TEMP_PCB
+ * 9:	IN14	TEMP_MOTOR
+ * 10:	IN15	ADC_EXT3
+ * 11:	IN13	AN_IN
+ * 12:	Vrefint
+ * 13:	IN0		SENS1
+ * 14:	IN1		SENS2
+ */
+
+#define HW_ADC_CHANNELS			15
+#define HW_ADC_INJ_CHANNELS		3
+#define HW_ADC_NBR_CONV			5
+
+// 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_TEMP_MOS		8
+#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					39000.0
+#endif
+#ifndef VIN_R2
+#define VIN_R2					2200.0
+#endif
+#ifndef CURRENT_AMP_GAIN
+#define CURRENT_AMP_GAIN		20.0
+#endif
+#ifndef CURRENT_SHUNT_RES
+#define CURRENT_SHUNT_RES		0.0005
+#endif
+
+// Input voltage
+#define GET_INPUT_VOLTAGE()		((V_REG / 4095.0) * (float)ADC_Value[ADC_IND_VIN_SENS] * ((VIN_R1 + VIN_R2) / VIN_R2))
+
+// NTC Termistors
+#define NTC_RES(adc_val)		((4095.0 * 10000.0) / adc_val - 10000.0)
+#define NTC_TEMP(adc_ind)		(1.0 / ((logf(NTC_RES(ADC_Value[adc_ind]) / 10000.0) / 3380.0) + (1.0 / 298.15)) - 273.15)
+
+#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)
+
+// Voltage on ADC channel
+#define ADC_VOLTS(ch)			((float)ADC_Value[ch] / 4096.0 * V_REG)
+
+// 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
+
+// Number of servo outputs
+#define HW_SERVO_NUM			1
+
+// 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
+
+// Permanent UART Peripheral (for NRF51)
+#define HW_UART_P_BAUD			115200
+#define HW_UART_P_DEV			SD4
+#define HW_UART_P_GPIO_AF		GPIO_AF_UART4
+#define HW_UART_P_TX_PORT		GPIOC
+#define HW_UART_P_TX_PIN		10
+#define HW_UART_P_RX_PORT		GPIOC
+#define HW_UART_P_RX_PIN		11
+
+// ICU Peripheral for servo decoding
+#define HW_USE_SERVO_TIM4
+#define HW_ICU_DEV				ICUD4
+#define HW_ICU_CHANNEL			ICU_CHANNEL_1
+#define HW_ICU_GPIO_AF			GPIO_AF_TIM4
+#define HW_ICU_GPIO				GPIOB
+#define HW_ICU_PIN				6
+
+// 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
+
+// SPI for DRV8323S
+#define DRV8323S_MOSI_GPIO		GPIOC
+#define DRV8323S_MOSI_PIN		12
+#define DRV8323S_MISO_GPIO		GPIOB
+#define DRV8323S_MISO_PIN		4
+#define DRV8323S_SCK_GPIO		GPIOB
+#define DRV8323S_SCK_PIN		3
+#define DRV8323S_CS_GPIO		GPIOC
+#define DRV8323S_CS_PIN			9
+
+// 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)
+
+// Default setting overrides
+#ifndef MCCONF_L_CURRENT_MAX
+#define MCCONF_L_CURRENT_MAX				60.0	// Current limit in Amperes (Upper)
+#endif
+#ifndef MCCONF_L_CURRENT_MIN
+#define MCCONF_L_CURRENT_MIN				-60.0	// Current limit in Amperes (Lower)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MAX
+#define MCCONF_L_IN_CURRENT_MAX				60.0	// Input current limit in Amperes (Upper)
+#endif
+#ifndef MCCONF_L_IN_CURRENT_MIN
+#define MCCONF_L_IN_CURRENT_MIN				-60.0	// Input current limit in Amperes (Lower)
+#endif
+#ifndef MCCONF_L_MAX_ABS_CURRENT
+#define MCCONF_L_MAX_ABS_CURRENT			120.0	// The maximum absolute current above which a fault is generated
+#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_FOC_SAMPLE_V0_V7
+#define MCCONF_FOC_SAMPLE_V0_V7			false	// Run control loop in both v0 and v7 (requires phase shunts)
+#endif
+
+// Setting limits
+#define HW_LIM_CURRENT			-60.0, 60.0
+#define HW_LIM_CURRENT_IN		-60.0, 60.0
+#define HW_LIM_CURRENT_ABS		0.0, 140.0
+#define HW_LIM_VIN				6.0, 57.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
+
+#endif /* HW_UAVC_QCUBE_H_ */
diff --git a/hwconf/hw_victor_r1a.c b/hwconf/hw_victor_r1a.c
index 4d50bbec1..d9d89325a 100644
--- a/hwconf/hw_victor_r1a.c
+++ b/hwconf/hw_victor_r1a.c
@@ -23,7 +23,6 @@
  */
 
 #include "hw.h"
-#ifdef HW_VERSION_VICTOR_R1A
 
 #include "ch.h"
 #include "hal.h"
@@ -231,5 +230,3 @@ void hw_try_restore_i2c(void) {
 		i2cReleaseBus(&HW_I2C_DEV);
 	}
 }
-
-#endif
diff --git a/hwconf/hw_victor_r1a.h b/hwconf/hw_victor_r1a.h
index e3519d980..d08047b77 100644
--- a/hwconf/hw_victor_r1a.h
+++ b/hwconf/hw_victor_r1a.h
@@ -114,7 +114,7 @@
 #define HW_SERVO_NUM			2
 
 // UART Peripheral
-#define HW_UART_DEV				UARTD6
+#define HW_UART_DEV				SD6
 #define HW_UART_GPIO_AF			GPIO_AF_USART6
 #define HW_UART_TX_PORT			GPIOC
 #define HW_UART_TX_PIN			6
diff --git a/hwconf/hwconf.mk b/hwconf/hwconf.mk
index 3fd12080e..648f182e3 100644
--- a/hwconf/hwconf.mk
+++ b/hwconf/hwconf.mk
@@ -1,21 +1,7 @@
-HWSRC =	hwconf/hw_40.c \
-	hwconf/hw_45.c \
-	hwconf/hw_r2.c \
-	hwconf/hw_46.c \
-	hwconf/hw_48.c \
-	hwconf/hw_49.c \
-	hwconf/hw_410.c \
-	hwconf/hw_60.c \
-	hwconf/hw_victor_r1a.c \
-	hwconf/hw_das_rs.c \
+HWSRC =	hwconf/hw.c \
 	hwconf/drv8301.c \
 	hwconf/drv8305.c \
-	hwconf/drv8320.c \
-	hwconf/hw_palta.c \
-	hwconf/hw_rh.c \
-	hwconf/hw_tp.c \
-	hwconf/hw_75_300.c \
-	hwconf/hw_mini4.c \
-	hwconf/hw_das_mini.c
+	hwconf/drv8320s.c \
+	hwconf/drv8323s.c
 
 HWINC = hwconf
diff --git a/libcanard/README.md b/libcanard/README.md
new file mode 100644
index 000000000..277387c0e
--- /dev/null
+++ b/libcanard/README.md
@@ -0,0 +1,91 @@
+# Libcanard
+[![Forum](https://img.shields.io/discourse/https/forum.uavcan.org/users.svg)](https://forum.uavcan.org)
+[![Build Status](https://travis-ci.org/UAVCAN/libcanard.svg?branch=master)](https://travis-ci.org/UAVCAN/libcanard)
+[![Coverity Scan](https://scan.coverity.com/projects/uavcan-libcanard/badge.svg)](https://scan.coverity.com/projects/uavcan-libcanard)
+
+Minimal implementation of the UAVCAN protocol stack in C for resource constrained applications.
+
+Get help on the **[UAVCAN Forum](https://forum.uavcan.org)**.
+
+## Usage
+
+If you're not using Git, you can just copy the entire library into your project tree.
+If you're using Git, it is recommended to add Libcanard to your project as a Git submodule,
+like this:
+
+```bash
+git submodule add https://github.com/UAVCAN/libcanard
+```
+
+The entire library is contained in three files:
+
+- `canard.c` - the only translation unit; add it to your build or compile it into a separate static library;
+- `canard.h` - the API header; include it in your application;
+- `canard_internals.h` - internal definitions of the library;
+keep this file in the same directory with `canard.c`.
+
+Add `canard.c` to your application build, add `libcanard` directory to the include paths,
+and you're ready to roll.
+
+Also you may want to use one of the available drivers for various CAN backends
+that are distributed with Libcanard - check out the `drivers/` directory to find out more.
+
+If you wish to override some of the default options, e.g., assert macros' definition,
+define the macro `CANARD_ENABLE_CUSTOM_BUILD_CONFIG` as a non-zero value
+(e.g. for GCC or Clang: `-DCANARD_ENABLE_CUSTOM_BUILD_CONFIG=1`)
+and provide your implementation in a file named `canard_build_config.h`.
+
+Example for Make:
+
+```make
+# Adding the library.
+INCLUDE += libcanard
+CSRC += libcanard/canard.c
+
+# Adding drivers, unless you want to use your own.
+# In this example we're using Linux SocketCAN drivers.
+INCLUDE += libcanard/drivers/socketcan
+CSRC += libcanard/drivers/socketcan/socketcan.c
+```
+
+There is no dedicated documentation for the library API, because it is simple enough to be self-documenting.
+Please check out the explanations provided in the comments in the header file to learn the basics.
+Most importantly, check out the demo application under `tests/demo.c`.
+Also use [code search to find real life usage examples](https://github.com/search?q=libcanard&type=Code&utf8=%E2%9C%93).
+
+At the moment the library does not provide means to automate (de)serialization of UAVCAN data structures,
+like other implementations (e.g. libuavcan for C++ or pyuavcan for Python) do.
+Therefore, data structures need to be parsed and assembled manually.
+The necessary examples are provided in the demo application.
+
+## Library Development
+
+This section is intended only for library developers and contributors.
+
+The library design document can be found in [DESIGN.md](DESIGN.md)
+
+Contributors, please follow the [Zubax C++ Coding Conventions](https://kb.zubax.com/x/84Ah).
+
+### Building and Running Tests
+
+```bash
+mkdir build && cd build
+cmake ../libcanard/tests    # Adjust path if necessary
+make
+./run_tests
+```
+
+### Submitting a Coverity Scan Build
+
+First, [get the Coverity build tool](https://scan.coverity.com/download?tab=cxx).
+Then build the tests with it:
+
+```bash
+export PATH=$PATH:<coverity-build-tool-directory>/bin/
+mkdir build && cd build
+cmake ../libcanard/tests -DCMAKE_BUILD_TYPE=Debug   # Adjust path if necessary
+cov-build --dir cov-int make -j8
+tar czvf libcanard.tgz cov-int
+```
+
+Then upload the resulting archive to Coverity.
diff --git a/libcanard/canard.c b/libcanard/canard.c
new file mode 100644
index 000000000..4efd01c6a
--- /dev/null
+++ b/libcanard/canard.c
@@ -0,0 +1,1585 @@
+/*
+ * Copyright (c) 2016-2017 UAVCAN Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Contributors: https://github.com/UAVCAN/libcanard/contributors
+ *
+ * Documentation: http://uavcan.org/Implementations/Libcanard
+ */
+
+#include "canard_internals.h"
+#include <string.h>
+
+
+#undef MIN
+#undef MAX
+#define MIN(a, b)   (((a) < (b)) ? (a) : (b))
+#define MAX(a, b)   (((a) > (b)) ? (a) : (b))
+
+
+#define TRANSFER_TIMEOUT_USEC                       2000000
+
+#define TRANSFER_ID_BIT_LEN                         5U
+#define ANON_MSG_DATA_TYPE_ID_BIT_LEN               2U
+
+#define SOURCE_ID_FROM_ID(x)                        ((uint8_t) (((x) >> 0U)  & 0x7FU))
+#define SERVICE_NOT_MSG_FROM_ID(x)                  ((bool)    (((x) >> 7U)  & 0x1U))
+#define REQUEST_NOT_RESPONSE_FROM_ID(x)             ((bool)    (((x) >> 15U) & 0x1U))
+#define DEST_ID_FROM_ID(x)                          ((uint8_t) (((x) >> 8U)  & 0x7FU))
+#define PRIORITY_FROM_ID(x)                         ((uint8_t) (((x) >> 24U) & 0x1FU))
+#define MSG_TYPE_FROM_ID(x)                         ((uint16_t)(((x) >> 8U)  & 0xFFFFU))
+#define SRV_TYPE_FROM_ID(x)                         ((uint8_t) (((x) >> 16U) & 0xFFU))
+
+#define MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, src_node_id, dst_node_id)             \
+    (((uint32_t)(data_type_id)) | (((uint32_t)(transfer_type)) << 16U) |                            \
+    (((uint32_t)(src_node_id)) << 18U) | (((uint32_t)(dst_node_id)) << 25U))
+
+#define TRANSFER_ID_FROM_TAIL_BYTE(x)               ((uint8_t)((x) & 0x1FU))
+
+// The extra cast to unsigned is needed to squelch warnings from clang-tidy
+#define IS_START_OF_TRANSFER(x)                     ((bool)(((uint32_t)(x) >> 7U) & 0x1U))
+#define IS_END_OF_TRANSFER(x)                       ((bool)(((uint32_t)(x) >> 6U) & 0x1U))
+#define TOGGLE_BIT(x)                               ((bool)(((uint32_t)(x) >> 5U) & 0x1U))
+
+
+struct CanardTxQueueItem
+{
+    CanardTxQueueItem* next;
+    CanardCANFrame frame;
+};
+
+
+/*
+ * API functions
+ */
+void canardInit(CanardInstance* out_ins,
+                void* mem_arena,
+                size_t mem_arena_size,
+                CanardOnTransferReception on_reception,
+                CanardShouldAcceptTransfer should_accept,
+                void* user_reference)
+{
+    CANARD_ASSERT(out_ins != NULL);
+
+    /*
+     * Checking memory layout.
+     * This condition is supposed to be true for all 32-bit and smaller platforms.
+     * If your application fails here, make sure it's not built in 64-bit mode.
+     * Refer to the design documentation for more info.
+     */
+    CANARD_ASSERT(CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE >= 6);
+
+    memset(out_ins, 0, sizeof(*out_ins));
+
+    out_ins->node_id = CANARD_BROADCAST_NODE_ID;
+    out_ins->on_reception = on_reception;
+    out_ins->should_accept = should_accept;
+    out_ins->rx_states = NULL;
+    out_ins->tx_queue = NULL;
+    out_ins->user_reference = user_reference;
+
+    size_t pool_capacity = mem_arena_size / CANARD_MEM_BLOCK_SIZE;
+    if (pool_capacity > 0xFFFFU)
+    {
+        pool_capacity = 0xFFFFU;
+    }
+
+    initPoolAllocator(&out_ins->allocator, mem_arena, (uint16_t)pool_capacity);
+}
+
+void* canardGetUserReference(CanardInstance* ins)
+{
+    CANARD_ASSERT(ins != NULL);
+    return ins->user_reference;
+}
+
+void canardSetLocalNodeID(CanardInstance* ins, uint8_t self_node_id)
+{
+    CANARD_ASSERT(ins != NULL);
+
+    if ((ins->node_id == CANARD_BROADCAST_NODE_ID) &&
+        (self_node_id >= CANARD_MIN_NODE_ID) &&
+        (self_node_id <= CANARD_MAX_NODE_ID))
+    {
+        ins->node_id = self_node_id;
+    }
+    else
+    {
+        CANARD_ASSERT(false);
+    }
+}
+
+uint8_t canardGetLocalNodeID(const CanardInstance* ins)
+{
+    return ins->node_id;
+}
+
+int16_t canardBroadcast(CanardInstance* ins,
+                        uint64_t data_type_signature,
+                        uint16_t data_type_id,
+                        uint8_t* inout_transfer_id,
+                        uint8_t priority,
+                        const void* payload,
+                        uint16_t payload_len)
+{
+    if (payload == NULL && payload_len > 0)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+    if (priority > CANARD_TRANSFER_PRIORITY_LOWEST)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+
+    uint32_t can_id = 0;
+    uint16_t crc = 0xFFFFU;
+
+    if (canardGetLocalNodeID(ins) == 0)
+    {
+        if (payload_len > 7)
+        {
+            return -CANARD_ERROR_NODE_ID_NOT_SET;
+        }
+
+        static const uint16_t DTIDMask = (1U << ANON_MSG_DATA_TYPE_ID_BIT_LEN) - 1U;
+
+        if ((data_type_id & DTIDMask) != data_type_id)
+        {
+            return -CANARD_ERROR_INVALID_ARGUMENT;
+        }
+
+        // anonymous transfer, random discriminator
+        const uint16_t discriminator = (uint16_t)((crcAdd(0xFFFFU, payload, payload_len)) & 0x7FFEU);
+        can_id = ((uint32_t) priority << 24U) | ((uint32_t) discriminator << 9U) |
+                 ((uint32_t) (data_type_id & DTIDMask) << 8U) | (uint32_t) canardGetLocalNodeID(ins);
+    }
+    else
+    {
+        can_id = ((uint32_t) priority << 24U) | ((uint32_t) data_type_id << 8U) | (uint32_t) canardGetLocalNodeID(ins);
+
+        if (payload_len > 7)
+        {
+            crc = crcAddSignature(crc, data_type_signature);
+            crc = crcAdd(crc, payload, payload_len);
+        }
+    }
+
+    const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len);
+
+    incrementTransferID(inout_transfer_id);
+
+    return result;
+}
+
+int16_t canardRequestOrRespond(CanardInstance* ins,
+                               uint8_t destination_node_id,
+                               uint64_t data_type_signature,
+                               uint8_t data_type_id,
+                               uint8_t* inout_transfer_id,
+                               uint8_t priority,
+                               CanardRequestResponse kind,
+                               const void* payload,
+                               uint16_t payload_len)
+{
+    if (payload == NULL && payload_len > 0)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+    if (priority > CANARD_TRANSFER_PRIORITY_LOWEST)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+    if (canardGetLocalNodeID(ins) == 0)
+    {
+        return -CANARD_ERROR_NODE_ID_NOT_SET;
+    }
+
+    const uint32_t can_id = ((uint32_t) priority << 24U) | ((uint32_t) data_type_id << 16U) |
+                            ((uint32_t) kind << 15U) | ((uint32_t) destination_node_id << 8U) |
+                            (1U << 7U) | (uint32_t) canardGetLocalNodeID(ins);
+    uint16_t crc = 0xFFFFU;
+
+    if (payload_len > 7)
+    {
+        crc = crcAddSignature(crc, data_type_signature);
+        crc = crcAdd(crc, payload, payload_len);
+    }
+
+    const int16_t result = enqueueTxFrames(ins, can_id, inout_transfer_id, crc, payload, payload_len);
+
+    if (kind == CanardRequest)                      // Response Transfer ID must not be altered
+    {
+        incrementTransferID(inout_transfer_id);
+    }
+
+    return result;
+}
+
+const CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins)
+{
+    if (ins->tx_queue == NULL)
+    {
+        return NULL;
+    }
+    return &ins->tx_queue->frame;
+}
+
+void canardPopTxQueue(CanardInstance* ins)
+{
+    CanardTxQueueItem* item = ins->tx_queue;
+    ins->tx_queue = item->next;
+    freeBlock(&ins->allocator, item);
+}
+
+void canardHandleRxFrame(CanardInstance* ins, const CanardCANFrame* frame, uint64_t timestamp_usec)
+{
+    const CanardTransferType transfer_type = extractTransferType(frame->id);
+    const uint8_t destination_node_id = (transfer_type == CanardTransferTypeBroadcast) ?
+                                        (uint8_t)CANARD_BROADCAST_NODE_ID :
+                                        DEST_ID_FROM_ID(frame->id);
+
+    // TODO: This function should maintain statistics of transfer errors and such.
+
+    if ((frame->id & CANARD_CAN_FRAME_EFF) == 0 ||
+        (frame->id & CANARD_CAN_FRAME_RTR) != 0 ||
+        (frame->id & CANARD_CAN_FRAME_ERR) != 0 ||
+        (frame->data_len < 1))
+    {
+        return;     // Unsupported frame, not UAVCAN - ignore
+    }
+
+    if (transfer_type != CanardTransferTypeBroadcast &&
+        destination_node_id != canardGetLocalNodeID(ins))
+    {
+        return;     // Address mismatch
+    }
+
+    const uint8_t priority = PRIORITY_FROM_ID(frame->id);
+    const uint8_t source_node_id = SOURCE_ID_FROM_ID(frame->id);
+    const uint16_t data_type_id = extractDataType(frame->id);
+    const uint32_t transfer_descriptor =
+            MAKE_TRANSFER_DESCRIPTOR(data_type_id, transfer_type, source_node_id, destination_node_id);
+
+    const uint8_t tail_byte = frame->data[frame->data_len - 1];
+
+    CanardRxState* rx_state = NULL;
+
+    if (IS_START_OF_TRANSFER(tail_byte))
+    {
+        uint64_t data_type_signature = 0;
+
+        if (ins->should_accept(ins, &data_type_signature, data_type_id, transfer_type, source_node_id))
+        {
+            rx_state = traverseRxStates(ins, transfer_descriptor);
+
+            if(rx_state == NULL)
+            {
+                return; // No allocator room for this frame
+            }
+
+            rx_state->calculated_crc = crcAddSignature(0xFFFFU, data_type_signature);
+        }
+        else
+        {
+            return;     // The application doesn't want this transfer
+        }
+    }
+    else
+    {
+        rx_state = findRxState(ins->rx_states, transfer_descriptor);
+
+        if (rx_state == NULL)
+        {
+            return;
+        }
+    }
+
+    CANARD_ASSERT(rx_state != NULL);    // All paths that lead to NULL should be terminated with return above
+
+    // Resolving the state flags:
+    const bool not_initialized = rx_state->timestamp_usec == 0;
+    const bool tid_timed_out = (timestamp_usec - rx_state->timestamp_usec) > TRANSFER_TIMEOUT_USEC;
+    const bool first_frame = IS_START_OF_TRANSFER(tail_byte);
+    const bool not_previous_tid =
+        computeTransferIDForwardDistance((uint8_t) rx_state->transfer_id, TRANSFER_ID_FROM_TAIL_BYTE(tail_byte)) > 1;
+
+    const bool need_restart =
+            (not_initialized) ||
+            (tid_timed_out) ||
+            (first_frame && not_previous_tid);
+
+    if (need_restart)
+    {
+        rx_state->transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte);
+        rx_state->next_toggle = 0;
+        releaseStatePayload(ins, rx_state);
+        if (!IS_START_OF_TRANSFER(tail_byte)) // missed the first frame
+        {
+            rx_state->transfer_id++;
+            return;
+        }
+    }
+
+    if (IS_START_OF_TRANSFER(tail_byte) && IS_END_OF_TRANSFER(tail_byte)) // single frame transfer
+    {
+        rx_state->timestamp_usec = timestamp_usec;
+        CanardRxTransfer rx_transfer = {
+            .timestamp_usec = timestamp_usec,
+            .payload_head = frame->data,
+            .payload_len = (uint8_t)(frame->data_len - 1U),
+            .data_type_id = data_type_id,
+            .transfer_type = transfer_type,
+            .transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte),
+            .priority = priority,
+            .source_node_id = source_node_id
+        };
+
+        ins->on_reception(ins, &rx_transfer);
+
+        prepareForNextTransfer(rx_state);
+        return;
+    }
+
+    if (TOGGLE_BIT(tail_byte) != rx_state->next_toggle)
+    {
+        return; // wrong toggle
+    }
+
+    if (TRANSFER_ID_FROM_TAIL_BYTE(tail_byte) != rx_state->transfer_id)
+    {
+        return; // unexpected tid
+    }
+
+    if (IS_START_OF_TRANSFER(tail_byte) && !IS_END_OF_TRANSFER(tail_byte))      // Beginning of multi frame transfer
+    {
+        if (frame->data_len <= 3)
+        {
+            return;     // Not enough data
+        }
+
+        // take off the crc and store the payload
+        rx_state->timestamp_usec = timestamp_usec;
+        const int16_t ret = bufferBlockPushBytes(&ins->allocator, rx_state, frame->data + 2,
+                                                 (uint8_t) (frame->data_len - 3));
+        if (ret < 0)
+        {
+            releaseStatePayload(ins, rx_state);
+            prepareForNextTransfer(rx_state);
+            return;
+        }
+        rx_state->payload_crc = (uint16_t)(((uint16_t) frame->data[0]) | (uint16_t)((uint16_t) frame->data[1] << 8U));
+        rx_state->calculated_crc = crcAdd((uint16_t)rx_state->calculated_crc,
+                                          frame->data + 2, (uint8_t)(frame->data_len - 3));
+    }
+    else if (!IS_START_OF_TRANSFER(tail_byte) && !IS_END_OF_TRANSFER(tail_byte))    // Middle of a multi-frame transfer
+    {
+        const int16_t ret = bufferBlockPushBytes(&ins->allocator, rx_state, frame->data,
+                                                 (uint8_t) (frame->data_len - 1));
+        if (ret < 0)
+        {
+            releaseStatePayload(ins, rx_state);
+            prepareForNextTransfer(rx_state);
+            return;
+        }
+        rx_state->calculated_crc = crcAdd((uint16_t)rx_state->calculated_crc,
+                                          frame->data, (uint8_t)(frame->data_len - 1));
+    }
+    else                                                                            // End of a multi-frame transfer
+    {
+        const uint8_t frame_payload_size = (uint8_t)(frame->data_len - 1);
+
+        uint8_t tail_offset = 0;
+
+        if (rx_state->payload_len < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE)
+        {
+            // Copy the beginning of the frame into the head, point the tail pointer to the remainder
+            for (size_t i = rx_state->payload_len;
+                 (i < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) && (tail_offset < frame_payload_size);
+                 i++, tail_offset++)
+            {
+                rx_state->buffer_head[i] = frame->data[tail_offset];
+            }
+        }
+        else
+        {
+            // Like above, except that the beginning goes into the last block of the storage
+            CanardBufferBlock* block = rx_state->buffer_blocks;
+            if (block != NULL)          // If there's no middle, that's fine, we'll use only head and tail
+            {
+                size_t offset = CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE;    // Payload offset of the first block
+                while (block->next != NULL)
+                {
+                    block = block->next;
+                    offset += CANARD_BUFFER_BLOCK_DATA_SIZE;
+                }
+                CANARD_ASSERT(block != NULL);
+
+                const size_t offset_within_block = rx_state->payload_len - offset;
+                CANARD_ASSERT(offset_within_block < CANARD_BUFFER_BLOCK_DATA_SIZE);
+
+                for (size_t i = offset_within_block;
+                     (i < CANARD_BUFFER_BLOCK_DATA_SIZE) && (tail_offset < frame_payload_size);
+                     i++, tail_offset++)
+                {
+                    block->data[i] = frame->data[tail_offset];
+                }
+            }
+        }
+
+        CanardRxTransfer rx_transfer = {
+            .timestamp_usec = timestamp_usec,
+            .payload_head = rx_state->buffer_head,
+            .payload_middle = rx_state->buffer_blocks,
+            .payload_tail = (tail_offset >= frame_payload_size) ? NULL : (&frame->data[tail_offset]),
+            .payload_len = (uint16_t)(rx_state->payload_len + frame_payload_size),
+            .data_type_id = data_type_id,
+            .transfer_type = transfer_type,
+            .transfer_id = TRANSFER_ID_FROM_TAIL_BYTE(tail_byte),
+            .priority = priority,
+            .source_node_id = source_node_id
+        };
+
+        rx_state->buffer_blocks = NULL;     // Block list ownership has been transferred to rx_transfer!
+
+        // CRC validation
+        rx_state->calculated_crc = crcAdd((uint16_t)rx_state->calculated_crc, frame->data, frame->data_len - 1U);
+        if (rx_state->calculated_crc == rx_state->payload_crc)
+        {
+            ins->on_reception(ins, &rx_transfer);
+        }
+
+        // Making sure the payload is released even if the application didn't bother with it
+        canardReleaseRxTransferPayload(ins, &rx_transfer);
+        prepareForNextTransfer(rx_state);
+        return;
+    }
+
+    rx_state->next_toggle = rx_state->next_toggle ? 0 : 1;
+}
+
+void canardCleanupStaleTransfers(CanardInstance* ins, uint64_t current_time_usec)
+{
+    CanardRxState* prev = ins->rx_states, * state = ins->rx_states;
+
+    while (state != NULL)
+    {
+        if ((current_time_usec - state->timestamp_usec) > TRANSFER_TIMEOUT_USEC)
+        {
+            if (state == ins->rx_states)
+            {
+                releaseStatePayload(ins, state);
+                ins->rx_states = ins->rx_states->next;
+                freeBlock(&ins->allocator, state);
+                state = ins->rx_states;
+                prev = state;
+            }
+            else
+            {
+                releaseStatePayload(ins, state);
+                prev->next = state->next;
+                freeBlock(&ins->allocator, state);
+                state = prev->next;
+            }
+        }
+        else
+        {
+            prev = state;
+            state = state->next;
+        }
+    }
+}
+
+int16_t canardDecodeScalar(const CanardRxTransfer* transfer,
+                           uint32_t bit_offset,
+                           uint8_t bit_length,
+                           bool value_is_signed,
+                           void* out_value)
+{
+    if (transfer == NULL || out_value == NULL)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+
+    if (bit_length < 1 || bit_length > 64)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+
+    if (bit_length == 1 && value_is_signed)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+
+    /*
+     * Reading raw bytes into the temporary storage.
+     * Luckily, C guarantees that every element is aligned at the beginning (lower address) of the union.
+     */
+    union
+    {
+        bool     boolean;       ///< sizeof(bool) is implementation-defined, so it has to be handled separately
+        uint8_t  u8;            ///< Also char
+        int8_t   s8;
+        uint16_t u16;
+        int16_t  s16;
+        uint32_t u32;
+        int32_t  s32;           ///< Also float, possibly double, possibly long double (depends on implementation)
+        uint64_t u64;
+        int64_t  s64;           ///< Also double, possibly float, possibly long double (depends on implementation)
+        uint8_t bytes[8];
+    } storage;
+
+    memset(&storage, 0, sizeof(storage));   // This is important
+
+    const int16_t result = descatterTransferPayload(transfer, bit_offset, bit_length, &storage.bytes[0]);
+    if (result <= 0)
+    {
+        return result;
+    }
+
+    CANARD_ASSERT((result > 0) && (result <= 64) && (result <= bit_length));
+
+    /*
+     * The bit copy algorithm assumes that more significant bits have lower index, so we need to shift some.
+     * Extra most significant bits will be filled with zeroes, which is fine.
+     * Coverity Scan mistakenly believes that the array may be overrun if bit_length == 64; however, this branch will
+     * not be taken if bit_length == 64, because 64 % 8 == 0.
+     */
+    if ((bit_length % 8) != 0)
+    {
+        // coverity[overrun-local]
+        storage.bytes[bit_length / 8U] = (uint8_t)(storage.bytes[bit_length / 8U] >> ((8U - (bit_length % 8U)) & 7U));
+    }
+
+    /*
+     * Determining the closest standard byte length - this will be needed for byte reordering and sign bit extension.
+     */
+    uint8_t std_byte_length = 0;
+    if      (bit_length == 1)   { std_byte_length = sizeof(bool); }
+    else if (bit_length <= 8)   { std_byte_length = 1; }
+    else if (bit_length <= 16)  { std_byte_length = 2; }
+    else if (bit_length <= 32)  { std_byte_length = 4; }
+    else if (bit_length <= 64)  { std_byte_length = 8; }
+    else
+    {
+        CANARD_ASSERT(false);
+        return -CANARD_ERROR_INTERNAL;
+    }
+
+    CANARD_ASSERT((std_byte_length > 0) && (std_byte_length <= 8));
+
+    /*
+     * Flipping the byte order if needed.
+     */
+    if (isBigEndian())
+    {
+        swapByteOrder(&storage.bytes[0], std_byte_length);
+    }
+
+    /*
+     * Extending the sign bit if needed. I miss templates.
+     * Note that we operate on unsigned values in order to avoid undefined behaviors.
+     */
+    if (value_is_signed && (std_byte_length * 8 != bit_length))
+    {
+        if (bit_length <= 8)
+        {
+            if ((storage.u8 & (1U << (bit_length - 1U))) != 0)                           // If the sign bit is set...
+            {
+                storage.u8 |= (uint8_t) 0xFFU & (uint8_t) ~((1U << bit_length) - 1U);   // ...set all bits above it.
+            }
+        }
+        else if (bit_length <= 16)
+        {
+            if ((storage.u16 & (1U << (bit_length - 1U))) != 0)
+            {
+                storage.u16 |= (uint16_t) 0xFFFFU & (uint16_t) ~((1U << bit_length) - 1U);
+            }
+        }
+        else if (bit_length <= 32)
+        {
+            if ((storage.u32 & (((uint32_t) 1) << (bit_length - 1U))) != 0)
+            {
+                storage.u32 |= (uint32_t) 0xFFFFFFFFUL & (uint32_t) ~((((uint32_t) 1) << bit_length) - 1U);
+            }
+        }
+        else if (bit_length < 64)   // Strictly less, this is not a typo
+        {
+            if ((storage.u64 & (((uint64_t) 1) << (bit_length - 1U))) != 0)
+            {
+                storage.u64 |= (uint64_t) 0xFFFFFFFFFFFFFFFFULL & (uint64_t) ~((((uint64_t) 1) << bit_length) - 1U);
+            }
+        }
+        else
+        {
+            CANARD_ASSERT(false);
+            return -CANARD_ERROR_INTERNAL;
+        }
+    }
+
+    /*
+     * Copying the result out.
+     */
+    if (value_is_signed)
+    {
+        if      (bit_length <= 8)   { *( (int8_t*) out_value) = storage.s8;  }
+        else if (bit_length <= 16)  { *((int16_t*) out_value) = storage.s16; }
+        else if (bit_length <= 32)  { *((int32_t*) out_value) = storage.s32; }
+        else if (bit_length <= 64)  { *((int64_t*) out_value) = storage.s64; }
+        else
+        {
+            CANARD_ASSERT(false);
+            return -CANARD_ERROR_INTERNAL;
+        }
+    }
+    else
+    {
+        if      (bit_length == 1)   { *(    (bool*) out_value) = storage.boolean; }
+        else if (bit_length <= 8)   { *( (uint8_t*) out_value) = storage.u8;  }
+        else if (bit_length <= 16)  { *((uint16_t*) out_value) = storage.u16; }
+        else if (bit_length <= 32)  { *((uint32_t*) out_value) = storage.u32; }
+        else if (bit_length <= 64)  { *((uint64_t*) out_value) = storage.u64; }
+        else
+        {
+            CANARD_ASSERT(false);
+            return -CANARD_ERROR_INTERNAL;
+        }
+    }
+
+    CANARD_ASSERT(result <= bit_length);
+    CANARD_ASSERT(result > 0);
+    return result;
+}
+
+void canardEncodeScalar(void* destination,
+                        uint32_t bit_offset,
+                        uint8_t bit_length,
+                        const void* value)
+{
+    /*
+     * This function can only fail due to invalid arguments, so it was decided to make it return void,
+     * and in the case of bad arguments try the best effort or just trigger an CANARD_ASSERTion failure.
+     * Maybe not the best solution, but it simplifies the API.
+     */
+    CANARD_ASSERT(destination != NULL);
+    CANARD_ASSERT(value != NULL);
+
+    if (bit_length > 64)
+    {
+        CANARD_ASSERT(false);
+        bit_length = 64;
+    }
+
+    if (bit_length < 1)
+    {
+        CANARD_ASSERT(false);
+        bit_length = 1;
+    }
+
+    /*
+     * Preparing the data in the temporary storage.
+     */
+    union
+    {
+        bool     boolean;
+        uint8_t  u8;
+        uint16_t u16;
+        uint32_t u32;
+        uint64_t u64;
+        uint8_t bytes[8];
+    } storage;
+
+    memset(&storage, 0, sizeof(storage));
+
+    uint8_t std_byte_length = 0;
+
+    // Extra most significant bits can be safely ignored here.
+    if      (bit_length == 1)   { std_byte_length = sizeof(bool);   storage.boolean = (*((bool*) value) != 0); }
+    else if (bit_length <= 8)   { std_byte_length = 1;              storage.u8  = *((uint8_t*) value);  }
+    else if (bit_length <= 16)  { std_byte_length = 2;              storage.u16 = *((uint16_t*) value); }
+    else if (bit_length <= 32)  { std_byte_length = 4;              storage.u32 = *((uint32_t*) value); }
+    else if (bit_length <= 64)  { std_byte_length = 8;              storage.u64 = *((uint64_t*) value); }
+    else
+    {
+        CANARD_ASSERT(false);
+    }
+
+    CANARD_ASSERT(std_byte_length > 0);
+
+    if (isBigEndian())
+    {
+        swapByteOrder(&storage.bytes[0], std_byte_length);
+    }
+
+    /*
+     * The bit copy algorithm assumes that more significant bits have lower index, so we need to shift some.
+     * Extra least significant bits will be filled with zeroes, which is fine.
+     * Extra most significant bits will be discarded here.
+     * Coverity Scan mistakenly believes that the array may be overrun if bit_length == 64; however, this branch will
+     * not be taken if bit_length == 64, because 64 % 8 == 0.
+     */
+    if ((bit_length % 8) != 0)
+    {
+        // coverity[overrun-local]
+        storage.bytes[bit_length / 8U] = (uint8_t)(storage.bytes[bit_length / 8U] << ((8U - (bit_length % 8U)) & 7U));
+    }
+
+    /*
+     * Now, the storage contains properly serialized scalar. Copying it out.
+     */
+    copyBitArray(&storage.bytes[0], 0, bit_length, (uint8_t*) destination, bit_offset);
+}
+
+void canardReleaseRxTransferPayload(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    while (transfer->payload_middle != NULL)
+    {
+        CanardBufferBlock* const temp = transfer->payload_middle->next;
+        freeBlock(&ins->allocator, transfer->payload_middle);
+        transfer->payload_middle = temp;
+    }
+
+    transfer->payload_middle = NULL;
+    transfer->payload_head = NULL;
+    transfer->payload_tail = NULL;
+    transfer->payload_len = 0;
+}
+
+CanardPoolAllocatorStatistics canardGetPoolAllocatorStatistics(CanardInstance* ins)
+{
+    return ins->allocator.statistics;
+}
+
+uint16_t canardConvertNativeFloatToFloat16(float value)
+{
+    CANARD_ASSERT(sizeof(float) == 4);
+
+    union FP32
+    {
+        uint32_t u;
+        float f;
+    };
+
+    const union FP32 f32inf = { 255UL << 23U };
+    const union FP32 f16inf = { 31UL << 23U };
+    const union FP32 magic = { 15UL << 23U };
+    const uint32_t sign_mask = 0x80000000UL;
+    const uint32_t round_mask = ~0xFFFUL;
+
+    union FP32 in;
+    in.f = value;
+    uint32_t sign = in.u & sign_mask;
+    in.u ^= sign;
+
+    uint16_t out = 0;
+
+    if (in.u >= f32inf.u)
+    {
+        out = (in.u > f32inf.u) ? (uint16_t)0x7FFFU : (uint16_t)0x7C00U;
+    }
+    else
+    {
+        in.u &= round_mask;
+        in.f *= magic.f;
+        in.u -= round_mask;
+        if (in.u > f16inf.u)
+        {
+            in.u = f16inf.u;
+        }
+        out = (uint16_t)(in.u >> 13U);
+    }
+
+    out |= (uint16_t)(sign >> 16U);
+
+    return out;
+}
+
+float canardConvertFloat16ToNativeFloat(uint16_t value)
+{
+    CANARD_ASSERT(sizeof(float) == 4);
+
+    union FP32
+    {
+        uint32_t u;
+        float f;
+    };
+
+    const union FP32 magic = { (254UL - 15UL) << 23U };
+    const union FP32 was_inf_nan = { (127UL + 16UL) << 23U };
+    union FP32 out;
+
+    out.u = (value & 0x7FFFU) << 13U;
+    out.f *= magic.f;
+    if (out.f >= was_inf_nan.f)
+    {
+        out.u |= 255UL << 23U;
+    }
+    out.u |= (value & 0x8000UL) << 16U;
+
+    return out.f;
+}
+
+/*
+ * Internal (static functions)
+ */
+CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a, uint8_t b)
+{
+    int16_t d = (int16_t)(b - a);
+    if (d < 0)
+    {
+        d = (int16_t)(d + (int16_t)(1U << TRANSFER_ID_BIT_LEN));
+    }
+    return d;
+}
+
+CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id)
+{
+    CANARD_ASSERT(transfer_id != NULL);
+
+    (*transfer_id)++;
+    if (*transfer_id >= 32)
+    {
+        *transfer_id = 0;
+    }
+}
+
+CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
+                                        uint32_t can_id,
+                                        uint8_t* transfer_id,
+                                        uint16_t crc,
+                                        const uint8_t* payload,
+                                        uint16_t payload_len)
+{
+    CANARD_ASSERT(ins != NULL);
+    CANARD_ASSERT((can_id & CANARD_CAN_EXT_ID_MASK) == can_id);            // Flags must be cleared
+
+    if (transfer_id == NULL)
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+
+    if ((payload_len > 0) && (payload == NULL))
+    {
+        return -CANARD_ERROR_INVALID_ARGUMENT;
+    }
+
+    int16_t result = 0;
+
+    if (payload_len < CANARD_CAN_FRAME_MAX_DATA_LEN)                        // Single frame transfer
+    {
+        CanardTxQueueItem* queue_item = createTxItem(&ins->allocator);
+        if (queue_item == NULL)
+        {
+            return -CANARD_ERROR_OUT_OF_MEMORY;
+        }
+
+        memcpy(queue_item->frame.data, payload, payload_len);
+
+        queue_item->frame.data_len = (uint8_t)(payload_len + 1);
+        queue_item->frame.data[payload_len] = (uint8_t)(0xC0U | (*transfer_id & 31U));
+        queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF;
+
+        pushTxQueue(ins, queue_item);
+        result++;
+    }
+    else                                                                    // Multi frame transfer
+    {
+        uint16_t data_index = 0;
+        uint8_t toggle = 0;
+        uint8_t sot_eot = 0x80;
+
+        CanardTxQueueItem* queue_item = NULL;
+
+        while (payload_len - data_index != 0)
+        {
+            queue_item = createTxItem(&ins->allocator);
+            if (queue_item == NULL)
+            {
+                return -CANARD_ERROR_OUT_OF_MEMORY;          // TODO: Purge all frames enqueued so far
+            }
+
+            uint8_t i = 0;
+            if (data_index == 0)
+            {
+                // add crc
+                queue_item->frame.data[0] = (uint8_t) (crc);
+                queue_item->frame.data[1] = (uint8_t) (crc >> 8U);
+                i = 2;
+            }
+            else
+            {
+                i = 0;
+            }
+
+            for (; i < (CANARD_CAN_FRAME_MAX_DATA_LEN - 1) && data_index < payload_len; i++, data_index++)
+            {
+                queue_item->frame.data[i] = payload[data_index];
+            }
+            // tail byte
+            sot_eot = (data_index == payload_len) ? (uint8_t)0x40 : sot_eot;
+
+            queue_item->frame.data[i] = (uint8_t)(sot_eot | ((uint32_t)toggle << 5U) | ((uint32_t)*transfer_id & 31U));
+            queue_item->frame.id = can_id | CANARD_CAN_FRAME_EFF;
+            queue_item->frame.data_len = (uint8_t)(i + 1);
+            pushTxQueue(ins, queue_item);
+
+            result++;
+            toggle ^= 1;
+            sot_eot = 0;
+        }
+    }
+
+    return result;
+}
+
+/**
+ * Puts frame on on the TX queue. Higher priority placed first
+ */
+CANARD_INTERNAL void pushTxQueue(CanardInstance* ins, CanardTxQueueItem* item)
+{
+    CANARD_ASSERT(ins != NULL);
+    CANARD_ASSERT(item->frame.data_len > 0);       // UAVCAN doesn't allow zero-payload frames
+
+    if (ins->tx_queue == NULL)
+    {
+        ins->tx_queue = item;
+        return;
+    }
+
+    CanardTxQueueItem* queue = ins->tx_queue;
+    CanardTxQueueItem* previous = ins->tx_queue;
+
+    while (queue != NULL)
+    {
+        if (isPriorityHigher(queue->frame.id, item->frame.id)) // lower number wins
+        {
+            if (queue == ins->tx_queue)
+            {
+                item->next = queue;
+                ins->tx_queue = item;
+            }
+            else
+            {
+                previous->next = item;
+                item->next = queue;
+            }
+            return;
+        }
+        else
+        {
+            if (queue->next == NULL)
+            {
+                queue->next = item;
+                return;
+            }
+            else
+            {
+                previous = queue;
+                queue = queue->next;
+            }
+        }
+    }
+}
+
+/**
+ * Creates new tx queue item from allocator
+ */
+CANARD_INTERNAL CanardTxQueueItem* createTxItem(CanardPoolAllocator* allocator)
+{
+    CanardTxQueueItem* item = (CanardTxQueueItem*) allocateBlock(allocator);
+    if (item == NULL)
+    {
+        return NULL;
+    }
+    memset(item, 0, sizeof(*item));
+    return item;
+}
+
+/**
+ * Returns true if priority of rhs is higher than id
+ */
+CANARD_INTERNAL bool isPriorityHigher(uint32_t rhs, uint32_t id)
+{
+    const uint32_t clean_id = id & CANARD_CAN_EXT_ID_MASK;
+    const uint32_t rhs_clean_id = rhs & CANARD_CAN_EXT_ID_MASK;
+
+    /*
+     * STD vs EXT - if 11 most significant bits are the same, EXT loses.
+     */
+    const bool ext = (id & CANARD_CAN_FRAME_EFF) != 0;
+    const bool rhs_ext = (rhs & CANARD_CAN_FRAME_EFF) != 0;
+    if (ext != rhs_ext)
+    {
+        uint32_t arb11 = ext ? (clean_id >> 18U) : clean_id;
+        uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18U) : rhs_clean_id;
+        if (arb11 != rhs_arb11)
+        {
+            return arb11 < rhs_arb11;
+        }
+        else
+        {
+            return rhs_ext;
+        }
+    }
+
+    /*
+     * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses.
+     */
+    const bool rtr = (id & CANARD_CAN_FRAME_RTR) != 0;
+    const bool rhs_rtr = (rhs & CANARD_CAN_FRAME_RTR) != 0;
+    if (clean_id == rhs_clean_id && rtr != rhs_rtr)
+    {
+        return rhs_rtr;
+    }
+
+    /*
+     * Plain ID arbitration - greater value loses.
+     */
+    return clean_id < rhs_clean_id;
+}
+
+/**
+ * preps the rx state for the next transfer. does not delete the state
+ */
+CANARD_INTERNAL void prepareForNextTransfer(CanardRxState* state)
+{
+    CANARD_ASSERT(state->buffer_blocks == NULL);
+    state->transfer_id++;
+    state->payload_len = 0;
+    state->next_toggle = 0;
+}
+
+/**
+ * returns data type from id
+ */
+CANARD_INTERNAL uint16_t extractDataType(uint32_t id)
+{
+    if (extractTransferType(id) == CanardTransferTypeBroadcast)
+    {
+        uint16_t dtid = MSG_TYPE_FROM_ID(id);
+        if (SOURCE_ID_FROM_ID(id) == CANARD_BROADCAST_NODE_ID)
+        {
+            dtid &= (1U << ANON_MSG_DATA_TYPE_ID_BIT_LEN) - 1U;
+        }
+        return dtid;
+    }
+    else
+    {
+        return (uint16_t) SRV_TYPE_FROM_ID(id);
+    }
+}
+
+/**
+ * returns transfer type from id
+ */
+CANARD_INTERNAL CanardTransferType extractTransferType(uint32_t id)
+{
+    const bool is_service = SERVICE_NOT_MSG_FROM_ID(id);
+    if (!is_service)
+    {
+        return CanardTransferTypeBroadcast;
+    }
+    else if (REQUEST_NOT_RESPONSE_FROM_ID(id) == 1)
+    {
+        return CanardTransferTypeRequest;
+    }
+    else
+    {
+        return CanardTransferTypeResponse;
+    }
+}
+
+/*
+ *  CanardRxState functions
+ */
+
+/**
+ * Traverses the list of CanardRxState's and returns a pointer to the CanardRxState
+ * with either the Id or a new one at the end
+ */
+CANARD_INTERNAL CanardRxState* traverseRxStates(CanardInstance* ins, uint32_t transfer_descriptor)
+{
+    CanardRxState* states = ins->rx_states;
+
+    if (states == NULL) // initialize CanardRxStates
+    {
+        states = createRxState(&ins->allocator, transfer_descriptor);
+        
+        if(states == NULL)
+        {
+            return NULL;
+        }
+
+        ins->rx_states = states;
+        return states;
+    }
+
+    states = findRxState(states, transfer_descriptor);
+    if (states != NULL)
+    {
+        return states;
+    }
+    else
+    {
+        return prependRxState(ins, transfer_descriptor);
+    }
+}
+
+/**
+ * returns pointer to the rx state of transfer descriptor or null if not found
+ */
+CANARD_INTERNAL CanardRxState* findRxState(CanardRxState* state, uint32_t transfer_descriptor)
+{
+    while (state != NULL)
+    {
+        if (state->dtid_tt_snid_dnid == transfer_descriptor)
+        {
+            return state;
+        }
+        state = state->next;
+    }
+    return NULL;
+}
+
+/**
+ * prepends rx state to the canard instance rx_states
+ */
+CANARD_INTERNAL CanardRxState* prependRxState(CanardInstance* ins, uint32_t transfer_descriptor)
+{
+    CanardRxState* state = createRxState(&ins->allocator, transfer_descriptor);
+
+    if(state == NULL)
+    {
+        return NULL;
+    }
+
+    state->next = ins->rx_states;
+    ins->rx_states = state;
+    return state;
+}
+
+CANARD_INTERNAL CanardRxState* createRxState(CanardPoolAllocator* allocator, uint32_t transfer_descriptor)
+{
+    CanardRxState init = {
+        .next = NULL,
+        .buffer_blocks = NULL,
+        .dtid_tt_snid_dnid = transfer_descriptor
+    };
+
+    CanardRxState* state = (CanardRxState*) allocateBlock(allocator);
+    if (state == NULL)
+    {
+        return NULL;
+    }
+    memcpy(state, &init, sizeof(*state));
+
+    return state;
+}
+
+CANARD_INTERNAL uint64_t releaseStatePayload(CanardInstance* ins, CanardRxState* rxstate)
+{
+    while (rxstate->buffer_blocks != NULL)
+    {
+        CanardBufferBlock* const temp = rxstate->buffer_blocks->next;
+        freeBlock(&ins->allocator, rxstate->buffer_blocks);
+        rxstate->buffer_blocks = temp;
+    }
+    rxstate->payload_len = 0;
+    return CANARD_OK;
+}
+
+/*
+ *  CanardBufferBlock functions
+ */
+
+/**
+ * pushes data into the rx state. Fills the buffer head, then appends data to buffer blocks
+ */
+CANARD_INTERNAL int16_t bufferBlockPushBytes(CanardPoolAllocator* allocator,
+                                             CanardRxState* state,
+                                             const uint8_t* data,
+                                             uint8_t data_len)
+{
+    uint16_t data_index = 0;
+
+    // if head is not full, add data to head
+    if ((CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE - state->payload_len) > 0)
+    {
+        for (uint16_t i = (uint16_t)state->payload_len;
+             i < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE && data_index < data_len;
+             i++, data_index++)
+        {
+            state->buffer_head[i] = data[data_index];
+        }
+        if (data_index >= data_len)
+        {
+            state->payload_len =
+                (uint16_t)(state->payload_len + data_len) & ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U);
+            return 1;
+        }
+    } // head is full.
+
+    uint16_t index_at_nth_block =
+        (uint16_t)(((state->payload_len) - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) % CANARD_BUFFER_BLOCK_DATA_SIZE);
+
+    // get to current block
+    CanardBufferBlock* block = NULL;
+
+    // buffer blocks uninitialized
+    if (state->buffer_blocks == NULL)
+    {
+        state->buffer_blocks = createBufferBlock(allocator);
+
+        if (state->buffer_blocks == NULL)
+        {
+            return -CANARD_ERROR_OUT_OF_MEMORY;
+        }
+
+        block = state->buffer_blocks;
+        index_at_nth_block = 0;
+    }
+    else
+    {
+        uint16_t nth_block = 1;
+
+        // get to block
+        block = state->buffer_blocks;
+        while (block->next != NULL)
+        {
+            nth_block++;
+            block = block->next;
+        }
+
+        const uint16_t num_buffer_blocks =
+            (uint16_t) (((((uint32_t)state->payload_len + data_len) - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) /
+                         CANARD_BUFFER_BLOCK_DATA_SIZE) + 1U);
+
+        if (num_buffer_blocks > nth_block && index_at_nth_block == 0)
+        {
+            block->next = createBufferBlock(allocator);
+            if (block->next == NULL)
+            {
+                return -CANARD_ERROR_OUT_OF_MEMORY;
+            }
+            block = block->next;
+        }
+    }
+
+    // add data to current block until it becomes full, add new block if necessary
+    while (data_index < data_len)
+    {
+        for (uint16_t i = index_at_nth_block;
+             i < CANARD_BUFFER_BLOCK_DATA_SIZE && data_index < data_len;
+             i++, data_index++)
+        {
+            block->data[i] = data[data_index];
+        }
+
+        if (data_index < data_len)
+        {
+            block->next = createBufferBlock(allocator);
+            if (block->next == NULL)
+            {
+                return -CANARD_ERROR_OUT_OF_MEMORY;
+            }
+            block = block->next;
+            index_at_nth_block = 0;
+        }
+    }
+
+    state->payload_len = (uint16_t)(state->payload_len + data_len) & ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U);
+
+    return 1;
+}
+
+CANARD_INTERNAL CanardBufferBlock* createBufferBlock(CanardPoolAllocator* allocator)
+{
+    CanardBufferBlock* block = (CanardBufferBlock*) allocateBlock(allocator);
+    if (block == NULL)
+    {
+        return NULL;
+    }
+    block->next = NULL;
+    return block;
+}
+
+/**
+ * Bit array copy routine, originally developed by Ben Dyer for Libuavcan. Thanks Ben.
+ */
+void copyBitArray(const uint8_t* src, uint32_t src_offset, uint32_t src_len,
+                        uint8_t* dst, uint32_t dst_offset)
+{
+    CANARD_ASSERT(src_len > 0U);
+
+    // Normalizing inputs
+    src += src_offset / 8U;
+    dst += dst_offset / 8U;
+
+    src_offset %= 8U;
+    dst_offset %= 8U;
+
+    const size_t last_bit = src_offset + src_len;
+    while (last_bit - src_offset)
+    {
+        const uint8_t src_bit_offset = (uint8_t)(src_offset % 8U);
+        const uint8_t dst_bit_offset = (uint8_t)(dst_offset % 8U);
+
+        const uint8_t max_offset = MAX(src_bit_offset, dst_bit_offset);
+        const uint32_t copy_bits = MIN(last_bit - src_offset, 8U - max_offset);
+
+        const uint8_t write_mask = (uint8_t)((uint8_t)(0xFF00U >> copy_bits) >> dst_bit_offset);
+        const uint8_t src_data = (uint8_t)(((uint32_t)src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset);
+
+        dst[dst_offset / 8U] =
+            (uint8_t)(((uint32_t)dst[dst_offset / 8U] & (uint32_t)~write_mask) | (uint32_t)(src_data & write_mask));
+
+        src_offset += copy_bits;
+        dst_offset += copy_bits;
+    }
+}
+
+CANARD_INTERNAL int16_t descatterTransferPayload(const CanardRxTransfer* transfer,
+                                                 uint32_t bit_offset,
+                                                 uint8_t bit_length,
+                                                 void* output)
+{
+    CANARD_ASSERT(transfer != 0);
+
+    if (bit_offset >= transfer->payload_len * 8)
+    {
+        return 0;       // Out of range, reading zero bits
+    }
+
+    if (bit_offset + bit_length > transfer->payload_len * 8)
+    {
+        bit_length = (uint8_t)(transfer->payload_len * 8U - bit_offset);
+    }
+
+    CANARD_ASSERT(bit_length > 0);
+
+    if ((transfer->payload_middle != NULL) || (transfer->payload_tail != NULL)) // Multi frame
+    {
+        /*
+         * This part is hideously complicated and probably should be redesigned.
+         * The objective here is to copy the requested number of bits from scattered storage into the temporary
+         * local storage. We go through great pains to ensure that all corner cases are handled correctly.
+         */
+        uint32_t input_bit_offset = bit_offset;
+        uint8_t output_bit_offset = 0;
+        uint8_t remaining_bit_length = bit_length;
+
+        // Reading head
+        if (input_bit_offset < CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8)
+        {
+            const uint8_t amount = (uint8_t)MIN(remaining_bit_length,
+                                                CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8U - input_bit_offset);
+
+            copyBitArray(&transfer->payload_head[0], input_bit_offset, amount, (uint8_t*) output, 0);
+
+            input_bit_offset += amount;
+            output_bit_offset = (uint8_t)(output_bit_offset + amount);
+            remaining_bit_length = (uint8_t)(remaining_bit_length - amount);
+        }
+
+        // Reading middle
+        uint32_t remaining_bits = transfer->payload_len * 8U - CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8U;
+        uint32_t block_bit_offset = CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE * 8U;
+        const CanardBufferBlock* block = transfer->payload_middle;
+
+        while ((block != NULL) && (remaining_bit_length > 0))
+        {
+            CANARD_ASSERT(remaining_bits > 0);
+            const uint32_t block_end_bit_offset = block_bit_offset + MIN(CANARD_BUFFER_BLOCK_DATA_SIZE * 8,
+                                                                         remaining_bits);
+
+            // Perform copy if we've reached the requested offset, otherwise jump over this block and try next
+            if (block_end_bit_offset > input_bit_offset)
+            {
+                const uint8_t amount = (uint8_t) MIN(remaining_bit_length, block_end_bit_offset - input_bit_offset);
+
+                CANARD_ASSERT(input_bit_offset >= block_bit_offset);
+                const uint32_t bit_offset_within_block = input_bit_offset - block_bit_offset;
+
+                copyBitArray(&block->data[0], bit_offset_within_block, amount, (uint8_t*) output, output_bit_offset);
+
+                input_bit_offset += amount;
+                output_bit_offset = (uint8_t)(output_bit_offset + amount);
+                remaining_bit_length = (uint8_t)(remaining_bit_length - amount);
+            }
+
+            CANARD_ASSERT(block_end_bit_offset > block_bit_offset);
+            remaining_bits -= block_end_bit_offset - block_bit_offset;
+            block_bit_offset = block_end_bit_offset;
+            block = block->next;
+        }
+
+        CANARD_ASSERT(remaining_bit_length <= remaining_bits);
+
+        // Reading tail
+        if ((transfer->payload_tail != NULL) && (remaining_bit_length > 0))
+        {
+            CANARD_ASSERT(input_bit_offset >= block_bit_offset);
+            const uint32_t offset = input_bit_offset - block_bit_offset;
+
+            copyBitArray(&transfer->payload_tail[0], offset, remaining_bit_length, (uint8_t*) output,
+                         output_bit_offset);
+
+            input_bit_offset += remaining_bit_length;
+            output_bit_offset = (uint8_t)(output_bit_offset + remaining_bit_length);
+            remaining_bit_length = 0;
+        }
+
+        CANARD_ASSERT(input_bit_offset <= transfer->payload_len * 8);
+        CANARD_ASSERT(output_bit_offset <= 64);
+        CANARD_ASSERT(remaining_bit_length == 0);
+    }
+    else                                                                    // Single frame
+    {
+        copyBitArray(&transfer->payload_head[0], bit_offset, bit_length, (uint8_t*) output, 0);
+    }
+
+    return bit_length;
+}
+
+CANARD_INTERNAL bool isBigEndian(void)
+{
+#if defined(BYTE_ORDER) && defined(BIG_ENDIAN)
+    return BYTE_ORDER == BIG_ENDIAN;                // Some compilers offer this neat shortcut
+#else
+    union
+    {
+        uint16_t a;
+        uint8_t b[2];
+    } u;
+    u.a = 1;
+    return u.b[1] == 1;                             // Some don't...
+#endif
+}
+
+CANARD_INTERNAL void swapByteOrder(void* data, size_t size)
+{
+    CANARD_ASSERT(data != NULL);
+
+    uint8_t* const bytes = (uint8_t*) data;
+
+    size_t fwd = 0;
+    size_t rev = size - 1;
+
+    while (fwd < rev)
+    {
+        const uint8_t x = bytes[fwd];
+        bytes[fwd] = bytes[rev];
+        bytes[rev] = x;
+        fwd++;
+        rev--;
+    }
+}
+
+/*
+ * CRC functions
+ */
+CANARD_INTERNAL uint16_t crcAddByte(uint16_t crc_val, uint8_t byte)
+{
+    crc_val ^= (uint16_t) ((uint16_t) (byte) << 8U);
+    for (uint8_t j = 0; j < 8; j++)
+    {
+        if (crc_val & 0x8000U)
+        {
+            crc_val = (uint16_t) ((uint16_t) (crc_val << 1U) ^ 0x1021U);
+        }
+        else
+        {
+            crc_val = (uint16_t) (crc_val << 1U);
+        }
+    }
+    return crc_val;
+}
+
+CANARD_INTERNAL uint16_t crcAddSignature(uint16_t crc_val, uint64_t data_type_signature)
+{
+    for (uint16_t shift_val = 0; shift_val < 64; shift_val = (uint16_t)(shift_val + 8U))
+    {
+        crc_val = crcAddByte(crc_val, (uint8_t) (data_type_signature >> shift_val));
+    }
+    return crc_val;
+}
+
+CANARD_INTERNAL uint16_t crcAdd(uint16_t crc_val, const uint8_t* bytes, size_t len)
+{
+    while (len--)
+    {
+        crc_val = crcAddByte(crc_val, *bytes++);
+    }
+    return crc_val;
+}
+
+/*
+ *  Pool Allocator functions
+ */
+CANARD_INTERNAL void initPoolAllocator(CanardPoolAllocator* allocator,
+                                       CanardPoolAllocatorBlock* buf,
+                                       uint16_t buf_len)
+{
+    size_t current_index = 0;
+    CanardPoolAllocatorBlock** current_block = &(allocator->free_list);
+    while (current_index < buf_len)
+    {
+        *current_block = &buf[current_index];
+        current_block = &((*current_block)->next);
+        current_index++;
+    }
+    *current_block = NULL;
+
+    allocator->statistics.capacity_blocks = buf_len;
+    allocator->statistics.current_usage_blocks = 0;
+    allocator->statistics.peak_usage_blocks = 0;
+}
+
+CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator)
+{
+    // Check if there are any blocks available in the free list.
+    if (allocator->free_list == NULL)
+    {
+        return NULL;
+    }
+
+    // Take first available block and prepares next block for use.
+    void* result = allocator->free_list;
+    allocator->free_list = allocator->free_list->next;
+
+    // Update statistics
+    allocator->statistics.current_usage_blocks++;
+    if (allocator->statistics.peak_usage_blocks < allocator->statistics.current_usage_blocks)
+    {
+        allocator->statistics.peak_usage_blocks = allocator->statistics.current_usage_blocks;
+    }
+
+    return result;
+}
+
+CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator, void* p)
+{
+    CanardPoolAllocatorBlock* block = (CanardPoolAllocatorBlock*) p;
+
+    block->next = allocator->free_list;
+    allocator->free_list = block;
+
+    CANARD_ASSERT(allocator->statistics.current_usage_blocks > 0);
+    allocator->statistics.current_usage_blocks--;
+}
diff --git a/libcanard/canard.h b/libcanard/canard.h
new file mode 100644
index 000000000..39a27b304
--- /dev/null
+++ b/libcanard/canard.h
@@ -0,0 +1,531 @@
+/*
+ * Copyright (c) 2016-2018 UAVCAN Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Contributors: https://github.com/UAVCAN/libcanard/contributors
+ *
+ * Documentation: http://uavcan.org/Implementations/Libcanard
+ */
+
+#ifndef CANARD_H
+#define CANARD_H
+
+#include <stdint.h>
+#include <stddef.h>
+#include <stdbool.h>
+//#include <assert.h>
+
+/// Build configuration header. Use it to provide your overrides.
+#if defined(CANARD_ENABLE_CUSTOM_BUILD_CONFIG) && CANARD_ENABLE_CUSTOM_BUILD_CONFIG
+# include "canard_build_config.h"
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/// Libcanard version. API will be backwards compatible within the same major version.
+#define CANARD_VERSION_MAJOR                        0
+#define CANARD_VERSION_MINOR                        2
+
+/// By default this macro resolves to the standard assert(). The user can redefine this if necessary.
+#ifndef CANARD_ASSERT
+//# define CANARD_ASSERT(x)   assert(x)
+#define CANARD_ASSERT(x)
+#endif
+
+#define CANARD_GLUE(a, b)           CANARD_GLUE_IMPL_(a, b)
+#define CANARD_GLUE_IMPL_(a, b)     a##b
+
+/// By default this macro expands to static_assert if supported by the language (C11, C++11, or newer).
+/// The user can redefine this if necessary.
+#ifndef CANARD_STATIC_ASSERT
+# if (defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)) ||\
+     (defined(__cplusplus) && (__cplusplus >= 201103L))
+#  define CANARD_STATIC_ASSERT(...) static_assert(__VA_ARGS__)
+# else
+#  define CANARD_STATIC_ASSERT(x, ...) typedef char CANARD_GLUE(_static_assertion_, __LINE__)[(x) ? 1 : -1]
+# endif
+#endif
+
+/// Error code definitions; inverse of these values may be returned from API calls.
+#define CANARD_OK                                   0
+// Value 1 is omitted intentionally, since -1 is often used in 3rd party code
+#define CANARD_ERROR_INVALID_ARGUMENT               2
+#define CANARD_ERROR_OUT_OF_MEMORY                  3
+#define CANARD_ERROR_NODE_ID_NOT_SET                4
+#define CANARD_ERROR_INTERNAL                       9
+
+/// The size of a memory block in bytes.
+#define CANARD_MEM_BLOCK_SIZE                       32U
+
+/// This will be changed when the support for CAN FD is added
+#define CANARD_CAN_FRAME_MAX_DATA_LEN               8U
+
+/// Node ID values. Refer to the specification for more info.
+#define CANARD_BROADCAST_NODE_ID                    0
+#define CANARD_MIN_NODE_ID                          1
+#define CANARD_MAX_NODE_ID                          127
+
+/// Refer to the type CanardRxTransfer
+#define CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE      (CANARD_MEM_BLOCK_SIZE - offsetof(CanardRxState, buffer_head))
+
+/// Refer to the type CanardBufferBlock
+#define CANARD_BUFFER_BLOCK_DATA_SIZE               (CANARD_MEM_BLOCK_SIZE - offsetof(CanardBufferBlock, data))
+
+/// Refer to canardCleanupStaleTransfers() for details.
+#define CANARD_RECOMMENDED_STALE_TRANSFER_CLEANUP_INTERVAL_USEC     1000000U
+
+/// Transfer priority definitions
+#define CANARD_TRANSFER_PRIORITY_HIGHEST            0
+#define CANARD_TRANSFER_PRIORITY_HIGH               8
+#define CANARD_TRANSFER_PRIORITY_MEDIUM             16
+#define CANARD_TRANSFER_PRIORITY_LOW                24
+#define CANARD_TRANSFER_PRIORITY_LOWEST             31
+
+/// Related to CanardCANFrame
+#define CANARD_CAN_EXT_ID_MASK                      0x1FFFFFFFU
+#define CANARD_CAN_STD_ID_MASK                      0x000007FFU
+#define CANARD_CAN_FRAME_EFF                        (1UL << 31U)         ///< Extended frame format
+#define CANARD_CAN_FRAME_RTR                        (1UL << 30U)         ///< Remote transmission (not used by UAVCAN)
+#define CANARD_CAN_FRAME_ERR                        (1UL << 29U)         ///< Error frame (not used by UAVCAN)
+
+#define CANARD_TRANSFER_PAYLOAD_LEN_BITS            10U
+#define CANARD_MAX_TRANSFER_PAYLOAD_LEN             ((1U << CANARD_TRANSFER_PAYLOAD_LEN_BITS) - 1U)
+
+
+/**
+ * This data type holds a standard CAN 2.0B data frame with 29-bit ID.
+ */
+typedef struct
+{
+    /**
+     * Refer to the following definitions:
+     *  - CANARD_CAN_FRAME_EFF
+     *  - CANARD_CAN_FRAME_RTR
+     *  - CANARD_CAN_FRAME_ERR
+     */
+    uint32_t id;
+    uint8_t data[CANARD_CAN_FRAME_MAX_DATA_LEN];
+    uint8_t data_len;
+} CanardCANFrame;
+
+/**
+ * Transfer types are defined by the UAVCAN specification.
+ */
+typedef enum
+{
+    CanardTransferTypeResponse  = 0,
+    CanardTransferTypeRequest   = 1,
+    CanardTransferTypeBroadcast = 2
+} CanardTransferType;
+
+/**
+ * Types of service transfers. These are not applicable to message transfers.
+ */
+typedef enum
+{
+    CanardResponse,
+    CanardRequest
+} CanardRequestResponse;
+
+/*
+ * Forward declarations.
+ */
+typedef struct CanardInstance CanardInstance;
+typedef struct CanardRxTransfer CanardRxTransfer;
+typedef struct CanardRxState CanardRxState;
+typedef struct CanardTxQueueItem CanardTxQueueItem;
+
+/**
+ * The application must implement this function and supply a pointer to it to the library during initialization.
+ * The library calls this function to determine whether the transfer should be received.
+ *
+ * If the application returns true, the value pointed to by 'out_data_type_signature' must be initialized with the
+ * correct data type signature, otherwise transfer reception will fail with CRC mismatch error. Please refer to the
+ * specification for more details about data type signatures. Signature for any data type can be obtained in many
+ * ways; for example, using the command line tool distributed with Libcanard (see the repository).
+ */
+typedef bool (* CanardShouldAcceptTransfer)(const CanardInstance* ins,          ///< Library instance
+                                            uint64_t* out_data_type_signature,  ///< Must be set by the application!
+                                            uint16_t data_type_id,              ///< Refer to the specification
+                                            CanardTransferType transfer_type,   ///< Refer to CanardTransferType
+                                            uint8_t source_node_id);            ///< Source node ID or Broadcast (0)
+
+/**
+ * This function will be invoked by the library every time a transfer is successfully received.
+ * If the application needs to send another transfer from this callback, it is highly recommended
+ * to call canardReleaseRxTransferPayload() first, so that the memory that was used for the block
+ * buffer can be released and re-used by the TX queue.
+ */
+typedef void (* CanardOnTransferReception)(CanardInstance* ins,                 ///< Library instance
+                                           CanardRxTransfer* transfer);         ///< Ptr to temporary transfer object
+
+/**
+ * INTERNAL DEFINITION, DO NOT USE DIRECTLY.
+ * A memory block used in the memory block allocator.
+ */
+typedef union CanardPoolAllocatorBlock_u
+{
+    char bytes[CANARD_MEM_BLOCK_SIZE];
+    union CanardPoolAllocatorBlock_u* next;
+} CanardPoolAllocatorBlock;
+
+/**
+ * This structure provides usage statistics of the memory pool allocator.
+ * This data helps to evaluate whether the allocated memory is sufficient for the application.
+ */
+typedef struct
+{
+    uint16_t capacity_blocks;               ///< Pool capacity in number of blocks
+    uint16_t current_usage_blocks;          ///< Number of blocks that are currently allocated by the library
+    uint16_t peak_usage_blocks;             ///< Maximum number of blocks used since initialization
+} CanardPoolAllocatorStatistics;
+
+/**
+ * INTERNAL DEFINITION, DO NOT USE DIRECTLY.
+ */
+typedef struct
+{
+    CanardPoolAllocatorBlock* free_list;
+    CanardPoolAllocatorStatistics statistics;
+} CanardPoolAllocator;
+
+/**
+ * INTERNAL DEFINITION, DO NOT USE DIRECTLY.
+ * Buffer block for received data.
+ */
+typedef struct CanardBufferBlock
+{
+    struct CanardBufferBlock* next;
+    uint8_t data[];
+} CanardBufferBlock;
+
+/**
+ * INTERNAL DEFINITION, DO NOT USE DIRECTLY.
+ */
+struct CanardRxState
+{
+    struct CanardRxState* next;
+
+    CanardBufferBlock* buffer_blocks;
+
+    uint64_t timestamp_usec;
+
+    const uint32_t dtid_tt_snid_dnid;
+
+    // We're using plain 'unsigned' here, because C99 doesn't permit explicit field type specification
+    unsigned calculated_crc : 16;
+    unsigned payload_len    : CANARD_TRANSFER_PAYLOAD_LEN_BITS;
+    unsigned transfer_id    : 5;
+    unsigned next_toggle    : 1;    // 16+10+5+1 = 32, aligned.
+
+    uint16_t payload_crc;
+
+    uint8_t buffer_head[];
+};
+CANARD_STATIC_ASSERT(offsetof(CanardRxState, buffer_head) <= 28, "Invalid memory layout");
+CANARD_STATIC_ASSERT(CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE >= 4, "Invalid memory layout");
+
+/**
+ * This is the core structure that keeps all of the states and allocated resources of the library instance.
+ * The application should never access any of the fields directly! Instead, API functions should be used.
+ */
+struct CanardInstance
+{
+    uint8_t node_id;                                ///< Local node ID; may be zero if the node is anonymous
+
+    CanardShouldAcceptTransfer should_accept;       ///< Function to decide whether the application wants this transfer
+    CanardOnTransferReception on_reception;         ///< Function the library calls after RX transfer is complete
+
+    CanardPoolAllocator allocator;                  ///< Pool allocator
+
+    CanardRxState* rx_states;                       ///< RX transfer states
+    CanardTxQueueItem* tx_queue;                    ///< TX frames awaiting transmission
+
+    void* user_reference;                           ///< User pointer that can link this instance with other objects
+};
+
+/**
+ * This structure represents a received transfer for the application.
+ * An instance of it is passed to the application via callback when the library receives a new transfer.
+ * Pointers to the structure and all its fields are invalidated after the callback returns.
+ */
+struct CanardRxTransfer
+{
+    /**
+     * Timestamp at which the first frame of this transfer was received.
+     */
+    uint64_t timestamp_usec;
+
+    /**
+     * Payload is scattered across three storages:
+     *  - Head points to CanardRxState.buffer_head (length of which is up to CANARD_PAYLOAD_HEAD_SIZE), or to the
+     *    payload field (possibly with offset) of the last received CAN frame.
+     *
+     *  - Middle is located in the linked list of dynamic blocks (only for multi-frame transfers).
+     *
+     *  - Tail points to the payload field (possibly with offset) of the last received CAN frame
+     *    (only for multi-frame transfers).
+     *
+     * The tail offset depends on how much data of the last frame was accommodated in the last allocated block.
+     *
+     * For single-frame transfers, middle and tail will be NULL, and the head will point at first byte
+     * of the payload of the CAN frame.
+     *
+     * In simple cases it should be possible to get data directly from the head and/or tail pointers.
+     * Otherwise it is advised to use canardDecodeScalar().
+     */
+    const uint8_t* payload_head;            ///< Always valid, i.e. not NULL.
+                                            ///< For multi frame transfers, the maximum size is defined in the constant
+                                            ///< CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE.
+                                            ///< For single-frame transfers, the size is defined in the
+                                            ///< field payload_len.
+    CanardBufferBlock* payload_middle;      ///< May be NULL if the buffer was not needed. Always NULL for single-frame
+                                            ///< transfers.
+    const uint8_t* payload_tail;            ///< Last bytes of multi-frame transfers. Always NULL for single-frame
+                                            ///< transfers.
+    uint16_t payload_len;                   ///< Effective length of the payload in bytes.
+
+    /**
+     * These fields identify the transfer for the application.
+     */
+    uint16_t data_type_id;                  ///< 0 to 255 for services, 0 to 65535 for messages
+    uint8_t transfer_type;                  ///< See CanardTransferType
+    uint8_t transfer_id;                    ///< 0 to 31
+    uint8_t priority;                       ///< 0 to 31
+    uint8_t source_node_id;                 ///< 1 to 127, or 0 if the source is anonymous
+};
+
+/**
+ * Initializes a library instance.
+ * Local node ID will be set to zero, i.e. the node will be anonymous.
+ *
+ * Typically, size of the memory pool should not be less than 1K, although it depends on the application. The
+ * recommended way to detect the required pool size is to measure the peak pool usage after a stress-test. Refer to
+ * the function canardGetPoolAllocatorStatistics().
+ */
+void canardInit(CanardInstance* out_ins,                    ///< Uninitialized library instance
+                void* mem_arena,                            ///< Raw memory chunk used for dynamic allocation
+                size_t mem_arena_size,                      ///< Size of the above, in bytes
+                CanardOnTransferReception on_reception,     ///< Callback, see CanardOnTransferReception
+                CanardShouldAcceptTransfer should_accept,   ///< Callback, see CanardShouldAcceptTransfer
+                void* user_reference);                      ///< Optional pointer for user's convenience, can be NULL
+
+/**
+ * Returns the value of the user pointer.
+ * The user pointer is configured once during initialization.
+ * It can be used to store references to any user-specific data, or to link the instance object with C++ objects.
+ */
+void* canardGetUserReference(CanardInstance* ins);
+
+/**
+ * Assigns a new node ID value to the current node.
+ * Node ID can be assigned only once.
+ */
+void canardSetLocalNodeID(CanardInstance* ins,
+                          uint8_t self_node_id);
+
+/**
+ * Returns node ID of the local node.
+ * Returns zero (broadcast) if the node ID is not set, i.e. if the local node is anonymous.
+ */
+uint8_t canardGetLocalNodeID(const CanardInstance* ins);
+
+/**
+ * Sends a broadcast transfer.
+ * If the node is in passive mode, only single frame transfers will be allowed (they will be transmitted as anonymous).
+ *
+ * For anonymous transfers, maximum data type ID is limited to 3 (see specification for details).
+ *
+ * Please refer to the specification for more details about data type signatures. Signature for any data type can be
+ * obtained in many ways; for example, using the command line tool distributed with Libcanard (see the repository).
+ *
+ * Pointer to the Transfer ID should point to a persistent variable (e.g. static or heap allocated, not on the stack);
+ * it will be updated by the library after every transmission. The Transfer ID value cannot be shared between
+ * transfers that have different descriptors! More on this in the transport layer specification.
+ *
+ * Returns the number of frames enqueued, or negative error code.
+ */
+int16_t canardBroadcast(CanardInstance* ins,            ///< Library instance
+                        uint64_t data_type_signature,   ///< See above
+                        uint16_t data_type_id,          ///< Refer to the specification
+                        uint8_t* inout_transfer_id,     ///< Pointer to a persistent variable containing the transfer ID
+                        uint8_t priority,               ///< Refer to definitions CANARD_TRANSFER_PRIORITY_*
+                        const void* payload,            ///< Transfer payload
+                        uint16_t payload_len);          ///< Length of the above, in bytes
+
+/**
+ * Sends a request or a response transfer.
+ * Fails if the node is in passive mode.
+ *
+ * Please refer to the specification for more details about data type signatures. Signature for any data type can be
+ * obtained in many ways; for example, using the command line tool distributed with Libcanard (see the repository).
+ *
+ * For Request transfers, the pointer to the Transfer ID should point to a persistent variable (e.g. static or heap
+ * allocated, not on the stack); it will be updated by the library after every request. The Transfer ID value
+ * cannot be shared between requests that have different descriptors! More on this in the transport layer
+ * specification.
+ *
+ * For Response transfers, the pointer to the Transfer ID will be treated as const (i.e. read-only), and normally it
+ * should point to the transfer_id field of the structure CanardRxTransfer.
+ *
+ * Returns the number of frames enqueued, or negative error code.
+ */
+int16_t canardRequestOrRespond(CanardInstance* ins,             ///< Library instance
+                               uint8_t destination_node_id,     ///< Node ID of the server/client
+                               uint64_t data_type_signature,    ///< See above
+                               uint8_t data_type_id,            ///< Refer to the specification
+                               uint8_t* inout_transfer_id,      ///< Pointer to a persistent variable with transfer ID
+                               uint8_t priority,                ///< Refer to definitions CANARD_TRANSFER_PRIORITY_*
+                               CanardRequestResponse kind,      ///< Refer to CanardRequestResponse
+                               const void* payload,             ///< Transfer payload
+                               uint16_t payload_len);           ///< Length of the above, in bytes
+
+/**
+ * Returns a pointer to the top priority frame in the TX queue.
+ * Returns NULL if the TX queue is empty.
+ * The application will call this function after canardBroadcast() or canardRequestOrRespond() to transmit generated
+ * frames over the CAN bus.
+ */
+const CanardCANFrame* canardPeekTxQueue(const CanardInstance* ins);
+
+/**
+ * Removes the top priority frame from the TX queue.
+ * The application will call this function after canardPeekTxQueue() once the obtained frame has been processed.
+ * Calling canardBroadcast() or canardRequestOrRespond() between canardPeekTxQueue() and canardPopTxQueue()
+ * is NOT allowed, because it may change the frame at the top of the TX queue.
+ */
+void canardPopTxQueue(CanardInstance* ins);
+
+/**
+ * Processes a received CAN frame with a timestamp.
+ * The application will call this function when it receives a new frame from the CAN bus.
+ */
+void canardHandleRxFrame(CanardInstance* ins,
+                         const CanardCANFrame* frame,
+                         uint64_t timestamp_usec);
+
+/**
+ * Traverses the list of transfers and removes those that were last updated more than timeout_usec microseconds ago.
+ * This function must be invoked by the application periodically, about once a second.
+ * Also refer to the constant CANARD_RECOMMENDED_STALE_TRANSFER_CLEANUP_INTERVAL_USEC.
+ */
+void canardCleanupStaleTransfers(CanardInstance* ins,
+                                 uint64_t current_time_usec);
+
+/**
+ * This function can be used to extract values from received UAVCAN transfers. It decodes a scalar value -
+ * boolean, integer, character, or floating point - from the specified bit position in the RX transfer buffer.
+ * Simple single-frame transfers can also be parsed manually.
+ *
+ * Returns the number of bits successfully decoded, which may be less than requested if operation ran out of
+ * buffer boundaries, or negated error code, such as invalid argument.
+ *
+ * Caveat:  This function works correctly only on platforms that use two's complement signed integer representation.
+ *          I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should
+ *          not affect portability in any way.
+ *
+ * The type of value pointed to by 'out_value' is defined as follows:
+ *
+ *  | bit_length | value_is_signed | out_value points to                      |
+ *  |------------|-----------------|------------------------------------------|
+ *  | 1          | false           | bool (may be incompatible with uint8_t!) |
+ *  | 1          | true            | N/A                                      |
+ *  | [2, 8]     | false           | uint8_t, or char                         |
+ *  | [2, 8]     | true            | int8_t, or char                          |
+ *  | [9, 16]    | false           | uint16_t                                 |
+ *  | [9, 16]    | true            | int16_t                                  |
+ *  | [17, 32]   | false           | uint32_t                                 |
+ *  | [17, 32]   | true            | int32_t, or 32-bit float                 |
+ *  | [33, 64]   | false           | uint64_t                                 |
+ *  | [33, 64]   | true            | int64_t, or 64-bit float                 |
+ */
+int16_t canardDecodeScalar(const CanardRxTransfer* transfer,    ///< The RX transfer where the data will be copied from
+                           uint32_t bit_offset,                 ///< Offset, in bits, from the beginning of the transfer
+                           uint8_t bit_length,                  ///< Length of the value, in bits; see the table
+                           bool value_is_signed,                ///< True if the value can be negative; see the table
+                           void* out_value);                    ///< Pointer to the output storage; see the table
+
+/**
+ * This function can be used to encode values for later transmission in a UAVCAN transfer. It encodes a scalar value -
+ * boolean, integer, character, or floating point - and puts it to the specified bit position in the specified
+ * contiguous buffer.
+ * Simple single-frame transfers can also be encoded manually.
+ *
+ * Caveat:  This function works correctly only on platforms that use two's complement signed integer representation.
+ *          I am not aware of any modern microarchitecture that uses anything else than two's complement, so it should
+ *          not affect portability in any way.
+ *
+ * The type of value pointed to by 'value' is defined as follows:
+ *
+ *  | bit_length | value points to                          |
+ *  |------------|------------------------------------------|
+ *  | 1          | bool (may be incompatible with uint8_t!) |
+ *  | [2, 8]     | uint8_t, int8_t, or char                 |
+ *  | [9, 16]    | uint16_t, int16_t                        |
+ *  | [17, 32]   | uint32_t, int32_t, or 32-bit float       |
+ *  | [33, 64]   | uint64_t, int64_t, or 64-bit float       |
+ */
+void canardEncodeScalar(void* destination,      ///< Destination buffer where the result will be stored
+                        uint32_t bit_offset,    ///< Offset, in bits, from the beginning of the destination buffer
+                        uint8_t bit_length,     ///< Length of the value, in bits; see the table
+                        const void* value);     ///< Pointer to the value; see the table
+
+/**
+ * This function can be invoked by the application to release pool blocks that are used
+ * to store the payload of the transfer.
+ *
+ * If the application needs to send new transfers from the transfer reception callback, this function should be
+ * invoked right before calling canardBroadcast() or canardRequestOrRespond(). Not releasing the buffers before
+ * transmission may cause higher peak usage of the memory pool.
+ *
+ * If the application didn't call this function before returning from the callback, the library will do that,
+ * so it is guaranteed that the memory will not leak.
+ */
+void canardReleaseRxTransferPayload(CanardInstance* ins,
+                                    CanardRxTransfer* transfer);
+
+/**
+ * Returns a copy of the pool allocator usage statistics.
+ * Refer to the type CanardPoolAllocatorStatistics.
+ * Use this function to determine worst case memory needs of your application.
+ */
+CanardPoolAllocatorStatistics canardGetPoolAllocatorStatistics(CanardInstance* ins);
+
+/**
+ * Float16 marshaling helpers.
+ * These functions convert between the native float and 16-bit float.
+ * It is assumed that the native float is IEEE 754 single precision float, otherwise results will be unpredictable.
+ * Vast majority of modern computers and microcontrollers use IEEE 754, so this limitation should not affect
+ * portability.
+ */
+uint16_t canardConvertNativeFloatToFloat16(float value);
+float canardConvertFloat16ToNativeFloat(uint16_t value);
+
+/// Abort the build if the current platform is not supported.
+CANARD_STATIC_ASSERT(((uint32_t)CANARD_MULTIFRAME_RX_PAYLOAD_HEAD_SIZE) < 32,
+                     "Platforms where sizeof(void*) > 4 are not supported. "
+                     "On AMD64 use 32-bit mode (e.g. GCC flag -m32).");
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/libcanard/canard.mk b/libcanard/canard.mk
new file mode 100644
index 000000000..a8c8a8c41
--- /dev/null
+++ b/libcanard/canard.mk
@@ -0,0 +1,8 @@
+CANARDSRC =	libcanard/canard.c \
+			libcanard/canard_driver.c \
+			libcanard/dsdl/uavcan/equipment/esc/esc_Status.c \
+			libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c \
+			libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c
+
+CANARDINC = libcanard \
+			libcanard/dsdl
diff --git a/libcanard/canard_driver.c b/libcanard/canard_driver.c
new file mode 100644
index 000000000..066dc0540
--- /dev/null
+++ b/libcanard/canard_driver.c
@@ -0,0 +1,366 @@
+/*
+	Copyright 2019 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 <string.h>
+#include <math.h>
+#include <stdio.h>
+
+#include "canard_driver.h"
+#include "canard.h"
+#include "uavcan/equipment/esc/Status.h"
+#include "uavcan/equipment/esc/RawCommand.h"
+#include "uavcan/equipment/esc/RPMCommand.h"
+
+#include "conf_general.h"
+#include "app.h"
+#include "comm_can.h"
+#include "commands.h"
+#include "mc_interface.h"
+#include "hw.h"
+#include "timeout.h"
+#include "terminal.h"
+
+// Constants
+#define APP_NODE_NAME									"org.vesc." HW_NAME
+
+#define UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE			((3015 + 7) / 8)
+#define UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE		0xee468a8121c46a9e
+#define UAVCAN_GET_NODE_INFO_DATA_TYPE_ID				1
+
+#define UAVCAN_NODE_STATUS_MESSAGE_SIZE					7
+#define UAVCAN_NODE_STATUS_DATA_TYPE_ID					341
+#define UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE			0x0f0868d0c1a7c6f1
+
+#define UAVCAN_NODE_HEALTH_OK							0
+#define UAVCAN_NODE_HEALTH_WARNING						1
+#define UAVCAN_NODE_HEALTH_ERROR						2
+#define UAVCAN_NODE_HEALTH_CRITICAL						3
+
+#define UAVCAN_NODE_MODE_OPERATIONAL					0
+#define UAVCAN_NODE_MODE_INITIALIZATION					1
+
+#define UNIQUE_ID_LENGTH_BYTES							16
+
+#define STATUS_MSGS_TO_STORE							10
+
+// Private datatypes
+typedef struct {
+	int id;
+	systime_t rx_time;
+	uavcan_equipment_esc_Status msg;
+} status_msg_wrapper_t;
+
+// Private variables
+static CanardInstance canard;
+static uint8_t canard_memory_pool[1024];
+static uint8_t node_health = UAVCAN_NODE_HEALTH_OK;
+static uint8_t node_mode = UAVCAN_NODE_MODE_OPERATIONAL;
+static int debug_level;
+static status_msg_wrapper_t stat_msgs[STATUS_MSGS_TO_STORE];
+
+// Threads
+static THD_WORKING_AREA(canard_thread_wa, 2048);
+static THD_FUNCTION(canard_thread, arg);
+
+// Private functions
+static void sendEscStatus(void);
+static void readUniqueID(uint8_t* out_uid);
+static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]);
+static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);
+static bool shouldAcceptTransfer(const CanardInstance* ins,
+		uint64_t* out_data_type_signature,
+		uint16_t data_type_id,
+		CanardTransferType transfer_type,
+		uint8_t source_node_id);
+static void terminal_debug_on(int argc, const char **argv);
+
+void canard_driver_init(void) {
+	debug_level = 0;
+
+	for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) {
+		stat_msgs[i].id = -1;
+	}
+
+	chThdCreateStatic(canard_thread_wa, sizeof(canard_thread_wa), NORMALPRIO, canard_thread, NULL);
+
+	terminal_register_command_callback(
+			"uavcan_debug",
+			"Enable UAVCAN debug prints (0 = off)",
+			"[level]",
+			terminal_debug_on);
+}
+
+static void sendEscStatus(void) {
+	uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE];
+	uavcan_equipment_esc_Status status;
+	status.current = mc_interface_get_tot_current();
+	status.error_count = mc_interface_get_fault();
+	status.esc_index = app_get_configuration()->uavcan_esc_index;
+	status.power_rating_pct = (fabsf(mc_interface_get_tot_current()) /
+			mc_interface_get_configuration()->l_current_max *
+			mc_interface_get_configuration()->l_current_max_scale) * 100.0;
+	status.rpm = mc_interface_get_rpm();
+	status.temperature = mc_interface_temp_fet_filtered() + 273.15;
+	status.voltage = GET_INPUT_VOLTAGE();
+
+	uavcan_equipment_esc_Status_encode(&status, buffer);
+
+	static uint8_t transfer_id;
+
+	canardBroadcast(&canard,
+			UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE,
+			UAVCAN_EQUIPMENT_ESC_STATUS_ID,
+			&transfer_id,
+			CANARD_TRANSFER_PRIORITY_LOW,
+			buffer,
+			UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE);
+}
+
+static void readUniqueID(uint8_t* out_uid) {
+	for (uint8_t i = 0; i < UNIQUE_ID_LENGTH_BYTES; i++) {
+		out_uid[i] = i;
+	}
+}
+
+static void makeNodeStatusMessage(uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE]) {
+	memset(buffer, 0, UAVCAN_NODE_STATUS_MESSAGE_SIZE);
+	const uint32_t uptime_sec = ST2S(chVTGetSystemTimeX());
+	canardEncodeScalar(buffer,  0, 32, &uptime_sec);
+	canardEncodeScalar(buffer, 32,  2, &node_health);
+	canardEncodeScalar(buffer, 34,  3, &node_mode);
+}
+
+/**
+ * This callback is invoked by the library when a new message or request or response is received.
+ */
+static void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer) {
+	if (debug_level > 0) {
+		commands_printf("UAVCAN transfer RX: NODE: %d Type: %d ID: %d",
+				transfer->source_node_id, transfer->transfer_type, transfer->data_type_id);
+	}
+
+	if ((transfer->transfer_type == CanardTransferTypeRequest) &&
+			(transfer->data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) {
+
+		uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE];
+		memset(buffer, 0, sizeof(buffer));
+
+		// NodeStatus
+		makeNodeStatusMessage(buffer);
+
+		// SoftwareVersion
+		buffer[7] = FW_VERSION_MAJOR;
+		buffer[8] = FW_VERSION_MINOR;
+		buffer[9] = 1; // Optional field flags, VCS commit is set
+		uint32_t u32 = 0;// GIT_HASH;
+		canardEncodeScalar(buffer, 80, 32, &u32);
+		// Image CRC skipped
+
+		// HardwareVersion
+		// Major skipped
+		// Minor skipped
+		readUniqueID(&buffer[24]);
+		// Certificate of authenticity skipped
+
+		// Name
+		const size_t name_len = strlen(APP_NODE_NAME);
+		memcpy(&buffer[41], APP_NODE_NAME, name_len);
+
+		const size_t total_size = 41 + name_len;
+
+		/*
+		 * Transmitting; in this case we don't have to release the payload because it's empty anyway.
+		 */
+		canardRequestOrRespond(ins,
+				transfer->source_node_id,
+				UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE,
+				UAVCAN_GET_NODE_INFO_DATA_TYPE_ID,
+				&transfer->transfer_id,
+				transfer->priority,
+				CanardResponse,
+				&buffer[0],
+				(uint16_t)total_size);
+	} else if ((transfer->transfer_type == CanardTransferTypeBroadcast) &&
+			(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)) {
+		uavcan_equipment_esc_RawCommand cmd;
+		uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE];
+		memset(buffer, 0, sizeof(buffer));
+		uint8_t *tmp = buffer;
+
+		if (uavcan_equipment_esc_RawCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0, true) >= 0) {
+			if (cmd.cmd.len > app_get_configuration()->uavcan_esc_index) {
+				mc_interface_set_duty(((float)cmd.cmd.data[app_get_configuration()->uavcan_esc_index]) / 8192.0);
+				timeout_reset();
+			}
+		}
+	} else if ((transfer->transfer_type == CanardTransferTypeBroadcast) &&
+			(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID)) {
+		uavcan_equipment_esc_RPMCommand cmd;
+		uint8_t buffer[UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE];
+		memset(buffer, 0, sizeof(buffer));
+		uint8_t *tmp = buffer;
+
+		if (uavcan_equipment_esc_RPMCommand_decode_internal(transfer, transfer->payload_len, &cmd, &tmp, 0, true) >= 0) {
+			if (cmd.rpm.len > app_get_configuration()->uavcan_esc_index) {
+				mc_interface_set_pid_speed(cmd.rpm.data[app_get_configuration()->uavcan_esc_index]);
+				timeout_reset();
+			}
+		}
+	} else if ((transfer->transfer_type == CanardTransferTypeBroadcast) &&
+			(transfer->data_type_id == UAVCAN_EQUIPMENT_ESC_STATUS_ID)) {
+		uavcan_equipment_esc_Status msg;
+		if (uavcan_equipment_esc_Status_decode_internal(transfer, transfer->payload_len, &msg, 0, 0, true) >= 0) {
+			for (int i = 0;i < STATUS_MSGS_TO_STORE;i++) {
+				status_msg_wrapper_t *msgw = &stat_msgs[i];
+				if (msgw->id == -1 || msgw->id == transfer->source_node_id) {
+					msgw->id = transfer->source_node_id;
+					msgw->rx_time = chVTGetSystemTimeX();
+					msgw->msg = msg;
+					break;
+				}
+			}
+		}
+	}
+}
+
+/**
+ * This callback is invoked by the library when it detects beginning of a new transfer on the bus that can be received
+ * by the local node.
+ * If the callback returns true, the library will receive the transfer.
+ * If the callback returns false, the library will ignore the transfer.
+ * All transfers that are addressed to other nodes are always ignored.
+ */
+static bool shouldAcceptTransfer(const CanardInstance* ins,
+		uint64_t* out_data_type_signature,
+		uint16_t data_type_id,
+		CanardTransferType transfer_type,
+		uint8_t source_node_id) {
+	(void)ins;
+	(void)source_node_id;
+
+	if (debug_level > 0) {
+		commands_printf("UAVCAN shouldAccept: NODE: %d Type: %d ID: %d",
+				source_node_id, transfer_type, data_type_id);
+	}
+
+	if ((transfer_type == CanardTransferTypeRequest) && (data_type_id == UAVCAN_GET_NODE_INFO_DATA_TYPE_ID)) {
+		*out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE;
+		return true;
+	}
+
+	if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID)) {
+		*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE;
+		return true;
+	}
+
+	if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID)) {
+		*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE;
+		return true;
+	}
+
+	if ((transfer_type == CanardTransferTypeBroadcast) && (data_type_id == UAVCAN_EQUIPMENT_ESC_STATUS_ID)) {
+		*out_data_type_signature = UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE;
+		return true;
+	}
+
+	return false;
+}
+
+static void terminal_debug_on(int argc, const char **argv) {
+	if (argc == 2) {
+		int level = -1;
+		sscanf(argv[1], "%d", &level);
+
+		if (level >= 0) {
+			debug_level = level;
+			commands_printf("UAVCAN debug level is now %d", debug_level);
+		} else {
+			commands_printf("Invalid argument(s).\n");
+		}
+	} else {
+		commands_printf("This command requires one argument.\n");
+	}
+}
+
+static THD_FUNCTION(canard_thread, arg) {
+	(void)arg;
+	chRegSetThreadName("UAVCAN");
+
+	canardInit(&canard, canard_memory_pool, sizeof(canard_memory_pool), onTransferReceived, shouldAcceptTransfer, NULL);
+
+	systime_t last_status_time = 0;
+	systime_t last_esc_status_time = 0;
+
+	for (;;) {
+		const app_configuration *conf = app_get_configuration();
+
+		if (!conf->uavcan_enable) {
+			chThdSleepMilliseconds(100);
+			continue;
+		}
+
+		canardSetLocalNodeID(&canard, conf->controller_id);
+
+		CANRxFrame *rxmsg;
+		while ((rxmsg = comm_can_get_rx_frame()) != 0) {
+			CanardCANFrame rx_frame;
+
+			if (rxmsg->IDE == CAN_IDE_EXT) {
+				rx_frame.id = rxmsg->EID | CANARD_CAN_FRAME_EFF;
+			} else {
+				rx_frame.id = rxmsg->SID;
+			}
+
+			rx_frame.data_len = rxmsg->DLC;
+			memcpy(rx_frame.data, rxmsg->data8, rxmsg->DLC);
+
+			canardHandleRxFrame(&canard, &rx_frame, ST2US(chVTGetSystemTimeX()));
+		}
+
+		for (const CanardCANFrame* txf = NULL; (txf = canardPeekTxQueue(&canard)) != NULL;) {
+			comm_can_transmit_eid(txf->id, txf->data, txf->data_len);
+			canardPopTxQueue(&canard);
+		}
+
+		if (ST2MS(chVTTimeElapsedSinceX(last_status_time)) >= 1000) {
+			last_status_time = chVTGetSystemTimeX();
+			canardCleanupStaleTransfers(&canard, ST2US(chVTGetSystemTimeX()));
+
+			uint8_t buffer[UAVCAN_NODE_STATUS_MESSAGE_SIZE];
+			makeNodeStatusMessage(buffer);
+
+			static uint8_t transfer_id;
+			canardBroadcast(&canard,
+					UAVCAN_NODE_STATUS_DATA_TYPE_SIGNATURE,
+					UAVCAN_NODE_STATUS_DATA_TYPE_ID,
+					&transfer_id,
+					CANARD_TRANSFER_PRIORITY_LOW,
+					buffer,
+					UAVCAN_NODE_STATUS_MESSAGE_SIZE);
+		}
+
+		if (ST2MS(chVTTimeElapsedSinceX(last_esc_status_time)) >= 1000 / conf->send_can_status_rate_hz &&
+				conf->send_can_status != CAN_STATUS_DISABLED) {
+			last_esc_status_time = chVTGetSystemTimeX();
+			sendEscStatus();
+		}
+
+		chThdSleepMilliseconds(1);
+	}
+}
diff --git a/libcanard/canard_driver.h b/libcanard/canard_driver.h
new file mode 100644
index 000000000..1fd0f1be0
--- /dev/null
+++ b/libcanard/canard_driver.h
@@ -0,0 +1,28 @@
+/*
+	Copyright 2019 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 LIBCANARD_CANARD_DRIVER_H_
+#define LIBCANARD_CANARD_DRIVER_H_
+
+#include "ch.h"
+#include "hal.h"
+
+void canard_driver_init(void);
+
+#endif /* LIBCANARD_CANARD_DRIVER_H_ */
diff --git a/libcanard/canard_internals.h b/libcanard/canard_internals.h
new file mode 100644
index 000000000..baf4b06e9
--- /dev/null
+++ b/libcanard/canard_internals.h
@@ -0,0 +1,154 @@
+/*
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2016-2017 UAVCAN Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ *
+ * Contributors: https://github.com/UAVCAN/libcanard/contributors
+ */
+
+/*
+ * This file holds function declarations that expose the library's internal definitions for unit testing.
+ * It is NOT part of the library's API and should not even be looked at by the user.
+ */
+
+#ifndef CANARD_INTERNALS_H
+#define CANARD_INTERNALS_H
+
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/// This macro is needed only for testing and development. Do not redefine this in production.
+#ifndef CANARD_INTERNAL
+# define CANARD_INTERNAL static
+#endif
+
+
+CANARD_INTERNAL CanardRxState* traverseRxStates(CanardInstance* ins,
+                                                uint32_t transfer_descriptor);
+
+CANARD_INTERNAL CanardRxState* createRxState(CanardPoolAllocator* allocator,
+                                             uint32_t transfer_descriptor);
+
+CANARD_INTERNAL CanardRxState* prependRxState(CanardInstance* ins,
+                                              uint32_t transfer_descriptor);
+
+CANARD_INTERNAL CanardRxState* findRxState(CanardRxState* state,
+                                           uint32_t transfer_descriptor);
+
+CANARD_INTERNAL int16_t bufferBlockPushBytes(CanardPoolAllocator* allocator,
+                                             CanardRxState* state,
+                                             const uint8_t* data,
+                                             uint8_t data_len);
+
+CANARD_INTERNAL CanardBufferBlock* createBufferBlock(CanardPoolAllocator* allocator);
+
+CANARD_INTERNAL CanardTransferType extractTransferType(uint32_t id);
+
+CANARD_INTERNAL uint16_t extractDataType(uint32_t id);
+
+CANARD_INTERNAL void pushTxQueue(CanardInstance* ins,
+                                 CanardTxQueueItem* item);
+
+CANARD_INTERNAL bool isPriorityHigher(uint32_t id,
+                                      uint32_t rhs);
+
+CANARD_INTERNAL CanardTxQueueItem* createTxItem(CanardPoolAllocator* allocator);
+
+CANARD_INTERNAL void prepareForNextTransfer(CanardRxState* state);
+
+CANARD_INTERNAL int16_t computeTransferIDForwardDistance(uint8_t a,
+                                                         uint8_t b);
+
+CANARD_INTERNAL void incrementTransferID(uint8_t* transfer_id);
+
+CANARD_INTERNAL uint64_t releaseStatePayload(CanardInstance* ins,
+                                             CanardRxState* rxstate);
+
+/// Returns the number of frames enqueued
+CANARD_INTERNAL int16_t enqueueTxFrames(CanardInstance* ins,
+                                        uint32_t can_id,
+                                        uint8_t* transfer_id,
+                                        uint16_t crc,
+                                        const uint8_t* payload,
+                                        uint16_t payload_len);
+
+CANARD_INTERNAL void copyBitArray(const uint8_t* src,
+                                  uint32_t src_offset,
+                                  uint32_t src_len,
+                                  uint8_t* dst,
+                                  uint32_t dst_offset);
+
+/**
+ * Moves specified bits from the scattered transfer storage to a specified contiguous buffer.
+ * Returns the number of bits copied, or negated error code.
+ */
+CANARD_INTERNAL int16_t descatterTransferPayload(const CanardRxTransfer* transfer,
+                                                 uint32_t bit_offset,
+                                                 uint8_t bit_length,
+                                                 void* output);
+
+CANARD_INTERNAL bool isBigEndian(void);
+
+CANARD_INTERNAL void swapByteOrder(void* data, unsigned size);
+
+/*
+ * Transfer CRC
+ */
+CANARD_INTERNAL uint16_t crcAddByte(uint16_t crc_val,
+                                    uint8_t byte);
+
+CANARD_INTERNAL uint16_t crcAddSignature(uint16_t crc_val,
+                                         uint64_t data_type_signature);
+
+CANARD_INTERNAL uint16_t crcAdd(uint16_t crc_val,
+                                const uint8_t* bytes,
+                                size_t len);
+
+/**
+ * Inits a memory allocator.
+ *
+ * @param [in] allocator The memory allocator to initialize.
+ * @param [in] buf The buffer used by the memory allocator.
+ * @param [in] buf_len The number of blocks in buf.
+ */
+CANARD_INTERNAL void initPoolAllocator(CanardPoolAllocator* allocator,
+                                       CanardPoolAllocatorBlock* buf,
+                                       uint16_t buf_len);
+
+/**
+ * Allocates a block from the given pool allocator.
+ */
+CANARD_INTERNAL void* allocateBlock(CanardPoolAllocator* allocator);
+
+/**
+ * Frees a memory block previously returned by canardAllocateBlock.
+ */
+CANARD_INTERNAL void freeBlock(CanardPoolAllocator* allocator,
+                               void* p);
+
+
+#ifdef __cplusplus
+}
+#endif
+#endif
diff --git a/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h b/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h
new file mode 100644
index 000000000..25d5443f8
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/esc/RPMCommand.h
@@ -0,0 +1,72 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1031.RPMCommand.uavcan
+ */
+
+#ifndef __UAVCAN_EQUIPMENT_ESC_RPMCOMMAND
+#define __UAVCAN_EQUIPMENT_ESC_RPMCOMMAND
+
+#include <stdint.h>
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/******************************* Source text **********************************
+#
+# Simple RPM setpoint.
+# The ESC should automatically clamp the setpoint according to the minimum and maximum supported RPM;
+# for example, given a ESC that operates in the range 100 to 10000 RPM, a setpoint of 1 RPM will be clamped to 100 RPM.
+# Negative values indicate reverse rotation.
+#
+
+int18[<=20] rpm
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+uavcan.equipment.esc.RPMCommand
+saturated int18[<=20] rpm
+******************************************************************************/
+
+#define UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_ID                 1031
+#define UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_NAME               "uavcan.equipment.esc.RPMCommand"
+#define UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_SIGNATURE          (0xCE0F9F621CF7E70BULL)
+
+#define UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_MAX_SIZE           ((365 + 7)/8)
+
+// Constants
+
+#define UAVCAN_EQUIPMENT_ESC_RPMCOMMAND_RPM_MAX_LENGTH                                   20
+
+typedef struct
+{
+    // FieldTypes
+    struct
+    {
+        uint8_t    len;                       // Dynamic array length
+        int32_t*   data;                      // Dynamic Array 18bit[20] max items
+    } rpm;
+
+} uavcan_equipment_esc_RPMCommand;
+
+extern
+uint32_t uavcan_equipment_esc_RPMCommand_encode(uavcan_equipment_esc_RPMCommand* source, void* msg_buf);
+
+extern
+int32_t uavcan_equipment_esc_RPMCommand_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RPMCommand* dest, uint8_t** dyn_arr_buf);
+
+extern
+uint32_t uavcan_equipment_esc_RPMCommand_encode_internal(uavcan_equipment_esc_RPMCommand* source, void* msg_buf, uint32_t offset, uint8_t root_item);
+
+extern
+int32_t uavcan_equipment_esc_RPMCommand_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RPMCommand* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif // __UAVCAN_EQUIPMENT_ESC_RPMCOMMAND
\ No newline at end of file
diff --git a/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h b/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h
new file mode 100644
index 000000000..af00d0f70
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/esc/RawCommand.h
@@ -0,0 +1,71 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1030.RawCommand.uavcan
+ */
+
+#ifndef __UAVCAN_EQUIPMENT_ESC_RAWCOMMAND
+#define __UAVCAN_EQUIPMENT_ESC_RAWCOMMAND
+
+#include <stdint.h>
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/******************************* Source text **********************************
+#
+# Raw ESC command normalized into [-8192, 8191]; negative values indicate reverse rotation.
+# The ESC should normalize the setpoint into its effective input range.
+# Non-zero setpoint value below minimum should be interpreted as min valid setpoint for the given motor.
+#
+
+int14[<=20] cmd
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+uavcan.equipment.esc.RawCommand
+saturated int14[<=20] cmd
+******************************************************************************/
+
+#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID                 1030
+#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_NAME               "uavcan.equipment.esc.RawCommand"
+#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_SIGNATURE          (0x217F5C87D7EC951DULL)
+
+#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_MAX_SIZE           ((285 + 7)/8)
+
+// Constants
+
+#define UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_CMD_MAX_LENGTH                                   20
+
+typedef struct
+{
+    // FieldTypes
+    struct
+    {
+        uint8_t    len;                       // Dynamic array length
+        int16_t*   data;                      // Dynamic Array 14bit[20] max items
+    } cmd;
+
+} uavcan_equipment_esc_RawCommand;
+
+extern
+uint32_t uavcan_equipment_esc_RawCommand_encode(uavcan_equipment_esc_RawCommand* source, void* msg_buf);
+
+extern
+int32_t uavcan_equipment_esc_RawCommand_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RawCommand* dest, uint8_t** dyn_arr_buf);
+
+extern
+uint32_t uavcan_equipment_esc_RawCommand_encode_internal(uavcan_equipment_esc_RawCommand* source, void* msg_buf, uint32_t offset, uint8_t root_item);
+
+extern
+int32_t uavcan_equipment_esc_RawCommand_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_RawCommand* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif // __UAVCAN_EQUIPMENT_ESC_RAWCOMMAND
\ No newline at end of file
diff --git a/libcanard/dsdl/uavcan/equipment/esc/Status.h b/libcanard/dsdl/uavcan/equipment/esc/Status.h
new file mode 100644
index 000000000..d877b4a03
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/esc/Status.h
@@ -0,0 +1,86 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1034.Status.uavcan
+ */
+
+#ifndef __UAVCAN_EQUIPMENT_ESC_STATUS
+#define __UAVCAN_EQUIPMENT_ESC_STATUS
+
+#include <stdint.h>
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/******************************* Source text **********************************
+#
+# Generic ESC status.
+# Unknown fields should be set to NAN.
+#
+
+uint32 error_count          # Resets when the motor restarts
+
+float16 voltage             # Volt
+float16 current             # Ampere. Can be negative in case of a regenerative braking.
+float16 temperature         # Kelvin
+
+int18 rpm                   # Negative value indicates reverse rotation
+
+uint7 power_rating_pct      # Instant demand factor in percent (percent of maximum power); range 0% to 127%.
+
+uint5 esc_index
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+uavcan.equipment.esc.Status
+saturated uint32 error_count
+saturated float16 voltage
+saturated float16 current
+saturated float16 temperature
+saturated int18 rpm
+saturated uint7 power_rating_pct
+saturated uint5 esc_index
+******************************************************************************/
+
+#define UAVCAN_EQUIPMENT_ESC_STATUS_ID                     1034
+#define UAVCAN_EQUIPMENT_ESC_STATUS_NAME                   "uavcan.equipment.esc.Status"
+#define UAVCAN_EQUIPMENT_ESC_STATUS_SIGNATURE              (0xA9AF28AEA2FBB254ULL)
+
+#define UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE               ((110 + 7)/8)
+
+// Constants
+
+typedef struct
+{
+    // FieldTypes
+    uint32_t   error_count;                   // bit len 32
+    float      voltage;                       // float16 Saturate
+    float      current;                       // float16 Saturate
+    float      temperature;                   // float16 Saturate
+    int32_t    rpm;                           // bit len 18
+    uint8_t    power_rating_pct;              // bit len 7
+    uint8_t    esc_index;                     // bit len 5
+
+} uavcan_equipment_esc_Status;
+
+extern
+uint32_t uavcan_equipment_esc_Status_encode(uavcan_equipment_esc_Status* source, void* msg_buf);
+
+extern
+int32_t uavcan_equipment_esc_Status_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_Status* dest, uint8_t** dyn_arr_buf);
+
+extern
+uint32_t uavcan_equipment_esc_Status_encode_internal(uavcan_equipment_esc_Status* source, void* msg_buf, uint32_t offset, uint8_t root_item);
+
+extern
+int32_t uavcan_equipment_esc_Status_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_esc_Status* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif // __UAVCAN_EQUIPMENT_ESC_STATUS
\ No newline at end of file
diff --git a/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c b/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c
new file mode 100644
index 000000000..ce4c19e7e
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/esc/esc_RPMCommand.c
@@ -0,0 +1,207 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1031.RPMCommand.uavcan
+ */
+#include "uavcan/equipment/esc/RPMCommand.h"
+#include "canard.h"
+
+#ifndef CANARD_INTERNAL_SATURATE
+#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
+#endif
+
+#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
+#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) );
+#endif
+
+#define CANARD_INTERNAL_ENABLE_TAO  ((uint8_t) 1)
+#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0)
+
+#if defined(__GNUC__)
+# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
+#else
+# define CANARD_MAYBE_UNUSED(x) x
+#endif
+
+/**
+  * @brief uavcan_equipment_esc_RPMCommand_encode_internal
+  * @param source : pointer to source data struct
+  * @param msg_buf: pointer to msg storage
+  * @param offset: bit offset to msg storage
+  * @param root_item: for detecting if TAO should be used
+  * @retval returns offset
+  */
+uint32_t uavcan_equipment_esc_RPMCommand_encode_internal(uavcan_equipment_esc_RPMCommand* source,
+  void* msg_buf,
+  uint32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(root_item))
+{
+    uint32_t c = 0;
+
+    // Dynamic Array (rpm)
+    if (! root_item)
+    {
+        // - Add array length
+        canardEncodeScalar(msg_buf, offset, 5, (void*)&source->rpm.len);
+        offset += 5;
+    }
+
+    // - Add array items
+    for (c = 0; c < source->rpm.len; c++)
+    {
+        canardEncodeScalar(msg_buf,
+                           offset,
+                           18,
+                           (void*)(source->rpm.data + c));// 131071
+        offset += 18;
+    }
+
+    return offset;
+}
+
+/**
+  * @brief uavcan_equipment_esc_RPMCommand_encode
+  * @param source : Pointer to source data struct
+  * @param msg_buf: Pointer to msg storage
+  * @retval returns message length as bytes
+  */
+uint32_t uavcan_equipment_esc_RPMCommand_encode(uavcan_equipment_esc_RPMCommand* source, void* msg_buf)
+{
+    uint32_t offset = 0;
+
+    offset = uavcan_equipment_esc_RPMCommand_encode_internal(source, msg_buf, offset, 1);
+
+    return (offset + 7 ) / 8;
+}
+
+/**
+  * @brief uavcan_equipment_esc_RPMCommand_decode_internal
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_esc_RPMCommand dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @param offset: Call with 0, bit offset to msg storage
+  * @param tao: is tail array optimization used
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_esc_RPMCommand_decode_internal(
+  const CanardRxTransfer* transfer,
+  uint16_t CANARD_MAYBE_UNUSED(payload_len),
+  uavcan_equipment_esc_RPMCommand* dest,
+  uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf),
+  int32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(tao))
+{
+    int32_t ret = 0;
+    uint32_t c = 0;
+
+    // Dynamic Array (rpm)
+    //  - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization
+    if (payload_len && tao == CANARD_INTERNAL_ENABLE_TAO)
+    {
+        //  - Calculate Array length from MSG length
+        dest->rpm.len = ((payload_len * 8) - offset ) / 18; // 18 bit array item size
+    }
+    else
+    {
+        // - Array length 5 bits
+        ret = canardDecodeScalar(transfer,
+                                 offset,
+                                 5,
+                                 false,
+                                 (void*)&dest->rpm.len); // 131071
+        if (ret != 5)
+        {
+            goto uavcan_equipment_esc_RPMCommand_error_exit;
+        }
+        offset += 5;
+    }
+
+    //  - Get Array
+    if (dyn_arr_buf)
+    {
+        dest->rpm.data = (int32_t*)*dyn_arr_buf;
+    }
+
+    for (c = 0; c < dest->rpm.len; c++)
+    {
+        if (dyn_arr_buf)
+        {
+            ret = canardDecodeScalar(transfer,
+                                     offset,
+                                     18,
+                                     true,
+                                     (void*)*dyn_arr_buf); // 131071
+            if (ret != 18)
+            {
+                goto uavcan_equipment_esc_RPMCommand_error_exit;
+            }
+            *dyn_arr_buf = (uint8_t*)(((int32_t*)*dyn_arr_buf) + 1);
+        }
+        offset += 18;
+    }
+    return offset;
+
+uavcan_equipment_esc_RPMCommand_error_exit:
+    if (ret < 0)
+    {
+        return ret;
+    }
+    else
+    {
+        return -CANARD_ERROR_INTERNAL;
+    }
+}
+
+/**
+  * @brief uavcan_equipment_esc_RPMCommand_decode
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_esc_RPMCommand dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_esc_RPMCommand_decode(const CanardRxTransfer* transfer,
+  uint16_t payload_len,
+  uavcan_equipment_esc_RPMCommand* dest,
+  uint8_t** dyn_arr_buf)
+{
+    const int32_t offset = 0;
+    int32_t ret = 0;
+
+    /* Backward compatibility support for removing TAO
+     *  - first try to decode with TAO DISABLED
+     *  - if it fails fall back to TAO ENABLED
+     */
+    uint8_t tao = CANARD_INTERNAL_DISABLE_TAO;
+
+    while (1)
+    {
+        // Clear the destination struct
+        for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_RPMCommand); c++)
+        {
+            ((uint8_t*)dest)[c] = 0x00;
+        }
+
+        ret = uavcan_equipment_esc_RPMCommand_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao);
+
+        if (ret >= 0)
+        {
+            break;
+        }
+
+        if (tao == CANARD_INTERNAL_ENABLE_TAO)
+        {
+            break;
+        }
+        tao = CANARD_INTERNAL_ENABLE_TAO;
+    }
+
+    return ret;
+}
diff --git a/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c b/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c
new file mode 100644
index 000000000..5781ee05d
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/esc/esc_RawCommand.c
@@ -0,0 +1,207 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1030.RawCommand.uavcan
+ */
+#include "uavcan/equipment/esc/RawCommand.h"
+#include "canard.h"
+
+#ifndef CANARD_INTERNAL_SATURATE
+#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
+#endif
+
+#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
+#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) );
+#endif
+
+#define CANARD_INTERNAL_ENABLE_TAO  ((uint8_t) 1)
+#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0)
+
+#if defined(__GNUC__)
+# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
+#else
+# define CANARD_MAYBE_UNUSED(x) x
+#endif
+
+/**
+  * @brief uavcan_equipment_esc_RawCommand_encode_internal
+  * @param source : pointer to source data struct
+  * @param msg_buf: pointer to msg storage
+  * @param offset: bit offset to msg storage
+  * @param root_item: for detecting if TAO should be used
+  * @retval returns offset
+  */
+uint32_t uavcan_equipment_esc_RawCommand_encode_internal(uavcan_equipment_esc_RawCommand* source,
+  void* msg_buf,
+  uint32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(root_item))
+{
+    uint32_t c = 0;
+
+    // Dynamic Array (cmd)
+    if (! root_item)
+    {
+        // - Add array length
+        canardEncodeScalar(msg_buf, offset, 5, (void*)&source->cmd.len);
+        offset += 5;
+    }
+
+    // - Add array items
+    for (c = 0; c < source->cmd.len; c++)
+    {
+        canardEncodeScalar(msg_buf,
+                           offset,
+                           14,
+                           (void*)(source->cmd.data + c));// 8191
+        offset += 14;
+    }
+
+    return offset;
+}
+
+/**
+  * @brief uavcan_equipment_esc_RawCommand_encode
+  * @param source : Pointer to source data struct
+  * @param msg_buf: Pointer to msg storage
+  * @retval returns message length as bytes
+  */
+uint32_t uavcan_equipment_esc_RawCommand_encode(uavcan_equipment_esc_RawCommand* source, void* msg_buf)
+{
+    uint32_t offset = 0;
+
+    offset = uavcan_equipment_esc_RawCommand_encode_internal(source, msg_buf, offset, 1);
+
+    return (offset + 7 ) / 8;
+}
+
+/**
+  * @brief uavcan_equipment_esc_RawCommand_decode_internal
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_esc_RawCommand dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @param offset: Call with 0, bit offset to msg storage
+  * @param tao: is tail array optimization used
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_esc_RawCommand_decode_internal(
+  const CanardRxTransfer* transfer,
+  uint16_t CANARD_MAYBE_UNUSED(payload_len),
+  uavcan_equipment_esc_RawCommand* dest,
+  uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf),
+  int32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(tao))
+{
+    int32_t ret = 0;
+    uint32_t c = 0;
+
+    // Dynamic Array (cmd)
+    //  - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization
+    if (payload_len && tao == CANARD_INTERNAL_ENABLE_TAO)
+    {
+        //  - Calculate Array length from MSG length
+        dest->cmd.len = ((payload_len * 8) - offset ) / 14; // 14 bit array item size
+    }
+    else
+    {
+        // - Array length 5 bits
+        ret = canardDecodeScalar(transfer,
+                                 offset,
+                                 5,
+                                 false,
+                                 (void*)&dest->cmd.len); // 8191
+        if (ret != 5)
+        {
+            goto uavcan_equipment_esc_RawCommand_error_exit;
+        }
+        offset += 5;
+    }
+
+    //  - Get Array
+    if (dyn_arr_buf)
+    {
+        dest->cmd.data = (int16_t*)*dyn_arr_buf;
+    }
+
+    for (c = 0; c < dest->cmd.len; c++)
+    {
+        if (dyn_arr_buf)
+        {
+            ret = canardDecodeScalar(transfer,
+                                     offset,
+                                     14,
+                                     true,
+                                     (void*)*dyn_arr_buf); // 8191
+            if (ret != 14)
+            {
+                goto uavcan_equipment_esc_RawCommand_error_exit;
+            }
+            *dyn_arr_buf = (uint8_t*)(((int16_t*)*dyn_arr_buf) + 1);
+        }
+        offset += 14;
+    }
+    return offset;
+
+uavcan_equipment_esc_RawCommand_error_exit:
+    if (ret < 0)
+    {
+        return ret;
+    }
+    else
+    {
+        return -CANARD_ERROR_INTERNAL;
+    }
+}
+
+/**
+  * @brief uavcan_equipment_esc_RawCommand_decode
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_esc_RawCommand dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_esc_RawCommand_decode(const CanardRxTransfer* transfer,
+  uint16_t payload_len,
+  uavcan_equipment_esc_RawCommand* dest,
+  uint8_t** dyn_arr_buf)
+{
+    const int32_t offset = 0;
+    int32_t ret = 0;
+
+    /* Backward compatibility support for removing TAO
+     *  - first try to decode with TAO DISABLED
+     *  - if it fails fall back to TAO ENABLED
+     */
+    uint8_t tao = CANARD_INTERNAL_DISABLE_TAO;
+
+    while (1)
+    {
+        // Clear the destination struct
+        for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_RawCommand); c++)
+        {
+            ((uint8_t*)dest)[c] = 0x00;
+        }
+
+        ret = uavcan_equipment_esc_RawCommand_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao);
+
+        if (ret >= 0)
+        {
+            break;
+        }
+
+        if (tao == CANARD_INTERNAL_ENABLE_TAO)
+        {
+            break;
+        }
+        tao = CANARD_INTERNAL_ENABLE_TAO;
+    }
+
+    return ret;
+}
diff --git a/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c b/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c
new file mode 100644
index 000000000..74adf45d6
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/esc/esc_Status.c
@@ -0,0 +1,262 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/esc/1034.Status.uavcan
+ */
+#include "uavcan/equipment/esc/Status.h"
+#include "canard.h"
+
+#ifndef CANARD_INTERNAL_SATURATE
+#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
+#endif
+
+#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
+#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) );
+#endif
+
+#define CANARD_INTERNAL_ENABLE_TAO  ((uint8_t) 1)
+#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0)
+
+#if defined(__GNUC__)
+# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
+#else
+# define CANARD_MAYBE_UNUSED(x) x
+#endif
+
+/**
+  * @brief uavcan_equipment_esc_Status_encode_internal
+  * @param source : pointer to source data struct
+  * @param msg_buf: pointer to msg storage
+  * @param offset: bit offset to msg storage
+  * @param root_item: for detecting if TAO should be used
+  * @retval returns offset
+  */
+uint32_t uavcan_equipment_esc_Status_encode_internal(uavcan_equipment_esc_Status* source,
+  void* msg_buf,
+  uint32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(root_item))
+{
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    canardEncodeScalar(msg_buf, offset, 32, (void*)&source->error_count); // 4294967295
+    offset += 32;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->voltage);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->voltage;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->current);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->current;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->temperature);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->temperature;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+    source->rpm = CANARD_INTERNAL_SATURATE(source->rpm, 131071)
+    canardEncodeScalar(msg_buf, offset, 18, (void*)&source->rpm); // 131071
+    offset += 18;
+
+    source->power_rating_pct = CANARD_INTERNAL_SATURATE_UNSIGNED(source->power_rating_pct, 127)
+    canardEncodeScalar(msg_buf, offset, 7, (void*)&source->power_rating_pct); // 127
+    offset += 7;
+
+    source->esc_index = CANARD_INTERNAL_SATURATE_UNSIGNED(source->esc_index, 31)
+    canardEncodeScalar(msg_buf, offset, 5, (void*)&source->esc_index); // 31
+    offset += 5;
+
+    return offset;
+}
+
+/**
+  * @brief uavcan_equipment_esc_Status_encode
+  * @param source : Pointer to source data struct
+  * @param msg_buf: Pointer to msg storage
+  * @retval returns message length as bytes
+  */
+uint32_t uavcan_equipment_esc_Status_encode(uavcan_equipment_esc_Status* source, void* msg_buf)
+{
+    uint32_t offset = 0;
+
+    offset = uavcan_equipment_esc_Status_encode_internal(source, msg_buf, offset, 1);
+
+    return (offset + 7 ) / 8;
+}
+
+/**
+  * @brief uavcan_equipment_esc_Status_decode_internal
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_esc_Status dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @param offset: Call with 0, bit offset to msg storage
+  * @param tao: is tail array optimization used
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_esc_Status_decode_internal(
+  const CanardRxTransfer* transfer,
+  uint16_t CANARD_MAYBE_UNUSED(payload_len),
+  uavcan_equipment_esc_Status* dest,
+  uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf),
+  int32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(tao))
+{
+    int32_t ret = 0;
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    ret = canardDecodeScalar(transfer, offset, 32, false, (void*)&dest->error_count);
+    if (ret != 32)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+    offset += 32;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->voltage = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->voltage = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->current = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->current = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->temperature = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->temperature = (float)tmp_float;
+#endif
+    offset += 16;
+
+    ret = canardDecodeScalar(transfer, offset, 18, true, (void*)&dest->rpm);
+    if (ret != 18)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+    offset += 18;
+
+    ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->power_rating_pct);
+    if (ret != 7)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+    offset += 7;
+
+    ret = canardDecodeScalar(transfer, offset, 5, false, (void*)&dest->esc_index);
+    if (ret != 5)
+    {
+        goto uavcan_equipment_esc_Status_error_exit;
+    }
+    offset += 5;
+    return offset;
+
+uavcan_equipment_esc_Status_error_exit:
+    if (ret < 0)
+    {
+        return ret;
+    }
+    else
+    {
+        return -CANARD_ERROR_INTERNAL;
+    }
+}
+
+/**
+  * @brief uavcan_equipment_esc_Status_decode
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_esc_Status dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_esc_Status_decode(const CanardRxTransfer* transfer,
+  uint16_t payload_len,
+  uavcan_equipment_esc_Status* dest,
+  uint8_t** dyn_arr_buf)
+{
+    const int32_t offset = 0;
+    int32_t ret = 0;
+
+    /* Backward compatibility support for removing TAO
+     *  - first try to decode with TAO DISABLED
+     *  - if it fails fall back to TAO ENABLED
+     */
+    uint8_t tao = CANARD_INTERNAL_DISABLE_TAO;
+
+    while (1)
+    {
+        // Clear the destination struct
+        for (uint32_t c = 0; c < sizeof(uavcan_equipment_esc_Status); c++)
+        {
+            ((uint8_t*)dest)[c] = 0x00;
+        }
+
+        ret = uavcan_equipment_esc_Status_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao);
+
+        if (ret >= 0)
+        {
+            break;
+        }
+
+        if (tao == CANARD_INTERNAL_ENABLE_TAO)
+        {
+            break;
+        }
+        tao = CANARD_INTERNAL_ENABLE_TAO;
+    }
+
+    return ret;
+}
diff --git a/libcanard/dsdl/uavcan/equipment/power/BatteryInfo.h b/libcanard/dsdl/uavcan/equipment/power/BatteryInfo.h
new file mode 100644
index 000000000..d8fbdc42b
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/power/BatteryInfo.h
@@ -0,0 +1,166 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/power/1092.BatteryInfo.uavcan
+ */
+
+#ifndef __UAVCAN_EQUIPMENT_POWER_BATTERYINFO
+#define __UAVCAN_EQUIPMENT_POWER_BATTERYINFO
+
+#include <stdint.h>
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/******************************* Source text **********************************
+#
+# Single battery info.
+#
+# Typical publishing rate should be around 0.2~1 Hz.
+#
+# Please refer to the Smart Battery data specification for some elaboration.
+#
+
+#
+# Primary parameters.
+# Some fields can be set to NAN if their values are unknown.
+# Full charge capacity is expected to slowly reduce as the battery is aging. Normally its estimate is updated after
+# every charging cycle.
+#
+float16 temperature             # [Kelvin]
+float16 voltage                 # [Volt]
+float16 current                 # [Ampere]
+float16 average_power_10sec     # [Watt]        Average power consumption over the last 10 seconds
+float16 remaining_capacity_wh   # [Watt hours]  Will be increasing during charging
+float16 full_charge_capacity_wh # [Watt hours]  Predicted battery capacity when it is fully charged. Falls with aging
+float16 hours_to_full_charge    # [Hours]       Charging is expected to complete in this time; zero if not charging
+
+#
+# Status flags.
+# Notes:
+#  - CHARGING must be always set as long as the battery is connected to a charger, even if the charging is complete.
+#  - CHARGED must be cleared immediately when the charger is disconnected.
+#
+uint11 STATUS_FLAG_IN_USE       = 1     # The battery is currently used as a power supply
+uint11 STATUS_FLAG_CHARGING     = 2     # Charger is active
+uint11 STATUS_FLAG_CHARGED      = 4     # Charging complete, but the charger is still active
+uint11 STATUS_FLAG_TEMP_HOT     = 8     # Battery temperature is above normal
+uint11 STATUS_FLAG_TEMP_COLD    = 16    # Battery temperature is below normal
+uint11 STATUS_FLAG_OVERLOAD     = 32    # Safe operating area violation
+uint11 STATUS_FLAG_BAD_BATTERY  = 64    # This battery should not be used anymore (e.g. low SOH)
+uint11 STATUS_FLAG_NEED_SERVICE = 128   # This battery requires maintenance (e.g. balancing, full recharge)
+uint11 STATUS_FLAG_BMS_ERROR    = 256   # Battery management system/controller error, smart battery interface error
+uint11 STATUS_FLAG_RESERVED_A   = 512   # Keep zero
+uint11 STATUS_FLAG_RESERVED_B   = 1024  # Keep zero
+uint11 status_flags
+
+#
+# State of Health (SOH) estimate, in percent.
+# http://en.wikipedia.org/wiki/State_of_health
+#
+uint7 STATE_OF_HEALTH_UNKNOWN = 127     # Use this constant if SOH cannot be estimated
+uint7 state_of_health_pct               # Health of the battery, in percent, optional
+
+#
+# Relative State of Charge (SOC) estimate, in percent.
+# http://en.wikipedia.org/wiki/State_of_charge
+#
+uint7 state_of_charge_pct               # Percent of the full charge [0, 100]. This field is required
+uint7 state_of_charge_pct_stdev         # SOC error standard deviation; use best guess if unknown
+
+#
+# Battery identification.
+# Model instance ID must be unique within the same battery model name.
+# Model name is a human-readable string that normally should include the vendor name, model name, and chemistry
+# type of this battery. This field should be assumed case-insensitive. Example: "Zubax Smart Battery v1.1 LiPo".
+#
+uint8 battery_id                        # Identifies the battery within this vehicle, e.g. 0 - primary battery
+uint32 model_instance_id                # Set to zero if not applicable
+uint8[<32] model_name                   # Battery model name
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+uavcan.equipment.power.BatteryInfo
+saturated float16 temperature
+saturated float16 voltage
+saturated float16 current
+saturated float16 average_power_10sec
+saturated float16 remaining_capacity_wh
+saturated float16 full_charge_capacity_wh
+saturated float16 hours_to_full_charge
+saturated uint11 status_flags
+saturated uint7 state_of_health_pct
+saturated uint7 state_of_charge_pct
+saturated uint7 state_of_charge_pct_stdev
+saturated uint8 battery_id
+saturated uint32 model_instance_id
+saturated uint8[<=31] model_name
+******************************************************************************/
+
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_ID              1092
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_NAME            "uavcan.equipment.power.BatteryInfo"
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_SIGNATURE       (0x249C26548A711966ULL)
+
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MAX_SIZE        ((437 + 7)/8)
+
+// Constants
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_IN_USE                 1 // 1
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_CHARGING               2 // 2
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_CHARGED                4 // 4
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_TEMP_HOT               8 // 8
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_TEMP_COLD             16 // 16
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_OVERLOAD              32 // 32
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_BAD_BATTERY           64 // 64
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_NEED_SERVICE         128 // 128
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_BMS_ERROR            256 // 256
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_RESERVED_A           512 // 512
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATUS_FLAG_RESERVED_B          1024 // 1024
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_STATE_OF_HEALTH_UNKNOWN          127 // 127
+
+#define UAVCAN_EQUIPMENT_POWER_BATTERYINFO_MODEL_NAME_MAX_LENGTH                         31
+
+typedef struct
+{
+    // FieldTypes
+    float      temperature;                   // float16 Saturate
+    float      voltage;                       // float16 Saturate
+    float      current;                       // float16 Saturate
+    float      average_power_10sec;           // float16 Saturate
+    float      remaining_capacity_wh;         // float16 Saturate
+    float      full_charge_capacity_wh;       // float16 Saturate
+    float      hours_to_full_charge;          // float16 Saturate
+    uint16_t   status_flags;                  // bit len 11
+    uint8_t    state_of_health_pct;           // bit len 7
+    uint8_t    state_of_charge_pct;           // bit len 7
+    uint8_t    state_of_charge_pct_stdev;     // bit len 7
+    uint8_t    battery_id;                    // bit len 8
+    uint32_t   model_instance_id;             // bit len 32
+    struct
+    {
+        uint8_t    len;                       // Dynamic array length
+        uint8_t*   data;                      // Dynamic Array 8bit[31] max items
+    } model_name;
+
+} uavcan_equipment_power_BatteryInfo;
+
+extern
+uint32_t uavcan_equipment_power_BatteryInfo_encode(uavcan_equipment_power_BatteryInfo* source, void* msg_buf);
+
+extern
+int32_t uavcan_equipment_power_BatteryInfo_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_power_BatteryInfo* dest, uint8_t** dyn_arr_buf);
+
+extern
+uint32_t uavcan_equipment_power_BatteryInfo_encode_internal(uavcan_equipment_power_BatteryInfo* source, void* msg_buf, uint32_t offset, uint8_t root_item);
+
+extern
+int32_t uavcan_equipment_power_BatteryInfo_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_power_BatteryInfo* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif // __UAVCAN_EQUIPMENT_POWER_BATTERYINFO
\ No newline at end of file
diff --git a/libcanard/dsdl/uavcan/equipment/power/CircuitStatus.h b/libcanard/dsdl/uavcan/equipment/power/CircuitStatus.h
new file mode 100644
index 000000000..fe45d534c
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/power/CircuitStatus.h
@@ -0,0 +1,82 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/power/1091.CircuitStatus.uavcan
+ */
+
+#ifndef __UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS
+#define __UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS
+
+#include <stdint.h>
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/******************************* Source text **********************************
+#
+# Generic electrical circuit info.
+#
+
+uint16 circuit_id
+
+float16 voltage
+float16 current
+
+uint8 ERROR_FLAG_OVERVOLTAGE  = 1
+uint8 ERROR_FLAG_UNDERVOLTAGE = 2
+uint8 ERROR_FLAG_OVERCURRENT  = 4
+uint8 ERROR_FLAG_UNDERCURRENT = 8
+uint8 error_flags
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+uavcan.equipment.power.CircuitStatus
+saturated uint16 circuit_id
+saturated float16 voltage
+saturated float16 current
+saturated uint8 error_flags
+******************************************************************************/
+
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ID            1091
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_NAME          "uavcan.equipment.power.CircuitStatus"
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_SIGNATURE     (0x8313D33D0DDDA115ULL)
+
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_MAX_SIZE      ((56 + 7)/8)
+
+// Constants
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ERROR_FLAG_OVERVOLTAGE           1 // 1
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ERROR_FLAG_UNDERVOLTAGE          2 // 2
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ERROR_FLAG_OVERCURRENT           4 // 4
+#define UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS_ERROR_FLAG_UNDERCURRENT          8 // 8
+
+typedef struct
+{
+    // FieldTypes
+    uint16_t   circuit_id;                    // bit len 16
+    float      voltage;                       // float16 Saturate
+    float      current;                       // float16 Saturate
+    uint8_t    error_flags;                   // bit len 8
+
+} uavcan_equipment_power_CircuitStatus;
+
+extern
+uint32_t uavcan_equipment_power_CircuitStatus_encode(uavcan_equipment_power_CircuitStatus* source, void* msg_buf);
+
+extern
+int32_t uavcan_equipment_power_CircuitStatus_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_power_CircuitStatus* dest, uint8_t** dyn_arr_buf);
+
+extern
+uint32_t uavcan_equipment_power_CircuitStatus_encode_internal(uavcan_equipment_power_CircuitStatus* source, void* msg_buf, uint32_t offset, uint8_t root_item);
+
+extern
+int32_t uavcan_equipment_power_CircuitStatus_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_power_CircuitStatus* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif // __UAVCAN_EQUIPMENT_POWER_CIRCUITSTATUS
\ No newline at end of file
diff --git a/libcanard/dsdl/uavcan/equipment/power/PrimaryPowerSupplyStatus.h b/libcanard/dsdl/uavcan/equipment/power/PrimaryPowerSupplyStatus.h
new file mode 100644
index 000000000..e6f33f612
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/power/PrimaryPowerSupplyStatus.h
@@ -0,0 +1,87 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/power/1090.PrimaryPowerSupplyStatus.uavcan
+ */
+
+#ifndef __UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS
+#define __UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS
+
+#include <stdint.h>
+#include "canard.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/******************************* Source text **********************************
+#
+# Primary power supply status.
+# Typical publishing rate should be around 1~2 Hz.
+#
+
+#
+# How many hours left to full discharge at average load over the last 10 seconds.
+#
+float16 hours_to_empty_at_10sec_avg_power               # [Hours]
+float16 hours_to_empty_at_10sec_avg_power_variance      # [Hours^2]
+
+#
+# True if the publishing node senses that an external power source can be used, e.g. to charge batteries.
+#
+bool external_power_available
+
+#
+# Remaining energy estimate in percent.
+#
+uint7 remaining_energy_pct              # [Percent]     Required
+uint7 remaining_energy_pct_stdev        # [Percent]     Error standard deviation. Use best guess if unknown.
+******************************************************************************/
+
+/********************* DSDL signature source definition ***********************
+uavcan.equipment.power.PrimaryPowerSupplyStatus
+saturated float16 hours_to_empty_at_10sec_avg_power
+saturated float16 hours_to_empty_at_10sec_avg_power_variance
+saturated bool external_power_available
+saturated uint7 remaining_energy_pct
+saturated uint7 remaining_energy_pct_stdev
+******************************************************************************/
+
+#define UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_ID 1090
+#define UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_NAME "uavcan.equipment.power.PrimaryPowerSupplyStatus"
+#define UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_SIGNATURE (0xBBA05074AD757480ULL)
+
+#define UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS_MAX_SIZE ((47 + 7)/8)
+
+// Constants
+
+typedef struct
+{
+    // FieldTypes
+    float      hours_to_empty_at_10sec_avg_power; // float16 Saturate
+    float      hours_to_empty_at_10sec_avg_power_variance; // float16 Saturate
+    bool       external_power_available;      // bit len 1
+    uint8_t    remaining_energy_pct;          // bit len 7
+    uint8_t    remaining_energy_pct_stdev;    // bit len 7
+
+} uavcan_equipment_power_PrimaryPowerSupplyStatus;
+
+extern
+uint32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_encode(uavcan_equipment_power_PrimaryPowerSupplyStatus* source, void* msg_buf);
+
+extern
+int32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_decode(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_power_PrimaryPowerSupplyStatus* dest, uint8_t** dyn_arr_buf);
+
+extern
+uint32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_encode_internal(uavcan_equipment_power_PrimaryPowerSupplyStatus* source, void* msg_buf, uint32_t offset, uint8_t root_item);
+
+extern
+int32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_decode_internal(const CanardRxTransfer* transfer, uint16_t payload_len, uavcan_equipment_power_PrimaryPowerSupplyStatus* dest, uint8_t** dyn_arr_buf, int32_t offset, uint8_t tao);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#endif // __UAVCAN_EQUIPMENT_POWER_PRIMARYPOWERSUPPLYSTATUS
\ No newline at end of file
diff --git a/libcanard/dsdl/uavcan/equipment/power/power_BatteryInfo.c b/libcanard/dsdl/uavcan/equipment/power/power_BatteryInfo.c
new file mode 100644
index 000000000..9d661f8e1
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/power/power_BatteryInfo.c
@@ -0,0 +1,441 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/power/1092.BatteryInfo.uavcan
+ */
+#include "uavcan/equipment/power/BatteryInfo.h"
+#include "canard.h"
+
+#ifndef CANARD_INTERNAL_SATURATE
+#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
+#endif
+
+#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
+#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) );
+#endif
+
+#define CANARD_INTERNAL_ENABLE_TAO  ((uint8_t) 1)
+#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0)
+
+#if defined(__GNUC__)
+# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
+#else
+# define CANARD_MAYBE_UNUSED(x) x
+#endif
+
+/**
+  * @brief uavcan_equipment_power_BatteryInfo_encode_internal
+  * @param source : pointer to source data struct
+  * @param msg_buf: pointer to msg storage
+  * @param offset: bit offset to msg storage
+  * @param root_item: for detecting if TAO should be used
+  * @retval returns offset
+  */
+uint32_t uavcan_equipment_power_BatteryInfo_encode_internal(uavcan_equipment_power_BatteryInfo* source,
+  void* msg_buf,
+  uint32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(root_item))
+{
+    uint32_t c = 0;
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->temperature);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->temperature;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->voltage);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->voltage;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->current);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->current;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->average_power_10sec);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->average_power_10sec;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->remaining_capacity_wh);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->remaining_capacity_wh;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->full_charge_capacity_wh);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->full_charge_capacity_wh;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->hours_to_full_charge);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->hours_to_full_charge;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+    source->status_flags = CANARD_INTERNAL_SATURATE_UNSIGNED(source->status_flags, 2047)
+    canardEncodeScalar(msg_buf, offset, 11, (void*)&source->status_flags); // 2047
+    offset += 11;
+
+    source->state_of_health_pct = CANARD_INTERNAL_SATURATE_UNSIGNED(source->state_of_health_pct, 127)
+    canardEncodeScalar(msg_buf, offset, 7, (void*)&source->state_of_health_pct); // 127
+    offset += 7;
+
+    source->state_of_charge_pct = CANARD_INTERNAL_SATURATE_UNSIGNED(source->state_of_charge_pct, 127)
+    canardEncodeScalar(msg_buf, offset, 7, (void*)&source->state_of_charge_pct); // 127
+    offset += 7;
+
+    source->state_of_charge_pct_stdev = CANARD_INTERNAL_SATURATE_UNSIGNED(source->state_of_charge_pct_stdev, 127)
+    canardEncodeScalar(msg_buf, offset, 7, (void*)&source->state_of_charge_pct_stdev); // 127
+    offset += 7;
+
+    canardEncodeScalar(msg_buf, offset, 8, (void*)&source->battery_id); // 255
+    offset += 8;
+
+    canardEncodeScalar(msg_buf, offset, 32, (void*)&source->model_instance_id); // 4294967295
+    offset += 32;
+
+    // Dynamic Array (model_name)
+    if (! root_item)
+    {
+        // - Add array length
+        canardEncodeScalar(msg_buf, offset, 5, (void*)&source->model_name.len);
+        offset += 5;
+    }
+
+    // - Add array items
+    for (c = 0; c < source->model_name.len; c++)
+    {
+        canardEncodeScalar(msg_buf,
+                           offset,
+                           8,
+                           (void*)(source->model_name.data + c));// 255
+        offset += 8;
+    }
+
+    return offset;
+}
+
+/**
+  * @brief uavcan_equipment_power_BatteryInfo_encode
+  * @param source : Pointer to source data struct
+  * @param msg_buf: Pointer to msg storage
+  * @retval returns message length as bytes
+  */
+uint32_t uavcan_equipment_power_BatteryInfo_encode(uavcan_equipment_power_BatteryInfo* source, void* msg_buf)
+{
+    uint32_t offset = 0;
+
+    offset = uavcan_equipment_power_BatteryInfo_encode_internal(source, msg_buf, offset, 1);
+
+    return (offset + 7 ) / 8;
+}
+
+/**
+  * @brief uavcan_equipment_power_BatteryInfo_decode_internal
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_power_BatteryInfo dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @param offset: Call with 0, bit offset to msg storage
+  * @param tao: is tail array optimization used
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_power_BatteryInfo_decode_internal(
+  const CanardRxTransfer* transfer,
+  uint16_t CANARD_MAYBE_UNUSED(payload_len),
+  uavcan_equipment_power_BatteryInfo* dest,
+  uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf),
+  int32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(tao))
+{
+    int32_t ret = 0;
+    uint32_t c = 0;
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->temperature = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->temperature = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->voltage = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->voltage = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->current = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->current = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->average_power_10sec = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->average_power_10sec = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->remaining_capacity_wh = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->remaining_capacity_wh = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->full_charge_capacity_wh = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->full_charge_capacity_wh = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->hours_to_full_charge = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->hours_to_full_charge = (float)tmp_float;
+#endif
+    offset += 16;
+
+    ret = canardDecodeScalar(transfer, offset, 11, false, (void*)&dest->status_flags);
+    if (ret != 11)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+    offset += 11;
+
+    ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->state_of_health_pct);
+    if (ret != 7)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+    offset += 7;
+
+    ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->state_of_charge_pct);
+    if (ret != 7)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+    offset += 7;
+
+    ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->state_of_charge_pct_stdev);
+    if (ret != 7)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+    offset += 7;
+
+    ret = canardDecodeScalar(transfer, offset, 8, false, (void*)&dest->battery_id);
+    if (ret != 8)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+    offset += 8;
+
+    ret = canardDecodeScalar(transfer, offset, 32, false, (void*)&dest->model_instance_id);
+    if (ret != 32)
+    {
+        goto uavcan_equipment_power_BatteryInfo_error_exit;
+    }
+    offset += 32;
+
+    // Dynamic Array (model_name)
+    //  - Last item in struct & Root item & (Array Size > 8 bit), tail array optimization
+    if (payload_len && tao == CANARD_INTERNAL_ENABLE_TAO)
+    {
+        //  - Calculate Array length from MSG length
+        dest->model_name.len = ((payload_len * 8) - offset ) / 8; // 8 bit array item size
+    }
+    else
+    {
+        // - Array length 5 bits
+        ret = canardDecodeScalar(transfer,
+                                 offset,
+                                 5,
+                                 false,
+                                 (void*)&dest->model_name.len); // 255
+        if (ret != 5)
+        {
+            goto uavcan_equipment_power_BatteryInfo_error_exit;
+        }
+        offset += 5;
+    }
+
+    //  - Get Array
+    if (dyn_arr_buf)
+    {
+        dest->model_name.data = (uint8_t*)*dyn_arr_buf;
+    }
+
+    for (c = 0; c < dest->model_name.len; c++)
+    {
+        if (dyn_arr_buf)
+        {
+            ret = canardDecodeScalar(transfer,
+                                     offset,
+                                     8,
+                                     false,
+                                     (void*)*dyn_arr_buf); // 255
+            if (ret != 8)
+            {
+                goto uavcan_equipment_power_BatteryInfo_error_exit;
+            }
+            *dyn_arr_buf = (uint8_t*)(((uint8_t*)*dyn_arr_buf) + 1);
+        }
+        offset += 8;
+    }
+    return offset;
+
+uavcan_equipment_power_BatteryInfo_error_exit:
+    if (ret < 0)
+    {
+        return ret;
+    }
+    else
+    {
+        return -CANARD_ERROR_INTERNAL;
+    }
+}
+
+/**
+  * @brief uavcan_equipment_power_BatteryInfo_decode
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_power_BatteryInfo dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_power_BatteryInfo_decode(const CanardRxTransfer* transfer,
+  uint16_t payload_len,
+  uavcan_equipment_power_BatteryInfo* dest,
+  uint8_t** dyn_arr_buf)
+{
+    const int32_t offset = 0;
+    int32_t ret = 0;
+
+    /* Backward compatibility support for removing TAO
+     *  - first try to decode with TAO DISABLED
+     *  - if it fails fall back to TAO ENABLED
+     */
+    uint8_t tao = CANARD_INTERNAL_DISABLE_TAO;
+
+    while (1)
+    {
+        // Clear the destination struct
+        for (uint32_t c = 0; c < sizeof(uavcan_equipment_power_BatteryInfo); c++)
+        {
+            ((uint8_t*)dest)[c] = 0x00;
+        }
+
+        ret = uavcan_equipment_power_BatteryInfo_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao);
+
+        if (ret >= 0)
+        {
+            break;
+        }
+
+        if (tao == CANARD_INTERNAL_ENABLE_TAO)
+        {
+            break;
+        }
+        tao = CANARD_INTERNAL_ENABLE_TAO;
+    }
+
+    return ret;
+}
diff --git a/libcanard/dsdl/uavcan/equipment/power/power_CircuitStatus.c b/libcanard/dsdl/uavcan/equipment/power/power_CircuitStatus.c
new file mode 100644
index 000000000..018929bfb
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/power/power_CircuitStatus.c
@@ -0,0 +1,216 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/power/1091.CircuitStatus.uavcan
+ */
+#include "uavcan/equipment/power/CircuitStatus.h"
+#include "canard.h"
+
+#ifndef CANARD_INTERNAL_SATURATE
+#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
+#endif
+
+#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
+#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) );
+#endif
+
+#define CANARD_INTERNAL_ENABLE_TAO  ((uint8_t) 1)
+#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0)
+
+#if defined(__GNUC__)
+# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
+#else
+# define CANARD_MAYBE_UNUSED(x) x
+#endif
+
+/**
+  * @brief uavcan_equipment_power_CircuitStatus_encode_internal
+  * @param source : pointer to source data struct
+  * @param msg_buf: pointer to msg storage
+  * @param offset: bit offset to msg storage
+  * @param root_item: for detecting if TAO should be used
+  * @retval returns offset
+  */
+uint32_t uavcan_equipment_power_CircuitStatus_encode_internal(uavcan_equipment_power_CircuitStatus* source,
+  void* msg_buf,
+  uint32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(root_item))
+{
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&source->circuit_id); // 65535
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->voltage);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->voltage;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->current);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->current;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+    canardEncodeScalar(msg_buf, offset, 8, (void*)&source->error_flags); // 255
+    offset += 8;
+
+    return offset;
+}
+
+/**
+  * @brief uavcan_equipment_power_CircuitStatus_encode
+  * @param source : Pointer to source data struct
+  * @param msg_buf: Pointer to msg storage
+  * @retval returns message length as bytes
+  */
+uint32_t uavcan_equipment_power_CircuitStatus_encode(uavcan_equipment_power_CircuitStatus* source, void* msg_buf)
+{
+    uint32_t offset = 0;
+
+    offset = uavcan_equipment_power_CircuitStatus_encode_internal(source, msg_buf, offset, 1);
+
+    return (offset + 7 ) / 8;
+}
+
+/**
+  * @brief uavcan_equipment_power_CircuitStatus_decode_internal
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_power_CircuitStatus dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @param offset: Call with 0, bit offset to msg storage
+  * @param tao: is tail array optimization used
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_power_CircuitStatus_decode_internal(
+  const CanardRxTransfer* transfer,
+  uint16_t CANARD_MAYBE_UNUSED(payload_len),
+  uavcan_equipment_power_CircuitStatus* dest,
+  uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf),
+  int32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(tao))
+{
+    int32_t ret = 0;
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&dest->circuit_id);
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_CircuitStatus_error_exit;
+    }
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_CircuitStatus_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->voltage = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->voltage = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_CircuitStatus_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->current = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->current = (float)tmp_float;
+#endif
+    offset += 16;
+
+    ret = canardDecodeScalar(transfer, offset, 8, false, (void*)&dest->error_flags);
+    if (ret != 8)
+    {
+        goto uavcan_equipment_power_CircuitStatus_error_exit;
+    }
+    offset += 8;
+    return offset;
+
+uavcan_equipment_power_CircuitStatus_error_exit:
+    if (ret < 0)
+    {
+        return ret;
+    }
+    else
+    {
+        return -CANARD_ERROR_INTERNAL;
+    }
+}
+
+/**
+  * @brief uavcan_equipment_power_CircuitStatus_decode
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_power_CircuitStatus dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_power_CircuitStatus_decode(const CanardRxTransfer* transfer,
+  uint16_t payload_len,
+  uavcan_equipment_power_CircuitStatus* dest,
+  uint8_t** dyn_arr_buf)
+{
+    const int32_t offset = 0;
+    int32_t ret = 0;
+
+    /* Backward compatibility support for removing TAO
+     *  - first try to decode with TAO DISABLED
+     *  - if it fails fall back to TAO ENABLED
+     */
+    uint8_t tao = CANARD_INTERNAL_DISABLE_TAO;
+
+    while (1)
+    {
+        // Clear the destination struct
+        for (uint32_t c = 0; c < sizeof(uavcan_equipment_power_CircuitStatus); c++)
+        {
+            ((uint8_t*)dest)[c] = 0x00;
+        }
+
+        ret = uavcan_equipment_power_CircuitStatus_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao);
+
+        if (ret >= 0)
+        {
+            break;
+        }
+
+        if (tao == CANARD_INTERNAL_ENABLE_TAO)
+        {
+            break;
+        }
+        tao = CANARD_INTERNAL_ENABLE_TAO;
+    }
+
+    return ret;
+}
diff --git a/libcanard/dsdl/uavcan/equipment/power/power_PrimaryPowerSupplyStatus.c b/libcanard/dsdl/uavcan/equipment/power/power_PrimaryPowerSupplyStatus.c
new file mode 100644
index 000000000..dc0d4ea30
--- /dev/null
+++ b/libcanard/dsdl/uavcan/equipment/power/power_PrimaryPowerSupplyStatus.c
@@ -0,0 +1,229 @@
+/*
+ * UAVCAN data structure definition for libcanard.
+ *
+ * Autogenerated, do not edit.
+ *
+ * Source file: /home/benjamin/Skrivbord/tmp/uavcan/libcanard/dsdl_compiler/pyuavcan/uavcan/dsdl_files/uavcan/equipment/power/1090.PrimaryPowerSupplyStatus.uavcan
+ */
+#include "uavcan/equipment/power/PrimaryPowerSupplyStatus.h"
+#include "canard.h"
+
+#ifndef CANARD_INTERNAL_SATURATE
+#define CANARD_INTERNAL_SATURATE(x, max) ( ((x) > max) ? max : ( (-(x) > max) ? (-max) : (x) ) );
+#endif
+
+#ifndef CANARD_INTERNAL_SATURATE_UNSIGNED
+#define CANARD_INTERNAL_SATURATE_UNSIGNED(x, max) ( ((x) > max) ? max : (x) );
+#endif
+
+#define CANARD_INTERNAL_ENABLE_TAO  ((uint8_t) 1)
+#define CANARD_INTERNAL_DISABLE_TAO ((uint8_t) 0)
+
+#if defined(__GNUC__)
+# define CANARD_MAYBE_UNUSED(x) x __attribute__((unused))
+#else
+# define CANARD_MAYBE_UNUSED(x) x
+#endif
+
+/**
+  * @brief uavcan_equipment_power_PrimaryPowerSupplyStatus_encode_internal
+  * @param source : pointer to source data struct
+  * @param msg_buf: pointer to msg storage
+  * @param offset: bit offset to msg storage
+  * @param root_item: for detecting if TAO should be used
+  * @retval returns offset
+  */
+uint32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_encode_internal(uavcan_equipment_power_PrimaryPowerSupplyStatus* source,
+  void* msg_buf,
+  uint32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(root_item))
+{
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->hours_to_empty_at_10sec_avg_power);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->hours_to_empty_at_10sec_avg_power;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+
+    // float16 special handling
+#ifndef CANARD_USE_FLOAT16_CAST
+    tmp_float = canardConvertNativeFloatToFloat16(source->hours_to_empty_at_10sec_avg_power_variance);
+#else
+    tmp_float = (CANARD_USE_FLOAT16_CAST)source->hours_to_empty_at_10sec_avg_power_variance;
+#endif
+    canardEncodeScalar(msg_buf, offset, 16, (void*)&tmp_float); // 32767
+    offset += 16;
+    source->external_power_available = CANARD_INTERNAL_SATURATE_UNSIGNED(source->external_power_available, 0)
+    canardEncodeScalar(msg_buf, offset, 1, (void*)&source->external_power_available); // 0
+    offset += 1;
+
+    source->remaining_energy_pct = CANARD_INTERNAL_SATURATE_UNSIGNED(source->remaining_energy_pct, 127)
+    canardEncodeScalar(msg_buf, offset, 7, (void*)&source->remaining_energy_pct); // 127
+    offset += 7;
+
+    source->remaining_energy_pct_stdev = CANARD_INTERNAL_SATURATE_UNSIGNED(source->remaining_energy_pct_stdev, 127)
+    canardEncodeScalar(msg_buf, offset, 7, (void*)&source->remaining_energy_pct_stdev); // 127
+    offset += 7;
+
+    return offset;
+}
+
+/**
+  * @brief uavcan_equipment_power_PrimaryPowerSupplyStatus_encode
+  * @param source : Pointer to source data struct
+  * @param msg_buf: Pointer to msg storage
+  * @retval returns message length as bytes
+  */
+uint32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_encode(uavcan_equipment_power_PrimaryPowerSupplyStatus* source, void* msg_buf)
+{
+    uint32_t offset = 0;
+
+    offset = uavcan_equipment_power_PrimaryPowerSupplyStatus_encode_internal(source, msg_buf, offset, 1);
+
+    return (offset + 7 ) / 8;
+}
+
+/**
+  * @brief uavcan_equipment_power_PrimaryPowerSupplyStatus_decode_internal
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_power_PrimaryPowerSupplyStatus dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @param offset: Call with 0, bit offset to msg storage
+  * @param tao: is tail array optimization used
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_decode_internal(
+  const CanardRxTransfer* transfer,
+  uint16_t CANARD_MAYBE_UNUSED(payload_len),
+  uavcan_equipment_power_PrimaryPowerSupplyStatus* dest,
+  uint8_t** CANARD_MAYBE_UNUSED(dyn_arr_buf),
+  int32_t offset,
+  uint8_t CANARD_MAYBE_UNUSED(tao))
+{
+    int32_t ret = 0;
+#ifndef CANARD_USE_FLOAT16_CAST
+    uint16_t tmp_float = 0;
+#else
+    CANARD_USE_FLOAT16_CAST tmp_float = 0;
+#endif
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_PrimaryPowerSupplyStatus_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->hours_to_empty_at_10sec_avg_power = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->hours_to_empty_at_10sec_avg_power = (float)tmp_float;
+#endif
+    offset += 16;
+
+    // float16 special handling
+    ret = canardDecodeScalar(transfer, offset, 16, false, (void*)&tmp_float);
+
+    if (ret != 16)
+    {
+        goto uavcan_equipment_power_PrimaryPowerSupplyStatus_error_exit;
+    }
+#ifndef CANARD_USE_FLOAT16_CAST
+    dest->hours_to_empty_at_10sec_avg_power_variance = canardConvertFloat16ToNativeFloat(tmp_float);
+#else
+    dest->hours_to_empty_at_10sec_avg_power_variance = (float)tmp_float;
+#endif
+    offset += 16;
+
+    ret = canardDecodeScalar(transfer, offset, 1, false, (void*)&dest->external_power_available);
+    if (ret != 1)
+    {
+        goto uavcan_equipment_power_PrimaryPowerSupplyStatus_error_exit;
+    }
+    offset += 1;
+
+    ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->remaining_energy_pct);
+    if (ret != 7)
+    {
+        goto uavcan_equipment_power_PrimaryPowerSupplyStatus_error_exit;
+    }
+    offset += 7;
+
+    ret = canardDecodeScalar(transfer, offset, 7, false, (void*)&dest->remaining_energy_pct_stdev);
+    if (ret != 7)
+    {
+        goto uavcan_equipment_power_PrimaryPowerSupplyStatus_error_exit;
+    }
+    offset += 7;
+    return offset;
+
+uavcan_equipment_power_PrimaryPowerSupplyStatus_error_exit:
+    if (ret < 0)
+    {
+        return ret;
+    }
+    else
+    {
+        return -CANARD_ERROR_INTERNAL;
+    }
+}
+
+/**
+  * @brief uavcan_equipment_power_PrimaryPowerSupplyStatus_decode
+  * @param transfer: Pointer to CanardRxTransfer transfer
+  * @param payload_len: Payload message length
+  * @param dest: Pointer to destination struct
+  * @param dyn_arr_buf: NULL or Pointer to memory storage to be used for dynamic arrays
+  *                     uavcan_equipment_power_PrimaryPowerSupplyStatus dyn memory will point to dyn_arr_buf memory.
+  *                     NULL will ignore dynamic arrays decoding.
+  * @retval offset or ERROR value if < 0
+  */
+int32_t uavcan_equipment_power_PrimaryPowerSupplyStatus_decode(const CanardRxTransfer* transfer,
+  uint16_t payload_len,
+  uavcan_equipment_power_PrimaryPowerSupplyStatus* dest,
+  uint8_t** dyn_arr_buf)
+{
+    const int32_t offset = 0;
+    int32_t ret = 0;
+
+    /* Backward compatibility support for removing TAO
+     *  - first try to decode with TAO DISABLED
+     *  - if it fails fall back to TAO ENABLED
+     */
+    uint8_t tao = CANARD_INTERNAL_DISABLE_TAO;
+
+    while (1)
+    {
+        // Clear the destination struct
+        for (uint32_t c = 0; c < sizeof(uavcan_equipment_power_PrimaryPowerSupplyStatus); c++)
+        {
+            ((uint8_t*)dest)[c] = 0x00;
+        }
+
+        ret = uavcan_equipment_power_PrimaryPowerSupplyStatus_decode_internal(transfer, payload_len, dest, dyn_arr_buf, offset, tao);
+
+        if (ret >= 0)
+        {
+            break;
+        }
+
+        if (tao == CANARD_INTERNAL_ENABLE_TAO)
+        {
+            break;
+        }
+        tao = CANARD_INTERNAL_ENABLE_TAO;
+    }
+
+    return ret;
+}
diff --git a/main.c b/main.c
index 9d74871cb..5fb30488c 100644
--- a/main.c
+++ b/main.c
@@ -209,6 +209,7 @@ int main(void) {
 	app_configuration appconf;
 	conf_general_read_app_configuration(&appconf);
 	app_set_configuration(&appconf);
+	app_uartcomm_start_permanent();
 
 #ifdef HW_HAS_PERMANENT_NRF
 	conf_general_permanent_nrf_found = nrf_driver_init();
@@ -223,6 +224,7 @@ int main(void) {
 				HW_SPI_PORT_SCK, HW_SPI_PIN_SCK,
 				HW_SPI_PORT_MOSI, HW_SPI_PIN_MOSI,
 				HW_SPI_PORT_MISO, HW_SPI_PIN_MISO);
+		HW_PERMANENT_NRF_FAILED_HOOK();
 	}
 #endif
 
diff --git a/mc_interface.c b/mc_interface.c
index 142b34c7e..2b9bf6488 100644
--- a/mc_interface.c
+++ b/mc_interface.c
@@ -30,8 +30,10 @@
 #include "commands.h"
 #include "encoder.h"
 #include "drv8301.h"
-#include "drv8320.h"
+#include "drv8320s.h"
+#include "drv8323s.h"
 #include "buffer.h"
+#include "gpdrive.h"
 #include <math.h>
 
 // Macros
@@ -135,9 +137,12 @@ void mc_interface_init(mc_configuration *configuration) {
 #ifdef HW_HAS_DRV8301
 	drv8301_set_oc_mode(configuration->m_drv8301_oc_mode);
 	drv8301_set_oc_adj(configuration->m_drv8301_oc_adj);
-#elif defined(HW_HAS_DRV8320)
-	drv8320_set_oc_mode(configuration->m_drv8301_oc_mode);
-	drv8320_set_oc_adj(configuration->m_drv8301_oc_adj);
+#elif defined(HW_HAS_DRV8320S)
+	drv8320s_set_oc_mode(configuration->m_drv8301_oc_mode);
+	drv8320s_set_oc_adj(configuration->m_drv8301_oc_adj);
+#elif defined(HW_HAS_DRV8323S)
+	drv8323s_set_oc_mode(configuration->m_drv8301_oc_mode);
+	drv8323s_set_oc_adj(configuration->m_drv8301_oc_adj);
 #endif
 
 	// Initialize encoder
@@ -151,6 +156,10 @@ void mc_interface_init(mc_configuration *configuration) {
 		encoder_init_as5047p_spi();
 		break;
 
+	case SENSOR_PORT_MODE_AD2S1205:
+		encoder_init_ad2s1205_spi();
+		break;
+
 	default:
 		break;
 	}
@@ -167,6 +176,10 @@ void mc_interface_init(mc_configuration *configuration) {
 		mcpwm_foc_init(&m_conf);
 		break;
 
+	case MOTOR_TYPE_GPD:
+		gpdrive_init(&m_conf);
+		break;
+
 	default:
 		break;
 	}
@@ -189,6 +202,10 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 			encoder_init_as5047p_spi();
 			break;
 
+		case SENSOR_PORT_MODE_AD2S1205:
+			encoder_init_ad2s1205_spi();
+			break;
+
 		default:
 			break;
 		}
@@ -202,21 +219,38 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 #ifdef HW_HAS_DRV8301
 	drv8301_set_oc_mode(configuration->m_drv8301_oc_mode);
 	drv8301_set_oc_adj(configuration->m_drv8301_oc_adj);
-#elif defined(HW_HAS_DRV8320)
-	drv8320_set_oc_mode(configuration->m_drv8301_oc_mode);
-	drv8320_set_oc_adj(configuration->m_drv8301_oc_adj);
+#elif defined(HW_HAS_DRV8320S)
+	drv8320s_set_oc_mode(configuration->m_drv8301_oc_mode);
+	drv8320s_set_oc_adj(configuration->m_drv8301_oc_adj);
+#elif defined(HW_HAS_DRV8323S)
+	drv8323s_set_oc_mode(configuration->m_drv8301_oc_mode);
+	drv8323s_set_oc_adj(configuration->m_drv8301_oc_adj);
 #endif
 
-	if (m_conf.motor_type == MOTOR_TYPE_FOC
-			&& configuration->motor_type != MOTOR_TYPE_FOC) {
-		mcpwm_foc_deinit();
-		m_conf = *configuration;
-		mcpwm_init(&m_conf);
-	} else if (m_conf.motor_type != MOTOR_TYPE_FOC
-			&& configuration->motor_type == MOTOR_TYPE_FOC) {
+	if (m_conf.motor_type != configuration->motor_type) {
 		mcpwm_deinit();
+		mcpwm_foc_deinit();
+		gpdrive_deinit();
+
 		m_conf = *configuration;
-		mcpwm_foc_init(&m_conf);
+
+		switch (m_conf.motor_type) {
+		case MOTOR_TYPE_BLDC:
+		case MOTOR_TYPE_DC:
+			mcpwm_init(&m_conf);
+			break;
+
+		case MOTOR_TYPE_FOC:
+			mcpwm_foc_init(&m_conf);
+			break;
+
+		case MOTOR_TYPE_GPD:
+			gpdrive_init(&m_conf);
+			break;
+
+		default:
+			break;
+		}
 	} else {
 		m_conf = *configuration;
 	}
@@ -233,6 +267,10 @@ void mc_interface_set_configuration(mc_configuration *configuration) {
 		mcpwm_foc_set_configuration(&m_conf);
 		break;
 
+	case MOTOR_TYPE_GPD:
+		gpdrive_set_configuration(&m_conf);
+		break;
+
 	default:
 		break;
 	}
@@ -250,6 +288,10 @@ bool mc_interface_dccal_done(void) {
 		ret = mcpwm_foc_is_dccal_done();
 		break;
 
+	case MOTOR_TYPE_GPD:
+		ret = gpdrive_is_dccal_done();
+		break;
+
 	default:
 		break;
 	}
@@ -444,6 +486,11 @@ void mc_interface_set_brake_current(float current) {
 		mcpwm_foc_set_brake_current(DIR_MULT * current);
 		break;
 
+	case MOTOR_TYPE_GPD:
+		// For timeout to stop the output
+		gpdrive_set_mode(GPD_OUTPUT_MODE_NONE);
+		break;
+
 	default:
 		break;
 	}
@@ -577,6 +624,10 @@ float mc_interface_get_sampling_frequency_now(void) {
 		ret = mcpwm_foc_get_sampling_frequency_now();
 		break;
 
+	case MOTOR_TYPE_GPD:
+		ret = gpdrive_get_switching_frequency_now();
+		break;
+
 	default:
 		break;
 	}
@@ -850,7 +901,11 @@ float mc_interface_get_last_inj_adc_isr_duration(void) {
 		break;
 
 	case MOTOR_TYPE_FOC:
-		ret = mcpwm_foc_get_last_inj_adc_isr_duration();
+		ret = mcpwm_foc_get_last_adc_isr_duration();
+		break;
+
+	case MOTOR_TYPE_GPD:
+		ret = gpdrive_get_last_adc_isr_duration();
 		break;
 
 	default:
@@ -861,6 +916,10 @@ float mc_interface_get_last_inj_adc_isr_duration(void) {
 }
 
 float mc_interface_read_reset_avg_motor_current(void) {
+	if (m_conf.motor_type == MOTOR_TYPE_GPD) {
+		return gpdrive_get_current_filtered();
+	}
+
 	float res = m_motor_current_sum / m_motor_current_iterations;
 	m_motor_current_sum = 0.0;
 	m_motor_current_iterations = 0.0;
@@ -868,6 +927,10 @@ float mc_interface_read_reset_avg_motor_current(void) {
 }
 
 float mc_interface_read_reset_avg_input_current(void) {
+	if (m_conf.motor_type == MOTOR_TYPE_GPD) {
+		return gpdrive_get_current_filtered() * gpdrive_get_modulation();
+	}
+
 	float res = m_input_current_sum / m_input_current_iterations;
 	m_input_current_sum = 0.0;
 	m_input_current_iterations = 0.0;
@@ -969,6 +1032,96 @@ float mc_interface_temp_motor_filtered(void) {
 	return m_temp_motor;
 }
 
+/**
+ * Get the battery level, based on battery settings in configuration. Notice that
+ * this function is based on remaining watt hours, and not amp hours.
+ *
+ * @param wh_left
+ * Pointer to where to store the remaining watt hours, can be null.
+ *
+ * @return
+ * Battery level, range 0 to 1
+ */
+float mc_interface_get_battery_level(float *wh_left) {
+	const volatile mc_configuration *conf = mc_interface_get_configuration();
+	const float v_in = GET_INPUT_VOLTAGE();
+	float battery_avg_voltage = 0.0;
+	float battery_avg_voltage_left = 0.0;
+	float ah_left = 0;
+
+	switch (conf->si_battery_type) {
+	case BATTERY_TYPE_LIION_3_0__4_2:
+		battery_avg_voltage = ((3.2 + 4.2) / 2.0) * (float)(conf->si_battery_cells);
+		battery_avg_voltage_left = ((3.2 * (float)(conf->si_battery_cells) + v_in) / 2.0);
+		ah_left = utils_map(v_in / (float)(conf->si_battery_cells),
+				3.2, 4.2, 0.0, conf->si_battery_ah);
+		break;
+
+	case BATTERY_TYPE_LIIRON_2_6__3_6:
+		battery_avg_voltage = ((2.8 + 3.6) / 2.0) * (float)(conf->si_battery_cells);
+		battery_avg_voltage_left = ((2.8 * (float)(conf->si_battery_cells) + v_in) / 2.0);
+		ah_left = utils_map(v_in / (float)(conf->si_battery_cells),
+				2.6, 3.6, 0.0, conf->si_battery_ah);
+		break;
+
+	case BATTERY_TYPE_LEAD_ACID:
+		// TODO: This does not really work for lead-acid batteries
+		battery_avg_voltage = ((2.1 + 2.36) / 2.0) * (float)(conf->si_battery_cells);
+		battery_avg_voltage_left = ((2.1 * (float)(conf->si_battery_cells) + v_in) / 2.0);
+		ah_left = utils_map(v_in / (float)(conf->si_battery_cells),
+				2.1, 2.36, 0.0, conf->si_battery_ah);
+		break;
+
+	default:
+		break;
+	}
+
+	const float wh_batt_tot = conf->si_battery_ah * battery_avg_voltage;
+	const float wh_batt_left = ah_left * battery_avg_voltage_left;
+
+	if (wh_left) {
+		*wh_left = wh_batt_left;
+	}
+
+	return wh_batt_left / wh_batt_tot;
+}
+
+/**
+ * Get the speed based on wheel diameter, gearing and motor pole settings.
+ *
+ * @return
+ * Speed, in m/s
+ */
+float mc_interface_get_speed(void) {
+	const volatile mc_configuration *conf = mc_interface_get_configuration();
+	const float rpm = mc_interface_get_rpm() / (conf->si_motor_poles / 2.0);
+	return (rpm / 60.0) * conf->si_wheel_diameter * M_PI / conf->si_gear_ratio;
+}
+
+/**
+ * Get the distance traveled based on wheel diameter, gearing and motor pole settings.
+ *
+ * @return
+ * Distance traveled since boot, in meters
+ */
+float mc_interface_get_distance(void) {
+	const volatile mc_configuration *conf = mc_interface_get_configuration();
+	const float tacho_scale = (conf->si_wheel_diameter * M_PI) / (3.0 * conf->si_motor_poles * conf->si_gear_ratio);
+	return mc_interface_get_tachometer_value(false) * tacho_scale;
+}
+
+/**
+ * Get the absolute distance traveled based on wheel diameter, gearing and motor pole settings.
+ *
+ * @return
+ * Absolute distance traveled since boot, in meters
+ */
+float mc_interface_get_distance_abs(void) {
+	const volatile mc_configuration *conf = mc_interface_get_configuration();
+	const float tacho_scale = (conf->si_wheel_diameter * M_PI) / (3.0 * conf->si_motor_poles * conf->si_gear_ratio);
+	return mc_interface_get_tachometer_abs_value(false) * tacho_scale;
+}
+
 // MC implementation functions
 
 /**
@@ -1050,10 +1203,14 @@ void mc_interface_fault_stop(mc_fault_code fault) {
 		if (fault == FAULT_CODE_DRV) {
 			fdata.drv8301_faults = drv8301_read_faults();
 		}
-#elif defined(HW_HAS_DRV8320)
-		if (fault == FAULT_CODE_DRV) {
-			fdata.drv8301_faults = drv8320_read_faults();
+#elif defined(HW_HAS_DRV8320S)
+ 		if (fault == FAULT_CODE_DRV) {
+			fdata.drv8301_faults = drv8320s_read_faults();
 		}
+#elif defined(HW_HAS_DRV8323S)
+		if (fault == FAULT_CODE_DRV) {
+			fdata.drv8301_faults = drv8323s_read_faults();
+ 		}
 #endif
 		terminal_add_fault_data(&fdata);
 	}
@@ -1070,6 +1227,10 @@ void mc_interface_fault_stop(mc_fault_code fault) {
 		mcpwm_foc_stop_pwm();
 		break;
 
+	case MOTOR_TYPE_GPD:
+		gpdrive_set_mode(GPD_OUTPUT_MODE_NONE);
+		break;
+
 	default:
 		break;
 	}
@@ -1338,9 +1499,12 @@ static void update_override_limits(volatile mc_configuration *conf) {
 	UTILS_LP_FAST(m_temp_fet, NTC_TEMP(ADC_IND_TEMP_MOS), 0.1);
 	UTILS_LP_FAST(m_temp_motor, NTC_TEMP_MOTOR(conf->m_ntc_motor_beta), 0.1);
 
+	const float l_current_min_tmp = conf->l_current_min * conf->l_current_min_scale;
+	const float l_current_max_tmp = conf->l_current_max * conf->l_current_max_scale;
+
 	// Temperature MOSFET
-	float lo_min_mos = conf->l_current_min;
-	float lo_max_mos = conf->l_current_max;
+	float lo_min_mos = l_current_min_tmp;
+	float lo_max_mos = l_current_max_tmp;
 	if (m_temp_fet < conf->l_temp_fet_start) {
 		// Keep values
 	} else if (m_temp_fet > conf->l_temp_fet_end) {
@@ -1348,25 +1512,25 @@ static void update_override_limits(volatile mc_configuration *conf) {
 		lo_max_mos = 0.0;
 		mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_FET);
 	} else {
-		float maxc = fabsf(conf->l_current_max);
-		if (fabsf(conf->l_current_min) > maxc) {
-			maxc = fabsf(conf->l_current_min);
+		float maxc = fabsf(l_current_max_tmp);
+		if (fabsf(l_current_min_tmp) > maxc) {
+			maxc = fabsf(l_current_min_tmp);
 		}
 
 		maxc = utils_map(m_temp_fet, conf->l_temp_fet_start, conf->l_temp_fet_end, maxc, 0.0);
 
-		if (fabsf(conf->l_current_min) > maxc) {
-			lo_min_mos = SIGN(conf->l_current_min) * maxc;
+		if (fabsf(l_current_min_tmp) > maxc) {
+			lo_min_mos = SIGN(l_current_min_tmp) * maxc;
 		}
 
-		if (fabsf(conf->l_current_max) > maxc) {
-			lo_max_mos = SIGN(conf->l_current_max) * maxc;
+		if (fabsf(l_current_max_tmp) > maxc) {
+			lo_max_mos = SIGN(l_current_max_tmp) * maxc;
 		}
 	}
 
 	// Temperature MOTOR
-	float lo_min_mot = conf->l_current_min;
-	float lo_max_mot = conf->l_current_max;
+	float lo_min_mot = l_current_min_tmp;
+	float lo_max_mot = l_current_max_tmp;
 	if (m_temp_motor < conf->l_temp_motor_start) {
 		// Keep values
 	} else if (m_temp_motor > conf->l_temp_motor_end) {
@@ -1374,19 +1538,19 @@ static void update_override_limits(volatile mc_configuration *conf) {
 		lo_max_mot = 0.0;
 		mc_interface_fault_stop(FAULT_CODE_OVER_TEMP_MOTOR);
 	} else {
-		float maxc = fabsf(conf->l_current_max);
-		if (fabsf(conf->l_current_min) > maxc) {
-			maxc = fabsf(conf->l_current_min);
+		float maxc = fabsf(l_current_max_tmp);
+		if (fabsf(l_current_min_tmp) > maxc) {
+			maxc = fabsf(l_current_min_tmp);
 		}
 
 		maxc = utils_map(m_temp_motor, conf->l_temp_motor_start, conf->l_temp_motor_end, maxc, 0.0);
 
-		if (fabsf(conf->l_current_min) > maxc) {
-			lo_min_mot = SIGN(conf->l_current_min) * maxc;
+		if (fabsf(l_current_min_tmp) > maxc) {
+			lo_min_mot = SIGN(l_current_min_tmp) * maxc;
 		}
 
-		if (fabsf(conf->l_current_max) > maxc) {
-			lo_max_mot = SIGN(conf->l_current_max) * maxc;
+		if (fabsf(l_current_max_tmp) > maxc) {
+			lo_max_mot = SIGN(l_current_max_tmp) * maxc;
 		}
 	}
 
@@ -1399,22 +1563,22 @@ static void update_override_limits(volatile mc_configuration *conf) {
 
 	float lo_fet_temp_accel = 0.0;
 	if (m_temp_fet < temp_fet_accel_start) {
-		lo_fet_temp_accel = conf->l_current_max;
+		lo_fet_temp_accel = l_current_max_tmp;
 	} else if (m_temp_fet > temp_fet_accel_end) {
 		lo_fet_temp_accel = 0.0;
 	} else {
 		lo_fet_temp_accel = utils_map(m_temp_fet, temp_fet_accel_start,
-				temp_fet_accel_end, conf->l_current_max, 0.0);
+				temp_fet_accel_end, l_current_max_tmp, 0.0);
 	}
 
 	float lo_motor_temp_accel = 0.0;
 	if (m_temp_motor < temp_motor_accel_start) {
-		lo_motor_temp_accel = conf->l_current_max;
+		lo_motor_temp_accel = l_current_max_tmp;
 	} else if (m_temp_motor > temp_motor_accel_end) {
 		lo_motor_temp_accel = 0.0;
 	} else {
 		lo_motor_temp_accel = utils_map(m_temp_motor, temp_motor_accel_start,
-				temp_motor_accel_end, conf->l_current_max, 0.0);
+				temp_motor_accel_end, l_current_max_tmp, 0.0);
 	}
 
 	// RPM max
@@ -1422,11 +1586,11 @@ static void update_override_limits(volatile mc_configuration *conf) {
 	const float rpm_pos_cut_start = conf->l_max_erpm * conf->l_erpm_start;
 	const float rpm_pos_cut_end = conf->l_max_erpm;
 	if (rpm_now < rpm_pos_cut_start) {
-		lo_max_rpm = conf->l_current_max;
+		lo_max_rpm = l_current_max_tmp;
 	} else if (rpm_now > rpm_pos_cut_end) {
 		lo_max_rpm = 0.0;
 	} else {
-		lo_max_rpm = utils_map(rpm_now, rpm_pos_cut_start, rpm_pos_cut_end, conf->l_current_max, 0.0);
+		lo_max_rpm = utils_map(rpm_now, rpm_pos_cut_start, rpm_pos_cut_end, l_current_max_tmp, 0.0);
 	}
 
 	// RPM min
@@ -1434,11 +1598,11 @@ static void update_override_limits(volatile mc_configuration *conf) {
 	const float rpm_neg_cut_start = conf->l_min_erpm * conf->l_erpm_start;
 	const float rpm_neg_cut_end = conf->l_min_erpm;
 	if (rpm_now > rpm_neg_cut_start) {
-		lo_min_rpm = conf->l_current_max;
+		lo_min_rpm = l_current_max_tmp;
 	} else if (rpm_now < rpm_neg_cut_end) {
 		lo_min_rpm = 0.0;
 	} else {
-		lo_min_rpm = utils_map(rpm_now, rpm_neg_cut_start, rpm_neg_cut_end, conf->l_current_max, 0.0);
+		lo_min_rpm = utils_map(rpm_now, rpm_neg_cut_start, rpm_neg_cut_end, l_current_max_tmp, 0.0);
 	}
 
 	float lo_max = utils_min_abs(lo_max_mos, lo_max_mot);
diff --git a/mc_interface.h b/mc_interface.h
index 968b9925d..ded1563c3 100644
--- a/mc_interface.h
+++ b/mc_interface.h
@@ -73,6 +73,10 @@ float mc_interface_get_last_sample_adc_isr_duration(void);
 void mc_interface_sample_print_data(debug_sampling_mode mode, uint16_t len, uint8_t decimation);
 float mc_interface_temp_fet_filtered(void);
 float mc_interface_temp_motor_filtered(void);
+float mc_interface_get_battery_level(float *wh_left);
+float mc_interface_get_speed(void);
+float mc_interface_get_distance(void);
+float mc_interface_get_distance_abs(void);
 
 // MC implementation functions
 void mc_interface_fault_stop(mc_fault_code fault);
diff --git a/mcconf/mcconf_default.h b/mcconf/mcconf_default.h
index bc4a2da0c..8d7d92077 100644
--- a/mcconf/mcconf_default.h
+++ b/mcconf/mcconf_default.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -42,10 +42,10 @@
 #define MCCONF_L_CURRENT_MIN			-60.0	// Current limit in Amperes (Lower)
 #endif
 #ifndef MCCONF_L_IN_CURRENT_MAX
-#define MCCONF_L_IN_CURRENT_MAX			60.0	// Input current limit in Amperes (Upper)
+#define MCCONF_L_IN_CURRENT_MAX			99.0	// Input current limit in Amperes (Upper)
 #endif
 #ifndef MCCONF_L_IN_CURRENT_MIN
-#define MCCONF_L_IN_CURRENT_MIN			-40.0	// Input current limit in Amperes (Lower)
+#define MCCONF_L_IN_CURRENT_MIN			-60.0	// Input current limit in Amperes (Lower)
 #endif
 #ifndef MCCONF_L_MAX_ABS_CURRENT
 #define MCCONF_L_MAX_ABS_CURRENT		130.0	// The maximum absolute current above which a fault is generated
@@ -102,10 +102,16 @@
 #define MCCONF_L_LIM_TEMP_ACCEL_DEC		0.15	// Decrease temperature limits this much during acceleration
 #endif
 #ifndef MCCONF_L_WATT_MAX
-#define MCCONF_L_WATT_MAX				15000.0	// Maximum wattage output
+#define MCCONF_L_WATT_MAX				1500000.0	// Maximum wattage output
 #endif
 #ifndef MCCONF_L_WATT_MIN
-#define MCCONF_L_WATT_MIN				-15000.0	// Minimum wattage output (braking)
+#define MCCONF_L_WATT_MIN				-1500000.0	// Minimum wattage output (braking)
+#endif
+#ifndef MCCONF_L_CURRENT_MAX_SCALE
+#define MCCONF_L_CURRENT_MAX_SCALE		1.0	// Maximum current scale
+#endif
+#ifndef MCCONF_L_CURRENT_MIN_SCALE
+#define MCCONF_L_CURRENT_MIN_SCALE		1.0	// Minimum current scale
 #endif
 
 // Speed PID parameters
@@ -240,7 +246,7 @@
 #define MCCONF_FOC_PLL_KP				2000.0
 #endif
 #ifndef MCCONF_FOC_PLL_KI
-#define MCCONF_FOC_PLL_KI				40000.0
+#define MCCONF_FOC_PLL_KI				30000.0
 #endif
 #ifndef MCCONF_FOC_MOTOR_L
 #define MCCONF_FOC_MOTOR_L				0.000007
@@ -324,6 +330,23 @@
 #define MCCONF_FOC_CURRENT_FILTER_CONST	0.1		// Filter constant for the filtered currents
 #endif
 
+// GPD
+#ifndef MCCONF_GPD_BUFFER_NOTIFY_LEFT
+#define MCCONF_GPD_BUFFER_NOTIFY_LEFT	200		// Notify when the buffer space is left than this
+#endif
+#ifndef MCCONF_GPD_BUFFER_INTERPOL
+#define MCCONF_GPD_BUFFER_INTERPOL		0		// Buffer interpolation
+#endif
+#ifndef MCCONF_GPD_CURRENT_FILTER_CONST
+#define MCCONF_GPD_CURRENT_FILTER_CONST	0.1		// Current filter constant
+#endif
+#ifndef MCCONF_GPD_CURRENT_KP
+#define MCCONF_GPD_CURRENT_KP			0.03
+#endif
+#ifndef MCCONF_GPD_CURRENT_KI
+#define MCCONF_GPD_CURRENT_KI			50.0
+#endif
+
 // Misc
 #ifndef MCCONF_M_FAULT_STOP_TIME
 #define MCCONF_M_FAULT_STOP_TIME		500	// Ignore commands for this duration in msec when faults occur
@@ -356,7 +379,7 @@
 #define MCCONF_M_BLDC_F_SW_MAX			40000 // Maximum switching frequency in bldc mode
 #endif
 #ifndef MCCONF_M_DC_F_SW
-#define MCCONF_M_DC_F_SW				35000 // Switching frequency in dc mode
+#define MCCONF_M_DC_F_SW				25000 // Switching frequency in dc mode
 #endif
 #ifndef MCCONF_M_NTC_MOTOR_BETA
 #define MCCONF_M_NTC_MOTOR_BETA			3380.0 // Beta value for motor termistor
@@ -365,4 +388,24 @@
 #define MCCONF_M_OUT_AUX_MODE			OUT_AUX_MODE_OFF // Auxiliary output mode
 #endif
 
+// Setup Info
+#ifndef MCCONF_SI_MOTOR_POLES
+#define MCCONF_SI_MOTOR_POLES			14 // Motor pole count
+#endif
+#ifndef MCCONF_SI_GEAR_RATIO
+#define MCCONF_SI_GEAR_RATIO			3 // Gear ratio
+#endif
+#ifndef MCCONF_SI_WHEEL_DIAMETER
+#define MCCONF_SI_WHEEL_DIAMETER		0.083 // Wheel Diameter
+#endif
+#ifndef MCCONF_SI_BATTERY_TYPE
+#define MCCONF_SI_BATTERY_TYPE			BATTERY_TYPE_LIION_3_0__4_2 // Battery Type
+#endif
+#ifndef MCCONF_SI_BATTERY_CELLS
+#define MCCONF_SI_BATTERY_CELLS			3 // Battery Cells
+#endif
+#ifndef MCCONF_SI_BATTERY_AH
+#define MCCONF_SI_BATTERY_AH			6.0 // Battery amp hours
+#endif
+
 #endif /* MCCONF_DEFAULT_H_ */
diff --git a/mcpwm.c b/mcpwm.c
index f1ac8d3be..09a48f182 100644
--- a/mcpwm.c
+++ b/mcpwm.c
@@ -87,8 +87,10 @@ static volatile float last_pwm_cycles_sums[6];
 static volatile bool dccal_done;
 static volatile bool sensorless_now;
 static volatile int hall_detect_table[8][7];
-static volatile bool init_done;
+static volatile bool init_done = false;
 static volatile mc_comm_mode comm_mode_next;
+static volatile float m_pll_phase;
+static volatile float m_pll_speed;
 
 #ifdef HW_HAS_3_SHUNTS
 static volatile int curr2_sum;
@@ -137,7 +139,7 @@ static void stop_pwm_hw(void);
 static void full_brake_ll(void);
 static void full_brake_hw(void);
 static void run_pid_control_speed(void);
-static void run_pid_control_pos(float dt);
+static void run_pid_control_pos(float dt, float pos_now);
 static void set_next_comm_step(int next_step);
 static void update_rpm_tacho(void);
 static void update_sensor_mode(void);
@@ -148,6 +150,8 @@ static void set_next_timer_settings(mc_timer_struct *settings);
 static void update_timer_attempt(void);
 static void set_switching_frequency(float frequency);
 static void do_dc_cal(void);
+static void pll_run(float phase, float dt, volatile float *phase_var,
+		volatile float *speed_var);
 
 // Defines
 #define IS_DETECTING()			(state == MC_STATE_DETECTING)
@@ -204,6 +208,8 @@ void mcpwm_init(volatile mc_configuration *configuration) {
 	memset((void*)hall_detect_table, 0, sizeof(hall_detect_table[0][0]) * 8 * 7);
 	update_sensor_mode();
 	comm_mode_next = conf->comm_mode;
+	m_pll_phase = 0.0;
+	m_pll_speed = 0.0;
 
 	mcpwm_init_hall_table((int8_t*)conf->hall_table);
 
@@ -436,6 +442,8 @@ void mcpwm_init(volatile mc_configuration *configuration) {
 
 	utils_sys_unlock_cnt();
 
+	CURRENT_FILTER_ON();
+
 	// Calibrate current offset
 	ENABLE_GATE();
 	DCCAL_OFF();
@@ -474,6 +482,10 @@ void mcpwm_init(volatile mc_configuration *configuration) {
 }
 
 void mcpwm_deinit(void) {
+	if (!init_done) {
+		return;
+	}
+
 	init_done = false;
 
 	WWDG_DeInit();
@@ -568,6 +580,17 @@ static void do_dc_cal(void) {
 	dccal_done = true;
 }
 
+static void pll_run(float phase, float dt, volatile float *phase_var,
+		volatile float *speed_var) {
+	UTILS_NAN_ZERO(*phase_var);
+	float delta_theta = phase - *phase_var;
+	utils_norm_angle_rad(&delta_theta);
+	UTILS_NAN_ZERO(*speed_var);
+	*phase_var += (*speed_var + conf->foc_pll_kp * delta_theta) * dt;
+	utils_norm_angle_rad((float*)phase_var);
+	*speed_var += conf->foc_pll_ki * delta_theta * dt;
+}
+
 /**
  * Use duty cycle control. Absolute values less than MCPWM_MIN_DUTY_CYCLE will
  * stop the motor.
@@ -645,7 +668,8 @@ void mcpwm_set_current(float current) {
 		return;
 	}
 
-	utils_truncate_number(&current, -conf->l_current_max, conf->l_current_max);
+	utils_truncate_number(&current, -conf->l_current_max * conf->l_current_max_scale,
+			conf->l_current_max * conf->l_current_max_scale);
 
 	control_mode = CONTROL_MODE_CURRENT;
 	current_set = current;
@@ -669,7 +693,7 @@ void mcpwm_set_brake_current(float current) {
 		return;
 	}
 
-	utils_truncate_number(&current, -fabsf(conf->l_current_min), fabsf(conf->l_current_min));
+	utils_truncate_number(&current, -fabsf(conf->lo_current_min), fabsf(conf->lo_current_min));
 
 	control_mode = CONTROL_MODE_CURRENT_BRAKE;
 	current_set = current;
@@ -730,7 +754,11 @@ float mcpwm_get_switching_frequency_now(void) {
  * The RPM value.
  */
 float mcpwm_get_rpm(void) {
-	return direction ? rpm_now : -rpm_now;
+	if (conf->motor_type == MOTOR_TYPE_DC) {
+		return m_pll_speed / ((2.0 * M_PI) / 60.0);
+	} else {
+		return direction ? rpm_now : -rpm_now;
+	}
 }
 
 mc_state mcpwm_get_state(void) {
@@ -1223,7 +1251,7 @@ static void run_pid_control_speed(void) {
 #endif
 }
 
-static void run_pid_control_pos(float dt) {
+static void run_pid_control_pos(float dt, float pos_now) {
 	static float i_term = 0;
 	static float prev_error = 0;
 	float p_term;
@@ -1237,7 +1265,7 @@ static void run_pid_control_pos(float dt) {
 	}
 
 	// Compute error
-	float error = utils_angle_difference(encoder_read_deg(), pos_pid_set_pos);
+	float error = utils_angle_difference(pos_now, pos_pid_set_pos);
 
 	// Compute parameters
 	p_term = error * conf->p_pid_kp;
@@ -1454,16 +1482,16 @@ void mcpwm_adc_inj_int_handler(void) {
 #endif
 
 	if (curr_samp_volt & (1 << 0)) {
-		curr0 = ADC_Value[ADC_IND_CURR1];
+		curr0 = GET_CURRENT1();
 	}
 
 	if (curr_samp_volt & (1 << 1)) {
-		curr1 = ADC_Value[ADC_IND_CURR2];
+		curr1 = GET_CURRENT2();
 	}
 
 #ifdef HW_HAS_3_SHUNTS
 	if (curr_samp_volt & (1 << 2)) {
-		curr2 = ADC_Value[ADC_IND_CURR3];
+		curr2 = GET_CURRENT3();
 	}
 #endif
 
@@ -1531,12 +1559,12 @@ void mcpwm_adc_inj_int_handler(void) {
 	if (conf->motor_type == MOTOR_TYPE_DC) {
 		if (direction) {
 #ifdef HW_HAS_3_SHUNTS
-			curr_tot_sample = -(float)(ADC_Value[ADC_IND_CURR3] - curr2_offset);
+			curr_tot_sample = -(float)(GET_CURRENT3() - curr2_offset);
 #else
-			curr_tot_sample = -(float)(ADC_Value[ADC_IND_CURR2] - curr1_offset);
+			curr_tot_sample = -(float)(GET_CURRENT2() - curr1_offset);
 #endif
 		} else {
-			curr_tot_sample = -(float)(ADC_Value[ADC_IND_CURR1] - curr0_offset);
+			curr_tot_sample = -(float)(GET_CURRENT1() - curr0_offset);
 		}
 	} else {
 		static int detect_now = 0;
@@ -1933,7 +1961,6 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
 		const float voltage_scale = 20.0 / input_voltage;
 		float ramp_step = conf->m_duty_ramp_step / (switching_frequency_now / 1000.0);
 		float ramp_step_no_lim = ramp_step;
-		const float rpm = mcpwm_get_rpm();
 
 		if (slow_ramping_cycles) {
 			slow_ramping_cycles--;
@@ -2063,10 +2090,13 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
 		}
 
 		// Don't start in the opposite direction when the RPM is too high even if the current is low enough.
-		if (dutycycle_now >= conf->l_min_duty && rpm < -conf->l_max_erpm_fbrake) {
-			dutycycle_now = -conf->l_min_duty;
-		} else if (dutycycle_now <= -conf->l_min_duty && rpm > conf->l_max_erpm_fbrake) {
-			dutycycle_now = conf->l_min_duty;
+		if (conf->motor_type != MOTOR_TYPE_DC) {
+			const float rpm = mcpwm_get_rpm();
+			if (dutycycle_now >= conf->l_min_duty && rpm < -conf->l_max_erpm_fbrake) {
+				dutycycle_now = -conf->l_min_duty;
+			} else if (dutycycle_now <= -conf->l_min_duty && rpm > conf->l_max_erpm_fbrake) {
+				dutycycle_now = conf->l_min_duty;
+			}
 		}
 
 		set_duty_cycle_ll(dutycycle_now);
@@ -2075,7 +2105,9 @@ void mcpwm_adc_int_handler(void *p, uint32_t flags) {
 	mc_interface_mc_timer_isr();
 
 	if (encoder_is_configured()) {
-		run_pid_control_pos(1.0 / switching_frequency_now);
+		float pos = encoder_read_deg();
+		run_pid_control_pos(1.0 / switching_frequency_now, pos);
+		pll_run(-pos * M_PI / 180.0, 1.0 / switching_frequency_now, &m_pll_phase, &m_pll_speed);
 	}
 
 	last_adc_isr_duration = (float)TIM12->CNT / 10000000.0;
diff --git a/mcpwm.h b/mcpwm.h
index fab4ec8ff..c15f567e3 100644
--- a/mcpwm.h
+++ b/mcpwm.h
@@ -67,6 +67,9 @@ mc_rpm_dep_struct mcpwm_get_rpm_dep(void);
 bool mcpwm_is_dccal_done(void);
 void mcpwm_switch_comm_mode(mc_comm_mode next);
 
+void drv8323s_dccal_on(void);
+void drv8323s_dccal_off(void);
+
 // Interrupt handlers
 void mcpwm_adc_inj_int_handler(void);
 void mcpwm_adc_int_handler(void *p, uint32_t flags);
diff --git a/mcpwm_foc.c b/mcpwm_foc.c
index 157a9281a..2db284365 100644
--- a/mcpwm_foc.c
+++ b/mcpwm_foc.c
@@ -89,6 +89,7 @@ static volatile float m_duty_cycle_set;
 static volatile float m_id_set;
 static volatile float m_iq_set;
 static volatile float m_openloop_speed;
+static volatile float m_openloop_phase;
 static volatile bool m_dccal_done;
 static volatile bool m_output_on;
 static volatile float m_pos_pid_set;
@@ -105,9 +106,9 @@ static volatile float m_pll_speed;
 static volatile mc_sample_t m_samples;
 static volatile int m_tachometer;
 static volatile int m_tachometer_abs;
-static volatile float last_inj_adc_isr_duration;
+static volatile float m_last_adc_isr_duration;
 static volatile float m_pos_pid_now;
-static volatile bool m_init_done;
+static volatile bool m_init_done = false;
 static volatile float m_gamma_now;
 
 #ifdef HW_HAS_3_SHUNTS
@@ -212,6 +213,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	m_id_set = 0.0;
 	m_iq_set = 0.0;
 	m_openloop_speed = 0.0;
+	m_openloop_phase = 0.0;
 	m_output_on = false;
 	m_pos_pid_set = 0.0;
 	m_speed_pid_set_rpm = 0.0;
@@ -226,7 +228,7 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 	m_pll_speed = 0.0;
 	m_tachometer = 0;
 	m_tachometer_abs = 0;
-	last_inj_adc_isr_duration = 0;
+	m_last_adc_isr_duration = 0;
 	m_pos_pid_now = 0.0;
 	m_gamma_now = 0.0;
 	memset((void*)&m_motor_state, 0, sizeof(motor_state_t));
@@ -424,6 +426,8 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 
 	utils_sys_unlock_cnt();
 
+	CURRENT_FILTER_ON();
+
 	// Calibrate current offset
 	ENABLE_GATE();
 	DCCAL_OFF();
@@ -455,6 +459,10 @@ void mcpwm_foc_init(volatile mc_configuration *configuration) {
 }
 
 void mcpwm_foc_deinit(void) {
+	if (!m_init_done) {
+		return;
+	}
+
 	m_init_done = false;
 
 	WWDG_DeInit();
@@ -653,7 +661,8 @@ void mcpwm_foc_set_openloop(float current, float rpm) {
 		return;
 	}
 
-	utils_truncate_number(&current, -m_conf->l_current_max, m_conf->l_current_max);
+	utils_truncate_number(&current, -m_conf->l_current_max * m_conf->l_current_max_scale,
+			m_conf->l_current_max * m_conf->l_current_max_scale);
 
 	m_control_mode = CONTROL_MODE_OPENLOOP;
 	m_iq_set = current;
@@ -664,6 +673,37 @@ void mcpwm_foc_set_openloop(float current, float rpm) {
 	}
 }
 
+/**
+ * Produce an openloop current at a fixed phase.
+ *
+ * @param current
+ * The current to use.
+ *
+ * @param phase
+ * The phase to use in degrees, range [0.0 360.0]
+ */
+void mcpwm_foc_set_openloop_phase(float current, float phase) {
+	if (fabsf(current) < m_conf->cc_min_current) {
+		m_control_mode = CONTROL_MODE_NONE;
+		m_state = MC_STATE_OFF;
+		stop_pwm_hw();
+		return;
+	}
+
+	utils_truncate_number(&current, -m_conf->l_current_max * m_conf->l_current_max_scale,
+			m_conf->l_current_max * m_conf->l_current_max_scale);
+
+	m_control_mode = CONTROL_MODE_OPENLOOP_PHASE;
+	m_iq_set = current;
+
+	m_openloop_phase = phase * M_PI / 180.0;
+	utils_norm_angle_rad((float*)&m_openloop_phase);
+
+	if (m_state != MC_STATE_RUNNING) {
+		m_state = MC_STATE_RUNNING;
+	}
+}
+
 float mcpwm_foc_get_duty_cycle_set(void) {
 	return m_duty_cycle_set;
 }
@@ -1083,8 +1123,15 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
 	c_sum = 0.0;
 
 	for (int i = 0;i < it_ofs;i++) {
-		m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
-		chThdSleepMilliseconds(500);
+		float step = (2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+		float override = (float)i * step;
+
+		while (m_phase_now_override != override) {
+			utils_step_towards((float*)&m_phase_now_override, override, step / 100.0);
+			chThdSleepMilliseconds(4);
+		}
+
+		chThdSleepMilliseconds(100);
 
 		float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
 		float s, c;
@@ -1098,8 +1145,15 @@ void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *r
 	}
 
 	for (int i = it_ofs;i > 0;i--) {
-		m_phase_now_override = ((float)i * 2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
-		chThdSleepMilliseconds(500);
+		float step = (2.0 * M_PI * m_conf->foc_encoder_ratio) / ((float)it_ofs);
+		float override = (float)i * step;
+
+		while (m_phase_now_override != override) {
+			utils_step_towards((float*)&m_phase_now_override, override, step / 100.0);
+			chThdSleepMilliseconds(4);
+		}
+
+		chThdSleepMilliseconds(100);
 
 		float diff = utils_angle_difference_rad(m_phase_now_encoder, m_phase_now_override);
 		float s, c;
@@ -1187,6 +1241,20 @@ float mcpwm_foc_measure_resistance(float current, int samples) {
 		if (cnt > 10000) {
 			break;
 		}
+
+		if (mc_interface_get_fault() != FAULT_CODE_NONE) {
+			m_id_set = 0.0;
+			m_iq_set = 0.0;
+			m_phase_override = false;
+			m_control_mode = CONTROL_MODE_NONE;
+			m_state = MC_STATE_OFF;
+			stop_pwm_hw();
+
+			timeout_configure(tout, tout_c);
+			mc_interface_unlock();
+
+			return 0.0;
+		}
 	}
 
 	const float current_avg = m_samples.avg_current_tot / (float)m_samples.sample_num;
@@ -1237,6 +1305,8 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr) {
 
 	mc_interface_lock();
 
+	CURRENT_FILTER_OFF();
+
 	int to_cnt = 0;
 	for (int i = 0;i < samples;i++) {
 		m_samples.measure_inductance_now = true;
@@ -1254,6 +1324,8 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr) {
 		}
 	}
 
+	CURRENT_FILTER_ON();
+
 	// Enable timeout
 	timeout_configure(tout, tout_c);
 
@@ -1271,6 +1343,50 @@ float mcpwm_foc_measure_inductance(float duty, int samples, float *curr) {
 	return ((avg_voltage * t) / avg_current) * 1e6 * (2.0 /  3.0);
 }
 
+/**
+ * Measure the motor inductance with short voltage pulses. The difference from the
+ * other function is that this one will aim for a specific measurement current. It
+ * will also use an appropriate switching frequency.
+ *
+ * @param curr_goal
+ * The measurement current to aim for.
+ *
+ * @param samples
+ * The number of samples to average over.
+ *
+ * @param *curr
+ * The current that was used for this measurement.
+ *
+ * @return
+ * The average d and q axis inductance in microhenry.
+ */
+float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr) {
+	const float f_sw_old = m_conf->foc_f_sw;
+	m_conf->foc_f_sw = 3000.0;
+
+	uint32_t top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
+	TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
+
+	float duty_last = 0.0;
+	for (float i = 0.02;i < 0.5;i *= 1.5) {
+		float i_tmp;
+		mcpwm_foc_measure_inductance(i, 10, &i_tmp);
+
+		duty_last = i;
+		if (i_tmp >= curr_goal) {
+			break;
+		}
+	}
+
+	float ind = mcpwm_foc_measure_inductance(duty_last, samples, curr);
+
+	m_conf->foc_f_sw = f_sw_old;
+	top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
+	TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
+
+	return ind;
+}
+
 /**
  * Automatically measure the resistance and inductance of the motor with small steps.
  *
@@ -1289,18 +1405,15 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
 	const float ki_old = m_conf->foc_current_ki;
 
 	m_conf->foc_f_sw = 10000.0;
-	m_conf->foc_current_kp = 0.01;
-	m_conf->foc_current_ki = 10.0;
+	m_conf->foc_current_kp = 0.001;
+	m_conf->foc_current_ki = 1.0;
 
 	uint32_t top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
 	TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
 
-	float res_tmp = 0.0;
 	float i_last = 0.0;
 	for (float i = 2.0;i < (m_conf->l_current_max / 2.0);i *= 1.5) {
-		res_tmp = mcpwm_foc_measure_resistance(i, 20);
-
-		if (i > (1.0 / res_tmp)) {
+		if (i > (1.0 / mcpwm_foc_measure_resistance(i, 20))) {
 			i_last = i;
 			break;
 		}
@@ -1311,23 +1424,7 @@ bool mcpwm_foc_measure_res_ind(float *res, float *ind) {
 	}
 
 	*res = mcpwm_foc_measure_resistance(i_last, 200);
-
-	m_conf->foc_f_sw = 3000.0;
-	top = SYSTEM_CORE_CLOCK / (int)m_conf->foc_f_sw;
-	TIMER_UPDATE_SAMP_TOP(MCPWM_FOC_CURRENT_SAMP_OFFSET, top);
-
-	float duty_last = 0.0;
-	for (float i = 0.02;i < 0.5;i *= 1.5) {
-		float i_tmp;
-		mcpwm_foc_measure_inductance(i, 20, &i_tmp);
-
-		duty_last = i;
-		if (i_tmp >= i_last) {
-			break;
-		}
-	}
-
-	*ind = mcpwm_foc_measure_inductance(duty_last, 200, 0);
+	*ind = mcpwm_foc_measure_inductance_current(i_last, 200, 0);
 
 	m_conf->foc_f_sw = f_sw_old;
 	m_conf->foc_current_kp = kp_old;
@@ -1456,8 +1553,8 @@ void mcpwm_foc_print_state(void) {
 	commands_printf("Obs_x2:       %.2f", (double)m_observer_x2);
 }
 
-float mcpwm_foc_get_last_inj_adc_isr_duration(void) {
-	return last_inj_adc_isr_duration;
+float mcpwm_foc_get_last_adc_isr_duration(void) {
+	return m_last_adc_isr_duration;
 }
 
 void mcpwm_foc_tim_sample_int_handler(void) {
@@ -1490,11 +1587,16 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 	// Reset the watchdog
 	WWDG_SetCounter(100);
 
-	int curr0 = ADC_Value[ADC_IND_CURR1];
-	int curr1 = ADC_Value[ADC_IND_CURR2];
+#ifdef AD2S1205_SAMPLE_GPIO
+	// force a position sample in the AD2S1205 resolver IC (falling edge)
+	palClearPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);
+#endif
+
+	int curr0 = GET_CURRENT1();
+	int curr1 = GET_CURRENT2();
 
 #ifdef HW_HAS_3_SHUNTS
-	int curr2 = ADC_Value[ADC_IND_CURR3];
+	int curr2 = GET_CURRENT3();
 #endif
 
 	m_curr0_sum += curr0;
@@ -1673,14 +1775,19 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 		float duty_set = m_duty_cycle_set;
 		bool control_duty = m_control_mode == CONTROL_MODE_DUTY;
 
-		// When the filtered duty cycle in sensorless mode becomes low in brake mode, the
-		// observer has lost tracking. Use duty cycle control with the lowest duty cycle
-		// to get as smooth braking as possible.
-		if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE
-				//				&& (m_conf->foc_sensor_mode != FOC_SENSOR_MODE_ENCODER) // Don't use this with encoders
-				&& fabsf(duty_filtered) < 0.03) {
+		// When the modulation is low in brake mode and the set brake current
+		// cannot be reached, short all phases to get more braking without
+		// applying active braking. Use a bit of hysteresis when leaving
+		// the shorted mode.
+		static bool was_full_brake = false;
+		if (m_control_mode == CONTROL_MODE_CURRENT_BRAKE &&
+				fabsf(duty_filtered) < m_conf->l_min_duty * 1.5 &&
+				(m_motor_state.i_abs * (was_full_brake ? 1.0 : 1.5)) < fabsf(m_iq_set)) {
 			control_duty = true;
 			duty_set = 0.0;
+			was_full_brake = true;
+		} else {
+			was_full_brake = false;
 		}
 
 		// Brake when set ERPM is below min ERPM
@@ -1786,14 +1893,16 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 			break;
 		}
 
-		// Force the phase to 0 in handbrake mode so that the current simply locks the rotor.
 		if (m_control_mode == CONTROL_MODE_HANDBRAKE) {
+			// Force the phase to 0 in handbrake mode so that the current simply locks the rotor.
 			m_motor_state.phase = 0.0;
 		} else if (m_control_mode == CONTROL_MODE_OPENLOOP) {
 			static float openloop_angle = 0.0;
 			openloop_angle += dt * m_openloop_speed;
 			utils_norm_angle_rad(&openloop_angle);
 			m_motor_state.phase = openloop_angle;
+		} else if (m_control_mode == CONTROL_MODE_OPENLOOP_PHASE) {
+			m_motor_state.phase = m_openloop_phase;
 		}
 
 		if (m_phase_override) {
@@ -1938,10 +2047,15 @@ void mcpwm_foc_adc_int_handler(void *p, uint32_t flags) {
 		run_pid_control_pos(m_pos_pid_now, m_pos_pid_set, dt);
 	}
 
+#ifdef AD2S1205_SAMPLE_GPIO
+	// Release sample in the AD2S1205 resolver IC.
+	palSetPad(AD2S1205_SAMPLE_GPIO, AD2S1205_SAMPLE_PIN);
+#endif
+
 	// MCIF handler
 	mc_interface_mc_timer_isr();
 
-	last_inj_adc_isr_duration = (float) TIM12->CNT / 10000000.0;
+	m_last_adc_isr_duration = (float) TIM12->CNT / 10000000.0;
 }
 
 // Private functions
@@ -2039,7 +2153,6 @@ static THD_FUNCTION(timer_thread, arg) {
 		run_pid_control_speed(dt);
 		chThdSleepMilliseconds(1);
 	}
-
 }
 
 static void do_dc_cal(void) {
@@ -2507,11 +2620,11 @@ static void run_pid_control_speed(float dt) {
 
 	// Optionally disable braking
 	if (!m_conf->s_pid_allow_braking) {
-		if (rpm > 0.0 && output < 0.0) {
+		if (rpm > 20.0 && output < 0.0) {
 			output = 0.0;
 		}
 
-		if (rpm < 0.0 && output > 0.0) {
+		if (rpm < -20.0 && output > 0.0) {
 			output = 0.0;
 		}
 	}
diff --git a/mcpwm_foc.h b/mcpwm_foc.h
index cd21afa0a..a34e45495 100644
--- a/mcpwm_foc.h
+++ b/mcpwm_foc.h
@@ -40,6 +40,7 @@ void mcpwm_foc_set_current(float current);
 void mcpwm_foc_set_brake_current(float current);
 void mcpwm_foc_set_handbrake(float current);
 void mcpwm_foc_set_openloop(float current, float rpm);
+void mcpwm_foc_set_openloop_phase(float current, float phase);
 float mcpwm_foc_get_duty_cycle_set(void);
 float mcpwm_foc_get_duty_cycle_now(void);
 float mcpwm_foc_get_pid_pos_set(void);
@@ -68,10 +69,11 @@ float mcpwm_foc_get_vq(void);
 void mcpwm_foc_encoder_detect(float current, bool print, float *offset, float *ratio, bool *inverted);
 float mcpwm_foc_measure_resistance(float current, int samples);
 float mcpwm_foc_measure_inductance(float duty, int samples, float *curr);
+float mcpwm_foc_measure_inductance_current(float curr_goal, int samples, float *curr);
 bool mcpwm_foc_measure_res_ind(float *res, float *ind);
 bool mcpwm_foc_hall_detect(float current, uint8_t *hall_table);
 void mcpwm_foc_print_state(void);
-float mcpwm_foc_get_last_inj_adc_isr_duration(void);
+float mcpwm_foc_get_last_adc_isr_duration(void);
 
 // Interrupt handlers
 void mcpwm_foc_tim_sample_int_handler(void);
diff --git a/mcuconf.h b/mcuconf.h
index 8bd71b3ea..66d39c935 100644
--- a/mcuconf.h
+++ b/mcuconf.h
@@ -17,7 +17,7 @@
 #ifndef _MCUCONF_H_
 #define _MCUCONF_H_
 
-#include "conf_general.h"
+#include "hw.h"
 
 /*
  * STM32F4xx drivers configuration.
@@ -39,7 +39,7 @@
 #define STM32F4xx_MCUCONF
 
 // Internal RC osc
-#ifdef HW_VERSION_RH
+#ifdef HW_USE_INTERNAL_RC
 #define STM32_NO_INIT                       FALSE
 #define STM32_HSI_ENABLED                   TRUE
 #define STM32_LSI_ENABLED                   TRUE
@@ -209,9 +209,7 @@
  */
 #define STM32_ICU_USE_TIM1                  FALSE
 #define STM32_ICU_USE_TIM2                  FALSE
-#if defined(HW_VERSION_60) || defined(HW_VERSION_DAS_RS) || defined(HW_VERSION_PALTA) || \
-	defined(HW_VERSION_RH) || defined(HW_VERSION_75_300) || defined(HW_VERSION_MINI4) || \
-    defined(HW_VERSION_DAS_MINI)
+#ifdef HW_USE_SERVO_TIM4
 #define STM32_ICU_USE_TIM3                  FALSE
 #define STM32_ICU_USE_TIM4                  TRUE
 #else
@@ -275,10 +273,10 @@
  */
 #define STM32_SERIAL_USE_USART1             FALSE
 #define STM32_SERIAL_USE_USART2             FALSE
-#define STM32_SERIAL_USE_USART3             FALSE
-#define STM32_SERIAL_USE_UART4              FALSE
+#define STM32_SERIAL_USE_USART3             TRUE
+#define STM32_SERIAL_USE_UART4              TRUE
 #define STM32_SERIAL_USE_UART5              FALSE
-#define STM32_SERIAL_USE_USART6             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
@@ -317,10 +315,10 @@
  */
 #define STM32_UART_USE_USART1               FALSE
 #define STM32_UART_USE_USART2               FALSE
-#define STM32_UART_USE_USART3               TRUE
+#define STM32_UART_USE_USART3               FALSE
 #define STM32_UART_USE_UART4                FALSE
 #define STM32_UART_USE_UART5                FALSE
-#define STM32_UART_USE_USART6               TRUE
+#define STM32_UART_USE_USART6               FALSE
 #define STM32_UART_USART1_RX_DMA_STREAM     STM32_DMA_STREAM_ID(2, 5)
 #define STM32_UART_USART1_TX_DMA_STREAM     STM32_DMA_STREAM_ID(2, 7)
 #define STM32_UART_USART2_RX_DMA_STREAM     STM32_DMA_STREAM_ID(1, 5)
@@ -354,7 +352,7 @@
 #define STM32_USB_USE_OTG2                  FALSE
 #define STM32_USB_OTG1_IRQ_PRIORITY         14
 #define STM32_USB_OTG2_IRQ_PRIORITY         14
-#define STM32_USB_OTG1_RX_FIFO_SIZE         512
+#define STM32_USB_OTG1_RX_FIFO_SIZE         1024
 #define STM32_USB_OTG2_RX_FIFO_SIZE         1024
 #define STM32_USB_OTG_THREAD_PRIO           LOWPRIO
 #define STM32_USB_OTG_THREAD_STACK_SIZE     128
diff --git a/nrf/nrf_driver.c b/nrf/nrf_driver.c
index 4a6ea6f42..002d613af 100644
--- a/nrf/nrf_driver.c
+++ b/nrf/nrf_driver.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -27,12 +27,14 @@
 #include "commands.h"
 #include "crc.h"
 #include "packet.h"
+#include "mc_interface.h"
+#include "app.h"
 
 // Settings
 #define MAX_PL_LEN				25
 #define RX_BUFFER_SIZE			PACKET_MAX_PL_LEN
 
-#define ALIVE_INTERVAL			50  // Send alive packets at this rate
+#define ALIVE_INTERVAL			100  // Send alive packets at this rate
 #define NRF_RESTART_TIMEOUT		500  // Restart the NRF if nothing has been received or acked for this time
 
 // Variables
@@ -45,12 +47,14 @@ static int nrf_restart_rx_time;
 static int nrf_restart_tx_time;
 
 static systime_t pairing_time_end = 0;
-static bool pairing_active = false;
+static volatile bool pairing_active = false;
 
 static volatile bool tx_running = false;
 static volatile bool tx_stop = true;
 static volatile bool rx_running = false;
 static volatile bool rx_stop = true;
+static volatile bool ext_nrf = false;
+static volatile int driver_paused = 0;
 
 // This is a hack to prevent race conditions when updating the appconf
 // from the nrf thread
@@ -66,6 +70,8 @@ bool nrf_driver_init(void) {
 		return true;
 	}
 
+	ext_nrf = false;
+
 	nrf_driver_stop();
 
 	if (!rfhelp_init()) {
@@ -89,53 +95,87 @@ bool nrf_driver_init(void) {
 	return true;
 }
 
+void nrf_driver_init_ext_nrf(void) {
+	ext_nrf = true;
+
+	if (!tx_running) {
+		tx_stop = false;
+		chThdCreateStatic(tx_thread_wa, sizeof(tx_thread_wa), NORMALPRIO - 1, tx_thread, NULL);
+	}
+}
+
 void nrf_driver_stop(void) {
-	if (from_nrf) {
+	if (from_nrf || ext_nrf) {
 		return;
 	}
 
-	tx_stop = true;
-	rx_stop = true;
-
-	if (rx_running || tx_running) {
+	if (rx_running) {
 		rfhelp_stop();
 	}
 
+	tx_stop = true;
+	rx_stop = true;
+
 	while (rx_running || tx_running) {
 		chThdSleepMilliseconds(1);
 	}
 }
 
 void nrf_driver_start_pairing(int ms) {
-	if (!rx_running) {
-		return;
-	}
+	if (ext_nrf) {
+		pairing_time_end = chVTGetSystemTimeX() + MS2ST(ms);
+
+		if (!pairing_active) {
+			pairing_active = true;
+
+			unsigned char data[5];
+			data[0] = COMM_EXT_NRF_ESB_SET_CH_ADDR;
+			data[1] = 67;
+			data[2] = 0xC6;
+			data[3] = 0xC5;
+			data[4] = 0x0;
+			commands_send_packet_nrf(data, 5);
+		}
+	} else {
+		if (!rx_running) {
+			return;
+		}
 
-	pairing_time_end = chVTGetSystemTimeX() + MS2ST(ms);
+		pairing_time_end = chVTGetSystemTimeX() + MS2ST(ms);
 
-	if (!pairing_active) {
-		pairing_active = true;
+		if (!pairing_active) {
+			pairing_active = true;
 
-		nrf_config conf = app_get_configuration()->app_nrf_conf;
-		conf.address[0] = 0xC6;
-		conf.address[1] = 0xC5;
-		conf.address[2] = 0x0;
-		conf.channel = 124;
-		conf.crc_type = NRF_CRC_1B;
-		conf.retries = 3;
-		conf.retry_delay = NRF_RETR_DELAY_1000US;
-		conf.send_crc_ack = true;
-		conf.speed = NRF_SPEED_250K;
+			nrf_config conf = app_get_configuration()->app_nrf_conf;
+			conf.address[0] = 0xC6;
+			conf.address[1] = 0xC5;
+			conf.address[2] = 0x0;
+			conf.channel = 67;
+			conf.crc_type = NRF_CRC_1B;
+			conf.retries = 3;
+			conf.retry_delay = NRF_RETR_DELAY_1000US;
+			conf.send_crc_ack = true;
+			conf.speed = NRF_SPEED_250K;
 
-		rfhelp_update_conf(&conf);
+			rfhelp_update_conf(&conf);
+		}
 	}
 }
 
 static int rf_tx_wrapper(char *data, int len) {
-	int res = rfhelp_send_data_crc(data, len);
+	int res = 0;
 
-	if (res == 0) {
-		nrf_restart_tx_time = NRF_RESTART_TIMEOUT;
+	if (ext_nrf) {
+		uint8_t buffer[len + 1];
+		buffer[0] = COMM_EXT_NRF_ESB_SEND_DATA;
+		memcpy(buffer + 1, data, len);
+		commands_send_packet_nrf(buffer, len + 1);
+	} else {
+		res = rfhelp_send_data_crc(data, len);
+
+		if (res == 0) {
+			nrf_restart_tx_time = NRF_RESTART_TIMEOUT;
+		}
 	}
 
 	return res;
@@ -156,13 +196,53 @@ static THD_FUNCTION(tx_thread, arg) {
 		nosend_cnt++;
 
 		if (nosend_cnt >= ALIVE_INTERVAL && !pairing_active) {
-			uint8_t pl[2];
+			uint8_t pl[18];
 			int32_t index = 0;
+			static uint8_t seq_cnt = 0;
+			seq_cnt++;
+
 			pl[index++] = MOTE_PACKET_ALIVE;
-			rf_tx_wrapper((char*)pl, index);
+			buffer_append_float16(pl, mc_interface_get_battery_level(0), 1e3, &index);
+			buffer_append_float32(pl, mc_interface_get_speed(), 1e3, &index);
+			buffer_append_float32(pl, mc_interface_get_distance_abs(), 1e3, &index);
+			buffer_append_float16(pl, mc_interface_temp_fet_filtered(), 1e1, &index);
+			buffer_append_float16(pl, mc_interface_temp_motor_filtered(), 1e1, &index);
+			pl[index++] = seq_cnt;
+
+			if (driver_paused == 0) {
+				rf_tx_wrapper((char*)pl, index);
+			}
+
 			nosend_cnt = 0;
 		}
 
+		if (driver_paused > 0) {
+			driver_paused--;
+		}
+
+		if (chVTGetSystemTimeX() > pairing_time_end && pairing_active) {
+			pairing_active = false;
+
+			if (ext_nrf) {
+				const app_configuration *appconf_ptr = app_get_configuration();
+				unsigned char data[5];
+				data[0] = COMM_EXT_NRF_ESB_SET_CH_ADDR;
+				data[1] = appconf_ptr->app_nrf_conf.channel;
+				data[2] = appconf_ptr->app_nrf_conf.address[0];
+				data[3] = appconf_ptr->app_nrf_conf.address[1];
+				data[4] = appconf_ptr->app_nrf_conf.address[2];
+				commands_send_packet_nrf(data, 5);
+			} else {
+				nrf_config conf = app_get_configuration()->app_nrf_conf;
+				rfhelp_update_conf(&conf);
+			}
+
+			unsigned char data[2];
+			data[0] = COMM_NRF_START_PAIRING;
+			data[1] = NRF_PAIR_FAIL;
+			commands_send_packet(data, 2);
+		}
+
 		chThdSleepMilliseconds(1);
 	}
 
@@ -175,6 +255,11 @@ static THD_FUNCTION(rx_thread, arg) {
 	rx_running = true;
 
 	for(;;) {
+		if (driver_paused != 0) {
+			chThdSleepMilliseconds(5);
+			continue;
+		}
+
 		if (rx_stop) {
 			rx_running = false;
 			return;
@@ -187,123 +272,9 @@ static THD_FUNCTION(rx_thread, arg) {
 		for(;;) {
 			int res = rfhelp_read_rx_data_crc((char*)buf, &len, &pipe);
 
-			chuck_data cdata;
-			int32_t ind = 0;
-			int buttons;
-
 			// If something was read
 			if (res >= 0) {
-				MOTE_PACKET packet = buf[0];
-
-				nrf_restart_rx_time = NRF_RESTART_TIMEOUT;
-
-				switch (packet) {
-				case MOTE_PACKET_BATT_LEVEL:
-					// TODO!
-					break;
-
-				case MOTE_PACKET_BUTTONS:
-					ind = 1;
-					mstate.js_x = buf[ind++];
-					mstate.js_y = buf[ind++];
-					buttons = buf[ind++];
-					mstate.bt_c = buttons & (1 << 0);
-					mstate.bt_z = buttons & (1 << 1);
-					mstate.bt_push = buttons & (1 << 2);
-					mstate.vbat = (float)buffer_get_int16(buf, &ind) / 1000.0;
-
-					cdata.js_x = 255 - mstate.js_x;
-					cdata.js_y = mstate.js_y;
-					cdata.bt_c = mstate.bt_c;
-					cdata.bt_z = mstate.bt_z;
-
-					app_nunchuk_update_output(&cdata);
-					break;
-
-				case MOTE_PACKET_FILL_RX_BUFFER:
-					memcpy(rx_buffer + buf[1], buf + 2, len - 2);
-					break;
-
-				case MOTE_PACKET_FILL_RX_BUFFER_LONG: {
-					int rxbuf_ind = (unsigned int)buf[1] << 8;
-					rxbuf_ind |= buf[2];
-					if (rxbuf_ind < RX_BUFFER_SIZE) {
-						memcpy(rx_buffer + rxbuf_ind, buf + 3, len - 3);
-					}
-				}
-				break;
-
-				case MOTE_PACKET_PROCESS_RX_BUFFER: {
-					ind = 1;
-					int rxbuf_len = (unsigned int)buf[ind++] << 8;
-					rxbuf_len |= (unsigned int)buf[ind++];
-
-					if (rxbuf_len > RX_BUFFER_SIZE) {
-						break;
-					}
-
-					uint8_t crc_high = buf[ind++];
-					uint8_t crc_low = buf[ind++];
-
-					memcpy(rx_buffer + rxbuf_len - (len - ind), buf + ind, len - ind);
-
-					if (crc16(rx_buffer, rxbuf_len)
-							== ((unsigned short) crc_high << 8
-									| (unsigned short) crc_low)) {
-
-						// Wait a bit in case retries are still made
-						chThdSleepMilliseconds(2);
-
-						commands_set_send_func(nrf_driver_send_buffer);
-						from_nrf = true;
-						commands_process_packet(rx_buffer, rxbuf_len);
-						from_nrf = false;
-					}
-				}
-				break;
-
-				case MOTE_PACKET_PROCESS_SHORT_BUFFER:
-					// Wait a bit in case retries are still made
-					chThdSleepMilliseconds(2);
-
-					commands_set_send_func(nrf_driver_send_buffer);
-					from_nrf = true;
-					commands_process_packet(buf + 1, len - 1);
-					from_nrf = false;
-					break;
-
-				case MOTE_PACKET_PAIRING_INFO: {
-					ind = 1;
-
-					app_configuration appconf = *app_get_configuration();
-					appconf.app_nrf_conf.address[0] = buf[ind++];
-					appconf.app_nrf_conf.address[1] = buf[ind++];
-					appconf.app_nrf_conf.address[2] = buf[ind++];
-					appconf.app_nrf_conf.channel = buf[ind++];
-					appconf.app_nrf_conf.crc_type = NRF_CRC_1B;
-					appconf.app_nrf_conf.retries = 3;
-					appconf.app_nrf_conf.retry_delay = NRF_RETR_DELAY_1000US;
-					appconf.app_nrf_conf.send_crc_ack = true;
-					appconf.app_nrf_conf.speed = NRF_SPEED_250K;
-
-					pairing_active = false;
-
-					from_nrf = true;
-					conf_general_store_app_configuration(&appconf);
-					app_set_configuration(&appconf);
-					commands_send_appconf(COMM_GET_APPCONF, &appconf);
-
-					unsigned char data[2];
-					data[0] = COMM_NRF_START_PAIRING;
-					data[1] = NRF_PAIR_OK;
-					commands_send_packet(data, 2);
-
-					from_nrf = false;
-				} break;
-
-				default:
-					break;
-				}
+				nrf_driver_process_packet(buf, len);
 			}
 
 			// Stop when there is no more data to read.
@@ -315,17 +286,6 @@ static THD_FUNCTION(rx_thread, arg) {
 			}
 		}
 
-		if (chVTGetSystemTimeX() > pairing_time_end && pairing_active) {
-			pairing_active = false;
-			nrf_config conf = app_get_configuration()->app_nrf_conf;
-			rfhelp_update_conf(&conf);
-
-			unsigned char data[2];
-			data[0] = COMM_NRF_START_PAIRING;
-			data[1] = NRF_PAIR_FAIL;
-			commands_send_packet(data, 2);
-		}
-
 		chThdSleepMilliseconds(5);
 
 		// Restart the nrf if nothing has been received for a while
@@ -408,3 +368,151 @@ void nrf_driver_send_buffer(unsigned char *data, unsigned int len) {
 		nosend_cnt = 0;
 	}
 }
+
+void nrf_driver_process_packet(unsigned char *buf, unsigned char len) {
+	MOTE_PACKET packet = buf[0];
+	chuck_data cdata;
+	int32_t ind = 0;
+	int buttons;
+
+	nrf_restart_rx_time = NRF_RESTART_TIMEOUT;
+
+	switch (packet) {
+	case MOTE_PACKET_BATT_LEVEL:
+		// TODO!
+		break;
+
+	case MOTE_PACKET_BUTTONS:
+		ind = 1;
+		mstate.js_x = buf[ind++];
+		mstate.js_y = buf[ind++];
+		buttons = buf[ind++];
+		mstate.bt_c = buttons & (1 << 0);
+		mstate.bt_z = buttons & (1 << 1);
+		mstate.bt_push = buttons & (1 << 2);
+		mstate.rev_has_state = buttons & (1 << 3);
+		mstate.is_rev = buttons & (1 << 4);
+		mstate.vbat = (float)buffer_get_int16(buf, &ind) / 1000.0;
+
+		cdata.js_x = 255 - mstate.js_x;
+		cdata.js_y = mstate.js_y;
+		cdata.bt_c = mstate.bt_c;
+		cdata.bt_z = mstate.bt_z;
+		cdata.rev_has_state = mstate.rev_has_state;
+		cdata.is_rev = mstate.is_rev;
+
+		app_nunchuk_update_output(&cdata);
+		break;
+
+	case MOTE_PACKET_FILL_RX_BUFFER:
+		memcpy(rx_buffer + buf[1], buf + 2, len - 2);
+		break;
+
+	case MOTE_PACKET_FILL_RX_BUFFER_LONG: {
+		int rxbuf_ind = (unsigned int)buf[1] << 8;
+		rxbuf_ind |= buf[2];
+		if (rxbuf_ind < RX_BUFFER_SIZE) {
+			memcpy(rx_buffer + rxbuf_ind, buf + 3, len - 3);
+		}
+	}
+	break;
+
+	case MOTE_PACKET_PROCESS_RX_BUFFER: {
+		ind = 1;
+		int rxbuf_len = (unsigned int)buf[ind++] << 8;
+		rxbuf_len |= (unsigned int)buf[ind++];
+
+		if (rxbuf_len > RX_BUFFER_SIZE) {
+			break;
+		}
+
+		uint8_t crc_high = buf[ind++];
+		uint8_t crc_low = buf[ind++];
+
+		memcpy(rx_buffer + rxbuf_len - (len - ind), buf + ind, len - ind);
+
+		if (crc16(rx_buffer, rxbuf_len)
+				== ((unsigned short) crc_high << 8
+						| (unsigned short) crc_low)) {
+
+			// Wait a bit in case retries are still made
+			chThdSleepMilliseconds(2);
+
+			commands_set_send_func(nrf_driver_send_buffer);
+			from_nrf = true;
+			commands_process_packet(rx_buffer, rxbuf_len);
+			from_nrf = false;
+		}
+	}
+	break;
+
+	case MOTE_PACKET_PROCESS_SHORT_BUFFER:
+		// Wait a bit in case retries are still made
+		chThdSleepMilliseconds(2);
+
+		commands_set_send_func(nrf_driver_send_buffer);
+		from_nrf = true;
+		commands_process_packet(buf + 1, len - 1);
+		from_nrf = false;
+		break;
+
+	case MOTE_PACKET_PAIRING_INFO: {
+		ind = 1;
+
+		if (!pairing_active) {
+			break;
+		}
+
+		pairing_active = false;
+
+		app_configuration appconf = *app_get_configuration();
+		appconf.app_nrf_conf.address[0] = buf[ind++];
+		appconf.app_nrf_conf.address[1] = buf[ind++];
+		appconf.app_nrf_conf.address[2] = buf[ind++];
+		appconf.app_nrf_conf.channel = buf[ind++];
+		appconf.app_nrf_conf.crc_type = NRF_CRC_1B;
+		appconf.app_nrf_conf.retries = 3;
+		appconf.app_nrf_conf.retry_delay = NRF_RETR_DELAY_1000US;
+		appconf.app_nrf_conf.send_crc_ack = true;
+		appconf.app_nrf_conf.speed = NRF_SPEED_250K;
+
+		if (ext_nrf) {
+			unsigned char data[5];
+			data[0] = COMM_EXT_NRF_ESB_SET_CH_ADDR;
+			data[1] = appconf.app_nrf_conf.channel;
+			data[2] = appconf.app_nrf_conf.address[0];
+			data[3] = appconf.app_nrf_conf.address[1];
+			data[4] = appconf.app_nrf_conf.address[2];
+			commands_send_packet_nrf(data, 5);
+		}
+
+		from_nrf = true;
+		conf_general_store_app_configuration(&appconf);
+		app_set_configuration(&appconf);
+
+		commands_send_appconf(COMM_GET_APPCONF, &appconf);
+
+		unsigned char data[2];
+		data[0] = COMM_NRF_START_PAIRING;
+		data[1] = NRF_PAIR_OK;
+		commands_send_packet(data, 2);
+
+		from_nrf = false;
+	} break;
+
+	default:
+		break;
+	}
+}
+
+bool nrf_driver_is_pairing(void) {
+	return pairing_active;
+}
+
+bool nrf_driver_ext_nrf_running(void) {
+	return ext_nrf;
+}
+
+void nrf_driver_pause(int ms) {
+	driver_paused = ms;
+}
diff --git a/nrf/nrf_driver.h b/nrf/nrf_driver.h
index 78ea4fb83..5a39ab117 100644
--- a/nrf/nrf_driver.h
+++ b/nrf/nrf_driver.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -24,8 +24,13 @@
 
 // Functions
 bool nrf_driver_init(void);
+void nrf_driver_init_ext_nrf(void);
 void nrf_driver_stop(void);
 void nrf_driver_start_pairing(int ms);
 void nrf_driver_send_buffer(unsigned char *data, unsigned int len);
+void nrf_driver_process_packet(unsigned char *buf, unsigned char len);
+bool nrf_driver_is_pairing(void);
+bool nrf_driver_ext_nrf_running(void);
+void nrf_driver_pause(int ms);
 
 #endif /* NRF_NRF_DRIVER_H_ */
diff --git a/packet.c b/packet.c
index bd3c81233..cc735d16c 100644
--- a/packet.c
+++ b/packet.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -21,143 +21,268 @@
 #include "packet.h"
 #include "crc.h"
 
+/**
+ * The latest update aims at achieving optimal re-synchronization in the
+ * case if lost data, at the cost of some performance.
+ */
+
+// Defines
+#define BUFFER_LEN				(PACKET_MAX_PL_LEN + 8)
+
+// Private types
 typedef struct {
-	volatile unsigned char rx_state;
 	volatile unsigned short rx_timeout;
 	void(*send_func)(unsigned char *data, unsigned int len);
 	void(*process_func)(unsigned char *data, unsigned int len);
-	unsigned int payload_length;
-	unsigned char rx_buffer[PACKET_MAX_PL_LEN];
-	unsigned char tx_buffer[PACKET_MAX_PL_LEN + 6];
-	unsigned int rx_data_ptr;
-	unsigned char crc_low;
-	unsigned char crc_high;
+	unsigned int rx_read_ptr;
+	unsigned int rx_write_ptr;
+	int bytes_left;
+	unsigned char rx_buffer[BUFFER_LEN];
+	unsigned char tx_buffer[BUFFER_LEN];
 } PACKET_STATE_t;
 
-static PACKET_STATE_t handler_states[PACKET_HANDLERS];
+// Private variables
+static PACKET_STATE_t m_handler_states[PACKET_HANDLERS];
+
+// Private functions
+static int try_decode_packet(unsigned char *buffer, unsigned int in_len,
+		void(*process_func)(unsigned char *data, unsigned int len), int *bytes_left);
 
 void packet_init(void (*s_func)(unsigned char *data, unsigned int len),
 		void (*p_func)(unsigned char *data, unsigned int len), int handler_num) {
-	handler_states[handler_num].send_func = s_func;
-	handler_states[handler_num].process_func = p_func;
+	memset(&m_handler_states[handler_num], 0, sizeof(PACKET_STATE_t));
+	m_handler_states[handler_num].send_func = s_func;
+	m_handler_states[handler_num].process_func = p_func;
+}
+
+void packet_reset(int handler_num) {
+	m_handler_states[handler_num].rx_read_ptr = 0;
+	m_handler_states[handler_num].rx_write_ptr = 0;
+	m_handler_states[handler_num].bytes_left = 0;
 }
 
 void packet_send_packet(unsigned char *data, unsigned int len, int handler_num) {
-	if (len > PACKET_MAX_PL_LEN) {
+	if (len == 0 || len > PACKET_MAX_PL_LEN) {
 		return;
 	}
 
 	int b_ind = 0;
+	PACKET_STATE_t *handler = &m_handler_states[handler_num];
 
-	if (len <= 256) {
-		handler_states[handler_num].tx_buffer[b_ind++] = 2;
-		handler_states[handler_num].tx_buffer[b_ind++] = len;
+	if (len <= 255) {
+		handler->tx_buffer[b_ind++] = 2;
+		handler->tx_buffer[b_ind++] = len;
+	} else if (len <= 65535) {
+		handler->tx_buffer[b_ind++] = 3;
+		handler->tx_buffer[b_ind++] = len >> 8;
+		handler->tx_buffer[b_ind++] = len & 0xFF;
 	} else {
-		handler_states[handler_num].tx_buffer[b_ind++] = 3;
-		handler_states[handler_num].tx_buffer[b_ind++] = len >> 8;
-		handler_states[handler_num].tx_buffer[b_ind++] = len & 0xFF;
+		handler->tx_buffer[b_ind++] = 4;
+		handler->tx_buffer[b_ind++] = len >> 16;
+		handler->tx_buffer[b_ind++] = (len >> 8) & 0x0F;
+		handler->tx_buffer[b_ind++] = len & 0xFF;
 	}
 
-	memcpy(handler_states[handler_num].tx_buffer + b_ind, data, len);
+	memcpy(handler->tx_buffer + b_ind, data, len);
 	b_ind += len;
 
 	unsigned short crc = crc16(data, len);
-	handler_states[handler_num].tx_buffer[b_ind++] = (uint8_t)(crc >> 8);
-	handler_states[handler_num].tx_buffer[b_ind++] = (uint8_t)(crc & 0xFF);
-	handler_states[handler_num].tx_buffer[b_ind++] = 3;
+	handler->tx_buffer[b_ind++] = (uint8_t)(crc >> 8);
+	handler->tx_buffer[b_ind++] = (uint8_t)(crc & 0xFF);
+	handler->tx_buffer[b_ind++] = 3;
 
-	if (handler_states[handler_num].send_func) {
-		handler_states[handler_num].send_func(handler_states[handler_num].tx_buffer, b_ind);
+	if (handler->send_func) {
+		handler->send_func(handler->tx_buffer, b_ind);
 	}
 }
 
 /**
- * Call this function every millisecond.
+ * Call this function every millisecond. This is not strictly necessary
+ * if the timeout is unimportant.
  */
 void packet_timerfunc(void) {
 	for (int i = 0;i < PACKET_HANDLERS;i++) {
-		if (handler_states[i].rx_timeout) {
-			handler_states[i].rx_timeout--;
+		if (m_handler_states[i].rx_timeout) {
+			m_handler_states[i].rx_timeout--;
 		} else {
-			handler_states[i].rx_state = 0;
+			packet_reset(i);
 		}
 	}
 }
 
 void packet_process_byte(uint8_t rx_data, int handler_num) {
-	switch (handler_states[handler_num].rx_state) {
-	case 0:
-		if (rx_data == 2) {
-			// 1 byte PL len
-			handler_states[handler_num].rx_state += 2;
-			handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-			handler_states[handler_num].rx_data_ptr = 0;
-			handler_states[handler_num].payload_length = 0;
-		} else if (rx_data == 3) {
-			// 2 byte PL len
-			handler_states[handler_num].rx_state++;
-			handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-			handler_states[handler_num].rx_data_ptr = 0;
-			handler_states[handler_num].payload_length = 0;
-		} else {
-			handler_states[handler_num].rx_state = 0;
+	PACKET_STATE_t *handler = &m_handler_states[handler_num];
+
+	handler->rx_timeout = PACKET_RX_TIMEOUT;
+
+	unsigned int data_len = handler->rx_write_ptr - handler->rx_read_ptr;
+
+	// Out of space (should not happen)
+	if (data_len >= BUFFER_LEN) {
+		handler->rx_write_ptr = 0;
+		handler->rx_read_ptr = 0;
+		handler->bytes_left = 0;
+		handler->rx_buffer[handler->rx_write_ptr++] = rx_data;
+		return;
+	}
+
+	// Everything has to be aligned, so shift buffer if we are out of space.
+	// (as opposed to using a circular buffer)
+	if (handler->rx_write_ptr >= BUFFER_LEN) {
+		memmove(handler->rx_buffer,
+				handler->rx_buffer + handler->rx_read_ptr,
+				data_len);
+
+		handler->rx_read_ptr = 0;
+		handler->rx_write_ptr = data_len;
+	}
+
+	handler->rx_buffer[handler->rx_write_ptr++] = rx_data;
+	data_len++;
+
+	if (handler->bytes_left > 1) {
+		handler->bytes_left--;
+		return;
+	}
+
+	// Try decoding the packet at various offsets until it succeeds, or
+	// until we run out of data.
+	for (;;) {
+		int res = try_decode_packet(handler->rx_buffer + handler->rx_read_ptr,
+				data_len, handler->process_func, &handler->bytes_left);
+
+		// More data is needed
+		if (res == -2) {
+			break;
 		}
-		break;
-
-	case 1:
-		handler_states[handler_num].payload_length = (unsigned int)rx_data << 8;
-		handler_states[handler_num].rx_state++;
-		handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-		break;
-
-	case 2:
-		handler_states[handler_num].payload_length |= (unsigned int)rx_data;
-		if (handler_states[handler_num].payload_length > 0 &&
-				handler_states[handler_num].payload_length <= PACKET_MAX_PL_LEN) {
-			handler_states[handler_num].rx_state++;
-			handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-		} else {
-			handler_states[handler_num].rx_state = 0;
+
+		if (res > 0) {
+			data_len -= res;
+			handler->rx_read_ptr += res;
+		} else if (res == -1) {
+			// Something went wrong. Move pointer forward and try again.
+			handler->rx_read_ptr++;
+			data_len--;
+		}
+	}
+
+	// Nothing left, move pointers to avoid memmove
+	if (data_len == 0) {
+		handler->rx_read_ptr = 0;
+		handler->rx_write_ptr = 0;
+	}
+}
+
+/**
+ * Try if it is possible to decode a packet from a buffer.
+ *
+ * @param buffer
+ * The buffer to try from
+ *
+ * @param in_len
+ * The length of the buffer
+ *
+ * @param process_func
+ * Call this function with the decoded packet on success. Set to null
+ * to disable.
+ *
+ * @param bytes_left
+ * This many additional bytes are required to tell more about the packet.
+ *
+ * @return
+ * >0: Success, number of bytes decoded from buffer (not payload length)
+ * -1: Invalid structure
+ * -2: OK so far, but not enough data
+ */
+static int try_decode_packet(unsigned char *buffer, unsigned int in_len,
+		void(*process_func)(unsigned char *data, unsigned int len), int *bytes_left) {
+	*bytes_left = 0;
+
+	if (in_len == 0) {
+		*bytes_left = 1;
+		return -2;
+	}
+
+	bool is_len_8b = buffer[0] == 2;
+	unsigned int data_start = buffer[0];
+
+#if PACKET_MAX_PL_LEN > 255
+	bool is_len_16b = buffer[0] == 3;
+#else
+#define is_len_16b false
+#endif
+
+#if PACKET_MAX_PL_LEN > 65535
+	bool is_len_24b = buffer[0] == 4;
+#else
+#define is_len_24b false
+#endif
+
+	// No valid start byte
+	if (!is_len_8b && !is_len_16b && !is_len_24b) {
+		return -1;
+	}
+
+	// Not enough data to determine length
+	if (in_len < data_start) {
+		*bytes_left = data_start - in_len;
+		return -2;
+	}
+
+	unsigned int len = 0;
+
+	if (is_len_8b) {
+		len = (unsigned int)buffer[1];
+
+		// No support for zero length packets
+		if (len < 1) {
+			return -1;
+		}
+	} else if (is_len_16b) {
+		len = (unsigned int)buffer[1] << 8 | (unsigned int)buffer[2];
+
+		// A shorter packet should use less length bytes
+		if (len < 255) {
+			return -1;
 		}
-		break;
+	} else if (is_len_24b) {
+		len = (unsigned int)buffer[1] << 16 |
+				(unsigned int)buffer[2] << 8 |
+				(unsigned int)buffer[3];
 
-	case 3:
-		handler_states[handler_num].rx_buffer[handler_states[handler_num].rx_data_ptr++] = rx_data;
-		if (handler_states[handler_num].rx_data_ptr == handler_states[handler_num].payload_length) {
-			handler_states[handler_num].rx_state++;
+		// A shorter packet should use less length bytes
+		if (len < 65535) {
+			return -1;
 		}
-		handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-		break;
-
-	case 4:
-		handler_states[handler_num].crc_high = rx_data;
-		handler_states[handler_num].rx_state++;
-		handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-		break;
-
-	case 5:
-		handler_states[handler_num].crc_low = rx_data;
-		handler_states[handler_num].rx_state++;
-		handler_states[handler_num].rx_timeout = PACKET_RX_TIMEOUT;
-		break;
-
-	case 6:
-		if (rx_data == 3) {
-			if (crc16(handler_states[handler_num].rx_buffer, handler_states[handler_num].payload_length)
-					== ((unsigned short)handler_states[handler_num].crc_high << 8
-							| (unsigned short)handler_states[handler_num].crc_low)) {
-				// Packet received!
-				if (handler_states[handler_num].process_func) {
-					handler_states[handler_num].process_func(handler_states[handler_num].rx_buffer,
-							handler_states[handler_num].payload_length);
-				}
-			}
+	}
+
+	// Too long packet
+	if (len > PACKET_MAX_PL_LEN) {
+		return -1;
+	}
+
+	// Need more data to determine rest of packet
+	if (in_len < (len + data_start + 3)) {
+		*bytes_left = (len + data_start + 3) - in_len;
+		return -2;
+	}
+
+	// Invalid stop byte
+	if (buffer[data_start + len + 2] != 3) {
+		return -1;
+	}
+
+	unsigned short crc_calc = crc16(buffer + data_start, len);
+	unsigned short crc_rx = (unsigned short)buffer[data_start + len] << 8
+							| (unsigned short)buffer[data_start + len + 1];
+
+	if (crc_calc == crc_rx) {
+		if (process_func) {
+			process_func(buffer + data_start, len);
 		}
-		handler_states[handler_num].rx_state = 0;
-		break;
 
-	default:
-		handler_states[handler_num].rx_state = 0;
-		break;
+		return len + data_start + 3;
+	} else {
+		return -1;
 	}
 }
diff --git a/packet.h b/packet.h
index b768d247c..f1875f555 100644
--- a/packet.h
+++ b/packet.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -21,15 +21,25 @@
 #define PACKET_H_
 
 #include <stdint.h>
+#include <stdbool.h>
 
 // Settings
+#ifndef PACKET_RX_TIMEOUT
 #define PACKET_RX_TIMEOUT		1000
-#define PACKET_HANDLERS			2
-#define PACKET_MAX_PL_LEN		1024
+#endif
+
+#ifndef PACKET_HANDLERS
+#define PACKET_HANDLERS			3
+#endif
+
+#ifndef PACKET_MAX_PL_LEN
+#define PACKET_MAX_PL_LEN		512
+#endif
 
 // Functions
 void packet_init(void (*s_func)(unsigned char *data, unsigned int len),
 		void (*p_func)(unsigned char *data, unsigned int len), int handler_num);
+void packet_reset(int handler_num);
 void packet_process_byte(uint8_t rx_data, int handler_num);
 void packet_timerfunc(void);
 void packet_send_packet(unsigned char *data, unsigned int len, int handler_num);
diff --git a/terminal.c b/terminal.c
index 31a8a6d3c..42e8c5dd9 100644
--- a/terminal.c
+++ b/terminal.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 - 2017 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -31,7 +31,9 @@
 #include "encoder.h"
 #include "drv8301.h"
 #include "drv8305.h"
-#include "drv8320.h"
+#include "drv8320s.h"
+#include "drv8323s.h"
+#include "app.h"
 
 #include <string.h>
 #include <stdio.h>
@@ -133,9 +135,13 @@ void terminal_process_string(char *str) {
 				if (fault_vec[i].fault == FAULT_CODE_DRV) {
 					commands_printf("DRV8301_FAULTS   : %s", drv8301_faults_to_string(fault_vec[i].drv8301_faults));
 				}
-#elif defined(HW_HAS_DRV8320)
+#elif defined(HW_HAS_DRV8320S)
+ 				if (fault_vec[i].fault == FAULT_CODE_DRV) {
+					commands_printf("DRV8320S_FAULTS  : %s", drv8320s_faults_to_string(fault_vec[i].drv8301_faults));
+				}
+#elif defined(HW_HAS_DRV8323S)
 				if (fault_vec[i].fault == FAULT_CODE_DRV) {
-					commands_printf("DRV8320_FAULTS   : %s", drv8320_faults_to_string(fault_vec[i].drv8301_faults));
+					commands_printf("DRV8323S_FAULTS  : %s", drv8323s_faults_to_string(fault_vec[i].drv8301_faults));
 				}
 #endif
 				commands_printf(" ");
@@ -326,7 +332,7 @@ void terminal_process_string(char *str) {
 				commands_printf("Invalid argument(s).\n");
 			}
 		} else {
-			commands_printf("This command requires one argument.\n");
+			commands_printf("This command requires four arguments.\n");
 		}
 	} else if (strcmp(argv[0], "measure_res_ind") == 0) {
 		mcconf.motor_type = MOTOR_TYPE_FOC;
@@ -391,6 +397,27 @@ void terminal_process_string(char *str) {
 		} else {
 			commands_printf("This command requires one argument.\n");
 		}
+	} else if (strcmp(argv[0], "measure_linkage_openloop") == 0) {
+		if (argc == 5) {
+			float current = -1.0;
+			float duty = -1.0;
+			float erpm_per_sec = -1.0;
+			float res = -1.0;
+			sscanf(argv[1], "%f", &current);
+			sscanf(argv[2], "%f", &duty);
+			sscanf(argv[3], "%f", &erpm_per_sec);
+			sscanf(argv[4], "%f", &res);
+
+			if (current > 0.0 && current <= mcconf.l_current_max && erpm_per_sec > 0.0 && duty > 0.02 && res >= 0.0) {
+				float linkage;
+				conf_general_measure_flux_linkage_openloop(current, duty, erpm_per_sec, res, &linkage);
+				commands_printf("Flux linkage: %.7f\n", (double)linkage);
+			} else {
+				commands_printf("Invalid argument(s).\n");
+			}
+		} else {
+			commands_printf("This command requires four arguments.\n");
+		}
 	} else if (strcmp(argv[0], "foc_state") == 0) {
 		mcpwm_foc_print_state();
 		commands_printf(" ");
@@ -413,6 +440,7 @@ void terminal_process_string(char *str) {
 			sscanf(argv[2], "%f", &erpm);
 
 			if (current >= 0.0 && erpm >= 0.0) {
+				timeout_reset();
 				mcpwm_foc_set_openloop(current, erpm);
 			} else {
 				commands_printf("Invalid argument(s).\n");
@@ -420,6 +448,190 @@ void terminal_process_string(char *str) {
 		} else {
 			commands_printf("This command requires two arguments.\n");
 		}
+	} else if (strcmp(argv[0], "nrf_ext_set_enabled") == 0) {
+		if (argc == 2) {
+			int enabled = -1;
+			sscanf(argv[1], "%d", &enabled);
+
+			if (enabled >= 0) {
+				uint8_t buffer[2];
+				buffer[0] = COMM_EXT_NRF_SET_ENABLED;
+				buffer[1] = enabled;
+				commands_send_packet_nrf(buffer, 2);
+			} else {
+				commands_printf("Invalid argument(s).\n");
+			}
+		} else {
+			commands_printf("This command requires one argument.\n");
+		}
+	} else if (strcmp(argv[0], "foc_sensors_detect_apply") == 0) {
+		if (argc == 2) {
+			float current = -1.0;
+			sscanf(argv[1], "%f", &current);
+
+			if (current > 0.0 && current <= mcconf.l_current_max) {
+				int res = conf_general_autodetect_apply_sensors_foc(current, true, true);
+
+				if (res == 0) {
+					commands_printf("No sensors found, using sensorless mode.\n");
+				} else if (res == 1) {
+					commands_printf("Found hall sensors, using them.\n");
+				} else if (res == 2) {
+					commands_printf("Found AS5047 encoder, using it.\n");
+				} else {
+					commands_printf("Detection error: %d\n", res);
+				}
+			} else {
+				commands_printf("Invalid argument(s).\n");
+			}
+		} else {
+			commands_printf("This command requires one argument.\n");
+		}
+	} else if (strcmp(argv[0], "rotor_lock_openloop") == 0) {
+		if (argc == 4) {
+			float current = -1.0;
+			float time = -1.0;
+			float angle = -1.0;
+			sscanf(argv[1], "%f", &current);
+			sscanf(argv[2], "%f", &time);
+			sscanf(argv[3], "%f", &angle);
+
+			if (current > 0.0 && current <= mcconf.l_current_max &&
+					angle >= 0.0 && angle <= 360.0) {
+				if (time <= 1e-6) {
+					timeout_reset();
+					mcpwm_foc_set_openloop_phase(current, angle);
+					commands_printf("OK\n");
+				} else {
+					int print_div = 0;
+					for (float t = 0.0;t < time;t += 0.002) {
+						timeout_reset();
+						mcpwm_foc_set_openloop_phase(current, angle);
+						chThdSleepMilliseconds(2);
+
+						print_div++;
+						if (print_div >= 200) {
+							print_div = 0;
+							commands_printf("T left: %.2f s", (double)(time - t));
+						}
+					}
+
+					commands_printf("Done\n");
+				}
+			} else {
+				commands_printf("Invalid argument(s).\n");
+			}
+		} else {
+			commands_printf("This command requires three arguments.\n");
+		}
+	} else if (strcmp(argv[0], "foc_detect_apply_all") == 0) {
+		if (argc == 2) {
+			float max_power_loss = -1.0;
+			sscanf(argv[1], "%f", &max_power_loss);
+
+			if (max_power_loss > 0.0) {
+				commands_printf("Running detection...");
+				int res = conf_general_detect_apply_all_foc(max_power_loss, true, true);
+
+				commands_printf("Res: %d", res);
+
+				if (res >= 0) {
+					commands_printf("Detection finished and applied. Results:");
+					mcconf = *mc_interface_get_configuration();
+					commands_printf("Motor Current       : %.1f A", (double)(mcconf.l_current_max));
+					commands_printf("Motor R             : %.2f mOhm", (double)(mcconf.foc_motor_r * 1e3));
+					commands_printf("Motor L             : %.2f microH", (double)(mcconf.foc_motor_l * 1e6));
+					commands_printf("Motor Flux Linkage  : %.3f mWb", (double)(mcconf.foc_motor_flux_linkage * 1e3));
+					commands_printf("Temp Comp           : %s", mcconf.foc_temp_comp ? "true" : "false");
+					if (mcconf.foc_temp_comp) {
+						commands_printf("Temp Comp Base Temp : %.1f degC", (double)mcconf.foc_temp_comp_base_temp);
+					}
+
+					if (res == 0) {
+						commands_printf("No sensors found, using sensorless mode.\n");
+					} else if (res == 1) {
+						commands_printf("Found hall sensors, using them.\n");
+					} else if (res == 2) {
+						commands_printf("Found AS5047 encoder, using it.\n");
+					} else {
+						commands_printf("Detection error: %d\n", res);
+					}
+				} else {
+					if (res == -10) {
+						commands_printf("Could not measure flux linkage.");
+					} else if (res == -11) {
+						commands_printf("Fault code occured during detection.");
+					}
+
+					commands_printf("Detection failed.\n");
+				}
+			} else {
+				commands_printf("Invalid argument(s).\n");
+			}
+		} else {
+			commands_printf("This command requires one argument.\n");
+		}
+	} 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);
+				found = true;
+			}
+		}
+
+		if (found) {
+			commands_printf("Done\n");
+		} else {
+			commands_printf("No CAN devices found\n");
+		}
+	} else if (strcmp(argv[0], "foc_detect_apply_all_can") == 0) {
+		if (argc == 2) {
+			float max_power_loss = -1.0;
+			sscanf(argv[1], "%f", &max_power_loss);
+
+			if (max_power_loss > 0.0) {
+				commands_printf("Running detection...");
+				int res = conf_general_detect_apply_all_foc_can(true, max_power_loss, 0.0, 0.0, 0.0, 0.0);
+
+				commands_printf("Res: %d", res);
+
+				if (res >= 0) {
+					commands_printf("Detection finished and applied. Results:");
+					mcconf = *mc_interface_get_configuration();
+					commands_printf("Motor Current       : %.1f A", (double)(mcconf.l_current_max));
+					commands_printf("Motor R             : %.2f mOhm", (double)(mcconf.foc_motor_r * 1e3));
+					commands_printf("Motor L             : %.2f microH", (double)(mcconf.foc_motor_l * 1e6));
+					commands_printf("Motor Flux Linkage  : %.3f mWb", (double)(mcconf.foc_motor_flux_linkage * 1e3));
+					commands_printf("Temp Comp           : %s", mcconf.foc_temp_comp ? "true" : "false");
+					if (mcconf.foc_temp_comp) {
+						commands_printf("Temp Comp Base Temp : %.1f degC", (double)mcconf.foc_temp_comp_base_temp);
+					}
+
+					if (res == 0) {
+						commands_printf("No sensors found, using sensorless mode.\n");
+					} else if (res == 1) {
+						commands_printf("Found hall sensors, using them.\n");
+					} else if (res == 2) {
+						commands_printf("Found AS5047 encoder, using it.\n");
+					} else {
+						commands_printf("Detection error: %d\n", res);
+					}
+				} else {
+					if (res == -10) {
+						commands_printf("Could not measure flux linkage.");
+					} else if (res == -11) {
+						commands_printf("Fault code occured during detection.");
+					}
+
+					commands_printf("Detection failed.\n");
+				}
+			} else {
+				commands_printf("Invalid argument(s).\n");
+			}
+		} else {
+			commands_printf("This command requires one argument.\n");
+		}
 	}
 
 	// The help command
@@ -484,7 +696,7 @@ void terminal_process_string(char *str) {
 		commands_printf("measure_ind [duty]");
 		commands_printf("  Send short voltage pulses, measure the current and calculate the motor inductance");
 
-		commands_printf("measure_linkage [current] [duty] [min_rpm] [motor_res]");
+		commands_printf("measure_linkage [current] [duty] [min_erpm] [motor_res]");
 		commands_printf("  Run the motor in BLDC delay mode and measure the flux linkage");
 		commands_printf("  example measure_linkage 5 0.5 700 0.076");
 		commands_printf("  tip: measure the resistance with measure_res first");
@@ -495,6 +707,11 @@ void terminal_process_string(char *str) {
 		commands_printf("measure_linkage_foc [duty]");
 		commands_printf("  Run the motor with FOC and measure the flux linkage.");
 
+		commands_printf("measure_linkage_openloop [current] [duty] [erpm_per_sec] [motor_res]");
+		commands_printf("  Run the motor in openloop FOC and measure the flux linkage");
+		commands_printf("  example measure_linkage 5 0.5 1000 0.076");
+		commands_printf("  tip: measure the resistance with measure_res first");
+
 		commands_printf("foc_state");
 		commands_printf("  Print some FOC state variables.");
 
@@ -504,6 +721,26 @@ void terminal_process_string(char *str) {
 		commands_printf("foc_openloop [current] [erpm]");
 		commands_printf("  Create an open loop rotating current vector.");
 
+		commands_printf("nrf_ext_set_enabled [enabled]");
+		commands_printf("  Enable or disable external NRF51822.");
+
+		commands_printf("foc_sensors_detect_apply [current]");
+		commands_printf("  Automatically detect FOC sensors, and apply settings on success.");
+
+		commands_printf("rotor_lock_openloop [current_A] [time_S] [angle_DEG]");
+		commands_printf("  Lock the motor with a current for a given time. Time 0 means forever, or");
+		commands_printf("  or until the heartbeat packets stop.");
+
+		commands_printf("foc_detect_apply_all [max_power_loss_W]");
+		commands_printf("  Detect and apply all motor settings, based on maximum resistive motor power losses.");
+
+		commands_printf("can_scan");
+		commands_printf("  Scan CAN-bus using ping commands, and print all devices that are found.");
+
+		commands_printf("foc_detect_apply_all_can [max_power_loss_W]");
+		commands_printf("  Detect and apply all motor settings, based on maximum resistive motor power losses. Also");
+		commands_printf("  initiates detection in all VESCs found on the CAN-bus.");
+
 		for (int i = 0;i < callback_write;i++) {
 			if (callbacks[i].arg_names) {
 				commands_printf("%s %s", callbacks[i].command, callbacks[i].arg_names);
diff --git a/tests/packet_recovery/Makefile b/tests/packet_recovery/Makefile
new file mode 100644
index 000000000..3f073337e
--- /dev/null
+++ b/tests/packet_recovery/Makefile
@@ -0,0 +1,32 @@
+TARGET = test
+LIBS = -lm
+CC = gcc
+CFLAGS = -O2 -g -Wall -Wextra -Wundef -std=gnu99 -I../../
+SOURCES = main.c ../../packet.c ../../crc.c
+HEADERS = ../../packet.h ../../crc.h
+OBJECTS = $(notdir $(SOURCES:.c=.o))
+
+.PHONY: default all clean
+
+default: $(TARGET)
+all: default
+
+%.o: %.c $(HEADERS)
+	$(CC) $(CFLAGS) -c $< -o $@
+	
+%.o: ../../%.c $(HEADERS)
+	$(CC) $(CFLAGS) -c $< -o $@
+
+.PRECIOUS: $(TARGET) $(OBJECTS)
+
+$(TARGET): $(OBJECTS)
+	$(CC) $(OBJECTS) -Wall $(LIBS) -o $@
+
+clean:
+	rm -f $(OBJECTS) $(TARGET)
+	
+test2:
+	echo $(OBJECTS)
+
+run: $(TARGET)
+	./$(TARGET)
diff --git a/tests/packet_recovery/main.c b/tests/packet_recovery/main.c
new file mode 100644
index 000000000..d40ea438e
--- /dev/null
+++ b/tests/packet_recovery/main.c
@@ -0,0 +1,97 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <time.h>
+
+#include "packet.h"
+
+const unsigned int rand_prepend = 50;
+static uint8_t buffer[250000];
+static unsigned int write = 0;
+
+void send_packet(unsigned char *data, unsigned int len) {
+	memcpy(buffer + write, data, len);
+	write += len;
+}
+
+void process_packet(unsigned char *data, unsigned int len) {
+	printf("Packet rx (%03d bytes): %s\r\n", len, (char*)data + rand_prepend);
+}
+
+void process_packet_perf(unsigned char *data, unsigned int len) {
+	(void)data;
+	(void)len;
+}
+
+int main(void) {
+	packet_init(send_packet, process_packet, 0);
+	
+	srand(104);
+	
+	for (int i = 0;i < 20;i++) {
+		char asd[rand_prepend + 100];
+		
+		for (unsigned int j = 0;j < rand_prepend;j++) {
+			asd[j] = rand();
+		}
+		
+		sprintf(asd + rand_prepend, "Offset: %d Test %d", write, i);
+		packet_send_packet((unsigned char*)asd, strlen(asd + rand_prepend) + rand_prepend + 1, 0);
+	}
+	
+	// Ability to recover
+	unsigned int offsets[] = {121, 1250, 1121, 1122, 1187, 1188, 1189, 1036, 1112, 1264};
+	for (unsigned int ofs = 0;ofs < sizeof(offsets) / sizeof(int);ofs++) {
+		printf("Decode from offset %d\r\n", offsets[ofs]);
+		
+		//packet_reset(0);
+		for(unsigned int i = offsets[ofs];i < write;i++) {
+			packet_process_byte(buffer[i], 0);
+		}
+		
+		printf("\r\n");
+	}
+	
+	// Corruption
+	printf("Corruption Test\r\n");
+	buffer[12] = 91;
+	buffer[13] = 0xFF;
+	buffer[14] = 0;
+	buffer[15] = 221;
+	buffer[800] = 0;
+	buffer[345] = 0xFF;
+	buffer[1200]++;
+	buffer[1342]++;
+	for(unsigned int i = 0;i < write;i++) {
+		packet_process_byte(buffer[i], 0);
+	}
+	
+	// Performance
+	printf("\r\nPerformance Test\r\n");
+	packet_init(send_packet, process_packet_perf, 0);
+	
+	srand(104);
+	write = 0;
+	unsigned char asd[500];
+	for (unsigned int i = 0;i < sizeof(asd);i++) {
+		asd[i] = rand();
+	}
+	
+	clock_t start, end;
+	double cpu_time_used;
+	
+	start = clock();
+	for (int i = 0;i < 1e6;i++) {
+		packet_send_packet(asd, sizeof(asd), 0);
+		for (unsigned int j = 0;j < write;j++) {
+			packet_process_byte(buffer[j], 0);
+		}
+		write = 0;
+	}
+	end = clock();
+	cpu_time_used = ((double) (end - start)) / CLOCKS_PER_SEC;
+	
+	printf("Time: %.3f s\r\n", cpu_time_used);
+	
+	return 0;
+}
diff --git a/utils.c b/utils.c
index dbf6593eb..4194f9605 100644
--- a/utils.c
+++ b/utils.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -143,7 +143,6 @@ void utils_deadband(float *value, float tres, float max) {
 		} else {
 			*value = -(k * -*value + max * (1.0 - k));
 		}
-
 	}
 }
 
@@ -623,3 +622,19 @@ void utils_sys_unlock_cnt(void) {
 		}
 	}
 }
+
+uint32_t utils_crc32c(uint8_t *data, uint32_t len) {
+	uint32_t crc = 0xFFFFFFFF;
+
+	for (uint32_t i = 0; i < len;i++) {
+		uint32_t byte = data[i];
+		crc = crc ^ byte;
+
+		for (int j = 7;j >= 0;j--) {
+			uint32_t mask = -(crc & 1);
+			crc = (crc >> 1) ^ (0x82F63B78 & mask);
+		}
+	}
+
+	return ~crc;
+}
diff --git a/utils.h b/utils.h
index 004650c18..f501a6028 100644
--- a/utils.h
+++ b/utils.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2019 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -21,6 +21,7 @@
 #define UTILS_H_
 
 #include <stdbool.h>
+#include <stdint.h>
 
 void utils_step_towards(float *value, float goal, float step);
 float utils_calc_ratio(float low, float high, float val);
@@ -48,6 +49,7 @@ void utils_byte_to_binary(int x, char *b);
 float utils_throttle_curve(float val, float curve_acc, float curve_brake, int mode);
 void utils_sys_lock_cnt(void);
 void utils_sys_unlock_cnt(void);
+uint32_t utils_crc32c(uint8_t *data, uint32_t len);
 
 // Return the sign of the argument. -1 if negative, 1 if zero or positive.
 #define SIGN(x)				((x < 0) ? -1 : 1)