From 267ac3cc6a37d2ccc43c705598ebac8b4c3ab3f7 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Mon, 23 Aug 2010 21:47:31 +0000 Subject: [PATCH] New RC lib for 328 git-svn-id: https://arducopter.googlecode.com/svn/trunk@284 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_RC/AP_RC.cpp | 237 ++++++++++++++++++ libraries/AP_RC/AP_RC.h | 22 ++ .../AP_RC/examples/AP_radio/AP_radio.pde | 48 ++++ libraries/AP_RC/keywords.txt | 4 + 4 files changed, 311 insertions(+) create mode 100644 libraries/AP_RC/AP_RC.cpp create mode 100644 libraries/AP_RC/AP_RC.h create mode 100644 libraries/AP_RC/examples/AP_radio/AP_radio.pde create mode 100644 libraries/AP_RC/keywords.txt diff --git a/libraries/AP_RC/AP_RC.cpp b/libraries/AP_RC/AP_RC.cpp new file mode 100644 index 0000000000..74dba32c54 --- /dev/null +++ b/libraries/AP_RC/AP_RC.cpp @@ -0,0 +1,237 @@ +/* + AP_Radio.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" + +#define CH1 0 +#define CH2 1 +#define CH3 2 +#define CH4 3 +#define THROTTLE_PIN 13 +#include + +#define CH3TRIM 1100 + +// 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 uint16_t timer1diff = 1500 * 2; +volatile uint16_t timer2diff = 1500 * 2; +volatile uint16_t timer3diff = 1100 * 2; +volatile uint16_t timer4diff = 1500 * 2; + + +#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; +//volatile uint16_t ap_rc_input[4]; + + +AP_RC::AP_RC() +{ + 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::read_pwm() +{ + input[CH1] = timer1diff; + input[CH2] = timer2diff; + input[CH3] = timer3diff; + input[CH4] = timer4diff; +} + +void AP_RC::init(int trims[]) +{ + // 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); + // apply initial values + set_ch_pwm(CH1, 1500); + set_ch_pwm(CH2, 1500); + 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 + // apply initial values + //OCR2A = (trims[CH3]-1000) / 4; + //OCR2B = trims[CH4] / 4; // center the rudder + set_ch_pwm(CH3, CH3TRIM); + set_ch_pwm(CH4, 1500); + + 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::set_ch_pwm(uint8_t ch, uint16_t pwm) +{ + switch(ch){ + case CH1: + pwm <<= 1; + OCR1A = pwm; + break; + + case CH2: + pwm <<= 1; + OCR1B = pwm; + break; + + case CH3: + // Jason's fancy 2µs hack + _timer_out = pwm % 512; + _timer_ovf_a = pwm / 512; + _timer_out >>= 1; + OCR2A = _timer_out; + break; + + case CH4: + _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); // Timer1 TOP = 40000 + else + timer1diff = (cnt - timer1count); + timer1diff >>= 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); // Timer1 TOP = 40000 + else + timer2diff = (cnt - timer2count); + timer2diff >>= 1; + } +} + +ISR(PCINT0_vect) +{ + int cnt = TCNT1; + +#if THROTTLE_PIN == 11 + if(PINB & 8){ // pin 11 +#else + if(PINB & 32){ // pin 13 +#endif + 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); // Timer1 TOP = 40000 + else + timer3diff = (cnt - timer3count); + timer3diff >>= 1; + + } + +#if THROTTLE_PIN == 11 + if(PINB & 32){ // pin 13 +#else + if(PINB & 8){ // pin 11 +#endif + 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); // Timer1 TOP = 40000 + else + timer4diff = (cnt - timer4count); + timer4diff >>= 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/AP_RC/AP_RC.h b/libraries/AP_RC/AP_RC.h new file mode 100644 index 0000000000..5f98e1e534 --- /dev/null +++ b/libraries/AP_RC/AP_RC.h @@ -0,0 +1,22 @@ +#ifndef AP_RC_h +#define AP_RC_h + +#include +#include "WProgram.h" + +class AP_RC +{ + public: + AP_RC(); + void set_ch_pwm(uint8_t ch, uint16_t pwm); + void init(int trims[]); + void read_pwm(); + + uint16_t input[4]; + + private: + uint16_t _timer_out; +}; + +#endif + diff --git a/libraries/AP_RC/examples/AP_radio/AP_radio.pde b/libraries/AP_RC/examples/AP_radio/AP_radio.pde new file mode 100644 index 0000000000..0c0cd5a88e --- /dev/null +++ b/libraries/AP_RC/examples/AP_radio/AP_radio.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 library test"); + + int trims[] = {1500,1500,1200,1500}; + rc.init(trims); + + delay(1000); +} +void loop() +{ + delay(20); + rc.read_pwm(); + for(int y=0; y<4; y++) { + rc.set_ch_pwm(y, rc.input[y]); // send to Servos + } + print_pwm(); +} + + +void print_pwm() +{ + Serial.print("ch1 "); + Serial.print(rc.input[CH_ROLL],DEC); + Serial.print("\tch2: "); + Serial.print(rc.input[CH_PITCH],DEC); + Serial.print("\tch3 :"); + Serial.print(rc.input[CH_THROTTLE],DEC); + Serial.print("\tch4 :"); + Serial.println(rc.input[CH_RUDDER],DEC); +} \ No newline at end of file diff --git a/libraries/AP_RC/keywords.txt b/libraries/AP_RC/keywords.txt new file mode 100644 index 0000000000..6d1f6c13ae --- /dev/null +++ b/libraries/AP_RC/keywords.txt @@ -0,0 +1,4 @@ +AP_RC KEYWORD1 +init KEYWORD2 +set_ch_pwm KEYWORD2 +read_pwm KEYWORD2