void FixedUpdate()
{
while (operations.Count > 0)
{
operations.TryDequeue(out Action action);
action.Invoke();
}
if (motorsArmed)
{
if (motorsControlMode == "stable")
{
float t = 50;
var target = Quaternion.Euler(targetRotation);
if (Quaternion.Dot(gameObject.transform.rotation, target) < 0)
{
target.w *= -1;
target.x *= -1;
target.y *= -1;
target.z *= -1;
}
target.x *= -1;
target.y *= -1;
target.z *= -1;
var error = target * gameObject.transform.rotation;
float pitchTarget = error.x * t;
float yawTarget = error.y * t;
float rollTarget = error.z * t;
Vector3 cur = transform.InverseTransformDirection(rb.angularVelocity); //localAngularVelocity
var rollControl = rollPID.ControlWithLimits(rollTarget, cur.z, Time.fixedTime);
var pitchControl = pitchPID.ControlWithLimits(pitchTarget, cur.x, Time.fixedTime);
var yawControl = yawPID.ControlWithLimits(yawTarget, cur.y, Time.fixedTime);
var depthControl = targetDepth == 0f ? 0 : depthPID.ControlWithLimits(-targetDepth, transform.position.y, Time.fixedTime);
SetMotors(rollControl, pitchControl, yawControl, depthControl, velocity.z, velocity.y, velocity.x);
}
else if (motorsControlMode == "acro")
{
Vector3 localangularvelocity = transform.InverseTransformDirection(rb.angularVelocity);
var rollControl = rollSpeedPID.ControlWithLimits(targetRotationSpeed.z, localangularvelocity.z, Time.fixedTime);
var pitchControl = pitchSpeedPID.ControlWithLimits(targetRotationSpeed.x, localangularvelocity.x, Time.fixedTime);
var yawControl = yawSpeedPID.ControlWithLimits(targetRotationSpeed.y, localangularvelocity.y, Time.fixedTime);
SetMotors(rollControl, pitchControl, yawControl, 0, velocity.z, velocity.y, velocity.x);
}
}
Module.FixedUpdateAll();
}
