From 63ea5dfb49d3ea5f0221630a25cd2261bf58ff0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Mar 2012 17:23:22 +1100 Subject: [PATCH] APM_RC: allow the fast RC speed to be passed as a parameter this will allow users to test different speeds --- libraries/APM_RC/APM_RC.h | 11 ++-- libraries/APM_RC/APM_RC_APM1.cpp | 94 ++++++++------------------------ libraries/APM_RC/APM_RC_APM1.h | 14 ++--- libraries/APM_RC/APM_RC_APM2.cpp | 71 ++++-------------------- libraries/APM_RC/APM_RC_APM2.h | 8 +-- 5 files changed, 46 insertions(+), 152 deletions(-) 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); };