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

5
libraries/APM_RC/APM_RC.h
View File

@ -40,10 +40,13 @@ class APM_RC_Class
virtual uint8_t GetState() = 0; virtual uint8_t GetState() = 0;
virtual void clearOverride(void) = 0; virtual void clearOverride(void) = 0;
virtual void Force_Out() = 0; virtual void Force_Out() = 0;
virtual void SetFastOutputChannels( uint32_t channelmask ) = 0; virtual void SetFastOutputChannels( uint32_t channelmask, uint16_t speed_hz = 400 ) = 0;
virtual void enable_out(uint8_t) = 0; virtual void enable_out(uint8_t) = 0;
virtual void disable_out(uint8_t) = 0; virtual void disable_out(uint8_t) = 0;
protected:
uint16_t _map_speed(uint16_t speed_hz) { return 2000000UL / speed_hz; }
}; };

82
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 SPEED CONTROL --------------------- */
// Output rate options: void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask, uint16_t speed_hz)
#define OUTPUT_SPEED_50HZ 0
#define OUTPUT_SPEED_200HZ 1
#define OUTPUT_SPEED_490HZ 2
void APM_RC_APM1::SetFastOutputChannels(uint32_t chmask)
{ {
if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) uint16_t icr = _map_speed(speed_hz);
_set_speed_ch1_ch2_ch9(OUTPUT_SPEED_490HZ);
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) if ((chmask & ( _BV(CH_1) | _BV(CH_2) | _BV(CH_9))) != 0) {
_set_speed_ch3_ch4_ch10(OUTPUT_SPEED_490HZ); ICR1 = icr;
if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0)
_set_speed_ch5_ch6(OUTPUT_SPEED_490HZ);
if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0)
_set_speed_ch7_ch8_ch11(OUTPUT_SPEED_490HZ);
}
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) if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_10))) != 0) {
{ ICR5 = icr;
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) #if 0
{ if ((chmask & ( _BV(CH_5) | _BV(CH_6))) != 0) {
switch(speed) { /* These channels intentionally left blank:
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). * Can't change output speed of ch5 (OCR4B) and ch6 (OCR4C).
* Timer 4 period controlled by OCR4A, and used for input * Timer 4 period controlled by OCR4A, and used for input
* capture on ICR4. * capture on ICR4.
* If the period of Timer 4 must be changed, the input capture * If the period of Timer 4 must be changed, the input capture
* code will have to be adjusted as well * code will have to be adjusted as well
*/ */
}
#endif
if ((chmask & ( _BV(CH_7) | _BV(CH_8) | _BV(CH_11))) != 0) {
ICR3 = icr;
}
} }
// allow HIL override of RC values // allow HIL override of RC values

8
libraries/APM_RC/APM_RC_APM1.h
View File

@ -18,7 +18,7 @@ class APM_RC_APM1 : public APM_RC_Class
bool setHIL(int16_t v[NUM_CHANNELS]); bool setHIL(int16_t v[NUM_CHANNELS]);
void clearOverride(void); void clearOverride(void);
void Force_Out(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 enable_out(uint8_t);
void disable_out(uint8_t); void disable_out(uint8_t);
@ -35,12 +35,6 @@ class APM_RC_APM1 : public APM_RC_Class
static volatile uint8_t _radio_status; static volatile uint8_t _radio_status;
int16_t _HIL_override[NUM_CHANNELS]; 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 #endif

67
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) { } void APM_RC_APM2::Force_Out6_Out7(void) { }
/* ---------------- OUTPUT SPEED CONTROL ------------------ */ /* ---------------- 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) uint16_t icr = _map_speed(speed_hz);
_set_speed_ch1_ch2(OUTPUT_SPEED_490HZ);
if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) if ((chmask & ( _BV(CH_1) | _BV(CH_2))) != 0) {
_set_speed_ch3_ch4_ch5(OUTPUT_SPEED_490HZ); 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;
} }
} if ((chmask & ( _BV(CH_3) | _BV(CH_4) | _BV(CH_5))) != 0) {
ICR4 = icr;
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_6) | _BV(CH_7) | _BV(CH_8))) != 0) {
ICR3 = icr;
}
} }
// allow HIL override of RC values // allow HIL override of RC values

6
libraries/APM_RC/APM_RC_APM2.h
View File

@ -20,7 +20,7 @@ class APM_RC_APM2 : public APM_RC_Class
bool setHIL(int16_t v[NUM_CHANNELS]); bool setHIL(int16_t v[NUM_CHANNELS]);
void clearOverride(void); void clearOverride(void);
void Force_Out(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 enable_out(uint8_t);
void disable_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 uint16_t _PWM_RAW[NUM_CHANNELS];
static volatile uint8_t _radio_status; static volatile uint8_t _radio_status;
int16_t _HIL_override[NUM_CHANNELS]; 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);
}; };