diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp index 20edb60347..536605434f 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.cpp +++ b/libraries/AP_RcChannel/AP_RcChannel.cpp @@ -14,7 +14,7 @@ #include "AP_RcChannel.h" #include -AP_RcChannel::AP_RcChannel(AP_Var::Key & key, const prog_char * name, APM_RC_Class & rc, const uint8_t & ch, +AP_RcChannel::AP_RcChannel(AP_Var::Key key, const prog_char * name, APM_RC_Class & rc, const uint8_t & ch, const float & scale, const float & center, const uint16_t & pwmMin, const uint16_t & pwmNeutral, const uint16_t & pwmMax, @@ -23,8 +23,8 @@ AP_RcChannel::AP_RcChannel(AP_Var::Key & key, const prog_char * name, APM_RC_Cla AP_Var_group(key,name), _rc(rc), ch(this,0,ch,PSTR("CH")), - scale(this,1,ch,PSTR("SCALE")), - center(this,2,ch,PSTR("CENTER")), + scale(this,1,scale,PSTR("SCALE")), + center(this,2,center,PSTR("CENTER")), pwmMin(this,3,pwmMin,PSTR("PMIN")), pwmMax(this,4,pwmMax,PSTR("PMAX")), pwmNeutral(this,5,pwmNeutral,PSTR("PNTRL")), diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/AP_RcChannel/AP_RcChannel.h index 23c972496c..de8dfe3cab 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ b/libraries/AP_RcChannel/AP_RcChannel.h @@ -18,7 +18,7 @@ class AP_RcChannel : public AP_Var_group { public: /// Constructor - AP_RcChannel(AP_Var::Key & key, const prog_char * name, APM_RC_Class & rc, const uint8_t & ch, + AP_RcChannel(AP_Var::Key key, const prog_char * name, APM_RC_Class & rc, const uint8_t & ch, const float & scale=45.0, const float & center=0.0, const uint16_t & pwmMin=1200, const uint16_t & pwmNeutral=1500, const uint16_t & pwmMax=1800, @@ -47,7 +47,6 @@ public: uint16_t getPwm() { return _pwm; } float getPosition() { return _pwmToPosition(_pwm); } float getNormalized() { return getPosition()/scale; } - const char * getName() { return _name; } // did our read come in 50µs below the min? bool failSafe() { _pwm < (pwmMin - 50); } diff --git a/libraries/AP_RcChannel/examples/Manual/Manual.pde b/libraries/AP_RcChannel/examples/Manual/Manual.pde index 72e0dc7761..0649b1f643 100644 --- a/libraries/AP_RcChannel/examples/Manual/Manual.pde +++ b/libraries/AP_RcChannel/examples/Manual/Manual.pde @@ -9,61 +9,86 @@ #include #include // ArduPilot Mega RC Library #include +#include -FastSerialPort0(Serial); // make sure this procees variable declarations +FastSerialPort0(Serial); // make sure this proceeds variable declarations // test settings -uint8_t nChannels = 8; -// channel configuration -AP_RcChannel rcCh[] = +class RadioTest { - AP_RcChannel("ROLL",APM_RC,0,45), - 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) +private: + float testPosition; + int8_t testSign; + enum + { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; +public: + RadioTest() : testPosition(2), testSign(1) + { + ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45)); + ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45)); + ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100)); + ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45)); + ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1)); + ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1)); + ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1)); + ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); + + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } + + void update() + { + // read the radio + for (int i=0;ireadRadio(); + + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (int i=0;icopy_name(name,7); + Serial.printf("%7s\t",name); + } + Serial.println(); + + // print pwm + Serial.printf("pwm :\t"); + for (int i=0;igetPwm()); + Serial.println(); + + // print position + Serial.printf("position :\t"); + for (int i=0;igetPosition()); + Serial.println(); + + delay(500); + } }; -// test position -float testPosition = 2; -int8_t testSign = 1; +RadioTest * test; void setup() { - Serial.begin(115200); - delay(2000); - - Serial.println("ArduPilot RC Channel test"); - - APM_RC.Init(); // APM Radio initialization - - delay(2000); - //eepromRegistry.print(Serial); // show eeprom map + test = new RadioTest; } void loop() { - // read the radio - for (int i=0;iupdate(); } diff --git a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde index 2a7a361aa8..2d5adef894 100644 --- a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde +++ b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde @@ -9,77 +9,103 @@ #include #include // ArduPilot Mega RC Library #include +#include -FastSerialPort0(Serial); // make sure this procees variable declarations +FastSerialPort0(Serial); // make sure this proceeds variable declarations // test settings -uint8_t nChannels = 8; -// channel configuration -AP_RcChannel rcCh[] = +class RadioTest { - AP_RcChannel("ROLL",APM_RC,0,45), - 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) +private: + float testPosition; + int8_t testSign; + enum + { + version, + rollKey, + pitchKey, + thrKey, + yawKey, + ch5Key, + ch6Key, + ch7Key, + ch8Key + }; + Vector ch; +public: + RadioTest() : testPosition(2), testSign(1) + { + ch.push_back(new AP_RcChannel(rollKey,PSTR("ROLL"),APM_RC,1,45)); + ch.push_back(new AP_RcChannel(pitchKey,PSTR("PITCH"),APM_RC,2,45)); + ch.push_back(new AP_RcChannel(thrKey,PSTR("THR"),APM_RC,3,100)); + ch.push_back(new AP_RcChannel(yawKey,PSTR("YAW"),APM_RC,4,45)); + ch.push_back(new AP_RcChannel(ch5Key,PSTR("CH5"),APM_RC,5,1)); + ch.push_back(new AP_RcChannel(ch6Key,PSTR("CH6"),APM_RC,6,1)); + ch.push_back(new AP_RcChannel(ch7Key,PSTR("CH7"),APM_RC,7,1)); + ch.push_back(new AP_RcChannel(ch8Key,PSTR("CH8"),APM_RC,8,1)); + + Serial.begin(115200); + delay(2000); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + delay(2000); + } + + void update() + { + // 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;isetNormalized(testPosition); + + // print test position + Serial.printf("\nnormalized position (%f)\n",testPosition); + + // print channel names + Serial.print("\t\t"); + static char name[7]; + for (int i=0;icopy_name(name,7); + Serial.printf("%7s\t",name); + } + Serial.println(); + + // print pwm + Serial.printf("pwm :\t"); + for (int i=0;igetPwm()); + Serial.println(); + + // print position + Serial.printf("position :\t"); + for (int i=0;igetPosition()); + Serial.println(); + + delay(500); + } }; -// test position -float testPosition = 2; -int8_t testSign = 1; +RadioTest * test; void setup() { - Serial.begin(115200); - delay(2000); - - Serial.println("ArduPilot RC Channel test"); - - APM_RC.Init(); // APM Radio initialization - - delay(2000); + test = new RadioTest; } 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;iupdate(); }