• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/APM_RC/APM_RC.cpp

Go to the documentation of this file.
00001 /*
00002         APM_RC.cpp - Radio Control Library for Ardupilot Mega. Arduino
00003         Code by Jordi Muņoz and Jose Julio. 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         RC Input : PPM signal on IC4 pin
00011         RC Output : 11 Servo outputs (standard 20ms frame)
00012 
00013         Methods:
00014                 Init() : Initialization of interrupts an Timers
00015                 OutpuCh(ch,pwm) : Output value to servos (range : 900-2100us) ch=0..10
00016                 InputCh(ch) : Read a channel input value.  ch=0..7
00017                 GetState() : Returns the state of the input. 1 => New radio frame to process
00018                              Automatically resets when we call InputCh to read channels
00019                 
00020 */
00021 #include "APM_RC.h"
00022 
00023 #include <avr/interrupt.h>
00024 #include "WProgram.h"
00025 
00026 #if !defined(__AVR_ATmega1280__) && !defined(__AVR_ATmega2560__)
00027 # error Please check the Tools/Board menu to ensure you have selected Arduino Mega as your target.
00028 #else
00029 
00030 // Variable definition for Input Capture interrupt
00031 volatile unsigned int ICR4_old;
00032 volatile unsigned char PPM_Counter=0;
00033 volatile uint16_t PWM_RAW[8] = {2400,2400,2400,2400,2400,2400,2400,2400};
00034 volatile unsigned char radio_status=0;
00035 
00036 /****************************************************
00037    Input Capture Interrupt ICP4 => PPM signal read
00038  ****************************************************/
00039 ISR(TIMER4_CAPT_vect)  
00040 {
00041   unsigned int Pulse;
00042   unsigned int Pulse_Width;
00043   
00044   Pulse=ICR4;
00045   if (Pulse<ICR4_old)     // Take care of the overflow of Timer4 (TOP=40000)
00046     Pulse_Width=(Pulse + 40000)-ICR4_old;  //Calculating pulse 
00047   else
00048     Pulse_Width=Pulse-ICR4_old;            //Calculating pulse 
00049   if (Pulse_Width>8000)   // SYNC pulse?
00050     PPM_Counter=0;
00051   else
00052     {
00053     PPM_Counter &= 0x07;  // For safety only (limit PPM_Counter to 7)
00054     PWM_RAW[PPM_Counter++]=Pulse_Width;  //Saving pulse. 
00055     if (PPM_Counter >= NUM_CHANNELS)
00056       radio_status = 1;
00057     }
00058   ICR4_old = Pulse;
00059 }
00060 
00061 
00062 // Constructors ////////////////////////////////////////////////////////////////
00063 
00064 APM_RC_Class::APM_RC_Class()
00065 {
00066 }
00067 
00068 // Public Methods //////////////////////////////////////////////////////////////
00069 void APM_RC_Class::Init(void)
00070 {
00071   // Init PWM Timer 1
00072   pinMode(11,OUTPUT); //     (PB5/OC1A)
00073   pinMode(12,OUTPUT); //OUT2 (PB6/OC1B)
00074   pinMode(13,OUTPUT); //OUT3 (PB7/OC1C)
00075 
00076   //Remember the registers not declared here remains zero by default... 
00077   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... 
00078   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
00079   OCR1A = 3000; //PB5, none
00080   //OCR1B = 3000; //PB6, OUT2
00081   //OCR1C = 3000; //PB7  OUT3
00082   ICR1 = 40000; //50hz freq...Datasheet says  (system_freq/prescaler)/target frequency. So (16000000hz/8)/50hz=40000,
00083 
00084   // Init PWM Timer 3
00085   pinMode(2,OUTPUT); //OUT7 (PE4/OC3B)
00086   pinMode(3,OUTPUT); //OUT6 (PE5/OC3C)
00087   pinMode(4,OUTPUT); //     (PE3/OC3A)
00088   TCCR3A =((1<<WGM31)|(1<<COM3A1)|(1<<COM3B1)|(1<<COM3C1));
00089   TCCR3B = (1<<WGM33)|(1<<WGM32)|(1<<CS31); 
00090   OCR3A = 3000; //PE3, NONE
00091   OCR3B = 3000; //PE4, OUT7
00092   OCR3C = 3000; //PE5, OUT6
00093   ICR3 = 40000; //50hz freq
00094 
00095   // Init PWM Timer 5
00096   pinMode(44,OUTPUT);  //OUT1 (PL5/OC5C)
00097   pinMode(45,OUTPUT);  //OUT0 (PL4/OC5B)
00098   pinMode(46,OUTPUT);  //     (PL3/OC5A)
00099   
00100   TCCR5A =((1<<WGM51)|(1<<COM5A1)|(1<<COM5B1)|(1<<COM5C1)); 
00101   TCCR5B = (1<<WGM53)|(1<<WGM52)|(1<<CS51);
00102   OCR5A = 3000; //PL3, 
00103   //OCR5B = 3000; //PL4, OUT0
00104   //OCR5C = 3000; //PL5, OUT1
00105   ICR5 = 40000; //50hz freq
00106 
00107   // Init PPM input and PWM Timer 4
00108   pinMode(49, INPUT);  // ICP4 pin (PL0) (PPM input)
00109   pinMode(7,OUTPUT);   //OUT5 (PH4/OC4B)
00110   pinMode(8,OUTPUT);   //OUT4 (PH5/OC4C)
00111       
00112   TCCR4A =((1<<WGM40)|(1<<WGM41)|(1<<COM4C1)|(1<<COM4B1)|(1<<COM4A1));  
00113   //Prescaler set to 8, that give us a resolution of 0.5us
00114   // Input Capture rising edge
00115   TCCR4B = ((1<<WGM43)|(1<<WGM42)|(1<<CS41)|(1<<ICES4));
00116   
00117   OCR4A = 40000; 
00118   OCR4B = 3000; //PH4, OUT5
00119   OCR4C = 3000; //PH5, OUT4
00120  
00121   //TCCR4B |=(1<<ICES4); //Changing edge detector (rising edge). 
00122   //TCCR4B &=(~(1<<ICES4)); //Changing edge detector. (falling edge)
00123   TIMSK4 |= (1<<ICIE4); // Enable Input Capture interrupt. Timer interrupt mask
00124 }
00125 
00126 void APM_RC_Class::OutputCh(unsigned char ch, uint16_t pwm)
00127 {
00128   pwm=constrain(pwm,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
00129   pwm<<=1;   // pwm*2;
00130  
00131  switch(ch)
00132   {
00133     case 0:  OCR5B=pwm; break;  //ch0
00134     case 1:  OCR5C=pwm; break;  //ch1
00135     case 2:  OCR1B=pwm; break;  //ch2
00136     case 3:  OCR1C=pwm; break;  //ch3
00137     case 4:  OCR4C=pwm; break;  //ch4
00138     case 5:  OCR4B=pwm; break;  //ch5
00139     case 6:  OCR3C=pwm; break;  //ch6
00140     case 7:  OCR3B=pwm; break;  //ch7
00141     case 8:  OCR5A=pwm; break;  //ch8,  PL3
00142     case 9:  OCR1A=pwm; break;  //ch9,  PB5
00143     case 10: OCR3A=pwm; break;  //ch10, PE3
00144   } 
00145 }
00146 
00147 uint16_t APM_RC_Class::InputCh(unsigned char ch)
00148 {
00149   uint16_t result;
00150   uint16_t result2;
00151   
00152   // Because servo pulse variables are 16 bits and the interrupts are running values could be corrupted.
00153   // We dont want to stop interrupts to read radio channels so we have to do two readings to be sure that the value is correct...
00154   result =  PWM_RAW[ch]>>1;  // Because timer runs at 0.5us we need to do value/2
00155   result2 =  PWM_RAW[ch]>>1;
00156   if (result != result2)
00157     result =  PWM_RAW[ch]>>1;   // if the results are different we make a third reading (this should be fine)
00158   
00159   // Limit values to a valid range
00160   result = constrain(result,MIN_PULSEWIDTH,MAX_PULSEWIDTH);
00161   radio_status=0; // Radio channel read
00162   return(result);
00163 }
00164 
00165 unsigned char APM_RC_Class::GetState(void)
00166 {
00167   return(radio_status);
00168 }
00169 
00170 // InstantPWM implementation
00171 // This function forces the PWM output (reset PWM) on Out0 and Out1 (Timer5). For quadcopters use
00172 void APM_RC_Class::Force_Out0_Out1(void)
00173 {
00174   if (TCNT5>5000)  // We take care that there are not a pulse in the output
00175     TCNT5=39990;   // This forces the PWM output to reset in 5us (10 counts of 0.5us). The counter resets at 40000
00176 }
00177 // This function forces the PWM output (reset PWM) on Out2 and Out3 (Timer1). For quadcopters use
00178 void APM_RC_Class::Force_Out2_Out3(void)
00179 {
00180   if (TCNT1>5000)
00181     TCNT1=39990;
00182 }
00183 // This function forces the PWM output (reset PWM) on Out6 and Out7 (Timer3). For quadcopters use
00184 void APM_RC_Class::Force_Out6_Out7(void)
00185 {
00186   if (TCNT3>5000)
00187     TCNT3=39990;
00188 }
00189 
00190 // make one instance for the user to use
00191 APM_RC_Class APM_RC;
00192 
00193 #endif // defined(ATMega1280)

Generated for ArduPilot Libraries by doxygen