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 // read input from APM_RC - create a control_in value 00057 void 00058 RC_Channel::set_pwm(int pwm) 00059 { 00060 //Serial.print(pwm,DEC); 00061 00062 if(_filter){ 00063 if(radio_in == 0) 00064 radio_in = pwm; 00065 else 00066 radio_in = ((pwm + radio_in) >> 1); // Small filtering 00067 }else{ 00068 radio_in = pwm; 00069 } 00070 00071 if(_type == RANGE){ 00072 //Serial.print("range "); 00073 control_in = pwm_to_range(); 00074 control_in = (control_in < dead_zone) ? 0 : control_in; 00075 if(scale_output){ 00076 control_in *= scale_output; 00077 } 00078 00079 }else{ 00080 control_in = pwm_to_angle(); 00081 control_in = (abs(control_in) < dead_zone) ? 0 : control_in; 00082 if(scale_output){ 00083 control_in *= scale_output; 00084 } 00085 } 00086 } 00087 00088 int 00089 RC_Channel::control_mix(float value) 00090 { 00091 return (1 - abs(control_in / _high)) * value + control_in; 00092 } 00093 00094 // are we below a threshold? 00095 bool 00096 RC_Channel::get_failsafe(void) 00097 { 00098 return (radio_in < (radio_min - 50)); 00099 } 00100 00101 // returns just the PWM without the offset from radio_min 00102 void 00103 RC_Channel::calc_pwm(void) 00104 { 00105 00106 if(_type == RANGE){ 00107 pwm_out = range_to_pwm(); 00108 radio_out = pwm_out + radio_min; 00109 }else{ 00110 pwm_out = angle_to_pwm(); 00111 radio_out = pwm_out + radio_trim; 00112 } 00113 radio_out = constrain(radio_out,radio_min, radio_max); 00114 } 00115 00116 // ------------------------------------------ 00117 00118 void 00119 RC_Channel::load_eeprom(void) 00120 { 00121 radio_min = eeprom_read_word((uint16_t *) _address); 00122 radio_max = eeprom_read_word((uint16_t *) (_address + 2)); 00123 radio_trim = eeprom_read_word((uint16_t *) (_address + 4)); 00124 //radio_min = _ee.read_int(_address); 00125 //radio_max = _ee.read_int(_address + 2); 00126 //radio_trim = _ee.read_int(_address + 4); 00127 } 00128 00129 void 00130 RC_Channel::save_eeprom(void) 00131 { 00132 eeprom_write_word((uint16_t *) _address, radio_min); 00133 eeprom_write_word((uint16_t *) (_address + 2), radio_max); 00134 eeprom_write_word((uint16_t *) (_address + 4), radio_trim); 00135 00136 //_ee.write_int(_address, radio_min); 00137 //_ee.write_int((_address + 2), radio_max); 00138 //_ee.write_int((_address + 4), radio_trim); 00139 } 00140 00141 // ------------------------------------------ 00142 void 00143 RC_Channel::save_trim(void) 00144 { 00145 eeprom_write_word((uint16_t *) (_address + 4), radio_trim); 00146 //_ee.write_int((_address + 4), radio_trim); 00147 } 00148 00149 // ------------------------------------------ 00150 00151 void 00152 RC_Channel::zero_min_max() 00153 { 00154 radio_min = radio_min = radio_in; 00155 } 00156 00157 void 00158 RC_Channel::update_min_max() 00159 { 00160 radio_min = min(radio_min, radio_in); 00161 radio_max = max(radio_max, radio_in); 00162 } 00163 00164 // ------------------------------------------ 00165 00166 int16_t 00167 RC_Channel::pwm_to_angle() 00168 { 00169 if(radio_in < radio_trim) 00170 return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min); 00171 else 00172 return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max - radio_trim); 00173 00174 //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim)); 00175 //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min)); 00176 } 00177 00178 00179 int16_t 00180 RC_Channel::angle_to_pwm() 00181 { 00182 if(servo_out < 0) 00183 return ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high; 00184 else 00185 return ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high; 00186 00187 //return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim)); 00188 //return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min)); 00189 } 00190 00191 // ------------------------------------------ 00192 00193 int16_t 00194 RC_Channel::pwm_to_range() 00195 { 00196 //return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min)))); 00197 return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min)); 00198 } 00199 00200 int16_t 00201 RC_Channel::range_to_pwm() 00202 { 00203 //return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min)); 00204 return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low); 00205 } 00206 00207 // ------------------------------------------ 00208 00209 float 00210 RC_Channel::norm_input() 00211 { 00212 if(radio_in < radio_trim) 00213 return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min); 00214 else 00215 return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim); 00216 } 00217 00218 float 00219 RC_Channel::norm_output() 00220 { 00221 if(radio_out < radio_trim) 00222 return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min); 00223 else 00224 return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); 00225 }