diff --git a/libraries/APM_RC/APM_RC.h b/libraries/APM_RC/APM_RC.h index 65fb36425f..5fc1c740d4 100644 --- a/libraries/APM_RC/APM_RC.h +++ b/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" diff --git a/libraries/APM_RC/APM_RC_APM1.cpp b/libraries/APM_RC/APM_RC_APM1.cpp index 3f17efd7b2..9339ad3a0e 100644 --- a/libraries/APM_RC/APM_RC_APM1.cpp +++ b/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 diff --git a/libraries/APM_RC/APM_RC_APM1.h b/libraries/APM_RC/APM_RC_APM1.h index 831aed510a..7818e5e175 100644 --- a/libraries/APM_RC/APM_RC_APM1.h +++ b/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 diff --git a/libraries/APM_RC/APM_RC_Purple.cpp b/libraries/APM_RC/APM_RC_Purple.cpp index 0557f5d5ef..1a3eb4a36d 100644 --- a/libraries/APM_RC/APM_RC_Purple.cpp +++ b/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 diff --git a/libraries/APM_RC/APM_RC_Purple.h b/libraries/APM_RC/APM_RC_Purple.h index 6b005b0642..298dbf5f9a 100644 --- a/libraries/APM_RC/APM_RC_Purple.h +++ b/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); };