diff --git a/libraries/RC/APM2_RC.cpp b/libraries/RC/APM2_RC.cpp new file mode 100644 index 0000000000..957b5e1086 --- /dev/null +++ b/libraries/RC/APM2_RC.cpp @@ -0,0 +1,306 @@ +#ifdef __AVR_ATmega1280__ +/* + APM2_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino + Code by Jordi Muńoz and Jose Julio. 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. + + RC Input : PPM signal on IC4 pin + RC Output : 11 Servo outputs (standard 20ms frame) + + Methods: + Init() : Initialization of interrupts an Timers + OutpuCh(ch, pwm) : Output value to servos (range : 900 - 2100us) ch = 0..10 + InputCh(ch) : Read a channel input value. ch = 0..7 + GetState() : Returns the state of the input. 1 => New radio frame to process + Automatically resets when we call InputCh to read channels + +*/ + +#include "APM2_RC.h" + +#define REVERSE 3050 + +// Variable definition for Input Capture interrupt +volatile uint16_t ICR4_old; +volatile uint8_t PPM_Counter = 0; +volatile uint16_t raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200}; + +// Constructors //////////////////////////////////////////////////////////////// +APM2_RC::APM2_RC() +{ + _direction_mask = 255; // move to super class + +} + +void +APM2_RC::init() +{ + // Init PWM Timer 1 + pinMode(11, OUTPUT); // (PB5 / OC1A) + pinMode(12, OUTPUT); // OUT2 (PB6 / OC1B) + pinMode(13, OUTPUT); // OUT3 (PB7 / OC1C) + + // Timer 3 + pinMode(2, OUTPUT); // OUT7 (PE4 / OC3B) + pinMode(3, OUTPUT); // OUT6 (PE5 / OC3C) + pinMode(4, OUTPUT); // (PE3 / OC3A) + + // Timer 5 + pinMode(44, OUTPUT); // OUT1 (PL5 / OC5C) + pinMode(45, OUTPUT); // OUT0 (PL4 / OC5B) + pinMode(46, OUTPUT); // (PL3 / OC5A) + + // Init PPM input and PWM Timer 4 + pinMode(49, INPUT); // ICP4 pin (PL0) (PPM input) + pinMode(7, OUTPUT); // OUT5 (PH4 / OC4B) + pinMode(8, OUTPUT); // OUT4 (PH5 / OC4C) + + //Remember the registers not declared here remains zero by default... + TCCR1A =((1 << WGM11) | (1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1)); // Please read page 131 of DataSheet, we are changing the registers settings of WGM11, COM1B1, COM1A1 to 1 thats all... + TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); // Prescaler set to 8, that give us a resolution of 0.5us, read page 134 of data sheet + OCR1A = 3000; // PB5, none + //OCR1B = 3000; // PB6, OUT2 + //OCR1C = 3000; // PB7 OUT3 + ICR1 = 40000; // 50hz freq...Datasheet says (system_freq / prescaler) / target frequency. So (16000000hz / 8) / 50hz = 40000, + + // Init PWM Timer 3 + TCCR3A =((1 << WGM31) | (1 << COM3A1) | (1 << COM3B1) | (1 << COM3C1)); + TCCR3B = (1 << WGM33) | (1 << WGM32) | (1 << CS31); + OCR3A = 3000; // PE3, NONE + //OCR3B = 3000; // PE4, OUT7 + //OCR3C = 3000; // PE5, OUT6 + ICR3 = 40000; // 50hz freq + + + // Init PWM Timer 5 + TCCR5A =((1 << WGM51) | (1 << COM5A1) | (1 << COM5B1) | (1 << COM5C1)); + TCCR5B = (1 << WGM53) | (1 << WGM52) | (1 << CS51); + OCR5A = 3000; // PL3, + //OCR5B = 3000; // PL4, OUT0 + //OCR5C = 3000; // PL5, OUT1 + ICR5 = 40000; // 50hz freq + + + // Init PPM input and PWM Timer 4 + TCCR4A = ((1 << WGM40) | (1 << WGM41) | (1 << COM4C1) | (1 << COM4B1) | (1 << COM4A1)); + TCCR4B = ((1 << WGM43) | (1 << WGM42) | (1 << CS41) | (1 << ICES4)); + OCR4A = 40000; // /50hz freq. + //OCR4B = 3000; // PH4, OUT5 + //OCR4C = 3000; // PH5, OUT4 + + //TCCR4B |=(1< PPM signal read + ****************************************************/ +ISR(TIMER4_CAPT_vect) +{ + uint16_t pulse; + uint16_t pulse_width; + + pulse = ICR4; + if (pulse < ICR4_old){ // Take care of the overflow of Timer4 (TOP = 40000) + pulse_width = (pulse + 40000) - ICR4_old; // Calculating pulse + }else{ + pulse_width = pulse - ICR4_old; // Calculating pulse + } + ICR4_old = pulse; + + + if (pulse_width > 8000){ // SYNC pulse + PPM_Counter = 0; + } else { + //PPM_Counter &= 0x07; // For safety only (limit PPM_Counter to 7) + raw[PPM_Counter++] = pulse_width >> 1; // Saving pulse. + } +} + + + +// InstantPWM implementation +// This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use +void APM2_RC::force_out_0_1(void) +{ + if (TCNT5 > 5000) // We take care that there are not a pulse in the output + TCNT5 = 39990; // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000 +} +// This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use +void APM2_RC::force_out_2_3(void) +{ + if (TCNT1 > 5000) + TCNT1 = 39990; +} + +// This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use +void APM2_RC::force_out_6_7(void) +{ + if (TCNT3 > 5000) + TCNT3 = 39990; +} +#endif \ No newline at end of file diff --git a/libraries/RC/APM2_RC.h b/libraries/RC/APM2_RC.h new file mode 100644 index 0000000000..e7541b474f --- /dev/null +++ b/libraries/RC/APM2_RC.h @@ -0,0 +1,36 @@ +#ifndef APM2_RC_h +#define APM2_RC_h + +#include +#include "WProgram.h" +#include "RC.h" + +class APM2_RC : public RC +{ + public: + APM2_RC(); + void read_pwm(); + void set_ch_pwm(uint8_t ch, uint16_t pwm); + void init(); + void set_throttle(float percent); + void trim(); + + void force_out_0_1(void); + void force_out_2_3(void); + void force_out_6_7(void); + + int16_t radio_in[8]; + int16_t radio_min[8]; + int16_t radio_trim[8]; + int16_t radio_max[8]; + + float servo_out[8]; + + private: + uint16_t _timer_out; +}; + +#endif + + +//volatile uint16_t raw[8] = {1200, 1200, 1200, 1200, 1200, 1200, 1200, 1200}; diff --git a/libraries/RC/AP_RC.cpp b/libraries/RC/AP_RC.cpp new file mode 100644 index 0000000000..2737c5ee4f --- /dev/null +++ b/libraries/RC/AP_RC.cpp @@ -0,0 +1,293 @@ +/* + AP_RC.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 "AP_RC.h" +#include +#define REVERSE 3050 + +// Variable definition for interrupt +volatile uint16_t timer1count = 0; +volatile uint16_t timer2count = 0; +volatile uint16_t timer3count = 0; +volatile uint16_t timer4count = 0; + +volatile int16_t timer1diff = 1500 * 2; +volatile int16_t timer2diff = 1500 * 2; +volatile int16_t timer3diff = 1100 * 2; +volatile int16_t timer4diff = 1500 * 2; + +//volatile uint16_t raw[8]; + +#define CH1_READ 1 +#define CH2_READ 2 +#define CH3_READ 4 +#define CH4_READ 8 + +volatile int8_t _rc_ch_read = 0; +volatile uint8_t _timer_ovf_a = 0; +volatile uint8_t _timer_ovf_b = 0; +volatile uint8_t _timer_ovf = 0; + + +AP_RC::AP_RC() +{ + _direction_mask = 255; // move to super class + pinMode(11,INPUT); // PB3 - MOSI/OC2 - Throttle in + pinMode(13,INPUT); // PB5 - SCK - Rudder in + pinMode(8, OUTPUT); // PB0 - AIN1 - OUTPUT THROTTLE + pinMode(9, OUTPUT); // PB1 - OC1A - Elevator PWM out + pinMode(10,OUTPUT); // PB2 - OC1B - Aileron PWM out + // set Analog out 4 to output + DDRC |= B00010000; +} + +void +AP_RC::init() +{ + // enable pin change interrupt 2 - PCINT23..16 + PCICR = _BV(PCIE2); + // enable pin change interrupt 0 - PCINT7..0 + PCICR |= _BV(PCIE0); + // enable in change interrupt on PB5 (digital pin 13) + PCMSK0 = _BV(PCINT3) | _BV(PCINT5); + // enable pin change interrupt on PD2,PD3 (digital pin 2,3) + PCMSK2 = _BV(PCINT18) | _BV(PCINT19); + + // Timer 1 + TCCR1A = ((1 << WGM11) | (1 << COM1B1) | (1 << COM1A1)); + TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); + // Loop value + ICR1 = 40000; + + // Throttle; + // Setting up the Timer 2 - 8 bit timer + TCCR2A = 0x0; // Normal Mode + TCCR2B = _BV(CS21) |_BV(CS20); //prescaler 32, at 16mhz (32/16) = 2, the counter will increment 1 every 2us + + // trim out the radio + for(int c = 0; c < 50; c++){ + delay(20); + read_pwm(); + } + + trim(); + + for(int y = 0; y < 4; y++) { + set_ch_pwm(y, radio_trim[y]); + } + + // enable throttle and Ch4 output + TIMSK1 |= _BV(ICIE1); // Timer / Counter1, Input Capture Interrupt Enable // PB0 - output throttle + TIMSK2 = _BV(TOIE1) | _BV(OCIE2A) | _BV(OCIE2B); // Timer / Counter2 Compare Match A +} + +void +AP_RC::read_pwm() +{ + if((_direction_mask & 1) == 0 ) + timer1diff = REVERSE - timer1diff; + if((_direction_mask & 2) == 0 ) + timer2diff = REVERSE - timer2diff; + if((_direction_mask & 4) == 0 ) + timer3diff = REVERSE - timer3diff; + if((_direction_mask & 8) == 0 ) + timer4diff = REVERSE - timer4diff; + + if(_mix_mode == 1){ + // elevons + int16_t ailerons = (timer1diff - radio_trim[CH1]) * .3; + int16_t elevator = (timer2diff - radio_trim[CH2]) * .7; + + radio_in[CH1] = (elevator - ailerons); // left + radio_in[CH2] = (elevator + ailerons); // right + + radio_in[CH1] += radio_trim[CH1]; + radio_in[CH2] += radio_trim[CH2]; + + //Serial.print("radio_in[CH1] "); + //Serial.print(radio_in[CH1],DEC); + //Serial.print(" \tradio_in[CH2] "); + //Serial.println(radio_in[CH2],DEC); + + }else{ + // normal + radio_in[CH1] = timer1diff; + radio_in[CH2] = timer2diff; + } + + radio_in[CH3] = (float)radio_in[CH3] * .9 + timer3diff * .1; + radio_in[CH4] = timer4diff; + + check_throttle_failsafe(radio_in[CH3]); + + // output servo + servo_out[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0; + servo_out[CH3] = constrain(servo_out[CH3], 0, 100); +} + +void +AP_RC::trim() +{ + uint8_t temp = _mix_mode; + _mix_mode = 0; + read_pwm(); + _mix_mode = temp; + + radio_trim[CH1] = radio_in[CH1]; + radio_trim[CH2] = radio_in[CH2]; + radio_trim[CH3] = radio_in[CH3]; + radio_trim[CH4] = radio_in[CH4]; + + //Serial.print("trim "); + //Serial.println(radio_trim[CH1], DEC); +} + +void +AP_RC::set_throttle(float percent) +{ + uint16_t out = (percent * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0; + out += radio_min[CH3]; + set_ch_pwm(CH3, out); +} + +void +AP_RC::set_ch_pwm(uint8_t ch, uint16_t pwm) +{ + switch(ch){ + case CH1: + if((_direction_mask & 1) == 0 ) + pwm = REVERSE - pwm; + pwm <<= 1; + OCR1A = pwm; + break; + + case CH2: + if((_direction_mask & 2) == 0 ) + pwm = REVERSE - pwm; + pwm <<= 1; + OCR1B = pwm; + break; + + case CH3: + if((_direction_mask & 4) == 0 ) + pwm = REVERSE - pwm; + // Jason's fancy 2µs hack + _timer_out = pwm % 512; + _timer_ovf_a = pwm / 512; + _timer_out >>= 1; + OCR2A = _timer_out; + break; + + case CH4: + if((_direction_mask & 8) == 0 ) + pwm = REVERSE - pwm; + _timer_out = pwm % 512; + _timer_ovf_b = pwm / 512; + _timer_out >>= 1; + OCR2B = _timer_out; + break; + } +} + + + +// radio PWM input timers +ISR(PCINT2_vect) { + int cnt = TCNT1; + if(PIND & B00000100){ // ch 1 (pin 2) is high + if ((_rc_ch_read & CH1_READ) != CH1_READ){ + _rc_ch_read |= CH1_READ; + timer1count = cnt; + } + }else if ((_rc_ch_read & CH1_READ) == CH1_READ){ // ch 1 (pin 2) is Low, and we were reading + _rc_ch_read &= B11111110; + if (cnt < timer1count) // Timer1 reset during the read of this pulse + timer1diff = (cnt + 40000 - timer1count) >> 1; // Timer1 TOP = 40000 + else + timer1diff = (cnt - timer1count) >> 1; + } + + if(PIND & B00001000){ // ch 2 (pin 3) is high + if ((_rc_ch_read & CH2_READ) != CH2_READ){ + _rc_ch_read |= CH2_READ; + timer2count = cnt; + } + }else if ((_rc_ch_read & CH2_READ) == CH2_READ){ // ch 1 (pin 2) is Low + _rc_ch_read &= B11111101; + if (cnt < timer2count) // Timer1 reset during the read of this pulse + timer2diff = (cnt + 40000 - timer2count) >> 1; // Timer1 TOP = 40000 + else + timer2diff = (cnt - timer2count) >> 1; + } +} + +ISR(PCINT0_vect) +{ + int cnt = TCNT1; + if(PINB & 8){ // pin 11 + if ((_rc_ch_read & CH3_READ) != CH3_READ){ + _rc_ch_read |= CH3_READ; + timer3count = cnt; + } + }else if ((_rc_ch_read & CH3_READ) == CH3_READ){ // ch 1 (pin 2) is Low + _rc_ch_read &= B11111011; + if (cnt < timer3count) // Timer1 reset during the read of this pulse + timer3diff = (cnt + 40000 - timer3count) >> 1; // Timer1 TOP = 40000 + else + timer3diff = (cnt - timer3count) >> 1; + } + + if(PINB & 32){ // pin 13 + if ((_rc_ch_read & CH4_READ) != CH4_READ){ + _rc_ch_read |= CH4_READ; + timer4count = cnt; + } + }else if ((_rc_ch_read & CH4_READ) == CH4_READ){ // ch 1 (pin 2) is Low + _rc_ch_read &= B11110111; + if (cnt < timer4count) // Timer1 reset during the read of this pulse + timer4diff = (cnt + 40000 - timer4count) >> 1; // Timer1 TOP = 40000 + else + timer4diff = (cnt - timer4count) >> 1; + } +} + + + +// Throttle Timer Interrupt +// ------------------------ +ISR(TIMER1_CAPT_vect) // Timer/Counter1 Capture Event +{ + //This is a timer 1 interrupts, executed every 20us + PORTB |= B00000001; //Putting the pin high! + PORTC |= B00010000; //Putting the pin high! + TCNT2 = 0; //restarting the counter of timer 2 + _timer_ovf = 0; +} + +ISR(TIMER2_OVF_vect) +{ + _timer_ovf++; +} + +ISR(TIMER2_COMPA_vect) // Timer/Counter2 Compare Match A +{ + if(_timer_ovf == _timer_ovf_a){ + PORTB &= B11111110; //Putting the pin low + } +} + +ISR(TIMER2_COMPB_vect) // Timer/Counter2 Compare Match B Rudder Servo +{ + if(_timer_ovf == _timer_ovf_b){ + PORTC &= B11101111; //Putting the pin low! + } +} + diff --git a/libraries/RC/AP_RC.h b/libraries/RC/AP_RC.h new file mode 100644 index 0000000000..05f49b6464 --- /dev/null +++ b/libraries/RC/AP_RC.h @@ -0,0 +1,30 @@ +#ifndef AP_RC_h +#define AP_RC_h + +#include +#include "WProgram.h" +#include "RC.h" + +class AP_RC : public RC +{ + public: + AP_RC(); + void read_pwm(); + void set_ch_pwm(uint8_t ch, uint16_t pwm); + void init(); + void set_throttle(float percent); + void trim(); + + int16_t radio_in[4]; + int16_t radio_min[4]; + int16_t radio_trim[4]; + int16_t radio_max[4]; + + float servo_out[4]; + + private: + uint16_t _timer_out; +}; + +#endif + diff --git a/libraries/RC/RC.cpp b/libraries/RC/RC.cpp new file mode 100644 index 0000000000..0f92dfd5b7 --- /dev/null +++ b/libraries/RC/RC.cpp @@ -0,0 +1,84 @@ +/* + AP_RC.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 "RC.h" + +/* +RC::RC()// : + _direction_mask(255) +{ +} +*/ + +// direction mask: 0 = normal, 1 = reversed servos +void +RC::set_channel_direction(uint8_t ch, int8_t dir) +{ + uint8_t bitflip = 1 << ch; + + if(dir == 1){ + _direction_mask |= bitflip; + }else{ + _direction_mask &= ~bitflip; + } +} + +void +RC::set_failsafe(uint16_t v) +{ + _fs_value = v; +} + +void +RC::set_mix_mode(uint8_t m) +{ + _mix_mode = m; +} + + +void +RC::check_throttle_failsafe(uint16_t throttle) +{ + //check for failsafe and debounce funky reads + // ------------------------------------------ + if (throttle < _fs_value){ + // we detect a failsafe from radio + // throttle has dropped below the mark + _fs_counter++; + if (_fs_counter == 9){ + + }else if(_fs_counter == 10) { + failsafe = 1; + }else if (_fs_counter > 10){ + _fs_counter = 11; + } + + }else if(_fs_counter > 0){ + // we are no longer in failsafe condition + // but we need to recover quickly + _fs_counter--; + if (_fs_counter > 3){ + _fs_counter = 3; + } + if (_fs_counter == 1){ + + }else if(_fs_counter == 0) { + failsafe = 0; + }else if (_fs_counter <0){ + _fs_counter = -1; + } + } +} + + + diff --git a/libraries/RC/RC.h b/libraries/RC/RC.h new file mode 100644 index 0000000000..fb45552b5e --- /dev/null +++ b/libraries/RC/RC.h @@ -0,0 +1,48 @@ +#ifndef RC_h +#define RC_h + +#include +#include "WProgram.h" + +#define CH1 0 +#define CH2 1 +#define CH3 2 +#define CH4 3 +#define CH5 4 +#define CH6 5 +#define CH7 6 +#define CH8 7 + +#define MIN_PULSEWIDTH 900 +#define MAX_PULSEWIDTH 2100 + +#define ELEVONS 1 + +class RC +{ + public: + // RC(); + virtual void read_pwm(); + virtual void set_ch_pwm(uint8_t ch, uint16_t pwm); + virtual void init(); + virtual void trim(); + virtual void set_channel_direction(uint8_t ch, int8_t dir); + virtual void set_throttle(float percent); + + + void set_failsafe(uint16_t fs); + void set_mix_mode(uint8_t mode); + + + uint8_t failsafe; + uint8_t reverse_throttle; + + protected: + void check_throttle_failsafe(uint16_t throttle); + uint8_t _fs_counter; + uint8_t _mix_mode; // 0 = normal, 1 = elevons + uint8_t _direction_mask; + uint16_t _fs_value; // PWM value to trigger failsafe flag +}; + +#endif diff --git a/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde b/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde new file mode 100644 index 0000000000..196fafc00a --- /dev/null +++ b/libraries/RC/examples/APM_RC_elevons/APM_RC_elevons.pde @@ -0,0 +1,59 @@ +/* + Example of APM2_RC library. + Code by Jordi MuĂ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +APM2_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot Mega RC library test"); + + //rc.set_channel_direction(CH_ROLL, -1); + //rc.set_channel_direction(CH_PITCH, -1); + //rc.set_channel_direction(CH_THROTTLE, -1); + //rc.set_channel_direction(CH_RUDDER, -1); + rc.set_mix_mode(1); // 1 = elevons, 0 = normal + rc.init(); + delay(1000); +} + +void loop() +{ + delay(20); + rc.read_pwm(); + for(int y = 0; y < 8; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + //print_pwm(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.print(rc.radio_in[CH_RUDDER], DEC); + Serial.print("\tch5 "); + Serial.print(rc.radio_in[4], DEC); + Serial.print("\tch6: "); + Serial.print(rc.radio_in[5], DEC); + Serial.print("\tch7 :"); + Serial.print(rc.radio_in[6], DEC); + Serial.print("\tch8 :"); + Serial.println(rc.radio_in[7], DEC); +} \ No newline at end of file diff --git a/libraries/RC/examples/APM_RC_test/APM_RC_test.pde b/libraries/RC/examples/APM_RC_test/APM_RC_test.pde new file mode 100644 index 0000000000..33c722863b --- /dev/null +++ b/libraries/RC/examples/APM_RC_test/APM_RC_test.pde @@ -0,0 +1,59 @@ +/* + Example of APM2_RC library. + Code by Jordi MuĂ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +APM2_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot Mega RC library test"); + + //rc.set_channel_direction(CH_ROLL, -1); + //rc.set_channel_direction(CH_PITCH, -1); + //rc.set_channel_direction(CH_THROTTLE, -1); + //rc.set_channel_direction(CH_RUDDER, -1); + rc.init(); + delay(1000); +} + +void loop() +{ + delay(20); + rc.read_pwm(); + + for(int y = 0; y < 8; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + //print_pwm(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.print(rc.radio_in[CH_RUDDER], DEC); + Serial.print("\tch5 "); + Serial.print(rc.radio_in[4], DEC); + Serial.print("\tch6: "); + Serial.print(rc.radio_in[5], DEC); + Serial.print("\tch7 :"); + Serial.print(rc.radio_in[6], DEC); + Serial.print("\tch8 :"); + Serial.println(rc.radio_in[7], DEC); +} \ No newline at end of file diff --git a/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde b/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde new file mode 100644 index 0000000000..810b2df8ac --- /dev/null +++ b/libraries/RC/examples/AP_RC_elevons/AP_RC_elevons.pde @@ -0,0 +1,48 @@ +/* + Example of AP_RC library. + Code by Jordi MuĂ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +AP_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot RC Elevon library test"); + + rc.set_mix_mode(1); // 1 = elevons, 0 = normal + rc.init(); + + delay(1000); +} +void loop() +{ + delay(60); + rc.read_pwm(); + for(int y = 0; y < 4; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + print_pwm(); +} + + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.println(rc.radio_in[CH_RUDDER], DEC); +} \ No newline at end of file diff --git a/libraries/RC/examples/AP_RC_test/AP_RC_test.pde b/libraries/RC/examples/AP_RC_test/AP_RC_test.pde new file mode 100644 index 0000000000..1130908340 --- /dev/null +++ b/libraries/RC/examples/AP_RC_test/AP_RC_test.pde @@ -0,0 +1,50 @@ +/* + Example of AP_RC library. + Code by Jordi MuĂ’oz and Jose Julio. DIYDrones.com + + Print Input values and send Output to the servos + (Works with last PPM_encoder firmware) +*/ + +#include // ArduPilot Mega RC Library +AP_RC rc; + +#define CH_ROLL 0 +#define CH_PITCH 1 +#define CH_THROTTLE 2 +#define CH_RUDDER 3 + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot RC library test"); + + //rc.set_channel_direction(CH_ROLL, -1); + //rc.set_channel_direction(CH_PITCH, -1); + //rc.set_channel_direction(CH_THROTTLE, -1); + //rc.set_channel_direction(CH_RUDDER, -1); + rc.init(); + delay(1000); +} + +void loop() +{ + delay(20); + rc.read_pwm(); + for(int y = 0; y < 4; y++) { + rc.set_ch_pwm(y, rc.radio_in[y]); // send to Servos + } + print_pwm(); +} + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.radio_in[CH_ROLL], DEC); + Serial.print("\tch2: "); + Serial.print(rc.radio_in[CH_PITCH], DEC); + Serial.print("\tch3 :"); + Serial.print(rc.radio_in[CH_THROTTLE], DEC); + Serial.print("\tch4 :"); + Serial.println(rc.radio_in[CH_RUDDER], DEC); +} \ No newline at end of file diff --git a/libraries/RC/keywords.txt b/libraries/RC/keywords.txt new file mode 100644 index 0000000000..6d1f6c13ae --- /dev/null +++ b/libraries/RC/keywords.txt @@ -0,0 +1,4 @@ +AP_RC KEYWORD1 +init KEYWORD2 +set_ch_pwm KEYWORD2 +read_pwm KEYWORD2