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;

Revised Yellow FPV Plane with gyro stability system added, Worked best with analogue inputs from Murata piezo gyro sensors of a dead KK board filtered taking a 10 point average. Yaw compensation is currently not used as there is no rudder presently.

http://www.youtube.com/watch?feature=player_embedded&v=IrSxP9YsLpYhttp://kiwitricopter.blogspot.co.nz/2012/11/arduino-rx-stabilization-code.html