diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h
index d388dcb412..7bfea34fd8 100644
--- a/libraries/APM_RC/APM_RC.h
+++ b/libraries/APM_RC/APM_RC.h
@@ -39,10 +39,13 @@ class APM_RC_Class
 	virtual uint16_t InputCh(uint8_t ch) = 0;
 	virtual uint8_t GetState() = 0;
 	virtual void clearOverride(void) = 0;
-    virtual void Force_Out() = 0;
-    virtual void SetFastOutputChannels( uint32_t channelmask ) = 0;
-  virtual void enable_out(uint8_t) = 0;
-  virtual void disable_out(uint8_t) = 0;
+	virtual void Force_Out() = 0;
+	virtual void SetFastOutputChannels( uint32_t channelmask, uint16_t speed_hz = 400 ) = 0;
+	virtual void enable_out(uint8_t) = 0;
+	virtual void disable_out(uint8_t) = 0;
+
+protected:
+	uint16_t _map_speed(uint16_t speed_hz) { return 2000000UL / speed_hz; }
 
 };
 
diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp
index dab7d05cd9..add4693172 100644
--- a/libraries/APM_RC/APM_RC_APM1.cpp
+++ b/libraries/APM_RC/APM_RC_APM1.cpp
@@ -247,84 +247,34 @@ void APM_RC_APM1::Force_Out6_Out7(void)
 
 /* --------------------- OUTPUT SPEED CONTROL --------------------- */
 
-// Output rate options:
-#define OUTPUT_SPEED_50HZ 0
-#define OUTPUT_SPEED_200HZ 1
-#define OUTPUT_SPEED_490HZ 2
-
-void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask)
+void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz)
 {
-    if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0)
-        _set_speed_ch1_ch2_ch9(OUTPUT_SPEED_490HZ);
+	uint16_t icr = _map_speed(speed_hz);
 
-    if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0)
-        _set_speed_ch3_ch4_ch10(OUTPUT_SPEED_490HZ);
+	if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) {
+		ICR1 = icr;
+	}
 
-    if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0)
-        _set_speed_ch5_ch6(OUTPUT_SPEED_490HZ);
+	if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) {
+		ICR5 = icr;
+	}
 
-    if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0)
-        _set_speed_ch7_ch8_ch11(OUTPUT_SPEED_490HZ);
+#if 0
+	if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0) {
+		/* These channels intentionally left blank:
+		 * Can't change output speed of ch5 (OCR4B) and ch6 (OCR4C).
+		 * Timer 4 period controlled by OCR4A, and used for input
+		 * capture on ICR4.
+		 * If the period of Timer 4 must be changed, the input capture
+		 * code will have to be adjusted as well
+		 */
+	}
+#endif
 
-}
+	if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0) {
+		ICR3 = icr;
+	}
 
-void APM_RC_APM1::_set_speed_ch1_ch2_ch9(uint8_t speed)
-{
-  switch(speed) {
-  case OUTPUT_SPEED_490HZ:
-    ICR1= 4096;
-    break;
-  case OUTPUT_SPEED_200HZ:
-    ICR1= 10000;
-    break;
-  case OUTPUT_SPEED_50HZ:
-  default:
-    ICR1 = 40000;
-    break;
-  }
-}
-
-void APM_RC_APM1::_set_speed_ch3_ch4_ch10(uint8_t speed)
-{
-  switch(speed) {
-  case OUTPUT_SPEED_490HZ:
-    ICR5= 4096;
-    break;
-  case OUTPUT_SPEED_200HZ:
-    ICR5= 10000;
-    break;
-  case OUTPUT_SPEED_50HZ:
-  default:
-    ICR5 = 40000;
-    break;
-  }
-}
-
-void APM_RC_APM1::_set_speed_ch7_ch8_ch11(uint8_t speed)
-{
-  switch(speed) {
-  case OUTPUT_SPEED_490HZ:
-    ICR3 = 4096;
-    break;
-  case OUTPUT_SPEED_200HZ:
-    ICR3 = 10000;
-    break;
-  case OUTPUT_SPEED_50HZ:
-  default:
-    ICR3 = 40000;
-    break;
-  }
-}
-
-void APM_RC_APM1::_set_speed_ch5_ch6(uint8_t speed)
-{
-  /* This function intentionally left blank:
-   * Can't change output speed of ch5 (OCR4B) and ch6 (OCR4C).
-   * Timer 4 period controlled by OCR4A, and used for input
-   * capture on ICR4.
-   * If the period of Timer 4 must be changed, the input capture
-   * code will have to be adjusted as well
-   */
 }
 
 // allow HIL override of RC values
diff --git a/libraries/APM_RC/APM_RC_APM1.h b/libraries/APM_RC/APM_RC_APM1.h
index 8ea59d353a..c448a3eddd 100644
--- a/libraries/APM_RC/APM_RC_APM1.h
+++ b/libraries/APM_RC/APM_RC_APM1.h
@@ -18,10 +18,10 @@ class APM_RC_APM1 : public APM_RC_Class
 	bool setHIL(int16_t v[NUM_CHANNELS]);
 	void clearOverride(void);
 	void Force_Out(void);
-	void SetFastOutputChannels(uint32_t chmask);
+	void SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz = 400);
 
-  void enable_out(uint8_t);
-  void disable_out(uint8_t);
+	void enable_out(uint8_t);
+	void disable_out(uint8_t);
 
 	void Force_Out0_Out1(void);
 	void Force_Out2_Out3(void);
@@ -34,13 +34,7 @@ class APM_RC_APM1 : public APM_RC_Class
     static volatile uint16_t _PWM_RAW[NUM_CHANNELS];
     static volatile uint8_t  _radio_status;
 
-	int16_t _HIL_override[NUM_CHANNELS];
-
-  void _set_speed_ch1_ch2_ch9(uint8_t speed);
-  void _set_speed_ch3_ch4_ch10(uint8_t speed);
-  void _set_speed_ch5_ch6(uint8_t speed);
-  void _set_speed_ch7_ch8_ch11(uint8_t speed);
-
+    int16_t _HIL_override[NUM_CHANNELS];
 };
 
 #endif
diff --git a/libraries/APM_RC/APM_RC_APM2.cpp b/libraries/APM_RC/APM_RC_APM2.cpp
index ca3a2da94d..c76c71c08c 100644
--- a/libraries/APM_RC/APM_RC_APM2.cpp
+++ b/libraries/APM_RC/APM_RC_APM2.cpp
@@ -227,71 +227,22 @@ void APM_RC_APM2::Force_Out2_Out3(void) { }
 void APM_RC_APM2::Force_Out6_Out7(void) { }
 
 /* ---------------- OUTPUT SPEED CONTROL ------------------ */
-// Output rate options:
-#define OUTPUT_SPEED_50HZ 0
-#define OUTPUT_SPEED_200HZ 1
-#define OUTPUT_SPEED_490HZ 2
 
-void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask)
+void APM_RC_APM2::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz)
 {
-    if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0)
-        _set_speed_ch1_ch2(OUTPUT_SPEED_490HZ);
+	uint16_t icr = _map_speed(speed_hz);
 
-    if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0)
-        _set_speed_ch3_ch4_ch5(OUTPUT_SPEED_490HZ);
+	if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) {
+		ICR1 = icr;
+	}
 
-    if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0)
-        _set_speed_ch6_ch7_ch8(OUTPUT_SPEED_490HZ);
-}
-
-void APM_RC_APM2::_set_speed_ch1_ch2(uint8_t speed)
-{
-  switch(speed) {
-  case OUTPUT_SPEED_490HZ:
-    ICR1 = 4096;
-    break;
-  case OUTPUT_SPEED_200HZ:
-    ICR1 = 10000;
-    break;
-  case OUTPUT_SPEED_50HZ:
-  default:
-    ICR1 = 40000;
-    break;
-  }
-}
-
-void APM_RC_APM2::_set_speed_ch3_ch4_ch5(uint8_t speed)
-{
-  switch(speed) {
-  case OUTPUT_SPEED_490HZ:
-    ICR4 = 4096;
-    break;
-  case OUTPUT_SPEED_200HZ:
-    ICR4 = 10000;
-    break;
-  case OUTPUT_SPEED_50HZ:
-  default:
-    ICR4 = 40000;
-    break;
-  }
-
-}
-
-void APM_RC_APM2::_set_speed_ch6_ch7_ch8(uint8_t speed)
-{
-  switch(speed) {
-  case OUTPUT_SPEED_490HZ:
-    ICR3 = 4096;
-    break;
-  case OUTPUT_SPEED_200HZ:
-    ICR3 = 10000;
-    break;
-  case OUTPUT_SPEED_50HZ:
-  default:
-    ICR3 = 40000;
-    break;
-  }
+	if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) {
+		ICR4 = icr;
+	}
 
+	if ((chmask & ( _BV(CH_6) | _BV(CH_7) | _BV(CH_8))) != 0) {
+		ICR3 = icr;
+	}
 }
 
 // allow HIL override of RC values
diff --git a/libraries/APM_RC/APM_RC_APM2.h b/libraries/APM_RC/APM_RC_APM2.h
index 17ca5503dc..f2a93a1f07 100644
--- a/libraries/APM_RC/APM_RC_APM2.h
+++ b/libraries/APM_RC/APM_RC_APM2.h
@@ -19,8 +19,8 @@ class APM_RC_APM2 : public APM_RC_Class
 	unsigned char GetState();
 	bool setHIL(int16_t v[NUM_CHANNELS]);
 	void clearOverride(void);
-    void Force_Out(void);
-    void SetFastOutputChannels(uint32_t chmask);
+	void Force_Out(void);
+	void SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz = 400);
 
   void enable_out(uint8_t);
   void disable_out(uint8_t);
@@ -35,10 +35,6 @@ class APM_RC_APM2 : public APM_RC_Class
     static volatile uint16_t _PWM_RAW[NUM_CHANNELS];
     static volatile uint8_t _radio_status;
     int16_t _HIL_override[NUM_CHANNELS];
-
-    void _set_speed_ch1_ch2(uint8_t speed);
-    void _set_speed_ch3_ch4_ch5(uint8_t speed);
-    void _set_speed_ch6_ch7_ch8(uint8_t speed);
 };