diff --git a/libraries/AP_RcChannel/AP_RcChannel.cpp b/libraries/AP_RcChannel/AP_RcChannel.cpp index f73c25f021..17a2a7ca34 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.cpp +++ b/libraries/AP_RcChannel/AP_RcChannel.cpp @@ -14,11 +14,9 @@ #include "AP_RcChannel.h" #include -void AP_RcChannel::readRadio(uint16_t pwmRadio) { +void AP_RcChannel::readRadio() { // apply reverse - if(_reverse) _pwmRadio = (_pwmNeutral - pwmRadio) + _pwmNeutral; - else _pwmRadio = pwmRadio; - + uint16_t pwmRadio = APM_RC.InputCh(_ch); setPwm(pwmRadio); } @@ -48,6 +46,7 @@ AP_RcChannel::setPwm(uint16_t pwm) _pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm; //Serial.printf("pwm after deadzone: %d\n", _pwm); + APM_RC.OutputCh(_ch,_pwm); } void @@ -59,10 +58,11 @@ AP_RcChannel::setPosition(float position) void AP_RcChannel::mixRadio(uint16_t infStart) { - float inf = abs( int16_t(_pwmRadio - _pwmNeutral) ); + uint16_t pwmRadio = APM_RC.InputCh(_ch); + float inf = abs( int16_t(pwmRadio - _pwmNeutral) ); inf = min(inf, infStart); inf = ((infStart - inf) /infStart); - setPwm(_pwm*inf + _pwmRadio); + setPwm(_pwm*inf + pwmRadio); } uint16_t diff --git a/libraries/AP_RcChannel/AP_RcChannel.h b/libraries/AP_RcChannel/AP_RcChannel.h index 94d73a671a..28cced6f0a 100644 --- a/libraries/AP_RcChannel/AP_RcChannel.h +++ b/libraries/AP_RcChannel/AP_RcChannel.h @@ -8,6 +8,7 @@ #include #include +#include /// @class AP_RcChannel /// @brief Object managing one RC channel @@ -18,30 +19,31 @@ public: /// Constructor /// - AP_RcChannel(const float & scale, const uint16_t & pwmMin, const uint16_t & pwmNeutral, + 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) : + _rc(rc), + _ch(ch), _scale(scale), _pwmMin(pwmMin), _pwmMax(pwmMax), _pwmNeutral(pwmNeutral), _pwmDeadZone(pwmDeadZone), _pwm(0), - _pwmRadio(0), _filter(filter), _reverse(reverse) { } // set servo state - void readRadio(uint16_t pwmRadio); + void readRadio(); void setPwm(uint16_t pwm); void setPosition(float position); void mixRadio(uint16_t infStart); // get servo state uint16_t getPwm() { return _pwm; } - uint16_t getPwmRadio() { return _pwmRadio; } float getPosition() { return _pwmToPosition(_pwm); } float getNormalized() { return getPosition()/_scale; } @@ -51,6 +53,8 @@ public: private: // configuration + const APM_RC_Class & _rc; + const uint16_t _ch; const float & _scale; const uint16_t & _pwmMin; const uint16_t & _pwmNeutral; @@ -60,8 +64,7 @@ private: 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 + uint16_t _pwm; // this is the internal state, position is just created when needed // private methods uint16_t _positionToPwm(const float & position); diff --git a/libraries/AP_RcChannel/examples/AP_RcChannel/AP_RcChannel.pde b/libraries/AP_RcChannel/examples/AP_RcChannel/AP_RcChannel.pde index e3ec93cc01..59751737a8 100644 --- a/libraries/AP_RcChannel/examples/AP_RcChannel/AP_RcChannel.pde +++ b/libraries/AP_RcChannel/examples/AP_RcChannel/AP_RcChannel.pde @@ -7,7 +7,6 @@ #include #include -#include // ArduPilot Mega RC Library #include // ArduPilot Mega RC Library #include @@ -27,7 +26,7 @@ FastSerialPort0(Serial); AP_RcChannel rc[] = { - AP_RcChannel(scale.get(),pwmMin.get(),pwmNeutral.get(),pwmMax.get(), + AP_RcChannel(APM_RC,CH_1,scale.get(),pwmMin.get(),pwmNeutral.get(),pwmMax.get(), pwmDeadZone.get(),filter.get(),reverse.get()) }; @@ -46,8 +45,8 @@ void setup() 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"); + 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);