From d0540d7373c36f10d77b6edc2e47d7bf34b96e2f Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Wed, 29 Dec 2010 08:26:21 +0000 Subject: [PATCH] RcChannel now owns channel config params. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1354 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Common/AP_Loop.h | 9 +- libraries/AP_Common/AP_Var.h | 54 ++++++++- libraries/AP_EEProm/AP_EEProm.h | 10 ++ libraries/AP_RcChannel/AP_RcChannel.cpp | 38 +++--- libraries/AP_RcChannel/AP_RcChannel.h | 87 ++++++++++---- .../examples/ServoSweep/ServoSweep.pde | 113 +++++------------- 6 files changed, 181 insertions(+), 130 deletions(-) diff --git a/libraries/AP_Common/AP_Loop.h b/libraries/AP_Common/AP_Loop.h index 1d1252896c..b4c6792851 100644 --- a/libraries/AP_Common/AP_Loop.h +++ b/libraries/AP_Common/AP_Loop.h @@ -21,11 +21,10 @@ #include "AP_Vector.h" -/** - * Start of apo namespace - * The above functions must be in the global - * namespace for c++ to function properly - */ +/// +// Start of apo namespace +// The above functions must be in the global +// namespace for c++ to function properly namespace apo { diff --git a/libraries/AP_Common/AP_Var.h b/libraries/AP_Common/AP_Var.h index 32495c681b..5154ebca4c 100644 --- a/libraries/AP_Common/AP_Var.h +++ b/libraries/AP_Common/AP_Var.h @@ -5,9 +5,12 @@ // Free Software Foundation; either version 2.1 of the License, or (at // your option) any later version. // - /// The AP variable interface. This allows different types +/// The AP variable interface. This allows different types /// of variables to be passed to blocks for floating point /// math, memory management, etc. +#ifndef AP_Var_H +#define AP_Var_H + class AP_VarI { public: @@ -15,9 +18,15 @@ public: /// Set the variable value as a float virtual void setF(const float & val) = 0; - /// Get the variable as a float + /// Get the variable value as a float virtual const float & getF() = 0; + /// Set the variable value as an Int16 + virtual void setI(const int16_t & val) = 0; + + /// Get the variable value as an Int16 + virtual const int16_t & getI() = 0; + /// Save a variable to eeprom virtual void save() = 0; @@ -48,14 +57,20 @@ public: { } + /// float copy constructor + AP_Var(AP_VarI & v) + { + setF(v.getF()); + } + /// Set the variable value - virtual void set(const type & val) { + void set(const type & val) { _data = val; if (_sync) save(); } /// Get the variable value. - virtual const type & get() { + const type & get() { if (_sync) load(); return _data; } @@ -70,6 +85,26 @@ public: return get(); } + /// Set the variable value as an Int16 + virtual void setI(const int16_t & val) { + set(val); + } + + /// Get the variable value as an Int16 + virtual const int16_t & getI() { + return get(); + } + + /// Set the variable value as an Int16 + virtual void setB(const bool & val) { + set(val); + } + + /// Get the variable value as an Int16 + virtual const bool & getB() { + return get(); + } + /// Save a variable to eeprom virtual void save() { @@ -93,3 +128,14 @@ protected: const char * _name; /// The variable name, useful for gcs and terminal output bool _sync; /// Whether or not to call save/load on get/set }; + +typedef AP_Var AP_Float; +typedef AP_Var AP_Int8; +typedef AP_Var AP_Uint8; +typedef AP_Var AP_Int16; +typedef AP_Var AP_Uint16; +typedef AP_Var AP_Int32; +typedef AP_Var AP_Unt32; +typedef AP_Var AP_Bool; + +#endif // AP_Var_H diff --git a/libraries/AP_EEProm/AP_EEProm.h b/libraries/AP_EEProm/AP_EEProm.h index 4f9ade3b24..0bdca66e40 100644 --- a/libraries/AP_EEProm/AP_EEProm.h +++ b/libraries/AP_EEProm/AP_EEProm.h @@ -100,4 +100,14 @@ private: uint16_t _address; /// EEProm address of variable }; +typedef AP_EEPromVar AP_EEPROM_Float; +typedef AP_EEPromVar AP_EEPROM_Int8; +typedef AP_EEPromVar AP_EEPROM_Uint8; +typedef AP_EEPromVar AP_EEPROM_Int16; +typedef AP_EEPromVar AP_EEPROM_Uint16; +typedef AP_EEPromVar AP_EEPROM_Int32; +typedef AP_EEPromVar AP_EEPROM_Unt32; +typedef AP_EEPromVar AP_EEPROM_Bool; + + #endif diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp index 17a2a7ca34..216d70d0b6 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.cpp +++ b/libraries/AP_RcChannel/AP_RcChannel.cpp @@ -16,22 +16,22 @@ void AP_RcChannel::readRadio() { // apply reverse - uint16_t pwmRadio = APM_RC.InputCh(_ch); + uint16_t pwmRadio = APM_RC.InputCh(getCh()); setPwm(pwmRadio); } void AP_RcChannel::setPwm(uint16_t pwm) { - //Serial.printf("reverse: %s\n", (_reverse)?"true":"false"); + //Serial.printf("reverse: %s\n", (getReverse())?"true":"false"); // apply reverse - if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral; + if(getReverse()) pwm = int16_t(getPwmNeutral()-pwm) + getPwmNeutral(); //Serial.printf("pwm after reverse: %d\n", pwm); // apply filter - if(_filter){ + if(getFilter()){ if(_pwm == 0) _pwm = pwm; else @@ -43,10 +43,10 @@ AP_RcChannel::setPwm(uint16_t pwm) //Serial.printf("pwm after filter: %d\n", _pwm); // apply deadzone - _pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm; + _pwm = (abs(_pwm - getPwmNeutral()) < getPwmDeadZone()) ? getPwmNeutral() : _pwm; //Serial.printf("pwm after deadzone: %d\n", _pwm); - APM_RC.OutputCh(_ch,_pwm); + APM_RC.OutputCh(getCh(),_pwm); } void @@ -58,8 +58,8 @@ AP_RcChannel::setPosition(float position) void AP_RcChannel::mixRadio(uint16_t infStart) { - uint16_t pwmRadio = APM_RC.InputCh(_ch); - float inf = abs( int16_t(pwmRadio - _pwmNeutral) ); + uint16_t pwmRadio = APM_RC.InputCh(getCh()); + float inf = abs( int16_t(pwmRadio - getPwmNeutral()) ); inf = min(inf, infStart); inf = ((infStart - inf) /infStart); setPwm(_pwm*inf + pwmRadio); @@ -69,12 +69,15 @@ uint16_t AP_RcChannel::_positionToPwm(const float & position) { uint16_t pwm; + float p = position - getCenter(); //Serial.printf("position: %f\n", position); - if(position < 0) - pwm = position * int16_t(_pwmNeutral - _pwmMin) / _scale + _pwmNeutral; + if(p < 0) + pwm = p * int16_t(getPwmNeutral() - getPwmMin()) / + getScale() + getPwmNeutral(); else - pwm = position * int16_t(_pwmMax - _pwmNeutral) / _scale + _pwmNeutral; - constrain(pwm,_pwmMin,_pwmMax); + pwm = p * int16_t(getPwmMax() - getPwmNeutral()) / + getScale() + getPwmNeutral(); + constrain(pwm,getPwmMin(),getPwmMax()); return pwm; } @@ -82,11 +85,14 @@ float AP_RcChannel::_pwmToPosition(const uint16_t & pwm) { float position; - if(pwm < _pwmNeutral) - position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmNeutral - _pwmMin); + if(pwm < getPwmNeutral()) + position = getScale() * int16_t(pwm - getPwmNeutral())/ + int16_t(getPwmNeutral() - getPwmMin()) + getCenter(); else - position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmMax - _pwmNeutral); - constrain(position,-_scale,_scale); + position = getScale() * int16_t(pwm -getPwmNeutral())/ + int16_t(getPwmMax() - getPwmNeutral()) + getCenter(); + constrain(position,-getScale()+getCenter(), + getScale()+getCenter()); return position; } diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/AP_RcChannel/AP_RcChannel.h index 11741fb980..02d08bcd46 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ b/libraries/AP_RcChannel/AP_RcChannel.h @@ -9,6 +9,9 @@ #include #include #include +#include +#include +#include /// @class AP_RcChannel /// @brief Object managing one RC channel @@ -17,49 +20,83 @@ class AP_RcChannel{ public: /// Constructor - AP_RcChannel(const APM_RC_Class & rc, const uint16_t & ch, - const float & scale, const uint16_t & pwmMin, const uint16_t & pwmNeutral, - const uint16_t & pwmMax, const uint16_t & pwmDeadZone, - const bool & filter, const bool & reverse) : + AP_RcChannel(const char * name, const 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, + const uint16_t & pwmDeadZone=100, + const bool & filter=false, const bool & reverse=false) : + _name(name), _rc(rc), - _ch(ch), - _scale(scale), - _pwmMin(pwmMin), - _pwmMax(pwmMax), - _pwmNeutral(pwmNeutral), - _pwmDeadZone(pwmDeadZone), + _ch(new AP_EEPROM_Uint8(ch,createName("CH"))), + _scale(new AP_EEPROM_Float(scale,createName("SCALE"))), + _center(new AP_EEPROM_Float(center,createName("CNTR"))), + _pwmMin(new AP_EEPROM_Uint16(pwmMin,createName("PMIN"))), + _pwmMax(new AP_EEPROM_Uint16(pwmMax,createName("PMAX"))), + _pwmNeutral(new AP_EEPROM_Uint16(pwmNeutral,createName("PNTRL"))), + _pwmDeadZone(new AP_EEPROM_Uint16(pwmDeadZone,createName("PDEAD"))), _pwm(0), - _filter(filter), - _reverse(reverse) + _filter(new AP_EEPROM_Bool(filter,createName("FLTR"))), + _reverse(new AP_EEPROM_Bool(reverse,createName("RVRS"))) { } - // set servo state + // set void readRadio(); void setPwm(uint16_t pwm); void setPosition(float position); void mixRadio(uint16_t infStart); + void setCh(const uint8_t & ch) { _ch->set(ch); } + void setScale(const float & scale) { _scale->set(scale); } + void setCenter(const float & center) { _center->set(center); } + void setPwmMin(const uint16_t & pwmMin) { _pwmMin->set(pwmMin); } + void setPwmNeutral(const uint16_t & pwmNeutral) { _pwmNeutral->set(pwmNeutral); } + void setPwmMax(const uint16_t & pwmMax) { _pwmMax->set(pwmMax); } + void setPwmDeadZone(const uint16_t & pwmDeadZone) { _pwmDeadZone->set(pwmDeadZone); } + void setFilter(const bool & filter) { _filter->set(filter); } - // get servo state + // get uint16_t getPwm() { return _pwm; } float getPosition() { return _pwmToPosition(_pwm); } - float getNormalized() { return getPosition()/_scale; } - + float getNormalized() { return getPosition()/_scale->get(); } + const char * getName() { return _name; } + const uint8_t & getCh() { return _ch->get(); } + const float & getScale() { return _scale->get(); } + const float & getCenter() { return _center->get(); } + const uint16_t & getPwmMin() { return _pwmMin->get(); } + const uint16_t & getPwmNeutral() { return _pwmNeutral->get(); } + const uint16_t & getPwmMax() { return _pwmMax->get(); } + const uint16_t & getPwmDeadZone() { return _pwmDeadZone->get(); } + const bool & getFilter() { return _filter->get(); } + const bool & getReverse() { return _reverse->get(); } + // did our read come in 50µs below the min? - bool failSafe() { _pwm < (_pwmMin - 50); } + bool failSafe() { _pwm < (_pwmMin->get() - 50); } private: + // createName + const char * createName(char * str) + { + char * newName; + strcpy(newName,_name); + strcat(newName,"_"); + strcat(newName,str); + return (const char * )newName; + } + // configuration + const char * _name; const APM_RC_Class & _rc; - const uint16_t _ch; - const float & _scale; - const uint16_t & _pwmMin; - const uint16_t & _pwmNeutral; - const uint16_t & _pwmMax; - const uint16_t & _pwmDeadZone; - const bool & _filter; - const bool & _reverse; + AP_Uint8 * _ch; + AP_Float * _scale; + AP_Float * _center; + AP_Uint16 * _pwmMin; + AP_Uint16 * _pwmNeutral; + AP_Uint16 * _pwmMax; + AP_Uint16 * _pwmDeadZone; + AP_Bool * _filter; + AP_Bool * _reverse; // internal states uint16_t _pwm; // this is the internal state, position is just created when needed diff --git a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde index f89c96da52..f4dc4a4d05 100644 --- a/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde +++ b/libraries/AP_RcChannel/examples/ServoSweep/ServoSweep.pde @@ -14,111 +14,64 @@ FastSerialPort0(Serial); // make sure this procees variable declarations // test settings -uint8_t nChannels = 8; -bool loadEEProm = false; -bool saveEEProm = false; +uint8_t nChannels = 1; // 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 rc[] = +{ + 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_tC,6,1), + AP_RcChannel("CH8",APM_RC,7,1) + */ +}; // test position float testPosition = 0; -uint16_t testPwm = 1500; int8_t testSign = 1; - void setup() { Serial.begin(115200); 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); - } } void loop() { // set channel positions Serial.println("In Loop"); - for (int i=0;isetPosition(testPosition); - + + for (int i=0;igetPwm()); + for (int i=0;igetPosition()); + for (int i=0;i 1) + for (int i=0;i 1) + { + testPosition = 1; + testSign = -1; + } + else if (testPosition < -1) + { + testPosition = -1; + testSign = 1; + } } + delay(500); }