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 AP_RcChannel::AP_RcChannel(const char * name, const APM_RC_Class & rc, const uint8_t & ch,
00018 const float & scale, const float & center,
00019 const uint16_t & pwmMin,
00020 const uint16_t & pwmNeutral, const uint16_t & pwmMax,
00021 const uint16_t & pwmDeadZone,
00022 const bool & filter, const bool & reverse) :
00023 _name(name),
00024 _rc(rc),
00025 _ch(new AP_EEPROM_Uint8(ch,"CH",name)),
00026 _scale(new AP_EEPROM_Float(scale,"SCALE",name)),
00027 _center(new AP_EEPROM_Float(center,"CNTR",name)),
00028 _pwmMin(new AP_EEPROM_Uint16(pwmMin,"PMIN",name)),
00029 _pwmMax(new AP_EEPROM_Uint16(pwmMax,"PMAX",name)),
00030 _pwmNeutral(new AP_EEPROM_Uint16(pwmNeutral,"PNTRL",name)),
00031 _pwmDeadZone(new AP_EEPROM_Uint16(pwmDeadZone,"PDEAD",name)),
00032 _pwm(0),
00033 _filter(new AP_EEPROM_Bool(filter,"FLTR",name)),
00034 _reverse(new AP_EEPROM_Bool(reverse,"REV",name))
00035 {
00036 }
00037
00038
00039 void AP_RcChannel::readRadio() {
00040
00041 uint16_t pwmRadio = APM_RC.InputCh(getCh());
00042 setPwm(pwmRadio);
00043 }
00044
00045 void
00046 AP_RcChannel::setPwm(uint16_t pwm)
00047 {
00048
00049
00050
00051
00052 if(getReverse()) pwm = int16_t(getPwmNeutral()-pwm) + getPwmNeutral();
00053
00054
00055
00056
00057 if(getFilter()){
00058 if(_pwm == 0)
00059 _pwm = pwm;
00060 else
00061 _pwm = ((pwm + _pwm) >> 1);
00062 }else{
00063 _pwm = pwm;
00064 }
00065
00066
00067
00068
00069 _pwm = (abs(_pwm - getPwmNeutral()) < getPwmDeadZone()) ? getPwmNeutral() : _pwm;
00070
00071
00072 APM_RC.OutputCh(getCh(),_pwm);
00073 }
00074
00075 void
00076 AP_RcChannel::setPosition(float position)
00077 {
00078 setPwm(_positionToPwm(position));
00079 }
00080
00081 void
00082 AP_RcChannel::mixRadio(uint16_t infStart)
00083 {
00084 uint16_t pwmRadio = APM_RC.InputCh(getCh());
00085 float inf = abs( int16_t(pwmRadio - getPwmNeutral()) );
00086 inf = min(inf, infStart);
00087 inf = ((infStart - inf) /infStart);
00088 setPwm(_pwm*inf + pwmRadio);
00089 }
00090
00091 uint16_t
00092 AP_RcChannel::_positionToPwm(const float & position)
00093 {
00094 uint16_t pwm;
00095
00096 float p = position - getCenter();
00097 if(p < 0)
00098 pwm = p * int16_t(getPwmNeutral() - getPwmMin()) /
00099 getScale() + getPwmNeutral();
00100 else
00101 pwm = p * int16_t(getPwmMax() - getPwmNeutral()) /
00102 getScale() + getPwmNeutral();
00103 constrain(pwm,getPwmMin(),getPwmMax());
00104 return pwm;
00105 }
00106
00107 float
00108 AP_RcChannel::_pwmToPosition(const uint16_t & pwm)
00109 {
00110 float position;
00111 if(pwm < getPwmNeutral())
00112 position = getScale() * int16_t(pwm - getPwmNeutral())/
00113 int16_t(getPwmNeutral() - getPwmMin()) + getCenter();
00114 else
00115 position = getScale() * int16_t(pwm -getPwmNeutral())/
00116 int16_t(getPwmMax() - getPwmNeutral()) + getCenter();
00117 constrain(position,-getScale()+getCenter(),
00118 getScale()+getCenter());
00119 return position;
00120 }
00121
00122