diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 378cbc973e..c4c4ef0fc0 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -1,7 +1,7 @@ /* RC_Channel.cpp - Radio library for Arduino Code by Jason Short. DIYDrones.com - + 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 @@ -18,7 +18,7 @@ #define RANGE 1 // setup the control preferences -void +void RC_Channel::set_range(int low, int high) { _type = RANGE; @@ -74,19 +74,19 @@ RC_Channel::set_pwm(int pwm) }else{ radio_in = pwm; } - + if(_type == RANGE){ //Serial.print("range "); control_in = pwm_to_range(); control_in = (control_in < dead_zone) ? 0 : control_in; - if(scale_output){ + if (fabs(scale_output) > 0){ control_in *= scale_output; } - + }else{ control_in = pwm_to_angle(); control_in = (abs(control_in) < dead_zone) ? 0 : control_in; - if(scale_output){ + if (fabs(scale_output) > 0){ control_in *= scale_output; } } @@ -117,7 +117,7 @@ RC_Channel::calc_pwm(void) pwm_out = angle_to_pwm(); radio_out = pwm_out + radio_trim; } - radio_out = constrain(radio_out,radio_min, radio_max); + radio_out = constrain(radio_out, radio_min.get(), radio_max.get()); } // ------------------------------------------ @@ -125,32 +125,21 @@ RC_Channel::calc_pwm(void) void RC_Channel::load_eeprom(void) { - radio_min = eeprom_read_word((uint16_t *) _address); - radio_max = eeprom_read_word((uint16_t *) (_address + 2)); - radio_trim = eeprom_read_word((uint16_t *) (_address + 4)); - //radio_min = _ee.read_int(_address); - //radio_max = _ee.read_int(_address + 2); - //radio_trim = _ee.read_int(_address + 4); + _group.load(); } void RC_Channel::save_eeprom(void) { - eeprom_write_word((uint16_t *) _address, radio_min); - eeprom_write_word((uint16_t *) (_address + 2), radio_max); - eeprom_write_word((uint16_t *) (_address + 4), radio_trim); - - //_ee.write_int(_address, radio_min); - //_ee.write_int((_address + 2), radio_max); - //_ee.write_int((_address + 4), radio_trim); + _group.save(); } // ------------------------------------------ +//XXX is this still in use? void RC_Channel::save_trim(void) { - eeprom_write_word((uint16_t *) (_address + 4), radio_trim); - //_ee.write_int((_address + 4), radio_trim); + _group.save(); } // ------------------------------------------ @@ -158,14 +147,14 @@ RC_Channel::save_trim(void) void RC_Channel::zero_min_max() { - radio_min = radio_min = radio_in; + radio_min = radio_max = radio_in; } void RC_Channel::update_min_max() { - radio_min = min(radio_min, radio_in); - radio_max = max(radio_max, radio_in); + radio_min = min(radio_min.get(), radio_in); + radio_max = max(radio_max.get(), radio_in); } // ------------------------------------------ @@ -177,7 +166,7 @@ RC_Channel::pwm_to_angle() 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)); } @@ -221,7 +210,7 @@ RC_Channel::range_to_pwm() // ------------------------------------------ -float +float RC_Channel::norm_input() { if(radio_in < radio_trim) @@ -230,7 +219,7 @@ RC_Channel::norm_input() return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim); } -float +float RC_Channel::norm_output() { if(radio_out < radio_trim) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 00653bc764..277e44ae76 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -6,31 +6,23 @@ #ifndef RC_Channel_h #define RC_Channel_h +#include #include /// @class RC_Channel /// @brief Object managing one RC channel class RC_Channel{ - public: + public: /// Constructor /// - /// A RC_Channel constructed in this fashion does not support save/restore. + /// @param key EEPROM storage key for the channel trim parameters. + /// @param name Optional name for the group. /// - RC_Channel() : - _address(0), - _reverse(1), - dead_zone(0), - scale_output(1.0) - {} - - /// Constructor - /// - /// @param address EEPROM base address at which RC_Channel parameters - /// are stored. Zero if the RC_Channel does not support - /// save/restore. - /// - RC_Channel(uint16_t address) : - _address(address), + RC_Channel(AP_Var::Key key, const prog_char *name) : + _group(key, name), + radio_min (&_group, 0, 1500, name ? "MIN" : 0), // suppress name if group has no name + radio_trim(&_group, 1, 1500, name ? "TRIM" : 0), + radio_max (&_group, 2, 1500, name ? "MAX" : 0), _high(1), _filter(true), _reverse(1), @@ -41,11 +33,11 @@ class RC_Channel{ // setup min and max radio values in CLI void update_min_max(); void zero_min_max(); - + // startup void load_eeprom(void); - void save_eeprom(void); - void save_trim(void); + void save_eeprom(void); + void save_trim(void); void set_filter(bool filter); // setup the control preferences @@ -56,22 +48,22 @@ class RC_Channel{ // read input from APM_RC - create a control_in value void set_pwm(int pwm); - + // pwm is stored here int16_t radio_in; // call after first set_pwm void trim(); - + // did our read come in 50µs below the min? bool get_failsafe(void); - + // value generated from PWM int16_t control_in; int16_t dead_zone; // used to keep noise down and create a dead zone. - + int control_mix(float value); - + // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 int16_t servo_out; @@ -82,10 +74,10 @@ class RC_Channel{ int16_t pwm_out; int16_t radio_out; - int16_t radio_min; - int16_t radio_trim; - int16_t radio_max; - + AP_Int16 radio_min; + AP_Int16 radio_trim; + AP_Int16 radio_max; + // includes offset from PWM //int16_t get_radio_out(void); @@ -97,18 +89,18 @@ class RC_Channel{ int16_t range_to_pwm(); float scale_output; - + private: + AP_Var_group _group; bool _filter; int8_t _reverse; - int16_t _address; ///< EEPROM address for save/restore of P/I/D - bool _type; + bool _type; int16_t _high; int16_t _low; }; -#endif +#endif