/* 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(2,INPUT); // PD2 - INT0 - CH 1 in pinMode(3,INPUT); // PD3 - INT1 - CH 2 in pinMode(11,INPUT); // PB3 - MOSI/OC2 - CH 3 in pinMode(13,INPUT); // PB5 - SCK - CH 4 in pinMode(10,OUTPUT); // PB2 - OC1B - CH 1 out pinMode(8, OUTPUT); // PB0 - AIN1 - CH 3 out pinMode(9, OUTPUT); // PB1 - OC1A - CH 2 out DDRC |= B00010000; // PC4 - - CH 4 out } 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(); } 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() { 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 servos for (uint8_t i = 0; i < 4; i++){ if (i == 3) continue; if(radio_in[i] >= radio_trim[i]) servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0; else servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0; } servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0; servo_in[CH3] = constrain(servo_out[CH3], 0, 100); } void AP_RC::output() { uint16_t out; for (uint8_t i = 0; i < 4; i++){ if (i == 3) continue; if(radio_in[i] >= radio_trim[i]) out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; else out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i]; set_ch_pwm(i, out); } out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0; out += radio_min[CH3]; set_ch_pwm(CH3, out); } void AP_RC::trim() { uint8_t temp = _mix_mode; _mix_mode = 0; read(); _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::twitch_servos(uint8_t times) { while (times > 0){ set_ch_pwm(CH1, radio_trim[CH1] + 100); set_ch_pwm(CH2, radio_trim[CH2] + 100); delay(400); set_ch_pwm(CH1, radio_trim[CH1] - 100); set_ch_pwm(CH2, radio_trim[CH2] - 100); delay(200); set_ch_pwm(CH1, radio_trim[CH1]); set_ch_pwm(CH2, radio_trim[CH2]); delay(30); times--; } } 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! } }