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.

Simple code snippet Arduino Nano

//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;
}

свернуть