//***** FORCE AND ANGLE OUTPUT CONTROL*****//
// Here is the actual control system.
// First we need to figure out how much force in the Y do we need to balance:
f32 yforce[5] = {0, (vbalance + vycorrecttion * .5), (vbalance + vycorrecttion * .5), (vbalance + vycorrecttion * .5), (vbalance + vycorrecttion * .5)} ;
//Next we adjust yforces for losses while rolling and pitching:
f32 lossfactor = abs(cos(plane.theta.X
EGTORAD)) * abs(cos(plane.theta.Z
EGTORAD));
yforce[1] /= lossfactor; yforce[2] /= lossfactor;
yforce[3] /= lossfactor; yforce[4] /= lossfactor;
// Next we add/subtract some force to produce roll
yforce[1] -= rollcorrection * rollcf ; yforce[3] -= rollcorrection* rollcf ;
yforce[2] += rollcorrection * rollcf ; yforce[4] += rollcorrection* rollcf ;
// Next we add/subtract some force to produce pitch
yforce[1] += pitchcorrection * pitchcf; yforce[2] += pitchcorrection * pitchcf;
yforce[3] -= pitchcorrection * pitchcf; yforce[4] -= pitchcorrection * pitchcf;
// Next we change diagonal forces to produce yaw
yforce[1] += yawcorrection * yawcf; yforce[4] += yawcorrection * yawcf;
yforce[2] -= yawcorrection * yawcf; yforce[3] -= yawcorrection * yawcf;
// Next we figure out how big our actual force output must be:
f32 totalforce[5] = {0, yforce[1] / cos(plane.f[1].theta), yforce[2] / cos(plane.f[2].theta),yforce[3] / cos(plane.f[3].theta),yforce[4] / cos(plane.f[4].theta)};
//// Increase by speedfactor:
totalforce[1] *= speedfactor; totalforce[2] *= speedfactor;totalforce[3] *= speedfactor;totalforce[4] *= speedfactor;
//
//// Recalculate y:
yforce[1] = totalforce[1]*cos(plane.f[1].theta); yforce[2] = totalforce[2]*cos(plane.f[2].theta);
yforce[3] = totalforce[3]*cos(plane.f[3].theta); yforce[4] = totalforce[4]*cos(plane.f[4].theta);
// Next we assign the X-component:
f32 xforce[5] = {0, totalforce[1] * sin(plane.f[1].theta), totalforce[2] * sin(plane.f[2].theta), totalforce[3] * sin(plane.f[3].theta), totalforce[4] * sin(plane.f[4].theta)};
// Next Assign Z component:
f32 zforce[5] = {0, 0, 0, 0, 0};