Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM_RC: allow the fast RC speed to be passed as a parameter 

this will allow users to test different speeds
This commit is contained in:
Andrew Tridgell 2012-03-02 17:23:22 +11:00
parent f781bd735b
commit 5f749325f0
5 changed files with 46 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/APM_RC/APM_RC.h
View File

@ -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; }
};

94
libraries/APM_RC/APM_RC_APM1.cpp
View File

@ -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

14
libraries/APM_RC/APM_RC_APM1.h
View File

@ -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

71
libraries/APM_RC/APM_RC_APM2.cpp
View File

@ -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

8
libraries/APM_RC/APM_RC_APM2.h
View File

@ -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);
};