public override void Drive(FlightCtrlState s)
        {
            if (controlHeading)
            {
                if (heading != headingLast)
                {
                    headingPID.Reset();
                    headingLast = heading;
                }

                double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
                headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
                if (s.wheelSteer == s.wheelSteerTrim)
                {
                    double act = headingPID.Compute(headingErr);
                    s.wheelSteer = Mathf.Clamp((float)act, -1, 1);
                }
            }
            if (controlSpeed)
            {
                if (speed != speedLast)
                {
                    speedPID.Reset();
                    speedLast = speed;
                }

                speedErr = speed - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.forward);
                if (s.wheelThrottle == s.wheelThrottleTrim)
                {
                    double act = speedPID.Compute(speedErr);
                    s.wheelThrottle = Mathf.Clamp((float)act, -1, 1);
                }
            }
        }
        public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
        {                                             // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
            if (orbit.referenceBody != lastBody)
            {
                WaypointIndex = -1; Waypoints.Clear();
            }
            MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);

            var brake = vessel.ActionGroups[KSPActionGroup.Brakes];             // keep brakes locked if they are

            curSpeed = Vector3d.Dot(vessel.srf_velocity, vesselState.forward);

            CalculateTraction();
            speedIntAcc = speedPID.intAccum;

            if (wp != null && wp.Body == orbit.referenceBody)
            {
                if (ControlHeading)
                {
                    heading = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
                }
                if (ControlSpeed)
                {
                    var nextWP   = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
                    var distance = Vector3.Distance(vessel.CoM, wp.Position);
                    if (wp.Target != null)
                    {
                        distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
                    }
                    //var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
                    var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed);                     // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
                    var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
                                    (nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
                                     (distance - wp.Radius > 50 ? turnSpeed.val : 1)));
                    minSpeed = (wp.Quicksave ? 1 : minSpeed);
                    // ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
                    var brakeFactor = Math.Max((curSpeed - minSpeed) * 1, 3);
                    var newSpeed    = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / brakeFactor, minSpeed));        // brake when getting closer
                    newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed);                     // reduce speed when turning a lot
                    if (LimitAcceleration)
                    {
                        newSpeed = curSpeed + Mathf.Clamp((float)(newSpeed - curSpeed), -1.5f, 0.5f);
                    }
//					newSpeed = tgtSpeed + Mathf.Clamp((float)(newSpeed - tgtSpeed), -Time.deltaTime * 8f, Time.deltaTime * 2f);
                    var radius = Math.Max(wp.Radius, 10);
                    if (distance < radius)
                    {
                        if (WaypointIndex + 1 >= Waypoints.Count)                         // last waypoint
                        {
                            newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
                            // ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
                            if (LoopWaypoints)
                            {
                                WaypointIndex = 0;
                            }
                            else
                            {
                                newSpeed = 0;
//								tgtSpeed.force(newSpeed);
                                if (curSpeed < brakeSpeedLimit)
                                {
                                    if (wp.Quicksave)
                                    {
                                        //if (s.mainThrottle > 0) { s.mainThrottle = 0; }
                                        if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
                                        {
                                            WaypointIndex  = -1;
                                            ControlHeading = ControlSpeed = false;
                                            QuickSaveLoad.QuickSave();
                                        }
                                    }
                                    else
                                    {
                                        WaypointIndex  = -1;
                                        ControlHeading = ControlSpeed = false;
                                    }
                                }
//								else {
//									Debug.Log("Is this even getting called?");
//									WaypointIndex++;
//								}
                            }
                        }
                        else
                        {
                            if (wp.Quicksave)
                            {
                                //if (s.mainThrottle > 0) { s.mainThrottle = 0; }
                                newSpeed = 0;
//								tgtSpeed.force(newSpeed);
                                if (curSpeed < brakeSpeedLimit)
                                {
                                    if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
                                    {
                                        WaypointIndex++;
                                        QuickSaveLoad.QuickSave();
                                    }
                                }
                            }
                            else
                            {
                                WaypointIndex++;
                            }
                        }
                    }
                    brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
                    // ^ brake if needed to prevent rolling, hopefully
                    tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
                }
            }

            if (ControlHeading)
            {
                headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);

                double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
                headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
                if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
                {
                    float  spd   = Mathf.Min((float)speed, (float)turnSpeed);                  // if a slower speed than the turnspeed is used also be more careful with the steering
                    float  limit = (Mathf.Abs((float)curSpeed) <= turnSpeed ? 1 : Mathf.Clamp((float)(spd / Mathf.Abs((float)curSpeed)), 0.35f, 1f));
                    double act   = headingPID.Compute(headingErr);
                    s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
                }
            }

            // Brake if there is no controler (Pilot eject from seat)
            if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
            {
                s.wheelThrottle = 0;
//				vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
                brake = true;
            }
            else if (ControlSpeed)
            {
                speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);

                speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vessel.srf_velocity, vesselState.forward);
                if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
                {
                    float act = (float)speedPID.Compute(speedErr);
                    s.wheelThrottle = !LimitAcceleration?Mathf.Clamp((float)act, -1, 1) :                       // I think I'm using these ( ? : ) a bit too much
                                          (traction == 0 ? 0 : (act < 0 ? Mathf.Clamp(act, -1f, 1f) : (lastThrottle + Mathf.Clamp(act - lastThrottle, -0.005f, 0.005f)) * (traction < tractionLimit ? -1 : 1)));

                    if (curSpeed < 0 & s.wheelThrottle < 0)
                    {
                        s.wheelThrottle = 0;
                    }                                                                                                        // don't go backwards
//					if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0) { s.wheelThrottle = Mathf.Clamp(act, -1f, 1f); }
                    if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) == 0) // StabilityControl && traction > 50 &&
//						vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
                    {
                        brake = true;
                    }
//					else if (!stabilityControl || traction <= 50 || speedErr > -0.2 || Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0) {
//						vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, (GameSettings.BRAKES.GetKey() && vessel.isActiveVessel));
//					}
                    lastThrottle = s.wheelThrottle;
                }
            }

            if (StabilityControl)
            {
                if (!core.attitude.users.Contains(this))
                {
                    core.attitude.users.Add(this);
//					line.enabled = true;
                }
//				float scale = Vector3.Distance(FlightCamera.fetch.mainCamera.transform.position, vessel.CoM) / 900f;
//				line.SetPosition(0, vessel.CoM);
//				line.SetPosition(1, vessel.CoM + hit.normal * 5);
//				line.SetWidth(0, scale + 0.1f);
                var fSpeed = (float)curSpeed;
//				if (Mathf.Abs(fSpeed) >= turnSpeed * 0.75) {
                Vector3 fwd = (Vector3)(traction > 0 ?                 // V when the speed is low go for the vessels forward, else with a bit of velocity
//				                        ((Mathf.Abs(fSpeed) <= turnSpeed ? vesselState.forward : vessel.srf_velocity / 4) - vessel.transform.right * s.wheelSteer) * Mathf.Sign(fSpeed) :
//				                        // ^ and then add the steering
                                        vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
                                        vessel.srf_velocity);                                                                  // in the air so follow velocity
                Vector3.OrthoNormalize(ref norm, ref fwd);
                var quat = Quaternion.LookRotation(fwd, norm);

//				if (traction > 0 || speed <= turnSpeed) {
//					var u = new Vector3(0, 1, 0);
//
//					var q = FlightGlobals.ship_rotation;
//					var q_s = quat;
//
//					var q_u = new Quaternion(u.x, u.y, u.z, 0);
//					var a = Quaternion.Dot(q, q_s * q_u);
//					var q_qs = Quaternion.Dot(q, q_s);
//					var b = (a == 0) ? Math.Sign(q_qs) : (q_qs / a);
//					var g = b / Mathf.Sqrt((b * b) + 1);
//					var gu = Mathf.Sqrt(1 - (g * g)) * u;
//					var q_d = new Quaternion() { w = g, x = gu.x, y = gu.y, z = gu.z };
//					var n = q_s * q_d;
//
//					quat = n;
//				}

                core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
//				}
            }

            if (BrakeOnEnergyDepletion)
            {
                var batteries  = vessel.Parts.FindAll(p => p.Resources.Contains("ElectricCharge") && p.Resources["ElectricCharge"].flowState);
                var energyLeft = batteries.Sum(p => p.Resources["ElectricCharge"].amount) / batteries.Sum(p => p.Resources["ElectricCharge"].maxAmount);
                var openSolars = vessel.mainBody.atmosphere &&                 // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
                                 vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.panelState != ModuleDeployableSolarPanel.panelStates.BROKEN &&
                                                                                                           p.panelState != ModuleDeployableSolarPanel.panelStates.RETRACTED).Count > 0;

                if (openSolars && energyLeft > 0.99)
                {
                    vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
                                                                                              p.panelState == ModuleDeployableSolarPanel.panelStates.EXTENDED).ForEach(p => p.Retract());
                }

                if (energyLeft < 0.05 && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0)
                {
                    s.wheelThrottle = 0;
                }                                                                                                                                 // save remaining energy by not using it for acceleration
                if (openSolars || energyLeft < 0.03)
                {
                    tgtSpeed = 0;
                }

                if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
                {
                    brake = true;
                }

                if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
                    vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.panelState == ModuleDeployableSolarPanel.panelStates.EXTENDED).Count > 0)
                {
                    waitingForDaylight = true;
                }
            }

//			brake = brake && (s.wheelThrottle == 0); // release brake if the user or AP want to drive
            if (s.wheelThrottle != 0 && Mathf.Sign(s.wheelThrottle) + Mathf.Sign((float)curSpeed) != 0)
            {
                brake = false;                 // the AP or user want to drive into the direction of momentum so release the brake
            }

            if (vessel.isActiveVessel)
            {
                if (GameSettings.BRAKES.GetKeyUp())
                {
                    brake = false;                     // release the brakes if the user lets go of them
                }
                if (GameSettings.BRAKES.GetKey())
                {
                    brake = true;                     // brake if the user brakes and we aren't about to flip
                }
            }

            tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
            vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && curSpeed > brakeSpeedLimit ? traction >= tractionLimit : true));
            // ^ brake but hopefully prevent flipping over, assuming the user set up the limit right
            if (brake && curSpeed < 0.1)
            {
                s.wheelThrottle = 0;
            }
        }
        public override void Drive(FlightCtrlState s)
        {
            if (core.GetComputerModule <MechJebModuleThrustWindow>().hidden&& core.GetComputerModule <MechJebModuleAscentGuidance>().hidden)
            {
                return;
            }

            if ((tmode != TMode.OFF) && (vesselState.thrustAvailable > 0))
            {
                double spd = 0;

                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, vessel.srf_velocity);
                        Vector3 dir   = -hsdir + vesselState.up * Math.Max(Math.Abs(spd), 20 * mainBody.GeeASL);
                        if ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) > 5000) && (hsdir.magnitude > Math.Max(Math.Abs(spd), 100 * mainBody.GeeASL) * 2))
                        {
                            tmode         = TMode.DIRECT;
                            trans_spd_act = 100;
                            rot           = -hsdir;
                        }
                        else
                        {
                            rot = dir.normalized;
                        }
                        core.attitude.attitudeTo(rot, AttitudeReference.INERTIAL, null);
                    }
                    break;
                }

                double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel;
                if ((tmode == TMode.KEEP_ORBITAL && Vector3d.Dot(vesselState.forward, vessel.obt_velocity) < 0) ||
                    (tmode == TMode.KEEP_SURFACE && Vector3d.Dot(vesselState.forward, vessel.srf_velocity) < 0))
                {
                    //allow thrust to declerate
                    t_err *= -1;
                }

                double t_act = pid.Compute(t_err);

                if ((tmode != TMode.KEEP_VERTICAL) ||
                    !trans_kill_h ||
                    (core.attitude.attitudeError < 2) ||
                    ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (core.attitude.attitudeError < 90)))
                {
                    if (tmode == TMode.DIRECT)
                    {
                        trans_prev_thrust = targetThrottle = trans_spd_act / 100.0F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = Mathf.Clamp01(trans_prev_thrust + (float)t_act);
                    }
                }
                else
                {
                    if ((core.attitude.attitudeError >= 2) && (vesselState.torqueThrustPYAvailable > Math.Min(vesselState.torqueAvailable.x, vesselState.torqueAvailable.z) * 10))
                    {
                        trans_prev_thrust = targetThrottle = 0.1F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = 0;
                    }
                }
            }

            // Only set throttle if a module need it. Othewise let the user or other mods set it
            // There is always at least 1 user : the module itself (why ?)
            if (users.Count() > 1)
            {
                s.mainThrottle = targetThrottle;
            }

            float throttleLimit = 1;

            limiter = LimitMode.None;

            if (limitThrottle)
            {
                if (maxThrottle < throttleLimit)
                {
                    limiter = LimitMode.Throttle;
                }
                throttleLimit = Mathf.Min(throttleLimit, (float)maxThrottle);
            }

            if (limitToTerminalVelocity)
            {
                float limit = TerminalVelocityThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.TerminalVelocity;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitToPreventOverheats)
            {
                float limit = TemperatureSafetyThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Temperature;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitAcceleration)
            {
                float limit = AccelerationLimitedThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Acceleration;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitToPreventFlameout)
            {
                // This clause benefits being last: if we don't need much air
                // due to prior limits, we can close some intakes.
                float limit = FlameoutSafetyThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Flameout;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (double.IsNaN(throttleLimit))
            {
                throttleLimit = 0;
            }
            throttleLimit = Mathf.Clamp01(throttleLimit);

            vesselState.throttleLimit = throttleLimit;

            if (s.mainThrottle < throttleLimit)
            {
                limiter = LimitMode.None;
            }

            s.mainThrottle = Mathf.Min(s.mainThrottle, throttleLimit);

            if (smoothThrottle)
            {
                s.mainThrottle = SmoothThrottle(s.mainThrottle);
            }

            if (double.IsNaN(s.mainThrottle))
            {
                s.mainThrottle = 0;
            }
            s.mainThrottle = Mathf.Clamp01(s.mainThrottle);

            if (s.Z == 0 && vesselState.rcsThrust)
            {
                s.Z = -s.mainThrottle;
            }

            lastThrottle = s.mainThrottle;
        }
        public override void Drive(FlightCtrlState s)
        {
            float threshold = 0.1F;
            bool  _userCommandingRotation = !(Mathfx.Approx(s.pitch, s.pitchTrim, threshold) &&
                                              Mathfx.Approx(s.yaw, s.yawTrim, threshold) &&
                                              Mathfx.Approx(s.roll, s.rollTrim, threshold));
            bool _userCommandingTranslation = !(Math.Abs(s.X) < threshold &&
                                                Math.Abs(s.Y) < threshold &&
                                                Math.Abs(s.Z) < threshold);

            if (_userCommandingRotation && !_userCommandingTranslation)
            {
                userCommandingRotationSmoothed = 2;
            }
            else if (userCommandingRotationSmoothed > 0)
            {
                userCommandingRotationSmoothed--;
            }

            if (core.GetComputerModule <MechJebModuleThrustWindow>().hidden&& core.GetComputerModule <MechJebModuleAscentGuidance>().hidden)
            {
                return;
            }

            if ((tmode != TMode.OFF) && (vesselState.thrustAvailable > 0))
            {
                double spd = 0;

                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.ProjectOnPlane(vesselState.surfaceVelocity, vesselState.up);
                        Vector3 dir   = -hsdir + vesselState.up * Math.Max(Math.Abs(spd), 20 * mainBody.GeeASL);
                        if ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) > 5000) && (hsdir.magnitude > Math.Max(Math.Abs(spd), 100 * mainBody.GeeASL) * 2))
                        {
                            tmode         = TMode.DIRECT;
                            trans_spd_act = 100;
                            rot           = -hsdir;
                        }
                        else
                        {
                            rot = dir.normalized;
                        }
                        core.attitude.attitudeTo(rot, AttitudeReference.INERTIAL, null);
                    }
                    break;
                }

                double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel;
                if ((tmode == TMode.KEEP_ORBITAL && Vector3d.Dot(vesselState.forward, vesselState.orbitalVelocity) < 0) ||
                    (tmode == TMode.KEEP_SURFACE && Vector3d.Dot(vesselState.forward, vesselState.surfaceVelocity) < 0))
                {
                    //allow thrust to declerate
                    t_err *= -1;
                }

                double t_act = pid.Compute(t_err);

                if ((tmode != TMode.KEEP_VERTICAL) ||
                    !trans_kill_h ||
                    (core.attitude.attitudeError < 2) ||
                    ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (core.attitude.attitudeError < 90)))
                {
                    if (tmode == TMode.DIRECT)
                    {
                        trans_prev_thrust = targetThrottle = trans_spd_act / 100.0F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = Mathf.Clamp01(trans_prev_thrust + (float)t_act);
                    }
                }
                else
                {
                    bool useGimbal = (vesselState.torqueFromEngine.x / vessel.ctrlState.mainThrottle > vesselState.torqueAvailable.x * 10) ||
                                     (vesselState.torqueFromEngine.z / vessel.ctrlState.mainThrottle > vesselState.torqueAvailable.z * 10);

                    bool useDiffThrottle = (vesselState.torqueFromDiffThrottle.x > vesselState.torqueAvailable.x * 10) ||
                                           (vesselState.torqueFromDiffThrottle.z > vesselState.torqueAvailable.z * 10);

                    if ((core.attitude.attitudeError >= 2) && (useGimbal || (useDiffThrottle && core.thrust.differentialThrottle)))
                    {
                        trans_prev_thrust = targetThrottle = 0.1F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = 0;
                    }
                }
            }

            // Only set throttle if a module need it. Othewise let the user or other mods set it
            // There is always at least 1 user : the module itself (why ?)
            if (users.Count() > 1)
            {
                s.mainThrottle = targetThrottle;
            }

            float throttleLimit = 1;

            limiter = LimitMode.None;

            if (limitThrottle)
            {
                if (maxThrottle < throttleLimit)
                {
                    limiter = LimitMode.Throttle;
                }
                throttleLimit = Mathf.Min(throttleLimit, (float)maxThrottle);
            }

            if (limitToTerminalVelocity)
            {
                float limit = TerminalVelocityThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.TerminalVelocity;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitDynamicPressure)
            {
                float limit = MaximumDynamicPressureThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.DynamicPressure;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitToPreventOverheats)
            {
                float limit = (float)TemperatureSafetyThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Temperature;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitAcceleration)
            {
                float limit = AccelerationLimitedThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Acceleration;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (electricThrottle && ElectricEngineRunning())
            {
                float limit = ElectricThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Electric;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitToPreventFlameout)
            {
                // This clause benefits being last: if we don't need much air
                // due to prior limits, we can close some intakes.
                float limit = FlameoutSafetyThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Flameout;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limiterMinThrottle && limiter != LimitMode.None && throttleLimit < minThrottle)
            {
                limiter       = LimitMode.MinThrottle;
                throttleLimit = (float)minThrottle;
            }

            if (double.IsNaN(throttleLimit))
            {
                throttleLimit = 0;
            }
            throttleLimit = Mathf.Clamp01(throttleLimit);

            vesselState.throttleLimit = throttleLimit;

            if (s.mainThrottle < throttleLimit)
            {
                limiter = LimitMode.None;
            }

            s.mainThrottle = Mathf.Min(s.mainThrottle, throttleLimit);

            if (smoothThrottle)
            {
                s.mainThrottle = SmoothThrottle(s.mainThrottle);
            }

            if (double.IsNaN(s.mainThrottle))
            {
                s.mainThrottle = 0;
            }
            s.mainThrottle = Mathf.Clamp01(s.mainThrottle);

            if (s.Z == 0 && core.rcs.rcsThrottle && vesselState.rcsThrust)
            {
                s.Z = -s.mainThrottle;
            }

            lastThrottle = s.mainThrottle;

            if (!core.attitude.enabled)
            {
                Vector3d act = new Vector3d(s.pitch, s.yaw, s.roll);
                differentialThrottleDemandedTorque = -Vector3d.Scale(act.xzy, vesselState.torqueFromDiffThrottle * s.mainThrottle * 0.5f);
            }
        }
示例#5
0
        public override void Drive(FlightCtrlState s)
        {
            UpdatePID();

            //SpeedHold (set AccelerationTarget automatically to hold speed)
            if (SpeedHoldEnabled)
            {
                double spd = vesselState.speedSurface;
                cur_acc = (spd - _spd) / Time.fixedDeltaTime;
                _spd    = spd;
                RealAccelerationTarget = (SpeedTarget - spd) / 4;
                a_err = (RealAccelerationTarget - cur_acc);
                AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
                double t_act = AccelerationPIDController.Compute(a_err);
                if (!double.IsNaN(t_act))
                {
                    core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
                }
                else
                {
                    core.thrust.targetThrottle = 0.0f;
                    AccelerationPIDController.Reset();
                }
            }

            //AltitudeHold (set VertSpeed automatically to hold altitude)
            if (AltitudeHoldEnabled)
            {
                RealVertSpeedTarget = convertAltitudeToVerticalSpeed(AltitudeTarget - vesselState.altitudeASL);
                RealVertSpeedTarget = UtilMath.Clamp(RealVertSpeedTarget, -VertSpeedTarget, VertSpeedTarget);
            }
            else
            {
                RealVertSpeedTarget = VertSpeedTarget;
            }

            pitch_err = roll_err = yaw_err = 0;
            pitch_act = roll_act = yaw_act = 0;

            //VertSpeedHold
            if (VertSpeedHoldEnabled)
            {
                // NOTE: 60-to-1 rule:
                // deltaAltitude = 2 * PI * r * deltaPitch / 360
                // Vvertical = 2 * PI * TAS * deltaPitch / 360
                // deltaPitch = Vvertical / Vhorizontal * 180 / PI
                double deltaVertSpeed = RealVertSpeedTarget - vesselState.speedVertical;
                double adjustment     = 180 / vesselState.speedSurface * deltaVertSpeed / Math.PI;
                RealPitchTarget = vesselState.vesselPitch + adjustment;

                RealPitchTarget = UtilMath.Clamp(RealPitchTarget, -PitchDownLimit, PitchUpLimit);
                pitch_err       = MuUtils.ClampDegrees180(RealPitchTarget - vesselState.vesselPitch);

                PitchPIDController.intAccum = UtilMath.Clamp(PitchPIDController.intAccum, -100 / PitKi, 100 / PitKi);
                pitch_act = PitchPIDController.Compute(pitch_err) / 100;

                //Debug.Log (p_act);
                if (double.IsNaN(pitch_act))
                {
                    PitchPIDController.Reset();
                }
                else
                {
                    s.pitch = Mathf.Clamp((float)pitch_act, -1, 1);
                }
            }

            //HeadingHold
            double curFlightPath = vesselState.HeadingFromDirection(vesselState.forward);

            // NOTE: we can not use vesselState.vesselHeading here because it interpolates headings internally
            //       i.e. turning from 1° to 359° will end up as (1+359)/2 = 180°
            //       see class MovingAverage for more details
            curr_yaw = vesselState.currentHeading - curFlightPath;

            if (HeadingHoldEnabled)
            {
                double toturn = MuUtils.ClampDegrees180(HeadingTarget - curFlightPath);

                if (Math.Abs(toturn) < 0.2)
                {
                    // yaw for small adjustments
                    RealYawTarget  = MuUtils.Clamp(toturn * 2, -YawLimit, YawLimit);
                    RealRollTarget = 0;
                }
                else
                {
                    // roll for large adjustments
                    RealYawTarget  = 0;
                    RealRollTarget = MuUtils.Clamp(toturn * 2, -RollLimit, RollLimit);
                }
            }
            else
            {
                RealRollTarget = RollTarget;
                RealYawTarget  = 0;
            }

            if (RollHoldEnabled)
            {
                RealRollTarget = UtilMath.Clamp(RealRollTarget, -BankAngle, BankAngle);
                RealRollTarget = UtilMath.Clamp(RealRollTarget, -RollLimit, RollLimit);
                roll_err       = MuUtils.ClampDegrees180(RealRollTarget - -vesselState.currentRoll);

                RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / RolKi, 100 / RolKi);
                roll_act = RollPIDController.Compute(roll_err) / 100;

                if (double.IsNaN(roll_act))
                {
                    RollPIDController.Reset();
                }
                else
                {
                    s.roll = Mathf.Clamp((float)roll_act, -1, 1);
                }
            }

            if (HeadingHoldEnabled)
            {
                RealYawTarget = UtilMath.Clamp(RealYawTarget, -YawLimit, YawLimit);
                yaw_err       = MuUtils.ClampDegrees180(RealYawTarget - curr_yaw);

                YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -100 / YawKi, 100 / YawKi);
                yaw_act = YawPIDController.Compute(yaw_err) / 100;

                if (double.IsNaN(yaw_act))
                {
                    YawPIDController.Reset();
                }
                else
                {
                    s.yaw = Mathf.Clamp((float)yaw_act, -1, 1);
                }
            }
        }
示例#6
0
        public override void Drive(FlightCtrlState s)
        {
            float threshold = 0.1F;
            bool  _userCommandingRotation = !(Mathfx.Approx(s.pitch, s.pitchTrim, threshold) &&
                                              Mathfx.Approx(s.yaw, s.yawTrim, threshold) &&
                                              Mathfx.Approx(s.roll, s.rollTrim, threshold));
            bool _userCommandingTranslation = !(Math.Abs(s.X) < threshold &&
                                                Math.Abs(s.Y) < threshold &&
                                                Math.Abs(s.Z) < threshold);

            if (_userCommandingRotation && !_userCommandingTranslation)
            {
                userCommandingRotationSmoothed = 2;
            }
            else if (userCommandingRotationSmoothed > 0)
            {
                userCommandingRotationSmoothed--;
            }

            if (core.GetComputerModule <MechJebModuleThrustWindow>().hidden&& core.GetComputerModule <MechJebModuleAscentGuidance>().hidden)
            {
                return;
            }

            if ((tmode != TMode.OFF) && (vesselState.thrustAvailable > 0))
            {
                double spd = 0;

                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.ProjectOnPlane(vesselState.surfaceVelocity, vesselState.up);
                        Vector3 dir   = -hsdir + vesselState.up * Math.Max(Math.Abs(spd), 20 * mainBody.GeeASL);
                        if ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) > 5000) && (hsdir.magnitude > Math.Max(Math.Abs(spd), 100 * mainBody.GeeASL) * 2))
                        {
                            tmode         = TMode.DIRECT;
                            trans_spd_act = 100;
                            rot           = -hsdir;
                        }
                        else
                        {
                            rot = dir.normalized;
                        }
                        core.attitude.attitudeTo(rot, AttitudeReference.INERTIAL, null);
                    }
                    break;
                }

                double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel;
                if ((tmode == TMode.KEEP_ORBITAL && Vector3d.Dot(vesselState.forward, vesselState.orbitalVelocity) < 0) ||
                    (tmode == TMode.KEEP_SURFACE && Vector3d.Dot(vesselState.forward, vesselState.surfaceVelocity) < 0))
                {
                    //allow thrust to declerate
                    t_err *= -1;
                }

                double t_act = pid.Compute(t_err);

                if ((tmode != TMode.KEEP_VERTICAL) ||
                    !trans_kill_h ||
                    (core.attitude.attitudeError < 2) ||
                    ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (core.attitude.attitudeError < 90)))
                {
                    if (tmode == TMode.DIRECT)
                    {
                        trans_prev_thrust = targetThrottle = trans_spd_act / 100.0F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = Mathf.Clamp01(trans_prev_thrust + (float)t_act);
                    }
                }
                else
                {
                    bool useGimbal = (vesselState.torqueGimbal.positive.x > vesselState.torqueAvailable.x * 10) ||
                                     (vesselState.torqueGimbal.positive.z > vesselState.torqueAvailable.z * 10);

                    bool useDiffThrottle = (vesselState.torqueDiffThrottle.x > vesselState.torqueAvailable.x * 10) ||
                                           (vesselState.torqueDiffThrottle.z > vesselState.torqueAvailable.z * 10);

                    if ((core.attitude.attitudeError >= 2) && (useGimbal || (useDiffThrottle && core.thrust.differentialThrottle)))
                    {
                        trans_prev_thrust = targetThrottle = 0.1F;
                        print(" targetThrottle = 0.1F");
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = 0;
                    }
                }
            }

            // Only set throttle if a module need it. Otherwise let the user or other mods set it
            // There is always at least 1 user : the module itself (why ?)
            if (users.Count > 1)
            {
                s.mainThrottle = targetThrottle;
            }

            throttleLimit      = 1;
            throttleFixedLimit = 1;

            limiter = LimitMode.None;

            if (limitThrottle)
            {
                if (maxThrottle < throttleLimit)
                {
                    setFixedLimit((float)maxThrottle, LimitMode.Throttle);
                }
            }

            if (limitToTerminalVelocity)
            {
                float limit = TerminalVelocityThrottle();
                if (limit < throttleLimit)
                {
                    setFixedLimit(limit, LimitMode.TerminalVelocity);
                }
            }

            if (limitDynamicPressure)
            {
                float limit = MaximumDynamicPressureThrottle();
                if (limit < throttleLimit)
                {
                    setFixedLimit(limit, LimitMode.DynamicPressure);
                }
            }

            if (limitToPreventOverheats)
            {
                float limit = (float)TemperatureSafetyThrottle();
                if (limit < throttleLimit)
                {
                    setFixedLimit(limit, LimitMode.Temperature);
                }
            }

            if (limitAcceleration)
            {
                float limit = AccelerationLimitedThrottle();
                if (limit < throttleLimit)
                {
                    setFixedLimit(limit, LimitMode.Acceleration);
                }
            }

            if (electricThrottle && ElectricEngineRunning())
            {
                float limit = ElectricThrottle();
                if (limit < throttleLimit)
                {
                    setFixedLimit(limit, LimitMode.Electric);
                }
            }

            if (limitToPreventFlameout)
            {
                // This clause benefits being last: if we don't need much air
                // due to prior limits, we can close some intakes.
                float limit = FlameoutSafetyThrottle();
                if (limit < throttleLimit)
                {
                    setFixedLimit(limit, LimitMode.Flameout);
                }
            }

            // Any limiters which can limit to non-zero values must come before this, any
            // limiters (like ullage) which enforce zero throttle should come after.  The
            // minThrottle setting has authority over any other limiter that sets non-zero throttle.
            if (limiterMinThrottle && limiter != LimitMode.None)
            {
                if (minThrottle > throttleFixedLimit)
                {
                    setFixedLimit((float)minThrottle, LimitMode.MinThrottle);
                }
                if (minThrottle > throttleLimit)
                {
                    setTempLimit((float)minThrottle, LimitMode.MinThrottle);
                }
            }

            /* auto-RCS ullaging up to very stable */
            if (autoRCSUllaging && s.mainThrottle > 0.0F && throttleLimit > 0.0F)
            {
                if (vesselState.lowestUllage < VesselState.UllageState.VeryStable)
                {
                    Debug.Log("MechJeb RCS auto-ullaging: found state below very stable: " + vesselState.lowestUllage);
                    if (vessel.hasEnabledRCSModules())
                    {
                        if (!vessel.ActionGroups[KSPActionGroup.RCS])
                        {
                            Debug.Log("MechJeb RCS auto-ullaging: enabling RCS action group for automatic ullaging");
                            vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
                        }
                        Debug.Log("MechJeb RCS auto-ullaging: firing RCS to stabilize ulllage");
                        setTempLimit(0.0F, LimitMode.UnstableIgnition);
                        s.Z = -1.0F;
                    }
                    else
                    {
                        Debug.Log("MechJeb RCS auto-ullaging: vessel has no enabled/staged RCS modules");
                    }
                }
            }

            /* prevent unstable ignitions */
            if (limitToPreventUnstableIgnition && s.mainThrottle > 0.0F && throttleLimit > 0.0F)
            {
                if (vesselState.lowestUllage < VesselState.UllageState.Stable)
                {
                    ScreenMessages.PostScreenMessage(preventingUnstableIgnitionsMessage);
                    Debug.Log("MechJeb Unstable Ignitions: preventing ignition in state: " + vesselState.lowestUllage);
                    setTempLimit(0.0F, LimitMode.UnstableIgnition);
                }
            }

            // we have to force the throttle here so that rssMode can work, otherwise we don't get our last throttle command
            // back on the next tick after disabling.  we save this before applying the throttle limits so that we preserve
            // the requested throttle, and not the limited throttle.
            if (core.rssMode)
            {
                SetFlightGlobals(s.mainThrottle);
            }

            if (double.IsNaN(throttleLimit))
            {
                throttleLimit = 1.0F;
            }
            throttleLimit = Mathf.Clamp01(throttleLimit);

            /* we do not _apply_ the "fixed" limit, the actual throttleLimit should always be the more limited and lower one */
            /* the purpose of the "fixed" limit is for external consumers like the node executor to consume */
            if (double.IsNaN(throttleFixedLimit))
            {
                throttleFixedLimit = 1.0F;
            }
            throttleFixedLimit = Mathf.Clamp01(throttleFixedLimit);

            vesselState.throttleLimit      = throttleLimit;
            vesselState.throttleFixedLimit = throttleFixedLimit;

            if (s.mainThrottle < throttleLimit)
            {
                limiter = LimitMode.None;
            }

            s.mainThrottle = Mathf.Min(s.mainThrottle, throttleLimit);

            if (smoothThrottle)
            {
                s.mainThrottle = SmoothThrottle(s.mainThrottle);
            }

            if (double.IsNaN(s.mainThrottle))
            {
                s.mainThrottle = 0;
            }

            s.mainThrottle = Mathf.Clamp01(s.mainThrottle);


            if (s.Z == 0 && core.rcs.rcsThrottle && vesselState.rcsThrust)
            {
                s.Z = -s.mainThrottle;
            }

            lastThrottle = s.mainThrottle;

            if (!core.attitude.enabled)
            {
                Vector3d act = new Vector3d(s.pitch, s.yaw, s.roll);
                differentialThrottleDemandedTorque = -Vector3d.Scale(act.xzy, vesselState.torqueDiffThrottle * s.mainThrottle * 0.5f);
            }
        }
示例#7
0
        public override void Drive(FlightCtrlState s)
        {
            //detect user input:
            if (s.mainThrottle < 1e-4 && lastThrottle > 1e-4)
            {
                targetThrottle = 0; //detect player pressing 'x'
            }
            else if (Mathf.Abs(s.mainThrottle - lastThrottle) > 1e-4)
            {
                targetThrottle = Mathf.Clamp01((s.mainThrottle - lastThrottle) + targetThrottle);
            }

            if ((tmode != TMode.OFF) && (vesselState.thrustAvailable > 0))
            {
                double spd = 0;

                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, vesselState.velocityVesselSurface);
                        Vector3 dir   = -hsdir + vesselState.up * Math.Max(Math.Abs(spd), 20 * mainBody.GeeASL);
                        if ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) > 5000) && (hsdir.magnitude > Math.Max(Math.Abs(spd), 100 * mainBody.GeeASL) * 2))
                        {
                            tmode         = TMode.DIRECT;
                            trans_spd_act = 100;
                            rot           = -hsdir;
                        }
                        else
                        {
                            rot = dir.normalized;
                        }
                        core.attitude.attitudeTo(rot, AttitudeReference.INERTIAL, null);
                    }
                    break;
                }

                double t_err = (trans_spd_act - spd) / vesselState.maxThrustAccel;
                if ((tmode == TMode.KEEP_ORBITAL && Vector3d.Dot(vesselState.forward, vesselState.velocityVesselOrbit) < 0) ||
                    (tmode == TMode.KEEP_SURFACE && Vector3d.Dot(vesselState.forward, vesselState.velocityVesselSurface) < 0))
                {
                    //allow thrust to declerate
                    t_err *= -1;
                }

                double t_act = pid.Compute(t_err);

                if ((tmode != TMode.KEEP_VERTICAL) ||
                    !trans_kill_h ||
                    (core.attitude.attitudeError < 2) ||
                    ((Math.Min(vesselState.altitudeASL, vesselState.altitudeTrue) < 1000) && (core.attitude.attitudeError < 90)))
                {
                    if (tmode == TMode.DIRECT)
                    {
                        trans_prev_thrust = targetThrottle = trans_spd_act / 100.0F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = Mathf.Clamp01(trans_prev_thrust + (float)t_act);
                    }
                }
                else
                {
                    if ((core.attitude.attitudeError >= 2) && (vesselState.torqueThrustPYAvailable > vesselState.torquePYAvailable * 10))
                    {
                        trans_prev_thrust = targetThrottle = 0.1F;
                    }
                    else
                    {
                        trans_prev_thrust = targetThrottle = 0;
                    }
                }
            }

            s.mainThrottle = targetThrottle;

            float throttleLimit = 1;

            limiter = LimitMode.None;

            if (limitThrottle)
            {
                if (maxThrottle < throttleLimit)
                {
                    limiter = LimitMode.Throttle;
                }
                throttleLimit = Mathf.Min(throttleLimit, (float)maxThrottle);
            }

            if (limitToTerminalVelocity)
            {
                float limit = TerminalVelocityThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.TerminalVelocity;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitToPreventOverheats)
            {
                float limit = TemperatureSafetyThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Temperature;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitAcceleration)
            {
                float limit = AccelerationLimitedThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Acceleration;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (limitToPreventFlameout)
            {
                // This clause benefits being last: if we don't need much air
                // due to prior limits, we can close some intakes.
                float limit = FlameoutSafetyThrottle();
                if (limit < throttleLimit)
                {
                    limiter = LimitMode.Flameout;
                }
                throttleLimit = Mathf.Min(throttleLimit, limit);
            }

            if (double.IsNaN(throttleLimit))
            {
                throttleLimit = 0;
            }
            throttleLimit = Mathf.Clamp01(throttleLimit);

            vesselState.throttleLimit = throttleLimit;

            if (s.mainThrottle < throttleLimit)
            {
                limiter = LimitMode.None;
            }

            s.mainThrottle = Mathf.Min(s.mainThrottle, throttleLimit);

            if (smoothThrottle)
            {
                s.mainThrottle = SmoothThrottle(s.mainThrottle);
            }

            if (double.IsNaN(s.mainThrottle))
            {
                s.mainThrottle = 0;
            }
            s.mainThrottle = Mathf.Clamp01(s.mainThrottle);

            lastThrottle = s.mainThrottle;
        }
示例#8
0
        public override void Drive(FlightCtrlState s)
        {
            if (!core.target.NormalTargetExists)
            {
                users.Clear();
                return;
            }

            core.attitude.attitudeTo(Vector3d.back, AttitudeReference.TARGET_ORIENTATION, this);

            Vector3d targetVel = core.target.Orbit.GetVel();

            Vector3d separation = core.target.RelativePosition;

            Vector3d zAxis      = core.target.DockingAxis;
            double   zSep       = -Vector3d.Dot(separation, zAxis); //positive if we are in front of the target, negative if behind
            Vector3d lateralSep = Vector3d.Exclude(zAxis, separation);

            double zApproachSpeed   = FixSpeed(vesselState.rcsThrustAvailable.GetMagnitude(-zAxis) * approachSpeedMult / vesselState.mass);
            double latApproachSpeed = FixSpeed(vesselState.rcsThrustAvailable.GetMagnitude(-lateralSep) * approachSpeedMult / vesselState.mass);

            if (zSep < 0)                                                                                //we're behind the target
            {
                if (lateralSep.magnitude < 10)                                                           //and we'll hit the target if we back up
                {
                    core.rcs.SetTargetWorldVelocity(targetVel + zApproachSpeed * lateralSep.normalized); //move away from the docking axis
                    status = "Moving away from docking axis at " + zApproachSpeed.ToString("F2") + " m/s to avoid hitting target on backing up";
                }
                else
                {
                    double backUpSpeed = FixSpeed(-zApproachSpeed * Math.Max(1, -zSep / 50));
                    core.rcs.SetTargetWorldVelocity(targetVel + backUpSpeed * zAxis); //back up
                    status = "Backing up at " + backUpSpeed.ToString("F2") + " m/s to get on the correct side of the target to dock.";
                }
                lateralPID.Reset();
            }
            else //we're in front of the target
            {
                //move laterally toward the docking axis
                lateralPID.max = latApproachSpeed * lateralSep.magnitude / 200;
                lateralPID.min = -lateralPID.max;
                Vector3d lateralVelocityNeeded = -lateralSep.normalized * lateralPID.Compute(lateralSep.magnitude);
                if (lateralVelocityNeeded.magnitude > latApproachSpeed)
                {
                    lateralVelocityNeeded *= (latApproachSpeed / lateralVelocityNeeded.magnitude);
                }

                double zVelocityNeeded = 0.1 + Math.Min(zApproachSpeed, zApproachSpeed * zSep / 200);

                if (lateralSep.magnitude > 0.2 && lateralSep.magnitude * 10 > zSep)
                {
                    //we're very far off the docking axis
                    if (zSep < lateralSep.magnitude)
                    {
                        //we're far off the docking axis, but our z separation is small. Back up to increase the z separation
                        zVelocityNeeded *= -1;
                        status           = "Backing at " + zVelocityNeeded.ToString("F2") + " m/s up and moving toward docking axis.";
                    }
                    else
                    {
                        //we're not extremely close in z, so just stay at this z distance while we fix the lateral separation
                        zVelocityNeeded = 0;
                        status          = "Holding still in Z and moving toward the docking axis at " + lateralVelocityNeeded.magnitude.ToString("F2") + " m/s.";
                    }
                }
                else
                {
                    if (zSep > 0.4)
                    {
                        //we're not extremely far off the docking axis. Approach the along z with a speed determined by our z separation
                        //but limited by how far we are off the axis
                        status = "Moving forward to dock at " + zVelocityNeeded.ToString("F2") + " m/s.";
                    }
                    else
                    {
                        // close enough, turn it off and let the magnetic dock work
                        users.Clear();
                        return;
                    }
                }

                Vector3d adjustment = lateralVelocityNeeded + zVelocityNeeded * zAxis;
                double   magnitude  = adjustment.magnitude;
                if (magnitude > 0)
                {
                    adjustment *= FixSpeed(magnitude) / magnitude;
                }
                core.rcs.SetTargetWorldVelocity(targetVel + adjustment);
            }
        }
        public override void Drive(FlightCtrlState s)
        {
            UpdatePID();

            //SpeedHold (set AccelerationTarget automatically to hold speed)
            if (SpeedHoldEnabled)
            {
                double spd = vesselState.speedSurface;
                cur_acc = (spd - _spd) / Time.fixedDeltaTime;
                _spd    = spd;
                RealAccelerationTarget = (SpeedTarget - spd) / 4;
                a_err = (RealAccelerationTarget - cur_acc);
                AccelerationPIDController.intAccum = MuUtils.Clamp(AccelerationPIDController.intAccum, -1 / AccKi, 1 / AccKi);
                double t_act = AccelerationPIDController.Compute(a_err);
                if (!double.IsNaN(t_act))
                {
                    core.thrust.targetThrottle = (float)MuUtils.Clamp(t_act, 0, 1);
                }
                else
                {
                    core.thrust.targetThrottle = 0.0f;
                    AccelerationPIDController.Reset();
                }
            }

            //AltitudeHold (set VertSpeed automatically to hold altitude)
            if (AltitudeHoldEnabled)
            {
                RealVertSpeedTarget = MuUtils.Clamp(fixVertSpeed(AltitudeTarget - vesselState.altitudeASL), -VertSpeedMax, VertSpeedMax);
            }
            else
            {
                RealVertSpeedTarget = VertSpeedTarget;
            }

            //VertSpeedHold
            if (VertSpeedHoldEnabled)
            {
                double vertspd = vesselState.speedVertical;
                v_err = RealVertSpeedTarget - vertspd;
                VertSpeedPIDController.intAccum = MuUtils.Clamp(VertSpeedPIDController.intAccum, -1 / (VerKi * VerPIDScale), 1 / (VerKi * VerPIDScale));
                double p_act = VertSpeedPIDController.Compute(v_err);
                //Log.dbg(p_act);
                if (double.IsNaN(p_act))
                {
                    VertSpeedPIDController.Reset();
                }
                else
                {
                    s.pitch = Mathf.Clamp((float)p_act, -1, 1);
                }
            }

            //HeadingHold
            double curHeading = vesselState.HeadingFromDirection(vesselState.forward);

            if (HeadingHoldEnabled)
            {
                double toturn = MuUtils.ClampDegrees180(HeadingTarget - curHeading);
                RealRollTarget = MuUtils.Clamp(toturn * 2, -RollMax, RollMax);
            }
            else
            {
                RealRollTarget = RollTarget;
            }

            if (RollHoldEnabled)
            {
                double roll_err = MuUtils.ClampDegrees180(vesselState.vesselRoll + RealRollTarget);
                RollPIDController.intAccum = MuUtils.Clamp(RollPIDController.intAccum, -100 / AccKi, 100 / AccKi);
                double roll_act = RollPIDController.Compute(roll_err);
                s.roll = Mathf.Clamp((float)roll_act / 100, -1, 1);
            }

            if (HeadingHoldEnabled)
            {
                double yaw_err = MuUtils.ClampDegrees180(HeadingTarget - curHeading);
                YawPIDController.intAccum = MuUtils.Clamp(YawPIDController.intAccum, -YawLimit * 100 / AccKi, YawLimit * 100 / AccKi);
                double yaw_act = YawPIDController.Compute(yaw_err);
                s.yaw = (float)MuUtils.Clamp(yaw_act / 100, -YawLimit, +YawLimit);
            }
        }
        public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
        {                                             // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up -- should be done now?
            if (orbit.referenceBody != lastBody)
            {
                WaypointIndex = -1; Waypoints.Clear();
            }
            MechJebWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);

            var brake = vessel.ActionGroups[KSPActionGroup.Brakes];             // keep brakes locked if they are

            curSpeed = Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);

            CalculateTraction();
            speedIntAcc = speedPID.intAccum;

            if (wp != null && wp.Body == orbit.referenceBody)
            {
                if (ControlHeading)
                {
                    heading.val = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
                }
                if (ControlSpeed)
                {
                    var nextWP   = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
                    var distance = Vector3.Distance(vessel.CoM, wp.Position);
                    if (wp.Target != null)
                    {
                        distance += (float)(wp.Target.srfSpeed * curSpeed) / 2;
                    }
                    // var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
                    var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed);                     // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
                    var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
                                    (nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
                                     (distance - wp.Radius > 50 ? turnSpeed.val : 1)));
                    minSpeed = (wp.Quicksave ? 1 : minSpeed);
                    // ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
                    var newSpeed = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / curSpeed, minSpeed));              // brake when getting closer
                    newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed);                     // reduce speed when turning a lot
                    var radius = Math.Max(wp.Radius, 10);
                    if (distance < radius)
                    {
                        if (WaypointIndex + 1 >= Waypoints.Count)                         // last waypoint
                        {
                            newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
                            // ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
                            if (LoopWaypoints)
                            {
                                WaypointIndex = 0;
                            }
                            else
                            {
                                newSpeed = 0;
                                brake    = true;
                                if (curSpeed < brakeSpeedLimit)
                                {
                                    if (wp.Quicksave)
                                    {
                                        if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
                                        {
                                            WaypointIndex  = -1;
                                            ControlHeading = ControlSpeed = false;
                                            QuickSaveLoad.QuickSave();
                                        }
                                    }
                                    else
                                    {
                                        WaypointIndex  = -1;
                                        ControlHeading = ControlSpeed = false;
                                    }
                                }
                            }
                        }
                        else
                        {
                            if (wp.Quicksave)
                            {
                                newSpeed = 0;
                                if (curSpeed < brakeSpeedLimit)
                                {
                                    if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
                                    {
                                        WaypointIndex++;
                                        QuickSaveLoad.QuickSave();
                                    }
                                }
                            }
                            else
                            {
                                WaypointIndex++;
                            }
                        }
                    }
                    brake = brake || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < brakeSpeedLimit && newSpeed < brakeSpeedLimit);
                    // ^ brake if needed to prevent rolling, hopefully
                    tgtSpeed = (newSpeed >= 0 ? newSpeed : 0);
                }
            }

            if (ControlHeading)
            {
                headingPID.intAccum = Mathf.Clamp((float)headingPID.intAccum, -1, 1);

                double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
                headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
                if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
                {
                    float limit = (Math.Abs(curSpeed) > turnSpeed ? Mathf.Clamp((float)((turnSpeed + 6) / Square(curSpeed)), 0.1f, 1f) : 1f);
                    // turnSpeed needs to be higher than curSpeed or it will never steer as much as it could even at 0.2m/s above it
                    double act = headingPID.Compute(headingErr);
                    if (traction >= tractionLimit)
                    {
                        s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
                        // prevents it from flying above a waypoint and landing with steering at max while still going fast
                    }
                }
            }

            // Brake if there is no controler (Pilot eject from seat)
            if (BrakeOnEject && vessel.GetReferenceTransformPart() == null)
            {
                s.wheelThrottle = 0;
                brake           = true;
            }
            else if (ControlSpeed)
            {
                speedPID.intAccum = Mathf.Clamp((float)speedPID.intAccum, -5, 5);

                speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed) - Vector3d.Dot(vesselState.surfaceVelocity, vesselState.forward);
                if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
                {
                    float act = (float)speedPID.Compute(speedErr);
                    s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
                    if (curSpeed < 0 & s.wheelThrottle < 0)
                    {
                        s.wheelThrottle = 0;
                    }                                                                                    // don't go backwards
                    if (Mathf.Sign(act) + Mathf.Sign(s.wheelThrottle) == 0)
                    {
                        s.wheelThrottle = Mathf.Clamp(act, -1f, 1f);
                    }
                    if (speedErr < -1 && StabilityControl && Mathf.Sign(s.wheelThrottle) + Math.Sign(curSpeed) == 0)
                    {
                        brake = true;
                    }
                    lastThrottle = Mathf.Clamp(s.wheelThrottle, -1, 1);
                }
            }

            if (StabilityControl)
            {
                if (!core.attitude.users.Contains(this))
                {
                    core.attitude.users.Add(this);
                }
                var     fSpeed = (float)curSpeed;
                Vector3 fwd    = (Vector3)(traction > 0 ?                                                                         // V when the speed is low go for the vessels forward, else with a bit of velocity
                                           vesselState.forward * 4 - vessel.transform.right * s.wheelSteer * Mathf.Sign(fSpeed) : // and then add the steering
                                           vesselState.surfaceVelocity);                                                          // in the air so follow velocity
                Vector3.OrthoNormalize(ref norm, ref fwd);
                var quat = Quaternion.LookRotation(fwd, norm);

                if (vesselState.torqueAvailable.sqrMagnitude > 0)
                {
                    core.attitude.attitudeTo(quat, AttitudeReference.INERTIAL, this);
                }
            }

            if (BrakeOnEnergyDepletion)
            {
                var batteries  = vessel.Parts.FindAll(p => p.Resources.Contains(PartResourceLibrary.ElectricityHashcode) && p.Resources.Get(PartResourceLibrary.ElectricityHashcode).flowState);
                var energyLeft = batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).amount) / batteries.Sum(p => p.Resources.Get(PartResourceLibrary.ElectricityHashcode).maxAmount);
                var openSolars = vessel.mainBody.atmosphere &&                 // true if in atmosphere and there are breakable solarpanels that aren't broken nor retracted
                                 vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable && p.deployState != ModuleDeployablePart.DeployState.BROKEN &&
                                                                                                           p.deployState != ModuleDeployablePart.DeployState.RETRACTED).Count > 0;

                if (openSolars && energyLeft > 0.99)
                {
                    vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.isBreakable &&
                                                                                              p.deployState == ModuleDeployablePart.DeployState.EXTENDED).ForEach(p => p.Retract());
                }

                if (energyLeft < 0.05 && Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0)
                {
                    s.wheelThrottle = 0;
                }                                                                                                                        // save remaining energy by not using it for acceleration
                if (openSolars || energyLeft < 0.03)
                {
                    tgtSpeed = 0;
                }

                if (curSpeed < brakeSpeedLimit && (energyLeft < 0.05 || openSolars))
                {
                    brake = true;
                }

                if (curSpeed < 0.1 && energyLeft < 0.05 && !waitingForDaylight &&
                    vessel.FindPartModulesImplementing <ModuleDeployableSolarPanel>().FindAll(p => p.deployState == ModuleDeployablePart.DeployState.EXTENDED).Count > 0)
                {
                    waitingForDaylight = true;
                }
            }

            if (s.wheelThrottle != 0 && (Math.Sign(s.wheelThrottle) + Math.Sign(curSpeed) != 0 || curSpeed < 1))
            {
                brake = false;                 // the AP or user want to drive into the direction of momentum so release the brake
            }

            if (vessel.isActiveVessel)
            {
                if (GameSettings.BRAKES.GetKeyUp())
                {
                    brake = false;                     // release the brakes if the user lets go of them
                }
                if (GameSettings.BRAKES.GetKey())
                {
                    brake = true;                     // brake if the user brakes and we aren't about to flip
                }
            }

            tractionLimit = (double)Mathf.Clamp((float)tractionLimit, 0, 100);
            vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, brake && (StabilityControl && (ControlHeading || ControlSpeed) ? traction >= tractionLimit : true));
            // only let go of the brake when losing traction if the AP is driving, otherwise assume the player knows when to let go of it
            // also to not constantly turn off the parking brake from going over a small bump
            if (brake && curSpeed < 0.1)
            {
                s.wheelThrottle = 0;
            }
        }
示例#11
0
        public override void Drive(FlightCtrlState s) // TODO put the brake in when running out of power to prevent nighttime solar failures on hills, or atleast try to
        {                                             // TODO make distance calculation for 'reached' determination consider the rover and waypoint on sealevel to prevent height differences from messing it up
            if (orbit.referenceBody != lastBody)
            {
                WaypointIndex = -1; Waypoints.Clear();
            }
            MechJebRoverWaypoint wp = (WaypointIndex > -1 && WaypointIndex < Waypoints.Count ? Waypoints[WaypointIndex] : null);

            var curSpeed = vesselState.speedSurface;

            etaSpeed.value = curSpeed;

            if (wp != null && wp.Body == orbit.referenceBody)
            {
                if (controlHeading)
                {
                    heading = Math.Round(HeadingToPos(vessel.CoM, wp.Position), 1);
                }
                if (controlSpeed)
                {
                    var nextWP   = (WaypointIndex < Waypoints.Count - 1 ? Waypoints[WaypointIndex + 1] : (LoopWaypoints ? Waypoints[0] : null));
                    var distance = Vector3.Distance(vessel.CoM, wp.Position);
                    //var maxSpeed = (wp.MaxSpeed > 0 ? Math.Min((float)speed, wp.MaxSpeed) : speed); // use waypoints maxSpeed if set and smaller than set the speed or just stick with the set speed
                    var maxSpeed = (wp.MaxSpeed > 0 ? wp.MaxSpeed : speed);                     // speed used to go towards the waypoint, using the waypoints maxSpeed if set or just stick with the set speed
                    var minSpeed = (wp.MinSpeed > 0 ? wp.MinSpeed :
                                    (nextWP != null ? TurningSpeed((nextWP.MaxSpeed > 0 ? nextWP.MaxSpeed : speed), heading - HeadingToPos(wp.Position, nextWP.Position)) :
                                     (distance - wp.Radius > 50 ? turnSpeed.val : 1)));
                    minSpeed = (wp.Quicksave ? 0 : minSpeed);
                    // ^ speed used to go through the waypoint, using half the set speed or maxSpeed as minSpeed for routing waypoints (all except the last)
                    var brakeFactor = Math.Max((curSpeed - minSpeed) * 1, 3);
                    var newSpeed    = Math.Min(maxSpeed, Math.Max((distance - wp.Radius) / brakeFactor, minSpeed)); // brake when getting closer
                    newSpeed = (newSpeed > turnSpeed ? TurningSpeed(newSpeed, headingErr) : newSpeed);              // reduce speed when turning a lot
                    var radius = Math.Max(wp.Radius, 10 / 0.8);                                                     // alternative radius so negative radii can still make it go full speed through waypoints for navigation reasons
                    if (distance < radius)
                    {
                        if (WaypointIndex + 1 >= Waypoints.Count)                           // last waypoint
                        {
                            newSpeed = new [] { newSpeed, (distance < radius * 0.8 ? 0 : 1) }.Min();
                            // ^ limit speed so it'll only go from 1m/s to full stop when braking to prevent accidents on moons
                            if (LoopWaypoints)
                            {
                                WaypointIndex = 0;
                            }
                            else
                            {
                                newSpeed = -0.25;
                                tgtSpeed.force(newSpeed);
                                if (curSpeed < 0.85)
                                {
                                    if (wp.Quicksave)
                                    {
                                        if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
                                        {
                                            WaypointIndex  = -1;
                                            controlHeading = controlSpeed = false;
                                            QuickSaveLoad.QuickSave();
                                        }
                                    }
                                    else
                                    {
                                        WaypointIndex  = -1;
                                        controlHeading = controlSpeed = false;
                                    }
                                }
//								else {
//									Debug.Log("Is this even getting called?");
//									WaypointIndex++;
//								}
                            }
                        }
                        else
                        {
                            if (wp.Quicksave)
                            {
                                newSpeed = -0.25;
                                tgtSpeed.force(newSpeed);
                                if (curSpeed < 0.85)
                                {
                                    if (FlightGlobals.ClearToSave() == ClearToSaveStatus.CLEAR)
                                    {
                                        WaypointIndex++;
                                        QuickSaveLoad.QuickSave();
                                    }
                                }
                            }
                            else
                            {
                                WaypointIndex++;
                            }
                        }
                    }
                    vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, (GameSettings.BRAKES.GetKey() && vessel.isActiveVessel) || ((s.wheelThrottle == 0 || !vessel.isActiveVessel) && curSpeed < 0.85 && newSpeed < 0.85));
                    // ^ brake if needed to prevent rolling, hopefully
                    tgtSpeed.value = Math.Round(newSpeed, 1);
                }
            }

            if (controlHeading)
            {
                if (heading != headingLast)
                {
                    headingPID.Reset();
                    headingLast = heading;
                }

                double instantaneousHeading = vesselState.rotationVesselSurface.eulerAngles.y;
                headingErr = MuUtils.ClampDegrees180(instantaneousHeading - heading);
                if (s.wheelSteer == s.wheelSteerTrim || FlightGlobals.ActiveVessel != vessel)
                {
                    float  spd   = Mathf.Min((float)speed, (float)turnSpeed);                  // if a slower speed than the turnspeed is used also be more careful with the steering
                    float  limit = (curSpeed <= turnSpeed ? 1 : Mathf.Clamp((float)((spd * spd) / (curSpeed * curSpeed)), 0.2f, 1f));
                    double act   = headingPID.Compute(headingErr);
                    s.wheelSteer = Mathf.Clamp((float)act, -limit, limit);
                }
            }
            // Brake if there is no controler (Pilot eject from seat)
            if (brakeOnEject && vessel.GetReferenceTransformPart() == null)
            {
                s.wheelThrottle = 0;
                vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
            }
            else if (controlSpeed)
            {
                if (speed != speedLast)
                {
                    speedPID.Reset();
                    speedLast = speed;
                }

                speedErr = (WaypointIndex == -1 ? speed.val : tgtSpeed.value) - Vector3d.Dot(vesselState.velocityVesselSurface, vesselState.forward);
                if (s.wheelThrottle == s.wheelThrottleTrim || FlightGlobals.ActiveVessel != vessel)
                {
                    double act = speedPID.Compute(speedErr);
                    s.wheelThrottle = Mathf.Clamp((float)act, -1, 1);
                }
            }
        }