diff --git a/apps/drivers/drv_pwm_output.h b/apps/drivers/drv_pwm_output.h
index b2fee65aca..c110cd5cbc 100644
--- a/apps/drivers/drv_pwm_output.h
+++ b/apps/drivers/drv_pwm_output.h
@@ -103,6 +103,9 @@ ORB_DECLARE(output_pwm);
 /** disarm all servo outputs (stop generating pulses) */
 #define PWM_SERVO_DISARM	_IOC(_PWM_SERVO_BASE, 1)
 
+/** set update rate in Hz */
+#define PWM_SERVO_SET_UPDATE_RATE	_IOC(_PWM_SERVO_BASE, 2)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index f227db1f76..78c0618194 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -503,6 +503,10 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		// up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		g_hil->set_pwm_rate(arg);
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp
index 819dbf2087..3b835904f6 100644
--- a/apps/drivers/px4fmu/fmu.cpp
+++ b/apps/drivers/px4fmu/fmu.cpp
@@ -470,6 +470,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		up_pwm_servo_arm(false);
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		set_pwm_rate(arg);
+		break;
+
 	case PWM_SERVO_SET(2):
 	case PWM_SERVO_SET(3):
 		if (_mode != MODE_4PWM) {
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index c1b52b07b4..b975cb5a17 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -629,6 +629,11 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		_send_needed = true;
 		break;
 
+	case PWM_SERVO_SET_UPDATE_RATE:
+		// not supported yet
+		ret = -EINVAL;
+		break;
+
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 
 		/* fake an update to the selected 'servo' channel */
diff --git a/nuttx/configs/px4fmu/nsh/defconfig b/nuttx/configs/px4fmu/nsh/defconfig
index bc724c0063..7ff9eaa8c8 100755
--- a/nuttx/configs/px4fmu/nsh/defconfig
+++ b/nuttx/configs/px4fmu/nsh/defconfig
@@ -245,18 +245,18 @@ CONFIG_USART6_SERIAL_CONSOLE=n
 
 #Mavlink messages can be bigger than 128
 CONFIG_USART1_TXBUFSIZE=512
-CONFIG_USART2_TXBUFSIZE=128
-CONFIG_USART3_TXBUFSIZE=128
-CONFIG_UART4_TXBUFSIZE=128
-CONFIG_UART5_TXBUFSIZE=64
+CONFIG_USART2_TXBUFSIZE=256
+CONFIG_USART3_TXBUFSIZE=256
+CONFIG_UART4_TXBUFSIZE=256
+CONFIG_UART5_TXBUFSIZE=256
 CONFIG_USART6_TXBUFSIZE=128
 
 CONFIG_USART1_RXBUFSIZE=512
-CONFIG_USART2_RXBUFSIZE=128
-CONFIG_USART3_RXBUFSIZE=128
-CONFIG_UART4_RXBUFSIZE=128
-CONFIG_UART5_RXBUFSIZE=128
-CONFIG_USART6_RXBUFSIZE=128
+CONFIG_USART2_RXBUFSIZE=256
+CONFIG_USART3_RXBUFSIZE=256
+CONFIG_UART4_RXBUFSIZE=256
+CONFIG_UART5_RXBUFSIZE=256
+CONFIG_USART6_RXBUFSIZE=256
 
 CONFIG_USART1_BAUD=57600
 CONFIG_USART2_BAUD=115200