N@NewHover
I@GyrR GyrP DeltR DeltP GpsZ K4 K6 K8 K2 KP KM SY SX SZ VY
O@ThrF ThrB ThrL ThrR TY DestZ
TY=0, T1=0, A=0
DestZ = DestZ + KP + KM
DiffZ = (DestZ - GpsZ)
GyrR > 70 -> K4=0,K2=0;
GyrR < -70 -> K6=0,K8=0;
TY = 5*(VY /0.5 - K4/5 + K6/5)
K4+K6+K8>1000 -> K8=K8*3,T1=1;
K4+K6+K2>1000 -> K2=K2*3,T1=1;
ThrR = 1*(-(10*GyrR + 150*DeltR)+40*DiffZ - 130*$GpsZ +2* K4)
ThrL = 1*(10*GyrR + 150*DeltR+40*DiffZ - 130*$GpsZ +2*K6)
ThrF = 1*(-(10*GyrP +400*DeltP)+40*DiffZ - 130*$GpsZ +2*K2)
ThrB = 1*(10*GyrP + 400*DeltP+40*DiffZ - 130*$GpsZ +1.3*2*K8)
SX<0 -> ThrL=ThrL-(SX+0.5*SZ+8*$SX+0*$SZ);
SX>0 -> ThrR=ThrR+(SX+0.5*SZ+8*$SX+0*$SZ);
SY<0 -> ThrB=ThrB+SZ-(SY+SZ+8*$SY+0*$SZ);
SY>0 -> ThrF=ThrF+(SY+0.5*SZ+8*$SY+0*$SZ);


based on 1000 u weight on a 4X4 plate
gyro + speedo + gps + k inputs
4 thrusters for stability, one for yaw
the rectified delta are on gates cause i have problems with angnorm (i am noob)
the Gyr things stands for gyro the S ones for speedo the Y for yaw and gps for gps
maybe some redundances i have to clean up , but it is ...

cheers