void FixedUpdate()
{
if (!m_PhotonView.isMine)
{
return;
}
float heightCommandCompensated = 0;
float heightFeedforwardCompensated = 0;
Vector3 anglesCommand = Vector3.zero;
switch (heightControl)
{
case ControlMode.Stabilized:
heightFeedforwardCompensated = applyAngleCompensation(heightFeedforward);
heightCommandCompensated = applyAngleCompensation(controlVerticalSpeed());
break;
case ControlMode.Manual:
heightFeedforwardCompensated = heightFeedforward + controlThrottle();
heightCommandCompensated = 0;
break;
}
switch (anglesControl)
{
case ControlMode.Stabilized:
anglesCommand = controlAngles();
break;
case ControlMode.Manual:
anglesCommand = controlRates();
break;
}
propellersController.MixRPM(heightFeedforwardCompensated, heightCommandCompensated, anglesCommand.x, anglesCommand.y, anglesCommand.z);
// Dbg.Trace (rb.angularVelocity.ToString());
Vector3 f = propellersController.getTotalForce();
Vector3 t = propellersController.getTotalTorque();
rb.AddRelativeForce(f);
rb.AddRelativeTorque(t);
Dbg.Trace(string.Format("{0:0.0000000}, {1:0.0000000}, {2:0.0000000}, {3:0.0000000}, {4:0.0000000}, {5:0.0000000}, " +
"{6:0.0000000}, {7:0.0000000}, {8:0.0000000}, {9:0.0000000}, {10:0.0000000}, {11:0.0000000}, " +
"{12:0.0000000}, {13:0.0000000}, {14:0.0000000}, {15:0.0000000}, {16:0.0000000}, {17:0.0000000}",
transform.eulerAngles.z, transform.eulerAngles.x, transform.eulerAngles.y, rollRef, pitchRef, yawRef,
-rb.angularVelocity.z, -rb.angularVelocity.x, rb.angularVelocity.y,
angularRatesRef.x, angularRatesRef.y, angularRatesRef.z, -t.z, -t.x, t.y,
rb.velocity.y, verticalSpeedRef, f.y));
acceleration = transform.rotation * f / rb.mass - 9.81f * Vector3.up;
}
