public void Update(PilotAddon.ControlInputs state)
{
// Run the controller once every timePerUpdate seconds
deltaTime += Time.fixedDeltaTime;
if (deltaTime < timePerUpdate)
{
return;
}
var internalVessel = vessel.InternalVessel;
var torque = vessel.AvailableTorqueVectors.Item1;
var moi = vessel.MomentOfInertiaVector;
// Compute the input and error for the controllers
var target = ComputeTargetAngularVelocity(torque, moi);
var current = ComputeCurrentAngularVelocity();
// If roll not set, or not close to target direction, set roll target velocity to 0
var currentDirection = ReferenceFrame.DirectionFromWorldSpace(internalVessel.ReferenceTransform.up);
if (double.IsNaN(TargetRoll) || Vector3.Angle(currentDirection, targetDirection) > RollThreshold)
{
target.y = 0;
}
// Autotune the controllers if enabled
if (AutoTune)
{
DoAutoTune(torque, moi);
}
// If vessel is sat on the pad, zero out the integral terms
if (internalVessel.situation == Vessel.Situations.PRELAUNCH)
{
PitchPID.ClearIntegralTerm();
RollPID.ClearIntegralTerm();
YawPID.ClearIntegralTerm();
}
// Run per-axis PID controllers
var output = new Vector3d(
PitchPID.Update(target.x, current.x, deltaTime),
RollPID.Update(target.y, current.y, deltaTime),
YawPID.Update(target.z, current.z, deltaTime));
state.Pitch = (float)output.x;
state.Roll = (float)output.y;
state.Yaw = (float)output.z;
deltaTime = 0;
}
public void Update(PilotAddon.ControlInputs state)
{
// Run the controller once every timePerUpdate seconds
deltaTime += Time.fixedDeltaTime;
if (deltaTime < timePerUpdate)
{
return;
}
var internalVessel = vessel.InternalVessel;
var torque = vessel.AvailableTorqueVectors.Item1;
var moi = vessel.MomentOfInertiaVector;
transitionMat = Matrix4x4.identity;
if (!AutoTorqueMat)
{
var ine = vessel.InertiaTensor;
Matrix4x4 InertialTensor = Matrix4x4.identity;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
InertialTensor[i + j * 4] = (float)ine[i * 3 + j];
}
}
//Debug.Log("InertialTensor\n" + InertialTensor);
Matrix4x4 mat = TorqueMat;
//Debug.Log("torqueMat\n" + mat);
/*
* mat[0] = (float)torque.x;
* mat[5] = (float)torque.y;
* mat[10] = (float)torque.z;
*/
Matrix4x4 w = InertialTensor.inverse * mat;
Func <float, float, float, float> lambda = (a1, a2, a3) => { return((float)Math.Pow((double)a1 * a1 + a2 * a2 + a3 * a3, 0.5)); };
var wx = lambda(w[0], w[1], w[2]);
var wy = lambda(w[4], w[5], w[6]);
var wz = lambda(w[8], w[9], w[10]);
//Debug.Log("x:"+wx+" y:"+wy+" z:"+wz);
mat[0] = w[0] / wx;
mat[1] = w[1] / wx;
mat[2] = w[2] / wx;
mat[4] = w[4] / wy;
mat[5] = w[5] / wy;
mat[6] = w[6] / wy;
mat[8] = w[8] / wz;
mat[9] = w[9] / wz;
mat[10] = w[10] / wz;
transitionMat = mat.inverse;
//Debug.Log("transitionMat\n" + transitionMat);
torque = new Vector3d(wx, wy, wz);
moi = new Vector3d(1.0f, 1.0f, 1.0f);
}
// Compute the input and error for the controllers
var target = ComputeTargetAngularVelocity(torque, moi);
var current = ComputeCurrentAngularVelocity();
// If roll not set, or not close to target direction, set roll target velocity to 0
var currentDirection = ReferenceFrame.DirectionFromWorldSpace(internalVessel.ReferenceTransform.up);
if (double.IsNaN(TargetRoll))
{
target.y = 0;
}
// Autotune the controllers if enabled
if (AutoTune)
{
DoAutoTune(torque, moi);
}
// If vessel is sat on the pad, zero out the integral terms
if (internalVessel.situation == Vessel.Situations.PRELAUNCH)
{
PitchPID.ClearIntegralTerm();
RollPID.ClearIntegralTerm();
YawPID.ClearIntegralTerm();
}
// Run per-axis PID controllers
var output = new Vector3d(
PitchPID.Update(target.x, current.x, deltaTime),
RollPID.Update(target.y, current.y, deltaTime),
YawPID.Update(target.z, current.z, deltaTime));
state.Pitch = (float)output.x;
state.Roll = (float)output.y;
state.Yaw = (float)output.z;
deltaTime = 0;
}