From 3ce38444b5a4c2b647dc5c287d0d0b29a93b8bc7 Mon Sep 17 00:00:00 2001 From: "james.goppert" Date: Sat, 25 Dec 2010 00:06:37 +0000 Subject: [PATCH] Added alternate RC_Channel class. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1253 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/RC_ChannelB/RC_ChannelB.cpp | 193 ++++++++++++++++++ libraries/RC_ChannelB/RC_ChannelB.h | 105 ++++++++++ .../examples/RC_ChannelB/RC_ChannelB.pde | 175 ++++++++++++++++ 3 files changed, 473 insertions(+) create mode 100644 libraries/RC_ChannelB/RC_ChannelB.cpp create mode 100644 libraries/RC_ChannelB/RC_ChannelB.h create mode 100644 libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde diff --git a/libraries/RC_ChannelB/RC_ChannelB.cpp b/libraries/RC_ChannelB/RC_ChannelB.cpp new file mode 100644 index 0000000000..de5de1a7b0 --- /dev/null +++ b/libraries/RC_ChannelB/RC_ChannelB.cpp @@ -0,0 +1,193 @@ +/* + RC_ChannelB.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 + version 2.1 of the License, or (at your option) any later version. + +*/ + +#include +#include +#include "WProgram.h" +#include "RC_ChannelB.h" + +#define ANGLE 0 +#define RANGE 1 + +// setup the control preferences +void +RC_ChannelB::set_range(int low, int high) +{ + _type = RANGE; + _high = high; + _low = low; +} + +void +RC_ChannelB::set_angle(int angle) +{ + _type = ANGLE; + _high = angle; +} + +void +RC_ChannelB::set_reverse(bool reverse) +{ + if (reverse) _reverse = -1; + else _reverse = 1; +} + +void +RC_ChannelB::set_filter(bool filter) +{ + _filter = filter; +} + +// call after first read +void +RC_ChannelB::trim() +{ + radio_trim = radio_in; + +} + +// read input from APM_RC - create a control_in value +void +RC_ChannelB::set_pwm(int pwm) +{ + //Serial.print(pwm,DEC); + + 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 +RC_ChannelB::control_mix(float value) +{ + return (1 - abs(control_in / _high)) * value + control_in; +} + +// are we below a threshold? +bool +RC_ChannelB::get_failsafe(void) +{ + return (radio_in < (radio_min - 50)); +} + +// returns just the PWM without the offset from radio_min +void +RC_ChannelB::calc_pwm(void) +{ + + if(_type == RANGE){ + pwm_out = range_to_pwm(); + }else{ + pwm_out = angle_to_pwm(); + } + radio_out = pwm_out + radio_min; +} + +// ------------------------------------------ + +void +RC_ChannelB::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)); +} + +void +RC_ChannelB::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); +} + +// ------------------------------------------ +void +RC_ChannelB::save_trim(void) +{ + eeprom_write_word((uint16_t *) (_address + 4), radio_trim); +} + +// ------------------------------------------ + +void +RC_ChannelB::update_min_max() +{ + radio_min = min(radio_min, radio_in); + radio_max = max(radio_max, radio_in); +} + +// ------------------------------------------ + +int16_t +RC_ChannelB::pwm_to_angle() +{ + if(radio_in < radio_trim) + return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min)); + else + return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim)); +} + +float +RC_ChannelB::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 +RC_ChannelB::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); +} + +int16_t +RC_ChannelB::angle_to_pwm() +{ + if(servo_out < 0) + return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim)); + else + return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min)); +} + +// ------------------------------------------ + +int16_t +RC_ChannelB::pwm_to_range() +{ + return _reverse * (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min)))); +} + +int16_t +RC_ChannelB::range_to_pwm() +{ + return (((float)servo_out / (float)(_high - _low)) * (float)(radio_max - radio_min)); +} + diff --git a/libraries/RC_ChannelB/RC_ChannelB.h b/libraries/RC_ChannelB/RC_ChannelB.h new file mode 100644 index 0000000000..1da8bd4735 --- /dev/null +++ b/libraries/RC_ChannelB/RC_ChannelB.h @@ -0,0 +1,105 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file RC_ChannelB.h +/// @brief RC_ChannelB manager, with EEPROM-backed storage of constants. + +#ifndef RC_ChannelB_h +#define RC_ChannelB_h + +#include + +/// @class RC_ChannelB +/// @brief Object managing one RC channel +class RC_ChannelB{ + public: + /// Constructor + /// + /// A RC_ChannelB constructed in this fashion does not support save/restore. + /// + RC_ChannelB() : + _address(0) + {} + + /// Constructor + /// + /// @param address EEPROM base address at which RC_ChannelB parameters + /// are stored. Zero if the RC_ChannelB does not support + /// save/restore. + /// + RC_ChannelB(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(void); + void save_eeprom(void); + void save_trim(void); + void set_filter(bool filter); + + // setup the control preferences + void set_range(int low, int high); + void set_angle(int angle); + void set_reverse(bool reverse); + + // 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; + + // generate PWM from servo_out value + void calc_pwm(void); + + // PWM is without the offset from radio_min + int16_t pwm_out; + int16_t radio_out; + + int16_t radio_min; + int16_t radio_trim; + int16_t radio_max; + + // includes offset from PWM + //int16_t get_radio_out(void); + + int16_t pwm_to_angle(); + float norm_input(); + float norm_output(); + int16_t angle_to_pwm(); + int16_t pwm_to_range(); + int16_t range_to_pwm(); + + private: + bool _filter; + int8_t _reverse; + + int16_t _address; ///< EEPROM address for save/restore of P/I/D + bool _type; + int16_t _high; + int16_t _low; +}; + +#endif + + + + diff --git a/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde b/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde new file mode 100644 index 0000000000..3b0e2c8025 --- /dev/null +++ b/libraries/RC_ChannelB/examples/RC_ChannelB/RC_ChannelB.pde @@ -0,0 +1,175 @@ +/* + Example of RC_Channel library. + Code by Jason Short. 2010 + DIYDrones.com + +*/ + +#include // ArduPilot Mega RC Library +#include // ArduPilot Mega RC Library + + +#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() +{ + Serial.begin(38400); + Serial.println("ArduPilot RC Channel test"); + APM_RC.Init(); // APM Radio initialization + + delay(1000); + // setup radio + + // read eepom or set manually + /* + rc_1.radio_min = 1100; + rc_1.radio_max = 1900; + + rc_2.radio_min = 1100; + rc_2.radio_max = 1900; + + rc_3.radio_min = 1100; + rc_3.radio_max = 1900; + + 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_4.set_pwm(APM_RC.InputCh(CH_4)); + + rc_1.trim(); + rc_2.trim(); + rc_4.trim(); + + +} + +void loop() +{ + delay(20); + 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_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(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc_1.control_in, DEC); + Serial.print("\tch2: "); + Serial.print(rc_2.control_in, DEC); + Serial.print("\tch3 :"); + Serial.print(rc_3.control_in, DEC); + Serial.print("\tch4 :"); + 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); +}