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