diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 656323d9df..49f8c59f01 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -9,15 +9,14 @@ */ +#include +#include +#include "WProgram.h" #include "RC_Channel.h" #define ANGLE 0 #define RANGE 1 -RC_Channel::RC_Channel() : _high(1) -{ -} - // setup the control preferences void RC_Channel::set_range(int low, int high) @@ -34,6 +33,12 @@ RC_Channel::set_angle(int angle) _high = angle; } +void +RC_Channel::set_filter(bool filter) +{ + _filter = filter; +} + // call after first read void RC_Channel::trim() @@ -48,11 +53,15 @@ RC_Channel::set_pwm(int pwm) { //Serial.print(pwm,DEC); - if(radio_in == 0) + if(_filter){ + if(radio_in == 0) + radio_in = pwm; + else + radio_in = ((pwm + radio_in) >> 1); // Small filtering + }else{ radio_in = pwm; - else - radio_in = ((pwm + radio_in) >> 1); // Small filtering - + } + if(_type == RANGE){ //Serial.print("range "); control_in = pwm_to_range(); @@ -70,7 +79,7 @@ RC_Channel::control_mix(float value) } // are we below a threshold? -boolean +bool RC_Channel::get_failsafe(void) { return (radio_in < (radio_min - 50)); @@ -90,28 +99,30 @@ RC_Channel::calc_pwm(void) } void -RC_Channel::load_eeprom(int address) +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 = eeprom_read_word((uint16_t *) _address); + radio_max = eeprom_read_word((uint16_t *) (_address + 2)); + radio_trim = eeprom_read_word((uint16_t *) (_address + 4)); } void -RC_Channel::save_eeprom(int address) +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); + 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); } // ------------------------------------------ + void RC_Channel::update_min_max() { radio_min = min(radio_min, radio_in); - radio_max = min(radio_max, radio_in); + radio_max = max(radio_max, radio_in); } + // ------------------------------------------ int16_t diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 1ab3e22147..a2c41b9b8c 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -1,27 +1,44 @@ -/* - RC_Channel library. - Code by Jason Short. 2010 - DIYDrones.com -*/ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file RC_Channel.h +/// @brief RC_Channel manager, with EEPROM-backed storage of constants. #ifndef RC_Channel_h #define RC_Channel_h -#include -#include -#include "WProgram.h" +#include -class RC_Channel -{ - public: - RC_Channel(); - - // setup in CLI +/// @class RC_Channel +/// @brief Object managing one RC channel +class RC_Channel{ + public: + /// Constructor + /// + /// A RC_Channel constructed in this fashion does not support save/restore. + /// + RC_Channel() : + _address(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), + _high(1), + _filter(true) + {} + + // setup min and max radio values in CLI void update_min_max(); // startup - void load_eeprom(int address); - void save_eeprom(int address); + void load_eeprom(void); + void save_eeprom(void); + void set_filter(bool filter); // setup the control preferences void set_range(int low, int high); @@ -37,7 +54,7 @@ class RC_Channel void trim(); // did our read come in 50µs below the min? - boolean get_failsafe(void); + bool get_failsafe(void); // value generated from PWM int16_t control_in; @@ -63,17 +80,14 @@ class RC_Channel //int16_t get_radio_out(void); private: - int16_t channel_filter(); + bool _filter; int16_t pwm_to_angle(); int16_t angle_to_pwm(); int16_t pwm_to_range(); int16_t range_to_pwm(); - byte _channel; - boolean _type; - boolean _filter; - - + int16_t _address; ///< EEPROM address for save/restore of P/I/D + bool _type; int16_t _high; int16_t _low; }; diff --git a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde index 813de81fc6..3b0e2c8025 100644 --- a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde +++ b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde @@ -8,15 +8,34 @@ #include // ArduPilot Mega RC Library #include // ArduPilot Mega RC Library -RC_Channel rc_1; -RC_Channel rc_2; -RC_Channel rc_3; -RC_Channel rc_4; + +#define EE_RADIO_1 0x00 // all gains stored from here +#define EE_RADIO_2 0x06 // all gains stored from here +#define EE_RADIO_3 0x0C // all gains stored from here +#define EE_RADIO_4 0x12 // all gains stored from here +#define EE_RADIO_5 0x18 // all gains stored from here +#define EE_RADIO_6 0x1E // all gains stored from here +#define EE_RADIO_7 0x24 // all gains stored from here +#define EE_RADIO_8 0x2A // all gains stored from here + + +RC_Channel rc_1(EE_RADIO_1); +RC_Channel rc_2(EE_RADIO_2); +RC_Channel rc_3(EE_RADIO_3); +RC_Channel rc_4(EE_RADIO_4); +RC_Channel rc_5(EE_RADIO_5); +RC_Channel rc_6(EE_RADIO_6); +RC_Channel rc_7(EE_RADIO_7); +RC_Channel rc_8(EE_RADIO_8); #define CH_1 0 #define CH_2 1 #define CH_3 2 #define CH_4 3 +#define CH_5 4 +#define CH_6 5 +#define CH_7 6 +#define CH_8 7 void setup() { @@ -28,6 +47,7 @@ void setup() // setup radio // read eepom or set manually + /* rc_1.radio_min = 1100; rc_1.radio_max = 1900; @@ -39,23 +59,40 @@ void setup() rc_4.radio_min = 1100; rc_4.radio_max = 1900; + */ + + + rc_1.load_eeprom(); + rc_2.load_eeprom(); + rc_3.load_eeprom(); + rc_4.load_eeprom(); + rc_5.load_eeprom(); + rc_6.load_eeprom(); + rc_7.load_eeprom(); + rc_8.load_eeprom(); + + print_radio_values(); // set type of output, symmetrical angles or a number range; rc_1.set_angle(4500); rc_2.set_angle(4500); rc_3.set_range(0,1000); rc_4.set_angle(3000); + rc_5.set_range(0,1000); + rc_6.set_range(0,1000); + rc_7.set_range(0,1000); + rc_8.set_range(0,1000); // set midpoint value rc_1.set_pwm(APM_RC.InputCh(CH_1)); rc_2.set_pwm(APM_RC.InputCh(CH_2)); - rc_3.set_pwm(APM_RC.InputCh(CH_3)); rc_4.set_pwm(APM_RC.InputCh(CH_4)); rc_1.trim(); rc_2.trim(); - rc_3.trim(); rc_4.trim(); + + } void loop() @@ -65,6 +102,10 @@ void loop() rc_2.set_pwm(APM_RC.InputCh(CH_2)); rc_3.set_pwm(APM_RC.InputCh(CH_3)); rc_4.set_pwm(APM_RC.InputCh(CH_4)); + rc_5.set_pwm(APM_RC.InputCh(CH_5)); + rc_6.set_pwm(APM_RC.InputCh(CH_6)); + rc_7.set_pwm(APM_RC.InputCh(CH_7)); + rc_8.set_pwm(APM_RC.InputCh(CH_8)); print_pwm(); } @@ -78,5 +119,57 @@ void print_pwm() Serial.print("\tch3 :"); Serial.print(rc_3.control_in, DEC); Serial.print("\tch4 :"); - Serial.println(rc_4.control_in, DEC); -} \ No newline at end of file + Serial.print(rc_4.control_in, DEC); + Serial.print("\tch5 :"); + Serial.print(rc_5.control_in, DEC); + Serial.print("\tch6 :"); + Serial.print(rc_6.control_in, DEC); + Serial.print("\tch7 :"); + Serial.print(rc_7.control_in, DEC); + Serial.print("\tch8 :"); + Serial.println(rc_8.control_in, DEC); +} + +void +print_radio_values() +{ + Serial.print("CH1: "); + Serial.print(rc_1.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_1.radio_max, DEC); + + Serial.print("CH2: "); + Serial.print(rc_2.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_2.radio_max, DEC); + + Serial.print("CH3: "); + Serial.print(rc_3.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_3.radio_max, DEC); + + Serial.print("CH4: "); + Serial.print(rc_4.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_4.radio_max, DEC); + + Serial.print("CH5: "); + Serial.print(rc_5.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_5.radio_max, DEC); + + Serial.print("CH6: "); + Serial.print(rc_6.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_6.radio_max, DEC); + + Serial.print("CH7: "); + Serial.print(rc_7.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_7.radio_max, DEC); + + Serial.print("CH8: "); + Serial.print(rc_8.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_8.radio_max, DEC); +}