diff --git a/libraries/AP_RcChannel/examples/Manual/Manual.pde b/libraries/AP_RcChannel/examples/Manual/Manual.pde index 85ae2143b1..49878253a4 100644 --- a/libraries/AP_RcChannel/examples/Manual/Manual.pde +++ b/libraries/AP_RcChannel/examples/Manual/Manual.pde @@ -15,90 +15,56 @@ FastSerialPort0(Serial); // make sure this procees variable declarations // test settings uint8_t nChannels = 8; -bool loadEEProm = false; -bool saveEEProm = false; // channel configuration -Vector< AP_EEPromVar * > scale; -Vector< AP_EEPromVar * > pwmMin; -Vector< AP_EEPromVar * > pwmNeutral; -Vector< AP_EEPromVar * > pwmMax; -Vector< AP_EEPromVar * > pwmDeadZone; -Vector< AP_Var * > filter; -Vector< AP_Var * > reverse; -Vector< AP_RcChannel * > rc; +AP_RcChannel rcCh[] = +{ + AP_RcChannel("ROLL",APM_RC,0,100.0), + AP_RcChannel("PITCH",APM_RC,1,45), + AP_RcChannel("THR",APM_RC,2,100), + AP_RcChannel("YAW",APM_RC,3,45), + AP_RcChannel("CH5",APM_RC,4,1), + AP_RcChannel("CH6",APM_RC,5,1), + AP_RcChannel("CH7",APM_RC,6,1), + AP_RcChannel("CH8",APM_RC,7,1) +}; + +// test position +float testPosition = 2; +int8_t testSign = 1; void setup() { Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization - // add channels - for (int i=0;i(1.0,strcat("SCALE",num))); - pwmMin.push_back(new AP_EEPromVar(1200,strcat("PWM_MIN",num))); - pwmNeutral.push_back(new AP_EEPromVar(1500,strcat("PWM_NEUTRAL",num))); - pwmMax.push_back(new AP_EEPromVar(1800,strcat("PWM_MAX",num))); - pwmDeadZone.push_back(new AP_EEPromVar(10,strcat("PWM_DEADZONE",num))); - filter.push_back(new AP_EEPromVar(false,strcat("FILTER",num))); - reverse.push_back(new AP_EEPromVar(false,strcat("REVERSE",num))); - - // save - if (saveEEProm) - { - scale[i]->save(); - pwmMin[i]->save(); - pwmNeutral[i]->save(); - pwmMax[i]->save(); - pwmDeadZone[i]->save(); - filter[i]->save(); - reverse[i]->save(); - } - - // load - if (loadEEProm) - { - scale[i]->load(); - pwmMin[i]->load(); - pwmNeutral[i]->load(); - pwmMax[i]->load(); - pwmDeadZone[i]->load(); - filter[i]->load(); - reverse[i]->load(); - } - - // find neutral position - AP_RcChannel * ch = new AP_RcChannel(APM_RC,i,scale[i]->get(), - pwmMin[i]->get(),pwmNeutral[i]->get(),pwmMax[i]->get(), - pwmDeadZone[i]->get(),filter[i]->get(),reverse[i]->get()); - - ch->readRadio(); - pwmNeutral[i]->set(ch->getPwm()); - - // add rc channel - rc.push_back(ch); - } + delay(2000); + eepromRegistry.print(Serial); // show eeprom map } void loop() { - // read radio - for (int i=0;ireadRadio(); - - // read test positions - Serial.printf("\npwm :\t"); - for (int i=0;igetPwm()); + // read the radio + for (int i=0;igetPosition()); + for (int i=0;i #include #include // ArduPilot Mega RC Library @@ -31,28 +30,45 @@ AP_RcChannel rcCh[] = }; // test position -float testPosition = 0; +float testPosition = 2; int8_t testSign = 1; void setup() { Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); APM_RC.Init(); // APM Radio initialization - eepromRegistry.print(Serial); // show eeprom map + delay(2000); } void loop() { + // update test value + testPosition += testSign*.1; + if (testPosition > 1) + { + eepromRegistry.print(Serial); // show eeprom map + testPosition = 1; + testSign = -1; + } + else if (testPosition < -1) + { + testPosition = -1; + testSign = 1; + } + // set channel positions for (int i=0;i 1) - { - testPosition = 1; - testSign = -1; - } - else if (testPosition < -1) - { - testPosition = -1; - testSign = 1; - } - - delay(100); + delay(500); } diff --git a/libraries/AP_RcChannel/examples/Usage/Usage.pde b/libraries/AP_RcChannel/examples/Usage/Usage.pde deleted file mode 100644 index 4e6b5c2eeb..0000000000 --- a/libraries/AP_RcChannel/examples/Usage/Usage.pde +++ /dev/null @@ -1,84 +0,0 @@ -/* - Example of RC_Channel library. - Code by James Goppert/ Jason Short. 2010 - DIYDrones.com - -*/ - -#include -#include -#include // ArduPilot Mega RC Library -#include -#include - -FastSerialPort0(Serial); // make sure this proceed variable declarations - -AP_EEPromVar scale(45.0,"RC1_SCALE"); -AP_EEPromVar pwmMin(1000,"RC1_PWMMIN"); -AP_EEPromVar pwmNeutral(1500,"RC1_PWMNEUTRAL"); -AP_EEPromVar pwmMax(2000,"RC1_PWMMAX"); -AP_EEPromVar pwmDeadZone(100,"RC1_PWMDEADZONE"); - -#define CH_1 0 - -// configuration -AP_Var filter(false,"RC1_FILTER"); -AP_Var reverse(false,"RC1_REVERSE"); - -float testPosition = 0; -uint16_t testPwm = 1500; - -AP_RcChannel rc[] = -{ - AP_RcChannel(APM_RC,CH_1,scale.get(),pwmMin.get(),pwmNeutral.get(),pwmMax.get(), - pwmDeadZone.get(),filter.get(),reverse.get()) - -}; - -void setup() -{ - Serial.begin(115200); - Serial.println("ArduPilot RC Channel test"); - APM_RC.Init(); // APM Radio initialization - - delay(1000); - - // configuratoin - scale.set(100); - Serial.printf("\nscale.set(100)\n"); - delay(2000); - - // find neutral radio position - rc[CH_1].readRadio(); - Serial.printf("\nrc[CH_1].readRadio()\n"); - Serial.printf("\npwmNeutral.set(rc[CH_1].getPwm())\n"); - pwmNeutral.set(rc[CH_1].getPwm()); - delay(2000); -} - -void loop() -{ - // get the min pwm - Serial.printf("\npwmMin.get(): %d\n", pwmMin.get()); - delay(2000); - - // set by pwm - rc[CH_1].setPwm(testPwm); - Serial.printf("\nrc[CH_1].setPwm(1500)\n"); - Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(), - rc[CH_1].getPosition()); - delay(2000); - - // set by position - rc[CH_1].setPosition(testPosition); - Serial.printf("\nrc[CH_1].setPosition(-50))\n"); - Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(), - rc[CH_1].getPosition()); - delay(2000); - - // update test value - testPosition += 10; - if (testPosition > 100) testPosition = -100; - testPwm += 100; - if (testPwm > 1800) testPosition = 1200; -}