/// <summary>
/// Function at regular time interval
/// </summary>
void FixedUpdate()
{
// wait 0.1 sec to avoid inizialization problem
if ((startAfter -= Time.deltaTime) > 0)
{
return;
}
if (stayOnFixedPoint)
{
Vector3 p = mag.worldToLocalPoint(routePosition, target.position);
targetZ = p.z;
targetX = p.x;
targetY = routePosition.y;
}
else
{
targetZ = mag.worldToLocalPoint(target.position, lookingAtPoint).z;
targetX = mag.worldToLocalPoint(routePosition, lookingAtPoint).x;
targetY = (routePosition.y + target.position.y) / 2f;
}
Vector3 thrustVector = Quaternion.AngleAxis(-45, Vector3.up) * new Vector3(targetX, targetY - transform.position.y, targetZ);
Vector3 xComponent = Quaternion.AngleAxis(-45, Vector3.up) * new Vector3(targetX, 0, 0);
Vector3 zComponent = Quaternion.AngleAxis(-45, Vector3.up) * new Vector3(0, 0, targetZ);
// drawing the direction vectors, for debugging
linedrawer.drawPosition(ticket3, thrustVector);
linedrawer.drawPosition(ticket1, xComponent);
linedrawer.drawPosition(ticket2, zComponent);
// calling the stabilization algorithms that will modify the rotation power
idealPitch = droneSettings.keepOnAbsRange(zStabilization(targetZ), 0.40f);
idealRoll = droneSettings.keepOnAbsRange(xStabilization(targetX), 0.40f);
idealYaw = mag.getYawToCenterOn(lookingAtPoint);
yStabilization(targetY);
pitchStabilization(idealPitch);
rollStabilization(idealRoll);
float yawErr = yawStabilization(idealYaw);
// if the drone has to rotate more than 0.22 (~40°) it stabilizes itself to a fixed point to avoid getting off the route and to increase stability
followTarget(yawErr < 0.22f);
// truncate and applies the power to the rotors
pV1 = keepOnRange01(pV1);
pV2 = keepOnRange01(pV2);
pO1 = keepOnRange01(pO1);
pO2 = keepOnRange01(pO2);
helixV1.setPower(denormalizePower(pV1));
helixV2.setPower(denormalizePower(pV2));
helixO1.setPower(denormalizePower(pO1));
helixO2.setPower(denormalizePower(pO2));
// Calculate the torque generated by each rotor and applies it to the drone
applyTorque(torqueGeneratedBy(helixV1) + torqueGeneratedBy(helixV2) + torqueGeneratedBy(helixO1) + torqueGeneratedBy(helixO2));
}