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

/home/jgoppert/Projects/ap/libraries/RC_Channel/RC_Channel.cpp

Go to the documentation of this file.
00001 /*
00002         RC_Channel.cpp - Radio library for Arduino
00003         Code by Jason Short. 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 "WProgram.h"
00015 #include "RC_Channel.h"
00016 
00017 #define ANGLE 0
00018 #define RANGE 1
00019 
00020 // setup the control preferences
00021 void    
00022 RC_Channel::set_range(int low, int high)
00023 {
00024         _type   = RANGE;
00025         _high   = high;
00026         _low    = low;
00027 }
00028 
00029 void
00030 RC_Channel::set_angle(int angle)
00031 {
00032         _type   = ANGLE;
00033         _high   = angle;
00034 }
00035 
00036 void
00037 RC_Channel::set_reverse(bool reverse)
00038 {
00039         if (reverse) _reverse = -1;
00040         else _reverse = 1;
00041 }
00042 
00043 void
00044 RC_Channel::set_filter(bool filter)
00045 {
00046         _filter = filter;
00047 }
00048 
00049 // call after first read
00050 void
00051 RC_Channel::trim()
00052 {
00053         radio_trim = radio_in;
00054         
00055 }
00056 
00057 // read input from APM_RC - create a control_in value
00058 void
00059 RC_Channel::set_pwm(int pwm)
00060 {
00061         //Serial.print(pwm,DEC);
00062 
00063         if(_filter){
00064                 if(radio_in == 0)
00065                         radio_in = pwm;
00066                 else
00067                         radio_in = ((pwm + radio_in) >> 1);             // Small filtering
00068         }else{
00069                 radio_in = pwm;
00070         }
00071         
00072         if(_type == RANGE){
00073                 //Serial.print("range ");
00074                 control_in = pwm_to_range();
00075                 control_in = (control_in < dead_zone) ? 0 : control_in;
00076                 if(scale_output){
00077                         control_in *= scale_output;
00078                 }
00079                 
00080         }else{
00081                 control_in = pwm_to_angle();
00082                 control_in = (abs(control_in) < dead_zone) ? 0 : control_in;
00083                 if(scale_output){
00084                         control_in *= scale_output;
00085                 }
00086         }
00087 }
00088 
00089 int
00090 RC_Channel::control_mix(float value)
00091 {
00092         return (1 - abs(control_in / _high)) * value + control_in;
00093 }
00094 
00095 // are we below a threshold?
00096 bool
00097 RC_Channel::get_failsafe(void)
00098 {
00099         return (radio_in < (radio_min - 50));
00100 }
00101 
00102 // returns just the PWM without the offset from radio_min
00103 void
00104 RC_Channel::calc_pwm(void)
00105 {
00106 
00107         if(_type == RANGE){
00108                 pwm_out = range_to_pwm();
00109         }else{
00110                 pwm_out = angle_to_pwm();
00111         }
00112         //if(scale_output){
00113         //      pwm_out *= scale_output;
00114         //}
00115         radio_out = pwm_out + radio_min;
00116         radio_out = constrain(radio_out,radio_min, radio_max);
00117 }
00118 
00119 // ------------------------------------------
00120 
00121 void
00122 RC_Channel::load_eeprom(void)
00123 {
00124         //radio_min     = eeprom_read_word((uint16_t *) _address);
00125         //radio_max     = eeprom_read_word((uint16_t *) (_address + 2));
00126         //radio_trim    = eeprom_read_word((uint16_t *) (_address + 4));
00127         radio_min       = _ee.read_int(_address);
00128         radio_max       = _ee.read_int(_address + 2);
00129         radio_trim      = _ee.read_int(_address + 4);
00130 }
00131 
00132 void
00133 RC_Channel::save_eeprom(void)
00134 {
00135         //eeprom_write_word((uint16_t *)        _address,                       radio_min);
00136         //eeprom_write_word((uint16_t *)        (_address + 2),         radio_max);
00137         //eeprom_write_word((uint16_t *)        (_address + 4),         radio_trim);
00138         
00139         _ee.write_int(_address,                 radio_min);
00140         _ee.write_int((_address + 2),   radio_max);
00141         _ee.write_int((_address + 4),   radio_trim);
00142 }
00143 
00144 // ------------------------------------------
00145 void
00146 RC_Channel::save_trim(void)
00147 {
00148         //eeprom_write_word((uint16_t *)        (_address + 4),         radio_trim);
00149         _ee.write_int((_address + 4),   radio_trim);
00150 }
00151 
00152 // ------------------------------------------
00153 
00154 void
00155 RC_Channel::zero_min_max()
00156 {
00157         radio_min = radio_min = radio_in;
00158 }
00159 
00160 void
00161 RC_Channel::update_min_max()
00162 {
00163         radio_min = min(radio_min, radio_in);
00164         radio_max = max(radio_max, radio_in);
00165 }
00166 
00167 // ------------------------------------------
00168 
00169 int16_t
00170 RC_Channel::pwm_to_angle()
00171 {
00172         if(radio_in < radio_trim)
00173                 return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min);
00174         else
00175                 return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max  - radio_trim);
00176                 
00177                 //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim));
00178                 //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min));
00179 }
00180 
00181 float 
00182 RC_Channel::norm_input()
00183 {
00184         if(radio_in < radio_trim)
00185                 return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min);
00186         else
00187                 return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max  - radio_trim);
00188 }
00189 
00190 float 
00191 RC_Channel::norm_output()
00192 {
00193         if(radio_out < radio_trim)
00194                 return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min);
00195         else
00196                 return (float)(radio_out - radio_trim) / (float)(radio_max  - radio_trim);
00197 }
00198 
00199 int16_t
00200 RC_Channel::angle_to_pwm()
00201 {
00202         if(servo_out < 0)
00203                 return ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high;
00204         else
00205                 return ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high;
00206 
00207                 //return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim));
00208                 //return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min));
00209 }
00210 
00211 // ------------------------------------------
00212 
00213 int16_t
00214 RC_Channel::pwm_to_range()
00215 {
00216         //return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min))));
00217         return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min));
00218 }
00219 
00220 int16_t
00221 RC_Channel::range_to_pwm()
00222 {
00223         //return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min));
00224         return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low);
00225 }
00226 
00227 
00228 

Generated for ArduPilot Libraries by doxygen