Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <math.h>
00013 #include <avr/eeprom.h>
00014 #include "AP_RcChannel.h"
00015 #include <AP_Common.h>
00016
00017 void AP_RcChannel::readRadio() {
00018
00019 uint16_t pwmRadio = APM_RC.InputCh(_ch);
00020 setPwm(pwmRadio);
00021 }
00022
00023 void
00024 AP_RcChannel::setPwm(uint16_t pwm)
00025 {
00026
00027
00028
00029 if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral;
00030
00031
00032
00033
00034 if(_filter){
00035 if(_pwm == 0)
00036 _pwm = pwm;
00037 else
00038 _pwm = ((pwm + _pwm) >> 1);
00039 }else{
00040 _pwm = pwm;
00041 }
00042
00043
00044
00045
00046 _pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm;
00047
00048
00049 APM_RC.OutputCh(_ch,_pwm);
00050 }
00051
00052 void
00053 AP_RcChannel::setPosition(float position)
00054 {
00055 setPwm(_positionToPwm(position));
00056 }
00057
00058 void
00059 AP_RcChannel::mixRadio(uint16_t infStart)
00060 {
00061 uint16_t pwmRadio = APM_RC.InputCh(_ch);
00062 float inf = abs( int16_t(pwmRadio - _pwmNeutral) );
00063 inf = min(inf, infStart);
00064 inf = ((infStart - inf) /infStart);
00065 setPwm(_pwm*inf + pwmRadio);
00066 }
00067
00068 uint16_t
00069 AP_RcChannel::_positionToPwm(const float & position)
00070 {
00071 uint16_t pwm;
00072
00073 if(position < 0)
00074 pwm = position * int16_t(_pwmNeutral - _pwmMin) / _scale + _pwmNeutral;
00075 else
00076 pwm = position * int16_t(_pwmMax - _pwmNeutral) / _scale + _pwmNeutral;
00077 constrain(pwm,_pwmMin,_pwmMax);
00078 return pwm;
00079 }
00080
00081 float
00082 AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
00083 {
00084 float position;
00085 if(pwm < _pwmNeutral)
00086 position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmNeutral - _pwmMin);
00087 else
00088 position = _scale * int16_t(pwm - _pwmNeutral)/ int16_t(_pwmMax - _pwmNeutral);
00089 constrain(position,-_scale,_scale);
00090 return position;
00091 }
00092
00093