//==========================================
//    4in/4out Mixer for Quadrocopter
//
//           Philip Sun 
//              iLuFa
//   
//    V 1.12 (2010/08/29)     
//
//==========================================

// 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).

//(V1.12)  more smooth to control (small  action)
//B0 = A1 - ŁGA0/2 + ŁGA3/2
//B1 = A1 - ŁGA0/2 - ŁGA3/2
//B2 = A1 + ŁGA0/2 - ŁGA2/2
//B3 = A1 + ŁGA0/2 + ŁGA2/2

//(V1.1)
//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

INT8  i,j,indata,indatb,TimerS,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;
      
      
delay_mS(5);                  // wait 

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

TimerS =0;
do{
      indatb = INPUT_A();
      indata=(indatb & templateA);}
      while(indata==0);
   
while(TimerS < 40) {
      indatb = INPUT_A();
      indata=(indatb & templateA);
      if(indata ==0) {TimerS++ ; delay_uS(50);} //50uS*40 =2mS
      else TimerS =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]/2) -(T[0]/2);
PWM[1] = T[1] +227 -(T[0]/2) -(T[3]/2);
PWM[2] = T[1] +(T[0]/2) -(T[2]/2);
PWM[3] = T[1] +(T[0]/2) +(T[2]/2) -227 ;


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