diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp index 66d4d420b1..dcddb1a5bd 100644 --- a/libraries/RC_Channel/RC_Channel.cpp +++ b/libraries/RC_Channel/RC_Channel.cpp @@ -10,21 +10,22 @@ * */ +#include #include -#include -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif + +#include +extern const AP_HAL::HAL& hal; + +#include + #include "RC_Channel.h" -/// global array with pointers to all APM RC channels, will be used by AP_Mount and AP_Camera classes -/// It points to RC input channels, both APM1 and APM2 only have 8 input channels. +#define NUM_CHANNELS 8 +/// global array with pointers to all APM RC channels, will be used by AP_Mount +/// and AP_Camera classes / It points to RC input channels, both APM1 and APM2 +/// only have 8 input channels. RC_Channel* rc_ch[NUM_CHANNELS]; -APM_RC_Class *RC_Channel::_apm_rc; - const AP_Param::GroupInfo RC_Channel::var_info[] PROGMEM = { // @Param: MIN // @DisplayName: RC min PWM @@ -310,25 +311,19 @@ RC_Channel::norm_output() return ret; } -void RC_Channel::set_apm_rc( APM_RC_Class * apm_rc ) +void RC_Channel::output() { - _apm_rc = apm_rc; -} - -void -RC_Channel::output() -{ - _apm_rc->OutputCh(_ch_out, radio_out); + hal.rcout->write(_ch_out, radio_out); } void RC_Channel::input() { - radio_in = _apm_rc->InputCh(_ch_out); + radio_in = hal.rcin->read(_ch_out); } void RC_Channel::enable_out() { - _apm_rc->enable_out(_ch_out); + hal.rcout->enable_ch(_ch_out); } diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 6f582d06ba..fe50750334 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -3,12 +3,15 @@ /// @file RC_Channel.h /// @brief RC_Channel manager, with EEPROM-backed storage of constants. -#ifndef RC_Channel_h -#define RC_Channel_h +#ifndef __RC_CHANNEL_H__ +#define __RC_CHANNEL_H__ #include #include -#include + +#define RC_CHANNEL_TYPE_ANGLE 0 +#define RC_CHANNEL_TYPE_RANGE 1 +#define RC_CHANNEL_TYPE_ANGLE_RAW 2 #define RC_CHANNEL_TYPE_ANGLE 0 #define RC_CHANNEL_TYPE_RANGE 1 @@ -91,11 +94,9 @@ public: int16_t pwm_to_range(); int16_t range_to_pwm(); - static void set_apm_rc(APM_RC_Class * apm_rc); void output(); void input(); void enable_out(); - static APM_RC_Class * _apm_rc; static const struct AP_Param::GroupInfo var_info[]; @@ -110,7 +111,7 @@ private: uint8_t _ch_out; }; -// This is ugly, but it fixes compilation on arduino +// This is ugly, but it fixes poorly architected library #include "RC_Channel_aux.h" #endif diff --git a/libraries/RC_Channel/RC_Channel_aux.cpp b/libraries/RC_Channel/RC_Channel_aux.cpp index dad1189f3f..81854dd9de 100644 --- a/libraries/RC_Channel/RC_Channel_aux.cpp +++ b/libraries/RC_Channel/RC_Channel_aux.cpp @@ -1,8 +1,11 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- -#include #include "RC_Channel_aux.h" +#include +#include +extern const AP_HAL::HAL& hal; + const AP_Param::GroupInfo RC_Channel_aux::var_info[] PROGMEM = { AP_NESTEDGROUPINFO(RC_Channel, 0), @@ -31,7 +34,7 @@ RC_Channel_aux::output_ch(unsigned char ch_nr) radio_out = radio_in; break; } - _apm_rc->OutputCh(ch_nr, radio_out); + hal.rcout->write(ch_nr, radio_out); } /// Update the _aux_channels array of pointers to rc_x channels diff --git a/libraries/RC_Channel/RC_Channel_aux.h b/libraries/RC_Channel/RC_Channel_aux.h index 4bf7bcc944..f3a73c801d 100644 --- a/libraries/RC_Channel/RC_Channel_aux.h +++ b/libraries/RC_Channel/RC_Channel_aux.h @@ -4,8 +4,8 @@ /// @brief RC_Channel manager for auxiliary channels (5..8), with EEPROM-backed storage of constants. /// @author Amilcar Lucas -#ifndef RC_CHANNEL_AUX_H_ -#define RC_CHANNEL_AUX_H_ +#ifndef __RC_CHANNEL_AUX_H__ +#define __RC_CHANNEL_AUX_H__ #include "RC_Channel.h" diff --git a/libraries/RC_Channel/examples/RC_Channel/Arduino.h b/libraries/RC_Channel/examples/RC_Channel/Arduino.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde index ed362eefe6..c5c4c91499 100644 --- a/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde +++ b/libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde @@ -4,45 +4,6 @@ * DIYDrones.com * */ - -#include -#include // ArduPilot Mega Common Library -#include -#include -#include // ArduPilot Mega Vector/Matrix math Library -#include // ArduPilot Mega RC Library -#include // ArduPilot Mega RC Library - -// define APM1 or APM2 -#define APM_HARDWARE_APM1 1 -#define APM_HARDWARE_APM2 2 - -// set your hardware type here -#define CONFIG_APM_HARDWARE APM_HARDWARE_APM2 - -Arduino_Mega_ISR_Registry isr_registry; - -//////////////////////////////////////////////////////////////////////////////// -// Serial ports -//////////////////////////////////////////////////////////////////////////////// -// -// Note that FastSerial port buffers are allocated at ::begin time, -// so there is not much of a penalty to defining ports that we don't -// use. -// -FastSerialPort0(Serial); // FTDI/console -FastSerialPort1(Serial1); // GPS port -FastSerialPort3(Serial3); // Telemetry port - -//////////////////////////////////////////// -// RC Hardware -//////////////////////////////////////////// -#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -APM_RC_APM2 APM_RC; -#else -APM_RC_APM1 APM_RC; -#endif - #define CH_1 0 #define CH_2 1 #define CH_3 2 @@ -52,6 +13,15 @@ APM_RC_APM1 APM_RC; #define CH_7 6 #define CH_8 7 +#include +#include +#include +#include +#include +#include + +#include +const AP_HAL::HAL& hal = AP_HAL_AVR_APM2; RC_Channel rc_1(CH_1); RC_Channel rc_2(CH_2); @@ -63,17 +33,11 @@ RC_Channel rc_7(CH_7); RC_Channel rc_8(CH_8); RC_Channel *rc = &rc_1; + void setup() { - Serial.begin(115200); - Serial.println("ArduPilot RC Channel test"); - isr_registry.init(); - APM_RC.Init( &isr_registry ); // APM Radio initialization - - - delay(500); - - // setup radio + hal.console->println("ArduPilot RC Channel test"); + hal.scheduler->delay(500); // interactive setup setup_radio(); @@ -81,26 +45,31 @@ void setup() print_radio_values(); // set type of output, symmetrical angles or a number range; + // XXX BROKEN rc_1.set_angle(4500); rc_1.set_dead_zone(80); + rc_1.set_type(RC_CHANNEL_TYPE_ANGLE_RAW); + + // XXX BROKEN rc_2.set_angle(4500); rc_2.set_dead_zone(80); + rc_2.set_type(RC_CHANNEL_TYPE_ANGLE_RAW); + rc_3.set_range(0,1000); rc_3.set_dead_zone(20); + rc_4.set_angle(6000); rc_4.set_dead_zone(500); + rc_4.set_type(RC_CHANNEL_TYPE_ANGLE_RAW); + rc_5.set_range(0,1000); rc_6.set_range(200,800); + rc_7.set_range(0,1000); + rc_8.set_range(0,1000); -#if 0 - for (byte i=0; i<8; i++) { - rc[i].set_reverse(false); - } -#endif - - for (byte i = 0; i < 30; i++) { + for (int i = 0; i < 30; i++) { read_radio(); } rc_1.trim(); @@ -110,32 +79,52 @@ void setup() void loop() { - delay(20); + hal.scheduler->delay(20); + debug_rcin(); read_radio(); print_pwm(); } +void debug_rcin() { + uint16_t channels[8]; + hal.rcin->read(channels, 8); + hal.console->printf_P( + PSTR("rcin: %u %u %u %u %u %u %u %u\r\n"), + channels[0], + channels[1], + channels[2], + channels[3], + channels[4], + channels[5], + channels[6], + channels[7]); +} void read_radio() { - for (byte i=0; i<8; i++) { - rc[i].set_pwm(APM_RC.InputCh(CH_1+i)); - } + rc_1.set_pwm(hal.rcin->read(CH_1)); + rc_2.set_pwm(hal.rcin->read(CH_2)); + rc_3.set_pwm(hal.rcin->read(CH_3)); + rc_4.set_pwm(hal.rcin->read(CH_4)); + rc_5.set_pwm(hal.rcin->read(CH_5)); + rc_6.set_pwm(hal.rcin->read(CH_6)); + rc_7.set_pwm(hal.rcin->read(CH_7)); + rc_8.set_pwm(hal.rcin->read(CH_8)); } void print_pwm() { - for (byte i=0; i<8; i++) { - Serial.printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].control_in); + for (int i=0; i<8; i++) { + hal.console->printf("ch%u: %4d ", (unsigned)i+1, (int)rc[i].control_in); } - Serial.printf("\n"); + hal.console->printf("\n"); } -void -print_radio_values() + +void print_radio_values() { - for (byte i=0; i<8; i++) { - Serial.printf("CH%u: %u|%u\n", + for (int i=0; i<8; i++) { + hal.console->printf("CH%u: %u|%u\n", (unsigned)i+1, (unsigned)rc[i].radio_min, (unsigned)rc[i].radio_max); @@ -146,11 +135,11 @@ print_radio_values() void setup_radio() { - Serial.println("\n\nRadio Setup:"); + hal.console->println("\n\nRadio Setup:"); uint8_t i; for(i = 0; i < 100;i++){ - delay(20); + hal.scheduler->delay(20); read_radio(); } @@ -181,10 +170,10 @@ setup_radio() rc_7.radio_trim = 1500; rc_8.radio_trim = 1500; - Serial.println("\nMove all controls to each extreme. Hit Enter to save:"); + hal.console->println("\nMove all controls to each extreme. Hit Enter to save:"); while(1){ - delay(20); + hal.scheduler->delay(20); // Filters radio input - adjust filters in the radio.pde file // ---------------------------------------------------------- read_radio(); @@ -198,13 +187,12 @@ setup_radio() rc_7.update_min_max(); rc_8.update_min_max(); - if(Serial.available() > 0){ - //rc_3.radio_max += 250; - Serial.flush(); - - Serial.println("Radio calibrated, Showing control values:"); - break; - } - } - return; + if(hal.console->available() > 0) { + hal.console->println("Radio calibrated, Showing control values:"); + break; + } + } + return; } + +AP_HAL_MAIN(); diff --git a/libraries/RC_Channel/examples/RC_Channel/nocore.inoflag b/libraries/RC_Channel/examples/RC_Channel/nocore.inoflag new file mode 100644 index 0000000000..e69de29bb2 diff --git a/libraries/RC_Channel/examples/RC_Channel2/Makefile b/libraries/RC_Channel/examples/RC_Channel2/Makefile deleted file mode 100644 index 85b4d8856b..0000000000 --- a/libraries/RC_Channel/examples/RC_Channel2/Makefile +++ /dev/null @@ -1,2 +0,0 @@ -BOARD = mega -include ../../../AP_Common/Arduino.mk