mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
RC_Channel: simplify the example
use an array of channels
This commit is contained in:
parent
bad02cab04
commit
dac569a393
@ -60,6 +60,7 @@ RC_Channel rc_5(CH_5);
|
|||||||
RC_Channel rc_6(CH_6);
|
RC_Channel rc_6(CH_6);
|
||||||
RC_Channel rc_7(CH_7);
|
RC_Channel rc_7(CH_7);
|
||||||
RC_Channel rc_8(CH_8);
|
RC_Channel rc_8(CH_8);
|
||||||
|
RC_Channel *rc = &rc_1;
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
{
|
{
|
||||||
@ -72,33 +73,6 @@ void setup()
|
|||||||
|
|
||||||
// setup radio
|
// setup radio
|
||||||
|
|
||||||
// read eepom or set manually
|
|
||||||
/*
|
|
||||||
* rc_1.radio_min = 1100;
|
|
||||||
* rc_1.radio_max = 1900;
|
|
||||||
*
|
|
||||||
* rc_2.radio_min = 1100;
|
|
||||||
* rc_2.radio_max = 1900;
|
|
||||||
*
|
|
||||||
* rc_3.radio_min = 1100;
|
|
||||||
* rc_3.radio_max = 1900;
|
|
||||||
*
|
|
||||||
* rc_4.radio_min = 1100;
|
|
||||||
* rc_4.radio_max = 1900;
|
|
||||||
*
|
|
||||||
* // or
|
|
||||||
*
|
|
||||||
* rc_1.load_eeprom();
|
|
||||||
* rc_2.load_eeprom();
|
|
||||||
* rc_3.load_eeprom();
|
|
||||||
* rc_4.load_eeprom();
|
|
||||||
* rc_5.load_eeprom();
|
|
||||||
* rc_6.load_eeprom();
|
|
||||||
* rc_7.load_eeprom();
|
|
||||||
* rc_8.load_eeprom();
|
|
||||||
*
|
|
||||||
*/
|
|
||||||
|
|
||||||
// interactive setup
|
// interactive setup
|
||||||
setup_radio();
|
setup_radio();
|
||||||
|
|
||||||
@ -120,6 +94,12 @@ void setup()
|
|||||||
rc_7.set_range(0,1000);
|
rc_7.set_range(0,1000);
|
||||||
rc_8.set_range(0,1000);
|
rc_8.set_range(0,1000);
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
for (byte i=0; i<8; i++) {
|
||||||
|
rc[i].set_reverse(false);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
for (byte i = 0; i < 30; i++) {
|
for (byte i = 0; i < 30; i++) {
|
||||||
read_radio();
|
read_radio();
|
||||||
}
|
}
|
||||||
@ -138,79 +118,28 @@ void loop()
|
|||||||
|
|
||||||
void read_radio()
|
void read_radio()
|
||||||
{
|
{
|
||||||
rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
for (byte i=0; i<8; i++) {
|
||||||
rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
rc[i].set_pwm(APM_RC.InputCh(CH_1+i));
|
||||||
rc_3.set_pwm(APM_RC.InputCh(CH_3));
|
}
|
||||||
rc_4.set_pwm(APM_RC.InputCh(CH_4));
|
|
||||||
rc_5.set_pwm(APM_RC.InputCh(CH_5));
|
|
||||||
rc_6.set_pwm(APM_RC.InputCh(CH_6));
|
|
||||||
rc_7.set_pwm(APM_RC.InputCh(CH_7));
|
|
||||||
rc_8.set_pwm(APM_RC.InputCh(CH_8));
|
|
||||||
//Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), rc_1.control_in, rc_2.control_in, rc_3.control_in, rc_4.control_in);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void print_pwm()
|
void print_pwm()
|
||||||
{
|
{
|
||||||
Serial.print("ch1 ");
|
for (byte i=0; i<8; i++) {
|
||||||
Serial.print(rc_1.control_in, DEC);
|
Serial.printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].control_in);
|
||||||
Serial.print("\tch2: ");
|
}
|
||||||
Serial.print(rc_2.control_in, DEC);
|
Serial.printf("\n");
|
||||||
Serial.print("\tch3 :");
|
|
||||||
Serial.print(rc_3.control_in, DEC);
|
|
||||||
Serial.print("\tch4 :");
|
|
||||||
Serial.print(rc_4.control_in, DEC);
|
|
||||||
Serial.print("\tch5 :");
|
|
||||||
Serial.print(rc_5.control_in, DEC);
|
|
||||||
Serial.print("\tch6 :");
|
|
||||||
Serial.print(rc_6.control_in, DEC);
|
|
||||||
Serial.print("\tch7 :");
|
|
||||||
Serial.print(rc_7.control_in, DEC);
|
|
||||||
Serial.print("\tch8 :");
|
|
||||||
Serial.println(rc_8.control_in, DEC);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
print_radio_values()
|
print_radio_values()
|
||||||
{
|
{
|
||||||
Serial.print("CH1: ");
|
for (byte i=0; i<8; i++) {
|
||||||
Serial.print(rc_1.radio_min, DEC);
|
Serial.printf("CH%u: %u|%u\n",
|
||||||
Serial.print(" | ");
|
(unsigned)i+1,
|
||||||
Serial.println(rc_1.radio_max, DEC);
|
(unsigned)rc[i].radio_min,
|
||||||
|
(unsigned)rc[i].radio_max);
|
||||||
Serial.print("CH2: ");
|
}
|
||||||
Serial.print(rc_2.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_2.radio_max, DEC);
|
|
||||||
|
|
||||||
Serial.print("CH3: ");
|
|
||||||
Serial.print(rc_3.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_3.radio_max, DEC);
|
|
||||||
|
|
||||||
Serial.print("CH4: ");
|
|
||||||
Serial.print(rc_4.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_4.radio_max, DEC);
|
|
||||||
|
|
||||||
Serial.print("CH5: ");
|
|
||||||
Serial.print(rc_5.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_5.radio_max, DEC);
|
|
||||||
|
|
||||||
Serial.print("CH6: ");
|
|
||||||
Serial.print(rc_6.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_6.radio_max, DEC);
|
|
||||||
|
|
||||||
Serial.print("CH7: ");
|
|
||||||
Serial.print(rc_7.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_7.radio_max, DEC);
|
|
||||||
|
|
||||||
Serial.print("CH8: ");
|
|
||||||
Serial.print(rc_8.radio_min, DEC);
|
|
||||||
Serial.print(" | ");
|
|
||||||
Serial.println(rc_8.radio_max, DEC);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@ -218,30 +147,16 @@ void
|
|||||||
setup_radio()
|
setup_radio()
|
||||||
{
|
{
|
||||||
Serial.println("\n\nRadio Setup:");
|
Serial.println("\n\nRadio Setup:");
|
||||||
uint8_t i;
|
|
||||||
|
|
||||||
for(i = 0; i < 100; i++) {
|
for (byte i = 0; i < 100; i++) {
|
||||||
delay(20);
|
delay(20);
|
||||||
read_radio();
|
read_radio();
|
||||||
}
|
}
|
||||||
|
|
||||||
rc_1.radio_min = rc_1.radio_in;
|
for (byte i=0; i<8; i++) {
|
||||||
rc_2.radio_min = rc_2.radio_in;
|
rc[i].radio_min = rc[i].radio_in;
|
||||||
rc_3.radio_min = rc_3.radio_in;
|
rc[i].radio_max = rc[i].radio_in;
|
||||||
rc_4.radio_min = rc_4.radio_in;
|
}
|
||||||
rc_5.radio_min = rc_5.radio_in;
|
|
||||||
rc_6.radio_min = rc_6.radio_in;
|
|
||||||
rc_7.radio_min = rc_7.radio_in;
|
|
||||||
rc_8.radio_min = rc_8.radio_in;
|
|
||||||
|
|
||||||
rc_1.radio_max = rc_1.radio_in;
|
|
||||||
rc_2.radio_max = rc_2.radio_in;
|
|
||||||
rc_3.radio_max = rc_3.radio_in;
|
|
||||||
rc_4.radio_max = rc_4.radio_in;
|
|
||||||
rc_5.radio_max = rc_5.radio_in;
|
|
||||||
rc_6.radio_max = rc_6.radio_in;
|
|
||||||
rc_7.radio_max = rc_7.radio_in;
|
|
||||||
rc_8.radio_max = rc_8.radio_in;
|
|
||||||
|
|
||||||
rc_1.radio_trim = rc_1.radio_in;
|
rc_1.radio_trim = rc_1.radio_in;
|
||||||
rc_2.radio_trim = rc_2.radio_in;
|
rc_2.radio_trim = rc_2.radio_in;
|
||||||
@ -260,14 +175,9 @@ setup_radio()
|
|||||||
// ----------------------------------------------------------
|
// ----------------------------------------------------------
|
||||||
read_radio();
|
read_radio();
|
||||||
|
|
||||||
rc_1.update_min_max();
|
for (byte i=0; i<8; i++) {
|
||||||
rc_2.update_min_max();
|
rc[i].update_min_max();
|
||||||
rc_3.update_min_max();
|
}
|
||||||
rc_4.update_min_max();
|
|
||||||
rc_5.update_min_max();
|
|
||||||
rc_6.update_min_max();
|
|
||||||
rc_7.update_min_max();
|
|
||||||
rc_8.update_min_max();
|
|
||||||
|
|
||||||
if(Serial.available() > 0) {
|
if(Serial.available() > 0) {
|
||||||
//rc_3.radio_max += 250;
|
//rc_3.radio_max += 250;
|
||||||
|
Loading…
Reference in New Issue
Block a user