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