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

/home/jgoppert/Projects/ap/libraries/AP_RcChannel/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 void AP_RcChannel::readRadio() {
00018         // apply reverse
00019         uint16_t pwmRadio = APM_RC.InputCh(_ch);
00020         setPwm(pwmRadio);
00021 }
00022 
00023 void
00024 AP_RcChannel::setPwm(uint16_t pwm)
00025 {
00026         //Serial.printf("reverse: %s\n", (_reverse)?"true":"false");
00027 
00028         // apply reverse
00029         if(_reverse) pwm = int16_t(_pwmNeutral-pwm) + _pwmNeutral;
00030 
00031         //Serial.printf("pwm after reverse: %d\n", pwm);
00032 
00033         // apply filter
00034         if(_filter){
00035                 if(_pwm == 0)
00036                         _pwm = pwm;
00037                 else
00038                         _pwm = ((pwm + _pwm) >> 1);             // Small filtering
00039         }else{
00040                 _pwm = pwm;
00041         }
00042 
00043         //Serial.printf("pwm after filter: %d\n", _pwm);
00044 
00045         // apply deadzone
00046         _pwm = (abs(_pwm - _pwmNeutral) < _pwmDeadZone) ? _pwmNeutral : _pwm;
00047 
00048         //Serial.printf("pwm after deadzone: %d\n", _pwm);
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         //Serial.printf("position: %f\n", position);
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 // ------------------------------------------

Generated for ArduPilot Libraries by doxygen