/* AP_RC_Channel.cpp - Radio library for Arduino Legacy Hardware Code by Jason Short. DIYDrones.com Improvements to implement channel curves by Ron Curry, 2012 This library is free software; you can redistribute it and / or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. */ #include #include #if defined(ARDUINO) && ARDUINO >= 100 #include "Arduino.h" #else #include "WProgram.h" #endif #include "AP_RC_Channel.h" #define ANGLE 0 #define RANGE 1 // setup the control preferences void AP_RC_Channel::set_range(int low, int high) { _type = RANGE; _high = high; _low = low; } void AP_RC_Channel::set_angle(int angle) { _type = ANGLE; _high = angle; } void AP_RC_Channel::set_reverse(bool reverse) { if (reverse) _reverse = -1; else _reverse = 1; } bool AP_RC_Channel::get_reverse(void) { if (_reverse==-1) return 1; else return 0; } void AP_RC_Channel::set_filter(bool filter) { _filter = filter; } // call after first read void AP_RC_Channel::trim() { radio_trim = radio_in; } //------------------------------------------------------------------------------- // Support for PWM translation (i.e. curves or "expo") // // Translation of the input PWM is done via a pointer "channel_curve" to an array that defines the PWM output value // for any given input value. The array is structured with element 0 equal to the number of elements // in the curve. If the length is zero then the array defines no curve. If the "channel_curve" pointer // is NULL that is interpretted as no curve defined and is the default state. // // Elements 1 to n of the array contain the values for the curve. These are defined in terms of the actual // PWM output pulsewidth desired for a given point on the curve with curve element 1 containing the value // for the lowest input value from the RC RX and element "n" containing the value for the highest input value // from the RX. // // Input PWM values are expected to be in the range of the radio calibration values "radio_min" to "radio_max". The // user must have already completed the radio calibration otherwise output will be inaccurage. Input PWM values // generate an index that falls between curve elements will cause the output to be interpolated in a linear fashion // between the curve elements. For example: A curve defined as element 0 = 2 (length), element 1 = 900, and // element 2 = 2100 would define a linear straight line output between 900 and 2100 for valid input values. // Additional elements could be inserted between element 1 and element 2 to define more complex // curves. - R. Curry 06-14-12 // Sets curve for channel output to user defined curve // Input: curve - A pointer to a user defined output curve for this channel void AP_RC_Channel::set_channel_curve(int *curve) { _channel_curve = curve; // Channel_curve points to array containing curve info } // Unsets the curve for this channel - i.e. no curve translation void AP_RC_Channel::unset_channel_curve() { _channel_curve = NULL; } // Apply the current curve to a PWM value // Input: PWM value in range of radio_min to radio_max // Output: Translated PWM value int AP_RC_Channel::apply_curve(int pwm) { float scale; int index1, index2; if (_channel_curve != NULL) { if (_channel_curve[0] > 0) // If the length of the curve isn't zero then use it { // Calculate the index into the channel curve table scale = ((float)(pwm - radio_min) / (float)(radio_max - radio_min)) * ((float)_channel_curve[0]-1); index1 = (int)scale; // get the index scale -= (float)index1; // scale now has the remainder for later if (index1 < 0) { // If the PWM value below our range then clamp to lowest table entry index1 = 0; scale = 0.0; } index2 = index1 + 1; // Point to the next entry beyond our current for interpolation if (index2 >= _channel_curve[0]) { // If we are beyond the end then clamp to highest entry index2 = _channel_curve[0] - 1; if (index1 >= _channel_curve[0]) { // Also check index 1 and clamp if necessary index1 = _channel_curve[0] -1; } } // Do the lookup and interpolation index1++; // curve values start at entry 1 index2++; pwm = ((_channel_curve[index1] * (1 - scale)) + (_channel_curve[index2] * scale)); // Get the pwm value from the curve and interpolate - done } } return pwm; // } //------------------------------------------------------------------------------- // read input from APM_RC - create a control_in value void AP_RC_Channel::set_pwm(int pwm) { // Serial.print(pwm,DEC); // Apply the curve - if any pwm = apply_curve(pwm); if(_filter){ if(radio_in == 0) radio_in = pwm; else radio_in = ((pwm + radio_in) >> 1); // Small filtering }else{ radio_in = pwm; } if(_type == RANGE){ //Serial.print("range "); control_in = pwm_to_range(); control_in = (control_in < dead_zone) ? 0 : control_in; }else{ control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; } } int AP_RC_Channel::control_mix(float value) { return (1 - abs(control_in / _high)) * value + control_in; } // are we below a threshold? bool AP_RC_Channel::get_failsafe(void) { return (radio_in < (radio_min - 50)); } // returns just the PWM without the offset from radio_min void AP_RC_Channel::calc_pwm(void) { if(_type == RANGE){ pwm_out = range_to_pwm(); radio_out = pwm_out + radio_min; }else{ pwm_out = angle_to_pwm(); radio_out = pwm_out + radio_trim; } // radio_out = constrain(radio_out, radio_min, radio_max); } // ------------------------------------------ void AP_RC_Channel::load_eeprom(void) { radio_min = eeprom_read_word((uint16_t *) _address); radio_max = eeprom_read_word((uint16_t *) (_address + 2)); load_trim(); } void AP_RC_Channel::save_eeprom(void) { eeprom_write_word((uint16_t *) _address, radio_min); eeprom_write_word((uint16_t *) (_address + 2), radio_max); save_trim(); } // ------------------------------------------ void AP_RC_Channel::save_trim(void) { eeprom_write_word((uint16_t *) (_address + 4), radio_trim); //_ee.write_int((_address + 4), radio_trim); } void AP_RC_Channel::load_trim(void) { radio_trim = eeprom_read_word((uint16_t *) (_address + 4)); //_ee.write_int((_address + 4), radio_trim); } // ------------------------------------------ void AP_RC_Channel::zero_min_max() { radio_min = radio_max = radio_in; } void AP_RC_Channel::update_min_max() { radio_min = min(radio_min, radio_in); radio_max = max(radio_max, radio_in); } // ------------------------------------------ int16_t AP_RC_Channel::pwm_to_angle() { if(radio_in < radio_trim) return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min); else return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max - radio_trim); //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim)); //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min)); } int16_t AP_RC_Channel::angle_to_pwm() { if(_reverse == -1) { if(servo_out < 0) return ( -1 * ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high); else return ( -1 * ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high); } else { if(servo_out > 0) return ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high; else return ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high; } //return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim)); //return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min)); } // ------------------------------------------ int16_t AP_RC_Channel::pwm_to_range() { //return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min)))); return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min)); } int16_t AP_RC_Channel::range_to_pwm() { //return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min)); return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low); } // ------------------------------------------ float AP_RC_Channel::norm_input() { if(radio_in < radio_trim) return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min); else return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim); } float AP_RC_Channel::norm_output() { if(radio_out < radio_trim) return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min); else return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); }