APM_RC: _set_speed functions implemented for APM_RC_APM1 and _Purple

* I have not tested any of these on real hardware.

libraries/APM_RC/APM_RC.h

@@ -13,8 +13,23 @@
 #define CH_6 5
 #define CH_7 6
 #define CH_8 7
+#define CH_9 8
 #define CH_10 9
 #define CH_11 10
+
+#define MSK_CH_1 (1 << CH_1)
+#define MSK_CH_2 (1 << CH_2)
+#define MSK_CH_3 (1 << CH_3)
+#define MSK_CH_4 (1 << CH_4)
+#define MSK_CH_5 (1 << CH_5)
+#define MSK_CH_6 (1 << CH_6)
+#define MSK_CH_7 (1 << CH_7)
+#define MSK_CH_8 (1 << CH_8)
+#define MSK_CH_9 (1 << CH_9)
+#define MSK_CH_10 (1 << CH_10)
+#define MSK_CH_11 (1 << CH_11)
+
+
 
 #define NUM_CHANNELS 8
 
@@ -27,6 +42,7 @@ class APM_RC_Class
 	virtual uint8_t GetState() = 0;
 	virtual void clearOverride(void) = 0;
     virtual void Force_Out() = 0;
+    virtual void SetFastOutputChannels( uint32_t channelmask );
 };
 
 #include "APM_RC_APM1.h"

libraries/APM_RC/APM_RC_APM1.cpp

@@ -207,6 +207,78 @@ void APM_RC_APM1::Force_Out6_Out7(void)
     TCNT3=39990;
 }
 
+/* --------------------- OUTPUT SPEED CONTROL --------------------- */
+
+// Output rate options:
+#define OUTPUT_SPEED_50HZ 0
+#define OUTPUT_SPEED_200HZ 1
+
+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);
+
+    if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_10 )) != 0)
+        _set_speed_ch3_ch4_ch10(OUTPUT_SPEED_200HZ);
+
+    if ((chmask & ( MSK_CH_5 | MSK_CH_6 )) != 0)
+        _set_speed_ch5_ch6(OUTPUT_SPEED_200HZ);
+
+    if ((chmask & ( MSK_CH_7 | MSK_CH_8 | MSK_CH_11 )) != 0)
+        _set_speed_ch7_ch8_ch11(OUTPUT_SPEED_200HZ);
+
+}
+
+void APM_RC_APM1::_set_speed_ch1_ch2_ch9(uint8_t speed)
+{
+  switch(speed) {
+  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_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_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
 // A value of -1 means no change
 // A value of 0 means no override, use the real RC values

libraries/APM_RC/APM_RC_APM1.h

@@ -18,6 +18,7 @@ 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 Force_Out0_Out1(void);
 	void Force_Out2_Out3(void);
@@ -31,6 +32,12 @@ class APM_RC_APM1 : public APM_RC_Class
     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);
+
 };
 
 #endif

libraries/APM_RC/APM_RC_Purple.cpp

@@ -199,6 +199,64 @@ void APM_RC_Purple::Force_Out0_Out1(void) { }
 void APM_RC_Purple::Force_Out2_Out3(void) { }
 void APM_RC_Purple::Force_Out6_Out7(void) { }
 
+/* ---------------- OUTPUT SPEED CONTROL ------------------ */
+// Output rate options:
+#define OUTPUT_SPEED_50HZ 0
+#define OUTPUT_SPEED_200HZ 1
+
+void APM_RC_Purple::SetFastOutputChannels(uint32_t chmask)
+{
+    if ((chmask & ( MSK_CH_1 | MSK_CH_2 )) != 0)
+        _set_speed_ch1_ch2(OUTPUT_SPEED_200HZ);
+
+    if ((chmask & ( MSK_CH_3 | MSK_CH_4 | MSK_CH_5 )) != 0)
+        _set_speed_ch3_ch4_ch5(OUTPUT_SPEED_200HZ);
+
+    if ((chmask & ( MSK_CH_6 | MSK_CH_7 | MSK_CH_8 )) != 0)
+        _set_speed_ch6_ch7_ch8(OUTPUT_SPEED_200HZ);
+}
+
+void APM_RC_Purple::_set_speed_ch1_ch2(uint8_t speed)
+{
+  switch(speed) {
+  case OUTPUT_SPEED_200HZ:
+    ICR1 = 10000;
+    break;
+  case OUTPUT_SPEED_50HZ:
+  default:
+    ICR1 = 40000;
+    break;
+  }
+}
+
+void APM_RC_Purple::_set_speed_ch3_ch4_ch5(uint8_t speed)
+{
+  switch(speed) {
+  case OUTPUT_SPEED_200HZ:
+    ICR4 = 10000;
+    break;
+  case OUTPUT_SPEED_50HZ:
+  default:
+    ICR4 = 40000;
+    break;
+  }
+
+}
+
+void APM_RC_Purple::_set_speed_ch6_ch7_ch8(uint8_t speed)
+{
+  switch(speed) {
+  case OUTPUT_SPEED_200HZ:
+    ICR3 = 10000;
+    break;
+  case OUTPUT_SPEED_50HZ:
+  default:
+    ICR3 = 40000;
+    break;
+  }
+
+}
+
 // allow HIL override of RC values
 // A value of -1 means no change
 // A value of 0 means no override, use the real RC values

libraries/APM_RC/APM_RC_Purple.h

@@ -20,6 +20,8 @@ class APM_RC_Purple : public APM_RC_Class
 	bool setHIL(int16_t v[NUM_CHANNELS]);
 	void clearOverride(void);
     void Force_Out(void);
+    void SetFastOutputChannels(uint32_t chmask);
+
 	void Force_Out0_Out1(void);
 	void Force_Out2_Out3(void);
 	void Force_Out6_Out7(void);
@@ -30,6 +32,10 @@ class APM_RC_Purple : 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);
 };