From 56b0be3124a7e8bbce7d87a2064f8d07249283f3 Mon Sep 17 00:00:00 2001
From: Pat Hickey <pat@moreproductive.org>
Date: Sat, 10 Dec 2011 13:06:06 -0800
Subject: [PATCH] APM_RC: APM1 fast output speed increased to 400hz.

---
 libraries/APM_RC/APM_RC_APM1.cpp | 18 ++++++++++++++----
 1 file changed, 14 insertions(+), 4 deletions(-)

diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp
index 9339ad3a0e..b2aa1464f1 100644
--- a/libraries/APM_RC/APM_RC_APM1.cpp
+++ b/libraries/APM_RC/APM_RC_APM1.cpp
@@ -212,26 +212,30 @@ void APM_RC_APM1::Force_Out6_Out7(void)
 // Output rate options:
 #define OUTPUT_SPEED_50HZ 0
 #define OUTPUT_SPEED_200HZ 1
+#define OUTPUT_SPEED_400HZ 2
 
 void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask)
 {
     if ((chmask & ( MSK_CH_1 | MSK_CH_2 | MSK_CH_9)) != 0)
-        _set_speed_ch1_ch2_ch9(OUTPUT_SPEED_200HZ);
+        _set_speed_ch1_ch2_ch9(OUTPUT_SPEED_400HZ);
 
     if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_10 )) != 0)
-        _set_speed_ch3_ch4_ch10(OUTPUT_SPEED_200HZ);
+        _set_speed_ch3_ch4_ch10(OUTPUT_SPEED_400HZ);
 
     if ((chmask & ( MSK_CH_5 | MSK_CH_6 )) != 0)
-        _set_speed_ch5_ch6(OUTPUT_SPEED_200HZ);
+        _set_speed_ch5_ch6(OUTPUT_SPEED_400HZ);
 
     if ((chmask & ( MSK_CH_7 | MSK_CH_8 | MSK_CH_11 )) != 0)
-        _set_speed_ch7_ch8_ch11(OUTPUT_SPEED_200HZ);
+        _set_speed_ch7_ch8_ch11(OUTPUT_SPEED_400HZ);
 
 }
 
 void APM_RC_APM1::_set_speed_ch1_ch2_ch9(uint8_t speed)
 {
   switch(speed) {
+  case OUTPUT_SPEED_400HZ:
+    ICR1= 5000;
+    break;
   case OUTPUT_SPEED_200HZ:
     ICR1= 10000;
     break;
@@ -245,6 +249,9 @@ void APM_RC_APM1::_set_speed_ch1_ch2_ch9(uint8_t speed)
 void APM_RC_APM1::_set_speed_ch3_ch4_ch10(uint8_t speed)
 {
   switch(speed) {
+  case OUTPUT_SPEED_400HZ:
+    ICR5= 5000;
+    break;
   case OUTPUT_SPEED_200HZ:
     ICR5= 10000;
     break;
@@ -258,6 +265,9 @@ void APM_RC_APM1::_set_speed_ch3_ch4_ch10(uint8_t speed)
 void APM_RC_APM1::_set_speed_ch7_ch8_ch11(uint8_t speed)
 {
   switch(speed) {
+  case OUTPUT_SPEED_400HZ:
+    ICR3 = 5000;
+    break;
   case OUTPUT_SPEED_200HZ:
     ICR3 = 10000;
     break;