• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

AP_RcChannel.cpp

Go to the documentation of this file.
00001 /*
00002         AP_RcChannel.cpp - Radio library for Arduino
00003         Code by Jason Short, James Goppert. DIYDrones.com
00004         
00005         This library is free software; you can redistribute it and / or
00006                 modify it under the terms of the GNU Lesser General Public
00007                 License as published by the Free Software Foundation; either
00008                 version 2.1 of the License, or (at your option) any later version.
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         // apply reverse
00041         uint16_t pwmRadio = APM_RC.InputCh(getCh());
00042         setPwm(pwmRadio);
00043 }
00044 
00045 void
00046 AP_RcChannel::setPwm(uint16_t pwm)
00047 {
00048         //Serial.printf("pwm in setPwm: %d\n", pwm);
00049         //Serial.printf("reverse: %s\n", (getReverse())?"true":"false");
00050         
00051         // apply reverse
00052         if(getReverse()) pwm = int16_t(getPwmNeutral()-pwm) + getPwmNeutral();
00053 
00054         //Serial.printf("pwm after reverse: %d\n", pwm);
00055 
00056         // apply filter
00057         if(getFilter()){
00058                 if(_pwm == 0)
00059                         _pwm = pwm;
00060                 else
00061                         _pwm = ((pwm + _pwm) >> 1);             // Small filtering
00062         }else{
00063                 _pwm = pwm;
00064         }
00065 
00066         //Serial.printf("pwm after filter: %d\n", _pwm);
00067 
00068         // apply deadzone
00069         _pwm = (abs(_pwm - getPwmNeutral()) < getPwmDeadZone()) ? getPwmNeutral() : _pwm;
00070 
00071         //Serial.printf("pwm after deadzone: %d\n", _pwm);
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         //Serial.printf("position: %f\n", position);
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 // ------------------------------------------

Generated for ArduPilot Libraries by doxygen