00001 /* 00002 AP_RC.cpp - Radio library for Arduino 00003 Code by Jason Short. DIYDrones.com 00004 00005 This library is free software; you can redistribute it and / or 00006 modify it under the terms of the GNU Lesser General Public 00007 License as published by the Free Software Foundation; either 00008 version 2.1 of the License, or (at your option) any later version. 00009 00010 */ 00011 00012 #include "AP_RC.h" 00013 #include "WProgram.h" 00014 #include <avr/interrupt.h> 00015 00016 // Variable definition for interrupt 00017 volatile uint16_t timer1count = 0; 00018 volatile uint16_t timer2count = 0; 00019 volatile uint16_t timer3count = 0; 00020 volatile uint16_t timer4count = 0; 00021 00022 volatile int16_t timer1diff = 1500 * 2; 00023 volatile int16_t timer2diff = 1500 * 2; 00024 volatile int16_t timer3diff = 1100 * 2; 00025 volatile int16_t timer4diff = 1500 * 2; 00026 00027 //volatile uint16_t raw[8]; 00028 #define CH1_READ 1 00029 #define CH2_READ 2 00030 #define CH3_READ 4 00031 #define CH4_READ 8 00032 00033 #define CH_1 0 00034 #define CH_2 1 00035 #define CH_3 2 00036 #define CH_4 3 00037 00038 volatile int8_t _rc_ch_read; 00039 volatile uint8_t _timer_out; 00040 volatile uint8_t _timer_ovf_a; 00041 volatile uint8_t _timer_ovf_b; 00042 volatile uint8_t _timer_ovf; 00043 00044 00045 AP_RC::AP_RC() 00046 { 00047 pinMode(2,INPUT); // PD2 - INT0 - CH 1 in 00048 pinMode(3,INPUT); // PD3 - INT1 - CH 2 in 00049 pinMode(11,INPUT); // PB3 - MOSI/OC2 - CH 3 in 00050 pinMode(13,INPUT); // PB5 - SCK - CH 4 in 00051 00052 pinMode(10,OUTPUT); // PB2 - OC1B - CH 1 out 00053 pinMode(8, OUTPUT); // PB0 - AIN1 - CH 3 out 00054 pinMode(9, OUTPUT); // PB1 - OC1A - CH 2 out 00055 DDRC |= B00010000; // PC4 - - CH 4 out 00056 } 00057 00058 void 00059 AP_RC::init() 00060 { 00061 // enable pin change interrupt 2 - PCINT23..16 00062 PCICR = _BV(PCIE2); 00063 // enable pin change interrupt 0 - PCINT7..0 00064 PCICR |= _BV(PCIE0); 00065 // enable in change interrupt on PB5 (digital pin 13) 00066 PCMSK0 = _BV(PCINT3) | _BV(PCINT5); 00067 // enable pin change interrupt on PD2,PD3 (digital pin 2,3) 00068 PCMSK2 = _BV(PCINT18) | _BV(PCINT19); 00069 00070 // Timer 1 00071 TCCR1A = ((1 << WGM11) | (1 << COM1B1) | (1 << COM1A1)); 00072 TCCR1B = (1 << WGM13) | (1 << WGM12) | (1 << CS11); 00073 // Loop value 00074 ICR1 = 40000; 00075 00076 // Throttle; 00077 // Setting up the Timer 2 - 8 bit timer 00078 TCCR2A = 0x0; // Normal Mode 00079 TCCR2B = _BV(CS21) |_BV(CS20); //prescaler 32, at 16mhz (32/16) = 2, the counter will increment 1 every 2us 00080 00081 // enable throttle and Ch4 output 00082 TIMSK1 |= _BV(ICIE1); // Timer / Counter1, Input Capture Interrupt Enable // PB0 - output throttle 00083 TIMSK2 = _BV(TOIE1) | _BV(OCIE2A) | _BV(OCIE2B); // Timer / Counter2 Compare Match A 00084 00085 /* 00086 set_ch_pwm(0, 1500); 00087 set_ch_pwm(1, 1500); 00088 set_ch_pwm(2, 1500); 00089 set_ch_pwm(3, 1500); 00090 */ 00091 } 00092 00093 uint16_t 00094 AP_RC::input_ch(uint8_t ch) 00095 { 00096 switch(ch){ 00097 case CH_1: 00098 return timer1diff; 00099 break; 00100 00101 case CH_2: 00102 return timer2diff; 00103 break; 00104 00105 case CH_3: 00106 return timer3diff; 00107 break; 00108 00109 case CH_4: 00110 return timer4diff; 00111 break; 00112 } 00113 } 00114 00115 void 00116 AP_RC::output_ch_pwm(uint8_t ch, uint16_t pwm) 00117 { 00118 switch(ch){ 00119 case CH_1: 00120 pwm <<= 1; 00121 OCR1A = pwm; 00122 break; 00123 00124 case CH_2: 00125 pwm <<= 1; 00126 OCR1B = pwm; 00127 break; 00128 00129 case CH_3: 00130 _timer_out = pwm % 512; 00131 _timer_ovf_a = pwm / 512; 00132 _timer_out >>= 1; 00133 OCR2A = _timer_out; 00134 break; 00135 00136 case CH_4: 00137 _timer_out = pwm % 512; 00138 _timer_ovf_b = pwm / 512; 00139 _timer_out >>= 1; 00140 OCR2B = _timer_out; 00141 break; 00142 } 00143 } 00144 00145 00146 00147 // radio PWM input timers 00148 ISR(PCINT2_vect) { 00149 int cnt = TCNT1; 00150 if(PIND & B00000100){ // ch 1 (pin 2) is high 00151 if ((_rc_ch_read & CH1_READ) != CH1_READ){ 00152 _rc_ch_read |= CH1_READ; 00153 timer1count = cnt; 00154 } 00155 }else if ((_rc_ch_read & CH1_READ) == CH1_READ){ // ch 1 (pin 2) is Low, and we were reading 00156 _rc_ch_read &= B11111110; 00157 if (cnt < timer1count) // Timer1 reset during the read of this pulse 00158 timer1diff = (cnt + 40000 - timer1count) >> 1; // Timer1 TOP = 40000 00159 else 00160 timer1diff = (cnt - timer1count) >> 1; 00161 } 00162 00163 if(PIND & B00001000){ // ch 2 (pin 3) is high 00164 if ((_rc_ch_read & CH2_READ) != CH2_READ){ 00165 _rc_ch_read |= CH2_READ; 00166 timer2count = cnt; 00167 } 00168 }else if ((_rc_ch_read & CH2_READ) == CH2_READ){ // ch 1 (pin 2) is Low 00169 _rc_ch_read &= B11111101; 00170 if (cnt < timer2count) // Timer1 reset during the read of this pulse 00171 timer2diff = (cnt + 40000 - timer2count) >> 1; // Timer1 TOP = 40000 00172 else 00173 timer2diff = (cnt - timer2count) >> 1; 00174 } 00175 } 00176 00177 ISR(PCINT0_vect) 00178 { 00179 int cnt = TCNT1; 00180 if(PINB & 8){ // pin 11 00181 if ((_rc_ch_read & CH3_READ) != CH3_READ){ 00182 _rc_ch_read |= CH3_READ; 00183 timer3count = cnt; 00184 } 00185 }else if ((_rc_ch_read & CH3_READ) == CH3_READ){ // ch 1 (pin 2) is Low 00186 _rc_ch_read &= B11111011; 00187 if (cnt < timer3count) // Timer1 reset during the read of this pulse 00188 timer3diff = (cnt + 40000 - timer3count) >> 1; // Timer1 TOP = 40000 00189 else 00190 timer3diff = (cnt - timer3count) >> 1; 00191 } 00192 00193 if(PINB & 32){ // pin 13 00194 if ((_rc_ch_read & CH4_READ) != CH4_READ){ 00195 _rc_ch_read |= CH4_READ; 00196 timer4count = cnt; 00197 } 00198 }else if ((_rc_ch_read & CH4_READ) == CH4_READ){ // ch 1 (pin 2) is Low 00199 _rc_ch_read &= B11110111; 00200 if (cnt < timer4count) // Timer1 reset during the read of this pulse 00201 timer4diff = (cnt + 40000 - timer4count) >> 1; // Timer1 TOP = 40000 00202 else 00203 timer4diff = (cnt - timer4count) >> 1; 00204 } 00205 } 00206 00207 00208 00209 // Throttle Timer Interrupt 00210 // ------------------------ 00211 ISR(TIMER1_CAPT_vect) // Timer/Counter1 Capture Event 00212 { 00213 //This is a timer 1 interrupts, executed every 20us 00214 PORTB |= B00000001; //Putting the pin high! 00215 PORTC |= B00010000; //Putting the pin high! 00216 TCNT2 = 0; //restarting the counter of timer 2 00217 _timer_ovf = 0; 00218 } 00219 00220 ISR(TIMER2_OVF_vect) 00221 { 00222 _timer_ovf++; 00223 } 00224 00225 ISR(TIMER2_COMPA_vect) // Timer/Counter2 Compare Match A 00226 { 00227 if(_timer_ovf == _timer_ovf_a){ 00228 PORTB &= B11111110; //Putting the pin low 00229 } 00230 } 00231 00232 ISR(TIMER2_COMPB_vect) // Timer/Counter2 Compare Match B Rudder Servo 00233 { 00234 if(_timer_ovf == _timer_ovf_b){ 00235 PORTC &= B11101111; //Putting the pin low! 00236 } 00237 }