//==========================================
//      4in/4out Mixer for Quadrocopter
//
//	Mod. by LDHUng	(V 1.13)
//	add initiation of Gyro esky 0704,
//	
//
//      original by  Philip Sun /  iLuFa      
//   
//      V 1.1 (2010/08/28)     
//
//==========================================

// Port A A0~A3 as Input: A0 (rudder),A1 (throttle), A2 (Eleon/Elevator),A3 (Aileon).
// port B B0~B3 as Output: B0 (motor left),B1(motor right), B2 (motor forward),B3 (motor back).
// B0 = A1 - ŁGA0 + ŁGA3
// B1 = A1 - ŁGA0 - ŁGA3
// B2 = A1 + ŁGA0 - ŁGA2
// B3 = A1 + ŁGA0 + ŁGA2

//==========================================
//20Mhz, resolution 6.6 uS, 151 stages
//port B B4 as output LED 1 = valid input
//==========================================

#include <16F84.h>

#FUSES NOWDT                    //No Watch Dog Timer
#FUSES HS                       //High speed Osc (> 4mhz)
#FUSES NOPUT                    //No Power Up Timer
#FUSES NOPROTECT                //Code not protected from reading

#use delay(clock=20000000)
//#use rs232(baud=9600,xmit=PIN_B6,rcv=PIN_B7)  //debug
#byte portb=0x06
INT8  i,j,indata,indatb,templateA;
INT16 TimerL;

INT R[4];
INT16 T[4],PWM[4];


//------------------------

void main(){     
   
   // 0   Init
   
   setup_timer_0(RTCC_INTERNAL|RTCC_DIV_1);
   templateA= 0b00001111  ;           // input 4 channels
   
   PWM[0]=227;
   PWM[1]=151;  //init throttle at lowerst position
   PWM[2]=227;
   PWM[3]=227;

   //LDHUng	,Inital Gyro esky 0704,It take about 15 second
   set_tris_b(0);
   for(TimerL=1;TimerL<=710; TimerL++)
        {
            portb = 0b00001111;
            delay_us(1500); //want servo to move to 90 degrees,center point servo
            portb = 0b00000000;
            if(TimerL%20<=10)
               portb = 0b00010000;
          delay_us(18500);
       }
   output_low(pin_B4);
   //LDHUng

   delay_mS(100);                  // wait

   // wait an inpit
   // To find an interval > 2 mS ,the syn single of PPM 

   TimerL =0;
   do
   {
      indatb = INPUT_A();
      indata=(indatb & templateA);
   }while(indata==0);

   while(TimerL < 40)
   {
      indatb = INPUT_A();
      indata=(indatb & templateA);
      if(indata ==0) {TimerL++ ; delay_uS(50);} //50uS*40 =2mS
      else TimerL =0;
   }
   
   // A   Determine the input sequence
   // Had find the begin of PPM string
   // store first channel of PWM in R0, second in R1..
   
   i=0;
   do 
   {
      do
      {
         indatb = INPUT_A();
         indata=(indatb & templateA );
      }while (indata ==0);
      
      if (indata ==1){  R[i]=1;  templateA=(templateA ^0b00000001);}
      if (indata ==2){  R[i]=2;  templateA=(templateA ^0b00000010);}  
      if (indata ==4){  R[i]=4;  templateA=(templateA ^0b00000100);}  
      if (indata ==8){  R[i]=8;  templateA=(templateA ^0b00001000);}  
      
      i++;
      
      do
      {
         indatb = INPUT_A();
         indata=(indatb & templateA );
      }while (! indata ==0);                  
   }
   while (! templateA==0);    // all 4in were detect and sort 
   
   
   //------------------------
   // B   Read 4 input one by one 
   
   // Begin of Endless loop
   // Read PWM one by one, R1~R4, take 5.5~9.5 mS
   // T0~T3 = 151~302 = 1mS~2mS +- 10%
   // delay_cycles(24) =33cycles 6.6uS,  
   
   do
   {
      output_low(pin_B4);    // Valid input indecator
      
      for  (i=0;i<4;++i)
      {
         timerL=0;
         j=R[i]; 
         
         if (j==1)
         {  
            do 
            {

            }                         //read first channel
            while (! input(pin_A0) );
            do 
            { 
               TimerL ++;               // first channel rise
               delay_cycles(24);     // make total 33 cycles
            }while ( input(PIN_A0) );      
            T[0]=TimerL;                  //first channel down
         } 
         
         if (j==2)
         {  
            do 
            {

            }while (! input(pin_A1) );
            do 
            { 
               TimerL ++;
               delay_cycles(24);
            }while ( input(PIN_A1) ); 
            T[1]=TimerL;
         }  
         
         if (j==4)
         {
            do
            {

            }while (! input(pin_A2) );
            do 
            { 
               TimerL ++;
               delay_cycles(24);
            }while ( input(PIN_A2) );
            T[2]=TimerL;
         }  
         
         if (j==8)
         {  
            do 
            {

            }while (! input(pin_A3) );
            do 
            { 
               TimerL ++;
               delay_cycles(24);
            }while ( input(PIN_A3) ); 
            T[3]=TimerL;
         }          
         
         output_high(pin_B4);          // Valid input
         
      }     
      
      
      //---------------------------
      // C   Calculating 
      // Calculating must less than 2.5mS
      
     // PWM[0] = T[1] +T[3] -T[0] -2;   // PWM[0] has ~15 uS deviation, cause unknow
      //PWM[1] = T[1] +454 -T[0] -T[3];
     // PWM[2] = T[1] +T[0] -T[2];
    //  PWM[3] = T[1] +T[0] +T[2] -454 ;

      //LDHUng	
      PWM[0] = T[1] +T[3] -T[0] -2;   
      PWM[1] = T[1] +454 -T[0] -T[3]+4;
      PWM[2] = T[1] +T[0] -T[2] +2;
      PWM[3] = T[1] +T[0] +T[2] -454 -2;
      //LDHUng

      for  (i=0;i<4;++i)
      { 
         if (PWM[i] < 130) 
            PWM[i] = 130;        // safe range control, 90%~110%
         if (PWM[i] > 325) 
            PWM[i] = 325; 
      } 
      
      //---------------------------
      // D   Out put 4 PWM
      // output 4 PWM, take 2.5mS
      // 1 unit = 33 cycles, 6.6 uS
      
      output_high(pin_B0);
      delay_cycles(10)  ;    //delay to match output_low 
      output_high(pin_B1);
      delay_cycles(10)  ;
      output_high(pin_B2);
      delay_cycles(10)  ;
      output_high(pin_B3);
      
      timerL=0;
      
      while (timerL < 330)
      {
         timerL++;
         
         if (timerL == PWM[0]) output_low(pin_B0);
         if (timerL == PWM[1]) output_low(pin_B1);
         if (timerL == PWM[2]) output_low(pin_B2);
         if (timerL == PWM[3]) output_low(pin_B3);
         delay_cycles(1);                //total 33 cycles
      }
   }while (true);       //End of endless loop
   
}  // End of Main