From c65981ab3ec53248d5ef0e5b02b5dffa24809cf7 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sat, 25 Dec 2010 19:03:38 +0000 Subject: [PATCH] Fixed RC_ChannelB errors. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1261 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_Common/AP_Common.h | 16 ++++----- libraries/RC_ChannelB/RC_ChannelB.cpp | 33 ++++++++++++++----- libraries/RC_ChannelB/RC_ChannelB.h | 8 ++--- .../examples/RC_ChannelB/RC_ChannelB.pde | 28 ++++++++++------ 4 files changed, 54 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Common/AP_Common.h b/libraries/AP_Common/AP_Common.h index 1efc9645b6..3ba51201dd 100644 --- a/libraries/AP_Common/AP_Common.h +++ b/libraries/AP_Common/AP_Common.h @@ -95,7 +95,7 @@ public: /** * The default constrcutor */ - AP_Var(type data, const char * name = "", bool sync=false) : + AP_Var(const type & data, const char * name = "", const bool & sync=false) : _data(data), _name(name), _sync(sync) { } @@ -103,7 +103,7 @@ public: /** * Set the variable value */ - void set(type val) { + void set(const type & val) { _data = val; if (_sync) save(); } @@ -111,7 +111,7 @@ public: /** * Get the variable value. */ - type get() { + const type & get() { if (_sync) load(); return _data; } @@ -119,15 +119,15 @@ public: /** * Set the variable value as a float */ - void setAsFloat(float val) { - set((type)val); + void setAsFloat(const float & val) { + set(val); } /** * Get the variable as a float */ - float getAsFloat() { - return (float)get(); + const float & getAsFloat() { + return get(); } @@ -154,7 +154,7 @@ public: * If sync is true the a load will always occure before a get and a save will always * occure before a set. */ - bool getSync() { return _sync; } + const bool & getSync() { return _sync; } void setSync(bool sync) { _sync = sync; } protected: diff --git a/libraries/RC_ChannelB/RC_ChannelB.cpp b/libraries/RC_ChannelB/RC_ChannelB.cpp index eb0dcf15b7..2cac7122c4 100644 --- a/libraries/RC_ChannelB/RC_ChannelB.cpp +++ b/libraries/RC_ChannelB/RC_ChannelB.cpp @@ -25,8 +25,12 @@ void RC_ChannelB::readRadio(uint16_t pwmRadio) { void RC_ChannelB::setPwm(uint16_t pwm) { + //Serial.printf("reverse: %s\n", (_reverse)?"true":"false"); + // apply reverse - if(_reverse) pwm = (_pwmNeutral-pwm) + _pwmNeutral; + if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral; + + //Serial.printf("pwm after reverse: %d\n", pwm); // apply filter if(_filter){ @@ -38,8 +42,12 @@ RC_ChannelB::setPwm(uint16_t pwm) _pwm = pwm; } + //Serial.printf("pwm after filter: %d\n", _pwm); + // apply deadzone _pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm; + + //Serial.printf("pwm after deadzone: %d\n", _pwm); } void @@ -51,28 +59,35 @@ RC_ChannelB::setPosition(float position) void RC_ChannelB::mixRadio(uint16_t infStart) { - float inf = abs(_pwmRadio - _pwmNeutral); + float inf = abs( int16_t(_pwmRadio - _pwmNeutral) ); inf = min(inf, infStart); inf = ((infStart - inf) /infStart); setPwm(_pwm*inf + _pwmRadio); } uint16_t -RC_ChannelB::_positionToPwm(float position) +RC_ChannelB::_positionToPwm(const float & position) { + uint16_t pwm; + //Serial.printf("position: %f\n", position); if(position < 0) - return (position / _scale) * (_pwmMin - _pwmNeutral); + pwm = position * int16_t(_pwmNeutral - _pwmMin) / _scale + _pwmNeutral; else - return (position / _scale) * (_pwmMax - _pwmNeutral); + pwm = position * int16_t(_pwmMax - _pwmNeutral) / _scale + _pwmNeutral; + constrain(pwm,_pwmMin,_pwmMax); + return pwm; } float -RC_ChannelB::_pwmToPosition(uint16_t pwm) +RC_ChannelB::_pwmToPosition(const uint16_t & pwm) { - if(_pwm < _pwmNeutral) - return _scale * (_pwm - _pwmNeutral)/(_pwmNeutral - _pwmMin); + float position; + if(pwm < _pwmNeutral) + position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmNeutral - _pwmMin); else - return _scale * (_pwm - _pwmNeutral)/(_pwmMax - _pwmNeutral); + position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmMax - _pwmNeutral); + constrain(position,-_scale,_scale); + return position; } // ------------------------------------------ diff --git a/libraries/RC_ChannelB/RC_ChannelB.h b/libraries/RC_ChannelB/RC_ChannelB.h index 158d07006d..6b414f25e8 100644 --- a/libraries/RC_ChannelB/RC_ChannelB.h +++ b/libraries/RC_ChannelB/RC_ChannelB.h @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /// @file RC_ChannelB.h -/// @brief RC_ChannelB manager, with EEPROM-backed storage of constants. +/// @brief RC_ChannelB manager #ifndef RC_ChannelB_h #define RC_ChannelB_h @@ -57,15 +57,15 @@ private: const uint16_t & _pwmMax; const uint16_t & _pwmDeadZone; const bool & _filter; - const int8_t & _reverse; + const bool & _reverse; // internal states uint16_t _pwm; // this is the internal state, positino is just created when needed uint16_t _pwmRadio; // radio pwm input // private methods - uint16_t _positionToPwm(float position); - float _pwmToPosition(uint16_t pwm); + uint16_t _positionToPwm(const float & position); + float _pwmToPosition(const uint16_t & pwm); }; #endif diff --git a/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde b/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde index 0cbd310c54..0c4d6419c1 100644 --- a/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde +++ b/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde @@ -20,8 +20,8 @@ AP_EEPromVar pwmDeadZone(100,"RC1_PWMDEADZONE"); #define CH_1 0 // configuration -AP_Var filter(true,"RC1_FILTER"); -AP_Var reverse(false,"RC1_REVERSE"); +AP_Var filter(false,"RC1_FILTER"); +AP_Var reverse(false,"RC1_REVERSE"); FastSerialPort0(Serial); @@ -39,26 +39,34 @@ void setup() APM_RC.Init(); // APM Radio initialization delay(1000); - // setup radio + + // configuratoin + scale.set(100); + Serial.printf("\nscale.set(100)\n"); + delay(2000); + + // find neutral radio position + rc[CH_1].readRadio(APM_RC.InputCh(CH_1)); + Serial.printf("\nrc[CH_1].readRadio(APM_RC.InputCh(CH_1))\n"); + Serial.printf("\npwmNeutral.set(rc[CH_1].getPwm())\n"); + pwmNeutral.set(rc[CH_1].getPwm()); + delay(2000); } void loop() { - rc[CH_1].readRadio(APM_RC.InputCh(CH_1)); - Serial.printf("\nrc[CH_1].readRadio(APM_RC.InputCh(CH_1))\n"); - Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(), rc[CH_1].getPosition()); + // get the min pwm + Serial.printf("\npwmMin.get(): %d\n", pwmMin.get()); delay(2000); - scale.set(100); - Serial.printf("\nscale.set(100)\n"); - - + // set by pwm rc[CH_1].setPwm(1500); 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(-50); Serial.printf("\nrc[CH_1].setPosition(-50))\n"); Serial.printf("pwm: %d position: %f\n",rc[CH_1].getPwm(),