public void tuneTf(Vector3d torque) { Vector3d ratio = new Vector3d( torque.x != 0 ? vesselState.MoI.x / torque.x : 0, torque.z != 0 ? vesselState.MoI.z / torque.z : 0, //y <=> z torque.y != 0 ? vesselState.MoI.y / torque.y : 0 //z <=> y ); TfV = 0.05 * ratio; Vector3d delayFactor = Vector3d.one + 2 * vesselState.torqueReactionSpeed; TfV.Scale(delayFactor); TfV = TfV.Clamp(2.0 * TimeWarp.fixedDeltaTime, 1.0); TfV = TfV.Clamp(TfMin, TfMax); //Tf = Mathf.Clamp((float)ratio.magnitude / 20f, 2 * TimeWarp.fixedDeltaTime, 1f); //Tf = Mathf.Clamp((float)Tf, (float)TfMin, (float)TfMax); }
public void Drive(FlightCtrlState s) { if (useSAS) { _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0); if (!vessel.ActionGroups[KSPActionGroup.SAS]) { vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true); vessel.Autopilot.SAS.LockHeading(_requestedAttitude); lastSAS = _requestedAttitude; } else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10) { vessel.Autopilot.SAS.LockHeading(_requestedAttitude); lastSAS = _requestedAttitude; } else { vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true); } } else { // Direction we want to be facing _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget; Transform vesselTransform = vessel.ReferenceTransform; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselTransform.rotation) * _requestedAttitude); Vector3d deltaEuler = delta.DeltaEuler(); // ( MoI / available torque ) factor: Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132); // Find out the real shorter way to turn were we wan to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0; // And the lowest roll // Thanks to Crzyrndm Vector3 normVec = Vector3.Cross(_requestedAttitude * Vector3.forward, vesselTransform.up); Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * _requestedAttitude; float rollError = Vector3.Angle(vesselTransform.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselTransform.forward)); error = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, rollError * Mathf.Deg2Rad ); error.Scale(_axisControl); Vector3d err = error + inertia.Reorder(132) / 2d; err = new Vector3d( Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); if (Tf_autoTune) tuneTf(torque); setPIDParameters(); // angular velocity limit: var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit), Math.Sqrt(NormFactor.y * Math.PI * kWlimit), Math.Sqrt(NormFactor.z * Math.PI * kWlimit)); pidAction = pid.Compute(err, omega, Wlimit); // deadband pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0f; pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0f; pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0f; // low pass filter, wf = 1/Tf: Vector3d act = lastAct; act.x += (pidAction.x - lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0)); act.y += (pidAction.y - lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0)); act.z += (pidAction.z - lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0)); lastAct = act; SetFlightCtrlState(act, deltaEuler, s, 1); act = new Vector3d(s.pitch, s.yaw, s.roll); // Feed the control torque to the differential throttle } }
private void drive(FlightCtrlState s) { stress = 0; if (part.State == PartStates.DEAD) { return; } if (controlModule != null) { if (controlModule.enabled) { controlModule.drive(s); } else { _controlModule = null; } } if (TimeWarp.CurrentRate > TimeWarp.MaxPhysicsRate) { return; } if ((tmode != TMode.OFF) && (vesselState.thrustAvailable > 0)) { double spd = 0; if (trans_land) { if (part.vessel.Landed || part.vessel.Splashed) { tmode = TMode.OFF; trans_land = false; } else { tmode = TMode.KEEP_VERTICAL; trans_kill_h = true; double minalt = Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue); if (vesselState.maxThrustAccel < vesselState.gravityForce.magnitude) { trans_spd_act = 0; } else { trans_spd_act = (float)-Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * minalt) * 0.50F; } if (!trans_land_gears && (minalt < 1000)) { foreach (Part p in part.vessel.parts) { if (p is LandingLeg) { LandingLeg l = (LandingLeg)p; if (l.legState == LandingLeg.LegStates.RETRACTED) { l.DeployOnActivate = true; l.force_activate(); } } } trans_land_gears = true; } if (minalt < 200) { minalt = Math.Min(vesselState.altitudeBottom, minalt); trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.50F, (float)minalt / 200); trans_spd_act = (float)Math.Min(-trans_land_touchdown_speed, trans_spd_act); // if (minalt < 3.0 * trans_land_touchdown_speed) trans_spd_act = -(float)trans_land_touchdown_speed; /* if ((minalt >= 90) && (vesselState.speedHorizontal > 1)) { trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.90F, (float)(minalt - 100) / 300); } else { trans_spd_act = -Mathf.Lerp(0, (float)Math.Sqrt((vesselState.maxThrustAccel - vesselState.gravityForce.magnitude) * 2 * 200) * 0.90F, (float)minalt / 300); } */ } } } switch (tmode) { case TMode.KEEP_ORBITAL: spd = vesselState.speedOrbital; break; case TMode.KEEP_SURFACE: spd = vesselState.speedSurface; break; case TMode.KEEP_VERTICAL: spd = vesselState.speedVertical; Vector3d rot = Vector3d.up; if (trans_kill_h) { Vector3 hsdir = Vector3.Exclude(vesselState.up, part.vessel.orbit.GetVel() - part.vessel.mainBody.getRFrmVel(vesselState.CoM)); Vector3 dir = -hsdir + vesselState.up * Math.Max(Math.Abs(spd), 20 * part.vessel.mainBody.GeeASL); if ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) > 5000) && (hsdir.magnitude > Math.Max(Math.Abs(spd), 100 * part.vessel.mainBody.GeeASL) * 2)) { tmode = TMode.DIRECT; trans_spd_act = 100; rot = -hsdir; } else { if (spd > 0) { rot = vesselState.up; } else { rot = dir.normalized; } } attitudeTo(rot, AttitudeReference.INERTIAL, null); } break; } double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel; t_integral += t_err * TimeWarp.fixedDeltaTime; double t_deriv = (t_err - t_prev_err) / TimeWarp.fixedDeltaTime; double t_act = (t_Kp * t_err) + (t_Ki * t_integral) + (t_Kd * t_deriv); t_prev_err = t_err; double int_error = Math.Abs(Vector3d.Angle(attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Vector3d.forward, vesselState.forward)); //print("int_error = " + int_error); if ((tmode != TMode.KEEP_VERTICAL) || (int_error < 2) || ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (int_error < 90))) { if (tmode == TMode.DIRECT) { trans_prev_thrust = s.mainThrottle = trans_spd_act / 100.0F; } else { trans_prev_thrust = s.mainThrottle = Mathf.Clamp(trans_prev_thrust + (float)t_act, 0, 1.0F); } } else { if ((int_error >= 2) && (vesselState.torqueThrustPYAvailable > vesselState.torquePYAvailable * 10)) { trans_prev_thrust = s.mainThrottle = 0.1F; } else { trans_prev_thrust = s.mainThrottle = 0; } } } if (attitudeActive) { int userCommanding = (Mathfx.Approx(s.pitch, 0, 0.1F) ? 0 : 1) + (Mathfx.Approx(s.yaw, 0, 0.1F) ? 0 : 2) + (Mathfx.Approx(s.roll, 0, 0.1F) ? 0 : 4); s.killRot = false; Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(part.vessel.transform.rotation) * target); /* if (Mathf.Abs(Quaternion.Angle(delta, Quaternion.identity)) > 30) { delta = Quaternion.Slerp(Quaternion.identity, delta, 0.1F); } */ Vector3d deltaEuler = delta.eulerAngles; Vector3d err = new Vector3d((deltaEuler.x > 180) ? (deltaEuler.x - 360.0F) : deltaEuler.x, -((deltaEuler.y > 180) ? (deltaEuler.y - 360.0F) : deltaEuler.y), (deltaEuler.z > 180) ? (deltaEuler.z - 360.0F) : deltaEuler.z) * Math.PI / 180.0F; Vector3d torque = new Vector3d(vesselState.torquePYAvailable + vesselState.torqueThrustPYAvailable * s.mainThrottle, vesselState.torqueRAvailable, vesselState.torquePYAvailable + vesselState.torqueThrustPYAvailable * s.mainThrottle); Vector3d inertia = Vector3d.Scale(MuUtils.Sign(vesselState.angularMomentum) * 1.1, Vector3d.Scale(Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), MuUtils.Invert(Vector3d.Scale(torque, vesselState.MoI)))); err += MuUtils.Reorder(inertia, 132); err.Scale(Vector3d.Scale(vesselState.MoI, MuUtils.Invert(torque))); integral += err * TimeWarp.fixedDeltaTime; Vector3d deriv = (err - prev_err) / TimeWarp.fixedDeltaTime; act = Kp * err + Ki * integral + Kd * deriv; prev_err = err; if (userCommanding != 0) { if (attitudeKILLROT) { attitudeTo(Quaternion.LookRotation(part.vessel.transform.up, -part.vessel.transform.forward), AttitudeReference.INERTIAL, null); } } if ((userCommanding & 4) > 0) { prev_err = new Vector3d(prev_err.x, prev_err.y, 0); integral = new Vector3d(integral.x, integral.y, 0); if (!attitudeRollMatters) { attitudeTo(Quaternion.LookRotation(attitudeTarget * Vector3d.forward, attitudeWorldToReference(-part.vessel.transform.forward, attitudeReference)), attitudeReference, null); _attitudeRollMatters = false; } } else { if (!double.IsNaN(act.z)) s.roll = Mathf.Clamp(s.roll + act.z, -1.0F, 1.0F); } if ((userCommanding & 3) > 0) { prev_err = new Vector3d(0, 0, prev_err.z); integral = new Vector3d(0, 0, integral.z); } else { if (!double.IsNaN(act.x)) s.pitch = Mathf.Clamp(s.pitch + act.x, -1.0F, 1.0F); if (!double.IsNaN(act.y)) s.yaw = Mathf.Clamp(s.yaw + act.y, -1.0F, 1.0F); } stress = Mathf.Min(Mathf.Abs(act.x), 1.0F) + Mathf.Min(Mathf.Abs(act.y), 1.0F) + Mathf.Min(Mathf.Abs(act.z), 1.0F); } if (double.IsNaN(s.mainThrottle)) { print("MechJeb: caught throttle = NaN"); s.mainThrottle = 0; } }
/// <summary> /// Automatically guides the ship to face a desired orientation /// </summary> /// <param name="target">The desired orientation</param> /// <param name="c">The FlightCtrlState for the current vessel.</param> /// <param name="fc">The flight computer carrying out the slew</param> /// <param name="ignoreRoll">[optional] to ignore the roll</param> public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll) { // Add support for roll-less targets later -- Starstrider42 bool fixedRoll = !ignoreRoll; Vessel vessel = fc.Vessel; Vector3d momentOfInertia = GetTrueMoI(vessel); Transform vesselReference = vessel.GetTransform(); //--------------------------------------- // Copied almost verbatim from MechJeb master on June 27, 2014 -- Starstrider42 Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target); Vector3d torque = GetTorque(vessel, c.mainThrottle); Vector3d spinMargin = GetStoppingAngle(vessel, torque); // Allow for zero torque around some but not all axes Vector3d normFactor; normFactor.x = (torque.x != 0 ? momentOfInertia.x / torque.x : 0.0); normFactor.y = (torque.y != 0 ? momentOfInertia.y / torque.y : 0.0); normFactor.z = (torque.z != 0 ? momentOfInertia.z / torque.z : 0.0); normFactor = SwapYZ(normFactor); // Find out the real shorter way to turn were we want to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselReference.transform.rotation.Inverse() * target * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); var rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0f; var err = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, fixedRoll ? ((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F ); err += SwapYZ(spinMargin); err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(normFactor); // angular velocity: Vector3d omega = SwapYZ(vessel.angularVelocity); omega.Scale(normFactor); Vector3d pidAction = fc.pid.Compute(err, omega); // low pass filter, wf = 1/Tf: Vector3d act = fc.lastAct + (pidAction - fc.lastAct) * (1 / ((fc.Tf / TimeWarp.fixedDeltaTime) + 1)); fc.lastAct = act; // end MechJeb import //--------------------------------------- float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f); float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision)); act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit); act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit); act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit); c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit); c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit); c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit); }
public override void Drive(FlightCtrlState s) { if (useSAS) { _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0); if (!part.vessel.ActionGroups[KSPActionGroup.SAS]) { part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true); part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude); lastSAS = _requestedAttitude; } else if (Quaternion.Angle(lastSAS, _requestedAttitude) > 10) { part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude); lastSAS = _requestedAttitude; } else { part.vessel.Autopilot.SAS.LockHeading(_requestedAttitude, true); } core.thrust.differentialThrottleDemandedTorque = Vector3d.zero; } else { // Direction we want to be facing _requestedAttitude = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vessel.GetTransform().rotation) * _requestedAttitude); 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 = vesselState.torqueAvailable + vesselState.torqueFromEngine * vessel.ctrlState.mainThrottle; if (core.thrust.differentialThrottleSuccess) torque += vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle / 2; Vector3d inertia = Vector3d.Scale( vesselState.angularMomentum.Sign(), Vector3d.Scale( Vector3d.Scale(vesselState.angularMomentum, vesselState.angularMomentum), Vector3d.Scale(torque, vesselState.MoI).Invert() ) ); // ( MoI / avaiable torque ) factor: Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132); // Find out the real shorter way to turn were we wan to. // Thanks to HoneyFox Vector3d tgtLocalUp = vessel.ReferenceTransform.transform.rotation.Inverse() * _requestedAttitude * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0f; Vector3d err = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, attitudeRollMatters ? ((delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z) * Math.PI / 180.0F : 0F ); err += inertia.Reorder(132) / 2; err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); pidAction = pid.Compute(err, omega); // low pass filter, wf = 1/Tf: Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1)); lastAct = act; SetFlightCtrlState(act, deltaEuler, s, 1); act = new Vector3d(s.pitch, s.yaw, s.roll); // Feed the control torque to the differential throttle if (core.thrust.differentialThrottleSuccess) core.thrust.differentialThrottleDemandedTorque = -Vector3d.Scale(act.xzy, vesselState.torqueFromDiffThrottle * vessel.ctrlState.mainThrottle); } }
public void UpdateStateVectors() { Direction targetdir = GetDirectionFromValue(); if (targetdir == null) { shared.Logger.LogWarning(string.Format("SteeringManager target direction is null, Value = {0}", Value)); return; } targetRot = targetdir.Rotation; centerOfMass = shared.Vessel.findWorldCenterOfMass(); vesselTransform = shared.Vessel.ReferenceTransform; // Found that the default rotation has top pointing forward, forward pointing down, and right pointing starboard. // This fixes that rotation. vesselRotation = vesselTransform.rotation * Quaternion.Euler(-90, 0, 0); vesselForward = vesselRotation * Vector3d.forward; vesselTop = vesselRotation * Vector3d.up; vesselStarboard = vesselRotation * Vector3d.right; vesselUp = (centerOfMass - shared.Vessel.mainBody.position).normalized; targetForward = targetRot * Vector3d.forward; targetTop = targetRot * Vector3d.up; targetStarboard = targetRot * Vector3d.right; Vector3d oldOmega = omega; // omega is angular rotation. need to correct the signs to agree with the facing axis omega = Quaternion.Inverse(vesselRotation) * shared.Vessel.rigidbody.angularVelocity; omega.x *= -1; //positive values pull the nose to the starboard. //omega.y *= -1; // positive values pull the nose up. omega.z *= -1; // positive values pull the starboard side up. // TODO: Currently adjustments to MOI are only enabled in debug compiles. Using this feature seems to be buggy, but it has potential // to be more resiliant against random spikes in angular velocity. if (sessionTime > lastSessionTime) { double dt = sessionTime - lastSessionTime; angularAcceleration = (omega - oldOmega) / dt; angularAcceleration = new Vector3d(angularAcceleration.x, angularAcceleration.z, angularAcceleration.y); if (enableMoiAdjust) { measuredMomentOfInertia = new Vector3d( controlTorque.x * accPitch / angularAcceleration.x, controlTorque.y * accRoll / angularAcceleration.y, controlTorque.z * accYaw / angularAcceleration.z); if (Math.Abs(accPitch) > EPSILON) { adjustMomentOfInertia.x = pitchMOICalc.Update(Math.Abs(measuredMomentOfInertia.x) / momentOfInertia.x); } if (Math.Abs(accYaw) > EPSILON) { adjustMomentOfInertia.z = yawMOICalc.Update(Math.Abs(measuredMomentOfInertia.z) / momentOfInertia.z); } if (Math.Abs(accRoll) > EPSILON) { adjustMomentOfInertia.y = rollMOICalc.Update(Math.Abs(measuredMomentOfInertia.y) / momentOfInertia.y); } } } momentOfInertia = shared.Vessel.findLocalMOI(centerOfMass); momentOfInertia.Scale(adjustMomentOfInertia); adjustTorque = Vector3d.zero; measuredTorque = Vector3d.Scale(momentOfInertia, angularAcceleration); if (sessionTime > lastSessionTime && EnableTorqueAdjust) { if (Math.Abs(accPitch) > EPSILON) { adjustTorque.x = Math.Min(Math.Abs(pitchTorqueCalc.Update(measuredTorque.x / accPitch)) - rawTorque.x, 0); //adjustTorque.x = Math.Abs(pitchTorqueCalc.Update(measuredTorque.x / accPitch) / rawTorque.x); } else adjustTorque.x = Math.Abs(pitchTorqueCalc.Update(pitchTorqueCalc.Mean)); if (Math.Abs(accYaw) > EPSILON) { adjustTorque.z = Math.Min(Math.Abs(yawTorqueCalc.Update(measuredTorque.z / accYaw)) - rawTorque.z, 0); //adjustTorque.z = Math.Abs(yawTorqueCalc.Update(measuredTorque.z / accYaw) / rawTorque.z); } else adjustTorque.z = Math.Abs(yawTorqueCalc.Update(yawTorqueCalc.Mean)); if (Math.Abs(accRoll) > EPSILON) { adjustTorque.y = Math.Min(Math.Abs(rollTorqueCalc.Update(measuredTorque.y / accRoll)) - rawTorque.y, 0); //adjustTorque.y = Math.Abs(rollTorqueCalc.Update(measuredTorque.y / accRoll) / rawTorque.y); } else adjustTorque.y = Math.Abs(rollTorqueCalc.Update(rollTorqueCalc.Mean)); } }
public override void Drive(FlightCtrlState s) { if (useSAS) { Quaternion target = attitudeGetReferenceRotation(attitudeReference) * attitudeTarget * Quaternion.Euler(90, 0, 0); if (!part.vessel.ActionGroups[KSPActionGroup.SAS]) { part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true); part.vessel.VesselSAS.LockHeading(target); lastSAS = target; } else if (Quaternion.Angle(lastSAS, target) > 10) { part.vessel.VesselSAS.LockHeading(target); lastSAS = target; } else { part.vessel.VesselSAS.LockHeading(target, true); } } else { // 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() ) ); // ( MoI / avaiable torque ) factor: Vector3d NormFactor = Vector3d.Scale(vesselState.MoI, torque.Invert()).Reorder(132); // angular error: Vector3d err = deltaEuler * Math.PI / 180.0F; err += inertia.Reorder(132) / 2; err = new Vector3d(Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); pidAction = pid.Compute(err, omega); // low pass filter, wf = 1/Tf: Vector3d act = lastAct + (pidAction - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1)); lastAct = act; SetFlightCtrlState(act, deltaEuler, s, 1); act = new Vector3d(s.pitch, s.yaw, s.roll); } }
public Vector3d Compute(Vector3d error, Vector3d omega) { derivativeAct = omega; derivativeAct.Scale(Kd); // integral actíon + Anti Windup intAccum.x = (Math.Abs(derivativeAct.x) < 0.6 * max) ? intAccum.x + (error.x * Ki.x * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.x; intAccum.y = (Math.Abs(derivativeAct.y) < 0.6 * max) ? intAccum.y + (error.y * Ki.y * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.y; intAccum.z = (Math.Abs(derivativeAct.z) < 0.6 * max) ? intAccum.z + (error.z * Ki.z * TimeWarp.fixedDeltaTime) : 0.9 * intAccum.z; propAct = error; propAct.Scale(Kp); Vector3d action = propAct + derivativeAct + intAccum; // action clamp action = new Vector3d(Math.Max(min, Math.Min(max, action.x)), Math.Max(min, Math.Min(max, action.y)), Math.Max(min, Math.Min(max, action.z))); return action; }
/// <summary> /// Automatically guides the ship to face a desired orientation /// </summary> /// <param name="target">The desired orientation</param> /// <param name="c">The FlightCtrlState for the current vessel.</param> /// <param name="fc">The flight computer carrying out the slew</param> /// <param name="ignoreRoll">[optional] to ignore the roll</param> public static void SteerShipToward(Quaternion target, FlightCtrlState c, FlightComputer fc, bool ignoreRoll) { // Add support for roll-less targets later -- Starstrider42 bool fixedRoll = !ignoreRoll; Vessel vessel = fc.Vessel; Vector3d momentOfInertia = vessel.MOI; Transform vesselReference = vessel.GetTransform(); Vector3d torque = GetVesselTorque(vessel); // ----------------------------------------------- // Copied from MechJeb master on 18.04.2016 with some modifications to adapt to RemoteTech Vector3d _axisControl = new Vector3d(); _axisControl.x = true ? 1 : 0; _axisControl.y = true ? 1 : 0; _axisControl.z = fixedRoll ? 1 : 0; Vector3d inertia = Vector3d.Scale( new Vector3d(vessel.angularMomentum.x, vessel.angularMomentum.y, vessel.angularMomentum.z).Sign(), Vector3d.Scale( Vector3d.Scale(vessel.angularMomentum, vessel.angularMomentum), Vector3d.Scale(torque, momentOfInertia).Invert() ) ); Vector3d TfV = new Vector3d(0.3, 0.3, 0.3); double kpFactor = 3; double kiFactor = 6; double kdFactor = 0.5; double kWlimit = 0.15; double deadband = 0.0001; Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(vesselReference.rotation) * target); Vector3d deltaEuler = delta.DeltaEuler(); // ( MoI / available torque ) factor: Vector3d NormFactor = Vector3d.Scale(momentOfInertia, torque.Invert()).Reorder(132); // Find out the real shorter way to turn were we want to. // Thanks to HoneyFox Vector3d tgtLocalUp = vesselReference.rotation.Inverse() * target * Vector3d.forward; Vector3d curLocalUp = Vector3d.up; double turnAngle = Math.Abs(Vector3d.Angle(curLocalUp, tgtLocalUp)); Vector2d rotDirection = new Vector2d(tgtLocalUp.x, tgtLocalUp.z); rotDirection = rotDirection.normalized * turnAngle / 180.0; // And the lowest roll // Thanks to Crzyrndm Vector3 normVec = Vector3.Cross(target * Vector3.forward, vesselReference.up); Quaternion targetDeRotated = Quaternion.AngleAxis((float)turnAngle, normVec) * target; float rollError = Vector3.Angle(vesselReference.right, targetDeRotated * Vector3.right) * Math.Sign(Vector3.Dot(targetDeRotated * Vector3.right, vesselReference.forward)); var error = new Vector3d( -rotDirection.y * Math.PI, rotDirection.x * Math.PI, rollError * Mathf.Deg2Rad ); error.Scale(_axisControl); Vector3d err = error + inertia.Reorder(132) / 2d; err = new Vector3d( Math.Max(-Math.PI, Math.Min(Math.PI, err.x)), Math.Max(-Math.PI, Math.Min(Math.PI, err.y)), Math.Max(-Math.PI, Math.Min(Math.PI, err.z))); err.Scale(NormFactor); // angular velocity: Vector3d omega; omega.x = vessel.angularVelocity.x; omega.y = vessel.angularVelocity.z; // y <=> z omega.z = vessel.angularVelocity.y; // z <=> y omega.Scale(NormFactor); //if (Tf_autoTune) // tuneTf(torque); Vector3d invTf = TfV.Invert(); fc.pid.Kd = kdFactor * invTf; fc.pid.Kp = (1 / (kpFactor * Math.Sqrt(2))) * fc.pid.Kd; fc.pid.Kp.Scale(invTf); fc.pid.Ki = (1 / (kiFactor * Math.Sqrt(2))) * fc.pid.Kp; fc.pid.Ki.Scale(invTf); fc.pid.intAccum = fc.pid.intAccum.Clamp(-5, 5); // angular velocity limit: var Wlimit = new Vector3d(Math.Sqrt(NormFactor.x * Math.PI * kWlimit), Math.Sqrt(NormFactor.y * Math.PI * kWlimit), Math.Sqrt(NormFactor.z * Math.PI * kWlimit)); Vector3d pidAction = fc.pid.Compute(err, omega, Wlimit); // deadband pidAction.x = Math.Abs(pidAction.x) >= deadband ? pidAction.x : 0.0; pidAction.y = Math.Abs(pidAction.y) >= deadband ? pidAction.y : 0.0; pidAction.z = Math.Abs(pidAction.z) >= deadband ? pidAction.z : 0.0; // low pass filter, wf = 1/Tf: Vector3d act = fc.lastAct; act.x += (pidAction.x - fc.lastAct.x) * (1.0 / ((TfV.x / TimeWarp.fixedDeltaTime) + 1.0)); act.y += (pidAction.y - fc.lastAct.y) * (1.0 / ((TfV.y / TimeWarp.fixedDeltaTime) + 1.0)); act.z += (pidAction.z - fc.lastAct.z) * (1.0 / ((TfV.z / TimeWarp.fixedDeltaTime) + 1.0)); fc.lastAct = act; // end MechJeb import //--------------------------------------- float precision = Mathf.Clamp((float)(torque.x * 20f / momentOfInertia.magnitude), 0.5f, 10f); float driveLimit = Mathf.Clamp01((float)(err.magnitude * 380.0f / precision)); act.x = Mathf.Clamp((float)act.x, -driveLimit, driveLimit); act.y = Mathf.Clamp((float)act.y, -driveLimit, driveLimit); act.z = Mathf.Clamp((float)act.z, -driveLimit, driveLimit); c.roll = Mathf.Clamp((float)(c.roll + act.z), -driveLimit, driveLimit); c.pitch = Mathf.Clamp((float)(c.pitch + act.x), -driveLimit, driveLimit); c.yaw = Mathf.Clamp((float)(c.yaw + act.y), -driveLimit, driveLimit); }