internal static bool Fly(global::Vessel vessel, PilotAddon.ControlInputs state)
{
// Get the auto-pilot object. Do nothing if there is no auto-pilot engaged for this vessel.
if (!engaged.ContainsKey(vessel.id))
{
return(false);
}
var autoPilot = engaged [vessel.id];
if (autoPilot == null)
{
return(false);
}
// If the client that engaged the auto-pilot has disconnected, disengage and reset the auto-pilot
if (autoPilot.requestingClient != null && !autoPilot.requestingClient.Connected)
{
autoPilot.attitudeController.ReferenceFrame = ReferenceFrame.Surface(vessel);
autoPilot.attitudeController.TargetPitch = 0;
autoPilot.attitudeController.TargetHeading = 0;
autoPilot.attitudeController.TargetRoll = double.NaN;
autoPilot.Disengage();
return(false);
}
// Run the auto-pilot
autoPilot.SAS = false;
autoPilot.attitudeController.Update(state);
return(true);
}
public void Update(PilotAddon.ControlInputs state)
{
var output = pid.Update(Error, Target, -1f, 1f);
state.Pitch = output.x;
state.Yaw = output.y;
state.Roll = output.z;
}
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;
}
internal static bool Fly(global::Vessel vessel, PilotAddon.ControlInputs state)
{
// Get the auto-pilot object. Do nothing if there is no auto-pilot engaged for this vessel.
if (!engaged.ContainsKey(vessel.id))
{
return(false);
}
var autoPilot = engaged [vessel.id];
if (autoPilot == null)
{
return(false);
}
// If the client that engaged the auto-pilot has disconnected, disengage and reset the auto-pilot
if (autoPilot.requestingClient != null && !autoPilot.requestingClient.Connected)
{
autoPilot.attitudeController.ReferenceFrame = ReferenceFrame.Surface(vessel);
autoPilot.attitudeController.TargetPitch = 0;
autoPilot.attitudeController.TargetHeading = 0;
autoPilot.attitudeController.TargetRoll = double.NaN;
autoPilot.Disengage();
return(false);
}
//auto shutdown engine
if (autoPilot.Speed > autoPilot.ShutdownSpeed)
{
autoPilot.vessel.Control.Throttle = 0.0f;
state.Throttle = 0.0f;
autoPilot.maxAcc = -1.0d;
}
if (autoPilot.maxAcc >= 0)
{
double deltaTime = Time.fixedDeltaTime;
if (deltaTime > 0.01)
{
double acc = autoPilot.vessel.Thrust / autoPilot.vessel.Mass;
float throttle = 1.0f + (float)autoPilot.throttleController.Update(autoPilot.maxAcc, acc, deltaTime);
state.Throttle = throttle.Clamp(0.01f, 1.0f);
}
}
// Run the auto-pilot
autoPilot.SAS = false;
autoPilot.attitudeController.Update(state);
return(true);
}
void DoAutoPiloting(PilotAddon.ControlInputs state)
{
SAS = false;
var currentDirection = ReferenceFrame.DirectionFromWorldSpace(InternalVessel.ReferenceTransform.up);
rotationRateController.ReferenceFrame = ReferenceFrame;
var targetRotation = Vector3d.zero;
if (targetDirection != Vector3d.zero)
{
targetRotation += (Vector3.Cross(targetDirection, currentDirection) * RotationSpeedMultiplier).ClampMagnitude(0f, MaxRotationSpeed);
}
if (!Double.IsNaN(TargetRoll))
{
float currentRoll = (float)ReferenceFrame.RotationFromWorldSpace(InternalVessel.ReferenceTransform.rotation).PitchHeadingRoll().z;
var rollError = GeometryExtensions.NormAngle(TargetRoll - currentRoll) * (Math.PI / 180f);
targetRotation += targetDirection * (rollError * RollSpeedMultiplier).Clamp(-MaxRollSpeed, MaxRollSpeed);
}
rotationRateController.Target = targetRotation;
rotationRateController.Update(state);
}
internal static bool Fly(global::Vessel vessel, PilotAddon.ControlInputs state)
{
// Get the auto-pilot object. Do nothing if there is no auto-pilot engaged for this vessel.
if (!engaged.ContainsKey(vessel.id))
{
return(false);
}
var autoPilot = engaged [vessel.id];
if (autoPilot == null)
{
return(false);
}
// If the client that engaged the auto-pilot has disconnected, disengage the auto-pilot
if (autoPilot.requestingClient != null && !autoPilot.requestingClient.Connected)
{
autoPilot.Disengage();
return(false);
}
// Run the auto-pilot
autoPilot.DoAutoPiloting(state);
return(true);
}
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;
}
