Planet Arduino

void UpdateServos()
{
      
    //Digital inputs TX code helper
    //TxVal[8] |= (digitalRead(5) << 0);//joy 2 push
    //TxVal[8] |= (digitalRead(6) << 1);//pb
    //TxVal[8] |= (digitalRead(7) << 2);//slide
    //TxVal[8] |= (digitalRead(8) << 3);//toggle
     
    //Throttle TxVal[1] 
    //Rotary pot TxVal[2]  
    //Joy 1 X TxVal[3] 
    //Joy 1 Y TxVal[4] 
    //Joy 2 X TxVal[5] 
    //Joy 2 Y TxVal[6] 
    //rssi TxVal[7]
    //digital TxVal[8]
    //micros() TxVal[9]
        
    //Use the pot as the gain for all channels for now    
    float GainPot = (float)(TxVal[2]) * 0.001f;    
      
    //Get the target values from the TX    
    int PitchTarg =  (TxVal[3] / 10);
    int RollTarg =  (TxVal[4] / 10);
    int YawTarg =  (TxVal[6] / 10);
    
    
    //Prime the Target WOZ values  
    if(PitchTargWOZ == 9999)
      PitchTargWOZ = PitchTarg;
      
    if(RollTargWOZ == 9999)
      RollTargWOZ = RollTarg;
      
    if(YawTargWOZ == 9999)
      YawTargWOZ = YawTarg;
      
    
    //Get the Centered target values   
    float PitchTargCentred = (float)(PitchTarg - PitchTargWOZ);
    float RollTargCentred =  (float)(RollTarg - RollTargWOZ);
    float YawTargCentred =  (float)(YawTarg - YawTargWOZ);
      
    //Calculate gains
    float PitchGain = GainPot * 1.0f;
    float RollGain = GainPot * 1.0f;
    float YawGain = GainPot * 1.0f;
    
    //Get Gyro values
    float PitchGyro = (float)(AnIn[2] - AnInWOZ[2]);
    float RollGyro = (float)(AnIn[1] - AnInWOZ[1]);
    float YawGyro = (float)(AnIn[0] - AnInWOZ[0]);
    
    //Calc P error
    float PitchError = (float)PitchTargCentred + PitchGyro;
    float RollError = (float)RollTargCentred + RollGyro;
    float YawError = (float)YawTargCentred + YawGyro;
        
    //Apply gains
    int PitchTrim = (int)(PitchError * PitchGain);    
    int RollTrim = (int)(RollError * RollGain);    
    int YawTrim = (int)(YawError * YawGain);
    
    //Constaring trim authority
    PitchTrim = constrain(PitchTrim, -30, 30);
    RollTrim = constrain(RollTrim, -30, 30);
    YawTrim = constrain(YawTrim, -30, 30);
    
    //Dump the trim value
    if((TxVal[9] & 0x4) == 0)
    {        
      PitchTrim = 0;
      RollTrim = 0;
      YawTrim = 0;      
    }
    
    
    
    //Calc flap anglke
    int Flaps = 0;
    
    //Apply flaps
    if((TxVal[9] & 0x8) == 0)
        Flaps = -25;
        
        
  
    //Throttle  
    val = TxVal[1] / 10;    
    val = map(val, 1, 179, 30, 179);
    val = constrain(val, 1, 165);         // scale it to use it with the servo (value between 0 and 180) 
    servo[0].write(val);                  // sets the servo position according to the scaled value 
       
       
    //Vee tail
    
    //Left Elevator Joy 1 Y TxVal[4]
    val = (YawTarg + YawTrim) + (PitchTargCentred + PitchTrim);   
    val = constrain(val, 15, 165); 
    val = map(val, 0, 179, 135, 45);       // scale it to use it with the servo (value between 0 and 180) 
    servo[1].write(val);                  // sets the servo position according to the scaled value 
    
    
    //Right Elevator Joy 1 Y TxVal[4]
    val =  (YawTarg + YawTrim) - (PitchTargCentred + PitchTrim);   
    val = constrain(val, 15, 165); 
    val = map(val, 0, 179, 135, 45);       // scale it to use it with the servo (value between 0 and 180) 
    servo[2].write(val);                  // sets the servo position according to the scaled value 
    
    
    
    //Left Flaperon           
    val = 90 + (RollTargCentred + Flaps) + RollTrim;  
    val = constrain(val, 15, 165); 
    val = map(val, 0, 179, 165, 15);      // scale it to use it with the servo (value between 0 and 180) 
    servo[3].write(val);                  // sets the servo position according to the scaled value 
    
    //Right Flaperon 
    val = 90 + (RollTargCentred - Flaps) + RollTrim;  
    val = constrain(val, 15, 165); 
    val = map(val, 0, 179, 165, 15);      // scale it to use it with the servo (value between 0 and 180) 
    servo[4].write(val);                  // sets the servo position according to the scaled value 
    
    
    //Joy 2 x nose Wheel
    val = (TxVal[6] / 10);  
    val = map(val, 0, 179, 55, 125); 
    servo[5].write(val);                 // sets the servo position according to the scaled value 
      
}