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