diff --git a/CHANGELOG b/CHANGELOG
index 032798a2..ffd34c59 100644
--- a/CHANGELOG
+++ b/CHANGELOG
@@ -12,6 +12,8 @@
 * Added TEMP_SENSOR_KTY84_130.
 * Major UAVCAN update. See: https://github.com/vedderb/bldc/pull/269
 * Avoid numerical instability when mapping is done over a narrow range. See: https://github.com/vedderb/bldc/issues/262
+* App Balance updates.
+* Added servo_out_enable appconf option, so that the PPM port can be used to control servos with the default firmware.
 
 === FW 5.02 ===
 * IMU calibration improvement.
diff --git a/appconf/appconf_default.h b/appconf/appconf_default.h
index 9fa9fe90..b490d23c 100644
--- a/appconf/appconf_default.h
+++ b/appconf/appconf_default.h
@@ -42,6 +42,9 @@
 #ifndef APPCONF_UAVCAN_RAW_MODE
 #define APPCONF_UAVCAN_RAW_MODE				UAVCAN_RAW_MODE_CURRENT
 #endif
+#ifndef APPCONF_SERVO_OUT_ENABLE
+#define APPCONF_SERVO_OUT_ENABLE			false
+#endif
 #ifndef APPCONF_SEND_CAN_STATUS_RATE_HZ
 #define APPCONF_SEND_CAN_STATUS_RATE_HZ		50
 #endif
diff --git a/applications/app.c b/applications/app.c
index eb4af77e..75c434b8 100644
--- a/applications/app.c
+++ b/applications/app.c
@@ -26,6 +26,7 @@
 #include "comm_can.h"
 #include "imu.h"
 #include "crc.h"
+#include "servo_simple.h"
 
 // Private variables
 static app_configuration appconf;
@@ -70,6 +71,14 @@ void app_set_configuration(app_configuration *conf) {
 
 	imu_init(&conf->imu_conf);
 
+	if (appconf.app_to_use != APP_PPM &&
+			appconf.app_to_use != APP_PPM_UART &&
+			appconf.servo_out_enable) {
+		servo_simple_init();
+	} else {
+		servo_simple_stop();
+	}
+
 	// Configure balance app before starting it.
 	app_balance_configure(&appconf.app_balance_conf, &appconf.imu_conf);
 
diff --git a/applications/app_ppm.c b/applications/app_ppm.c
index 8909c18c..120bb6ee 100644
--- a/applications/app_ppm.c
+++ b/applications/app_ppm.c
@@ -29,9 +29,6 @@
 #include "comm_can.h"
 #include <math.h>
 
-// Only available if servo output is not active
-#if !SERVO_OUT_ENABLE
-
 // Settings
 #define MAX_CAN_AGE						0.1
 #define MIN_PULSES_WITHOUT_POWER		50
@@ -54,10 +51,8 @@ static float input_val = 0.0;
 static volatile float direction_hyst = 0;
 
 // Private functions
-#endif
 
 void app_ppm_configure(ppm_config *conf) {
-#if !SERVO_OUT_ENABLE
 	config = *conf;
 	pulses_without_power = 0;
 
@@ -66,20 +61,14 @@ void app_ppm_configure(ppm_config *conf) {
 	}
 
 	direction_hyst = config.max_erpm_for_dir * 0.20;
-#else
-	(void)conf;
-#endif
 }
 
 void app_ppm_start(void) {
-#if !SERVO_OUT_ENABLE
 	stop_now = false;
 	chThdCreateStatic(ppm_thread_wa, sizeof(ppm_thread_wa), NORMALPRIO, ppm_thread, NULL);
-#endif
 }
 
 void app_ppm_stop(void) {
-#if !SERVO_OUT_ENABLE
 	stop_now = true;
 
 	if (is_running) {
@@ -90,18 +79,12 @@ void app_ppm_stop(void) {
 	while(is_running) {
 		chThdSleepMilliseconds(1);
 	}
-#endif
 }
 
 float app_ppm_get_decoded_level(void) {
-#if !SERVO_OUT_ENABLE
 	return input_val;
-#else
-	return 0.0;
-#endif
 }
 
-#if !SERVO_OUT_ENABLE
 static void servodec_func(void) {
 	ppm_rx = true;
 	chSysLockFromISR();
@@ -532,4 +515,3 @@ static THD_FUNCTION(ppm_thread, arg) {
 
 	}
 }
-#endif
diff --git a/commands.c b/commands.c
index 3e8aba43..4fefbc22 100644
--- a/commands.c
+++ b/commands.c
@@ -451,10 +451,8 @@ void commands_process_packet(unsigned char *data, unsigned int len,
 	} break;
 
 	case COMM_SET_SERVO_POS: {
-#if SERVO_OUT_ENABLE
 		int32_t ind = 0;
 		servo_simple_set_output(buffer_get_float16(data, 1000.0, &ind));
-#endif
 	} break;
 
 	case COMM_SET_MCCONF: {
diff --git a/conf_general.h b/conf_general.h
index 10965044..8fb40a7e 100644
--- a/conf_general.h
+++ b/conf_general.h
@@ -24,7 +24,7 @@
 #define FW_VERSION_MAJOR			5
 #define FW_VERSION_MINOR			03
 // Set to 0 for building a release and iterate during beta test builds
-#define FW_TEST_VERSION_NUMBER		16
+#define FW_TEST_VERSION_NUMBER		17
 
 #include "datatypes.h"
 
@@ -74,8 +74,8 @@
 //#define HW60_IS_MK4
 #define HW60_IS_MK5
 
-//#define HW_SOURCE "hw_60.c"
-//#define HW_HEADER "hw_60.h"
+#define HW_SOURCE "hw_60.c"
+#define HW_HEADER "hw_60.h"
 
 //#define HW_SOURCE "hw_r2.c"
 //#define HW_HEADER "hw_r2.h"
@@ -123,8 +123,8 @@
 //#define HW_SOURCE "hw_binar_v1.c"
 //#define HW_HEADER "hw_binar_v1.h"
 
-#define HW_SOURCE "hw_hd60.c"
-#define HW_HEADER "hw_hd60.h"
+//#define HW_SOURCE "hw_hd60.c"
+//#define HW_HEADER "hw_hd60.h"
 
 //#define HW_SOURCE "hw_hd75.c"
 //#define HW_HEADER "hw_hd75.h"
@@ -258,9 +258,6 @@
 /*
  * Servo output driver
  */
-#ifndef SERVO_OUT_ENABLE
-#define SERVO_OUT_ENABLE			0		// Enable servo output
-#endif
 #define SERVO_OUT_PULSE_MIN_US		1000	// Minimum pulse length in microseconds
 #define SERVO_OUT_PULSE_MAX_US		2000	// Maximum pulse length in microseconds
 #define SERVO_OUT_RATE_HZ			50		// Update rate in Hz
diff --git a/confgenerator.c b/confgenerator.c
index 665e831a..101e18de 100644
--- a/confgenerator.c
+++ b/confgenerator.c
@@ -194,6 +194,7 @@ int32_t confgenerator_serialize_appconf(uint8_t *buffer, const app_configuration
 	buffer[ind++] = conf->can_mode;
 	buffer[ind++] = (uint8_t)conf->uavcan_esc_index;
 	buffer[ind++] = conf->uavcan_raw_mode;
+	buffer[ind++] = conf->servo_out_enable;
 	buffer[ind++] = conf->app_to_use;
 	buffer[ind++] = conf->app_ppm_conf.ctrl_type;
 	buffer_append_float32_auto(buffer, conf->app_ppm_conf.pid_max_erpm, &ind);
@@ -535,6 +536,7 @@ bool confgenerator_deserialize_appconf(const uint8_t *buffer, app_configuration
 	conf->can_mode = buffer[ind++];
 	conf->uavcan_esc_index = buffer[ind++];
 	conf->uavcan_raw_mode = buffer[ind++];
+	conf->servo_out_enable = buffer[ind++];
 	conf->app_to_use = buffer[ind++];
 	conf->app_ppm_conf.ctrl_type = buffer[ind++];
 	conf->app_ppm_conf.pid_max_erpm = buffer_get_float32_auto(buffer, &ind);
@@ -860,6 +862,7 @@ void confgenerator_set_defaults_appconf(app_configuration *conf) {
 	conf->can_mode = APPCONF_CAN_MODE;
 	conf->uavcan_esc_index = APPCONF_UAVCAN_ESC_INDEX;
 	conf->uavcan_raw_mode = APPCONF_UAVCAN_RAW_MODE;
+	conf->servo_out_enable = APPCONF_SERVO_OUT_ENABLE;
 	conf->app_to_use = APPCONF_APP_TO_USE;
 	conf->app_ppm_conf.ctrl_type = APPCONF_PPM_CTRL_TYPE;
 	conf->app_ppm_conf.pid_max_erpm = APPCONF_PPM_PID_MAX_ERPM;
diff --git a/confgenerator.h b/confgenerator.h
index e76855db..69454c0e 100644
--- a/confgenerator.h
+++ b/confgenerator.h
@@ -9,7 +9,7 @@
 
 // Constants
 #define MCCONF_SIGNATURE		2835115562
-#define APPCONF_SIGNATURE		3708020089
+#define APPCONF_SIGNATURE		1729624904
 
 // Functions
 int32_t confgenerator_serialize_mcconf(uint8_t *buffer, const mc_configuration *conf);
diff --git a/datatypes.h b/datatypes.h
index e8f66b9c..fe1010bf 100644
--- a/datatypes.h
+++ b/datatypes.h
@@ -786,6 +786,7 @@ typedef struct {
 	bool pairing_done;
 	bool permanent_uart_enabled;
 	SHUTDOWN_MODE shutdown_mode;
+	bool servo_out_enable;
 
 	// CAN modes
 	CAN_MODE can_mode;
diff --git a/hwconf/hw.h b/hwconf/hw.h
index 256e7770..be2988f2 100644
--- a/hwconf/hw.h
+++ b/hwconf/hw.h
@@ -491,6 +491,16 @@
 #define HW_PAS2_PIN				HW_UART_TX_PIN
 #endif
 
+#ifndef HW_ICU_TIMER
+#ifdef HW_USE_SERVO_TIM4
+#define HW_ICU_TIMER			TIM4
+#define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE)
+#else
+#define HW_ICU_TIMER			TIM3
+#define HW_ICU_TIM_CLK_EN()		RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE)
+#endif
+#endif
+
 // Functions
 void hw_init_gpio(void);
 void hw_setup_adc_channels(void);
diff --git a/hwconf/hw_stormcore_100d.h b/hwconf/hw_stormcore_100d.h
index 56bb73d7..0f0f2eef 100644
--- a/hwconf/hw_stormcore_100d.h
+++ b/hwconf/hw_stormcore_100d.h
@@ -273,6 +273,7 @@
 
 // ICU Peripheral for servo decoding
 #define HW_ICU_TIMER			TIM9
+#define HW_ICU_TIM_CLK_EN()		RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE)
 #define HW_ICU_DEV				ICUD9
 #define HW_ICU_CHANNEL			ICU_CHANNEL_1
 #define HW_ICU_GPIO_AF			GPIO_AF_TIM9
diff --git a/hwconf/hw_stormcore_60d.h b/hwconf/hw_stormcore_60d.h
index feb958b9..2ce81519 100644
--- a/hwconf/hw_stormcore_60d.h
+++ b/hwconf/hw_stormcore_60d.h
@@ -301,6 +301,7 @@
 
 // ICU Peripheral for servo decoding
 #define HW_ICU_TIMER			TIM9
+#define HW_ICU_TIM_CLK_EN()		RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE)
 #define HW_ICU_DEV				ICUD9
 #define HW_ICU_CHANNEL			ICU_CHANNEL_1
 #define HW_ICU_GPIO_AF			GPIO_AF_TIM9
diff --git a/hwconf/hw_unity.h b/hwconf/hw_unity.h
index 9b68d5b1..f96437ad 100644
--- a/hwconf/hw_unity.h
+++ b/hwconf/hw_unity.h
@@ -199,6 +199,7 @@
 
 // ICU Peripheral for servo decoding
 #define HW_ICU_TIMER			TIM9
+#define HW_ICU_TIM_CLK_EN()		RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM9, ENABLE)
 #define HW_ICU_DEV				ICUD9
 #define HW_ICU_CHANNEL			ICU_CHANNEL_1
 #define HW_ICU_GPIO_AF			GPIO_AF_TIM9
diff --git a/main.c b/main.c
index f75d1e44..d618e5fb 100644
--- a/main.c
+++ b/main.c
@@ -268,10 +268,6 @@ int main(void) {
 #endif
 #endif
 
-#if SERVO_OUT_ENABLE
-	servo_simple_init();
-#endif
-
 	// Threads
 	chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
 	chThdCreateStatic(flash_integrity_check_thread_wa, sizeof(flash_integrity_check_thread_wa), LOWPRIO, flash_integrity_check_thread, NULL);
diff --git a/servo_simple.c b/servo_simple.c
index 0c4c0436..a9835d83 100644
--- a/servo_simple.c
+++ b/servo_simple.c
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2021 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -24,10 +24,15 @@
 #include "stm32f4xx_conf.h"
 #include "utils.h"
 
+/**
+ * TODO: Use the chibios driver instead of the ST-driver.
+ */
+
 // Settings
 #define TIM_CLOCK			1000000 // Hz
 
-#if SERVO_OUT_ENABLE
+// Private variables
+static volatile bool m_is_running = false;
 
 void servo_simple_init(void) {
 	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
@@ -63,9 +68,28 @@ void servo_simple_init(void) {
 	servo_simple_set_output(0.5);
 
 	TIM_Cmd(HW_ICU_TIMER, ENABLE);
+
+	m_is_running = true;
+}
+
+void servo_simple_stop(void) {
+	if (m_is_running) {
+		palSetPadMode(HW_ICU_GPIO, HW_ICU_PIN, PAL_MODE_INPUT);
+		TIM_Cmd(HW_ICU_TIMER, DISABLE);
+		TIM_DeInit(HW_ICU_TIMER);
+		m_is_running = false;
+	}
+}
+
+bool servo_simple_is_running(void) {
+	return m_is_running;
 }
 
 void servo_simple_set_output(float out) {
+	if (!m_is_running) {
+		return;
+	}
+
 	utils_truncate_number(&out, 0.0, 1.0);
 
 	float us = (float)SERVO_OUT_PULSE_MIN_US + out *
@@ -78,5 +102,3 @@ void servo_simple_set_output(float out) {
 		HW_ICU_TIMER->CCR2 = (uint32_t)us;
 	}
 }
-
-#endif
diff --git a/servo_simple.h b/servo_simple.h
index a0ccc31e..5590c5c8 100644
--- a/servo_simple.h
+++ b/servo_simple.h
@@ -1,5 +1,5 @@
 /*
-	Copyright 2016 Benjamin Vedder	benjamin@vedder.se
+	Copyright 2016 - 2021 Benjamin Vedder	benjamin@vedder.se
 
 	This file is part of the VESC firmware.
 
@@ -20,8 +20,12 @@
 #ifndef SERVO_SIMPLE_H_
 #define SERVO_SIMPLE_H_
 
+#include <stdbool.h>
+
 // Functions
 void servo_simple_init(void);
+void servo_simple_stop(void);
+bool servo_simple_is_running(void);
 void servo_simple_set_output(float out);
 
 #endif /* SERVO_SIMPLE_H_ */