public override void Drive(FlightCtrlState s) { Vector3d worldVelocityDelta = vesselState.velocityVesselOrbit - targetVelocity; worldVelocityDelta += TimeWarp.fixedDeltaTime * vesselState.gravityForce; //account for one frame's worth of gravity Vector3d velocityDelta = Quaternion.Inverse(vessel.GetTransform().rotation) * worldVelocityDelta; if (!conserveFuel || (velocityDelta.magnitude > conserveThreshold)) { if (!vessel.ActionGroups[KSPActionGroup.RCS]) { vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true); } Vector3d rcs = new Vector3d(); foreach (Vector6.Direction dir in Enum.GetValues(typeof(Vector6.Direction))) { if (vesselState.rcsThrustAvailable[dir] > 0) { double dV = Vector3d.Dot(velocityDelta, Vector6.directions[dir]) / (vesselState.rcsThrustAvailable[dir] * TimeWarp.fixedDeltaTime / vesselState.mass); if (dV > 0) { rcs += Vector6.directions[dir] * dV; } } } //rcs = pid.Compute(rcs, rcs - lastAct); // Having an Omega would be nice but each test made it worse rcs = pid.Compute(rcs); //rcs = lastAct + (rcs - lastAct) * (1 / ((0.5 / TimeWarp.fixedDeltaTime) + 1)); lastAct = rcs; s.X = Mathf.Clamp((float)rcs.x, -1, 1); s.Y = Mathf.Clamp((float)rcs.z, -1, 1); //note that z and s.Z = Mathf.Clamp((float)rcs.y, -1, 1); //y must be swapped } else if (conserveFuel) { if (vessel.ActionGroups[KSPActionGroup.RCS]) { vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, false); } } base.Drive(s); }
public override void Drive(FlightCtrlState s) { // Used in the killRot activation calculation and drive_limit calculation double precision = Math.Max(0.5, Math.Min(10.0, (Math.Min(vesselState.torqueAvailable.x, vesselState.torqueAvailable.z) + vesselState.torqueThrustPYAvailable * s.mainThrottle) * 20.0 / vesselState.MoI.magnitude)); // Reset the PID controller during roll to keep pitch and yaw errors // from accumulating on the wrong axis. double rollDelta = Mathf.Abs((float)(vesselState.vesselRoll - lastResetRoll)); if (rollDelta > 180) { rollDelta = 360 - rollDelta; } if (rollDelta > 5) { pid.Reset(); lastResetRoll = vesselState.vesselRoll; } // Direction we want to be facing Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * target); Vector3d deltaEuler = new Vector3d( (delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x, -((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y), (delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z ); Vector3d torque = new Vector3d( vesselState.torqueAvailable.x + vesselState.torqueThrustPYAvailable * s.mainThrottle, vesselState.torqueAvailable.y, vesselState.torqueAvailable.z + vesselState.torqueThrustPYAvailable * s.mainThrottle ); Vector3d inertia = Vector3d.Scale( vesselState.angularMomentum.Sign(), Vector3d.Scale( Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), Vector3d.Scale(torque, vesselState.MoI).Invert() ) ); Vector3d err = deltaEuler * Math.PI / 180.0F; err += inertia.Reorder(132); err.Scale(Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132)); Vector3d act = pid.Compute(err); float drive_limit = Mathf.Clamp01((float)(err.magnitude * drive_factor / precision)); act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit); act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit); act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit); act = lastAct + (act - lastAct) * (TimeWarp.fixedDeltaTime / Tf); SetFlightCtrlState(act, deltaEuler, s, precision, drive_limit); act = new Vector3d(s.pitch, s.yaw, s.roll); lastAct = act; stress = Math.Abs(act.x) + Math.Abs(act.y) + Math.Abs(act.z); }