diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index a104b9de2ef6f1137a6efab7ead77368cacfd675..aa4e1140c66ad8bcbfc316920f36796ab2d64c2f 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -260,16 +260,17 @@ struct pwm_output_rc_config {
 #define PWM_SERVO_MODE_3PWM1CAP     5
 #define PWM_SERVO_MODE_4PWM         6
 #define PWM_SERVO_MODE_4PWM1CAP     7
-#define PWM_SERVO_MODE_5PWM         8
-#define PWM_SERVO_MODE_5PWM1CAP     9
-#define PWM_SERVO_MODE_6PWM        10
-#define PWM_SERVO_MODE_8PWM        11
-#define PWM_SERVO_MODE_14PWM       12
-#define PWM_SERVO_MODE_4CAP        13
-#define PWM_SERVO_MODE_5CAP        14
-#define PWM_SERVO_MODE_6CAP        15
-#define PWM_SERVO_ENTER_TEST_MODE  16
-#define PWM_SERVO_EXIT_TEST_MODE   17
+#define PWM_SERVO_MODE_4PWM2CAP     8
+#define PWM_SERVO_MODE_5PWM         9
+#define PWM_SERVO_MODE_5PWM1CAP    10
+#define PWM_SERVO_MODE_6PWM        11
+#define PWM_SERVO_MODE_8PWM        12
+#define PWM_SERVO_MODE_14PWM       13
+#define PWM_SERVO_MODE_4CAP        14
+#define PWM_SERVO_MODE_5CAP        15
+#define PWM_SERVO_MODE_6CAP        16
+#define PWM_SERVO_ENTER_TEST_MODE  17
+#define PWM_SERVO_EXIT_TEST_MODE   18
 #define PWM_SERVO_SET_MODE         _PX4_IOC(_PWM_SERVO_BASE, 34)
 
 /*
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index a25ea7e9c1c189e485b50e444b4f90d231f34420..120ef57ad5372258d80ddfc71daac3d66eedda8e 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -92,6 +92,7 @@ enum PortMode {
 	PORT_PWM1,
 	PORT_PWM3CAP1,
 	PORT_PWM4CAP1,
+	PORT_PWM4CAP2,
 	PORT_PWM5CAP1,
 	PORT_PWM2CAP2,
 	PORT_CAPTURE,
@@ -115,6 +116,7 @@ public:
 		MODE_3PWM1CAP,
 		MODE_4PWM,
 		MODE_4PWM1CAP,
+		MODE_4PWM2CAP,
 		MODE_5PWM,
 		MODE_5PWM1CAP,
 		MODE_6PWM,
@@ -612,6 +614,23 @@ PX4FMU::set_mode(Mode mode)
 
 		break;
 
+#if defined(BOARD_HAS_CAPTURE)
+
+	case MODE_4PWM2CAP:
+		PX4_DEBUG("MODE_4PWM2CAP");
+		up_input_capture_set(5, Rising, 0, NULL, NULL);
+
+		/* default output rates */
+		_pwm_default_rate = 400;
+		_pwm_alt_rate = 50;
+		_pwm_alt_rate_channels = 0;
+		_pwm_mask = 0x0f;
+		_pwm_initialized = false;
+		_num_outputs = 4;
+
+		break;
+#endif
+
 #if defined(BOARD_HAS_CAPTURE)
 
 	case MODE_5PWM1CAP:
@@ -1460,6 +1479,7 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg)
 	case MODE_2PWM2CAP:
 	case MODE_3PWM1CAP:
 	case MODE_4PWM1CAP:
+	case MODE_4PWM2CAP:
 	case MODE_5PWM1CAP:
 #if defined(BOARD_HAS_PWM) && BOARD_HAS_PWM >= 6
 	case MODE_6PWM:
@@ -1965,6 +1985,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 
 		case MODE_4PWM:
 		case MODE_4PWM1CAP:
+		case MODE_4PWM2CAP:
 			*(unsigned *)arg = 4;
 			break;
 
@@ -2076,6 +2097,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				ret = set_mode(MODE_4PWM1CAP);
 				break;
 
+			case PWM_SERVO_MODE_4PWM2CAP:
+				ret = set_mode(MODE_4PWM2CAP);
+				break;
+
 			case PWM_SERVO_MODE_5PWM:
 				ret = set_mode(MODE_5PWM);
 				break;
@@ -2297,7 +2322,8 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
 	input_capture_stats_t *stats = (input_capture_stats_t *)arg;
 
 	if (_mode == MODE_3PWM1CAP || _mode == MODE_2PWM2CAP ||
-	    _mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP) {
+	    _mode == MODE_4PWM1CAP || _mode == MODE_5PWM1CAP ||
+	    _mode == MODE_4PWM2CAP) {
 
 		pconfig = (input_capture_config_t *)arg;
 	}
@@ -2379,6 +2405,7 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
 			break;
 
 		case MODE_2PWM2CAP:
+		case MODE_4PWM2CAP:
 			*(unsigned *)arg = 2;
 			break;
 
@@ -2405,6 +2432,10 @@ PX4FMU::capture_ioctl(struct file *filp, int cmd, unsigned long arg)
 			set_mode(MODE_4PWM1CAP);
 			break;
 
+		case MODE_4PWM2CAP:
+			set_mode(MODE_4PWM2CAP);
+			break;
+
 		case MODE_5PWM1CAP:
 			set_mode(MODE_5PWM1CAP);
 			break;
@@ -2509,6 +2540,12 @@ PX4FMU::fmu_new_mode(PortMode new_mode)
 		servo_mode = PX4FMU::MODE_4PWM1CAP;
 		break;
 
+	case PORT_PWM4CAP2:
+		/* select 4-pin PWM mode 2 capture */
+		servo_mode = PX4FMU::MODE_4PWM2CAP;
+		mode_with_input = true;
+		break;
+
 #  endif
 
 	case PORT_PWM3:
@@ -2535,6 +2572,7 @@ PX4FMU::fmu_new_mode(PortMode new_mode)
 		/* select 2-pin PWM mode 2 capture */
 		servo_mode = PX4FMU::MODE_2PWM2CAP;
 		break;
+
 #  endif
 #endif
 
@@ -2911,6 +2949,9 @@ int PX4FMU::custom_command(int argc, char *argv[])
 
 	} else if (!strcmp(verb, "mode_pwm4cap1")) {
 		new_mode = PORT_PWM4CAP1;
+
+	} else if (!strcmp(verb, "mode_pwm4cap2")) {
+		new_mode = PORT_PWM4CAP2;
 #  endif
 
 	} else if (!strcmp(verb, "mode_pwm3")) {
@@ -3014,6 +3055,7 @@ mixer files.
   PRINT_MODULE_USAGE_COMMAND("mode_pwm5cap1");
 	PRINT_MODULE_USAGE_COMMAND("mode_pwm4");
   PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap1");
+  PRINT_MODULE_USAGE_COMMAND("mode_pwm4cap2");
   PRINT_MODULE_USAGE_COMMAND("mode_pwm3");
   PRINT_MODULE_USAGE_COMMAND("mode_pwm3cap1");
 	PRINT_MODULE_USAGE_COMMAND("mode_pwm2");
@@ -3073,6 +3115,8 @@ int PX4FMU::print_status()
 
   	case MODE_4PWM1CAP: mode_str = "pwm4cap1"; break;
 
+	case MODE_4PWM2CAP: mode_str = "pwm4cap2"; break;
+
   	case MODE_5PWM: mode_str = "pwm5"; break;
 
   	case MODE_5PWM1CAP: mode_str = "pwm5cap1"; break;