/// <summary>
/// If the mission is on a client controlled mission, we disable every incoming input
/// </summary>
/// <param name="s">S.</param>
private void onFlyByWire(FlightCtrlState s)
{
Status status = calculateStatus(currentMission, false, activeVessel);
if(status.isClientControlled) {
s.fastThrottle = 0;
s.gearDown = false;
s.gearUp = false;
s.headlight = false;
s.killRot = false;
s.mainThrottle = 0;
s.pitch = 0;
s.pitchTrim = 0;
s.roll = 0;
s.rollTrim = 0;
s.X = 0;
s.Y = 0;
s.yaw = 0;
s.yawTrim = 0;
s.Z = 0;
s.wheelSteer = 0;
s.wheelSteerTrim = 0;
s.wheelThrottle = 0;
s.wheelThrottleTrim = 0;
s.NeutralizeAll ();
}
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (!core.landing.PredictionReady)
return this;
Vector3d deltaV;
try
{
deltaV = core.landing.ComputeCourseCorrection(true);
}
catch(ArgumentException e)
{
status = e.Message;
return new DecelerationBurn(core);
}
if (core.landing.rcsAdjustment)
{
if (deltaV.magnitude > 3)
core.rcs.enabled = true;
else if (deltaV.magnitude < 0.01)
core.rcs.enabled = false;
if (core.rcs.enabled)
core.rcs.SetWorldVelocityError(deltaV);
}
return this;
}
protected override void _Drive(FlightCtrlState s)
{
Vessel vessel = base.Autopilot.vessel;
AttitudeController attitudeController = base.Autopilot.AutopilotController.AttitudeController;
if (vessel.altitude < 10000) {
attitudeController.Attitude = Attitude.Up;
base.Autopilot.AutopilotController.AutoStagingController.Enable ();
s.mainThrottle = 1.0f;
} else if (vessel.altitude < 80000) {
attitudeController.Pitch = 90;
attitudeController.Yaw = 45;
attitudeController.Attitude = Attitude.UserDefined;
s.mainThrottle = Mathf.Clamp01 (100000.0f - (float)vessel.orbit.ApA);
} else {
double ut = Planetarium.GetUniversalTime () + vessel.orbit.timeToAp;
double deltaV = Math.Sqrt (vessel.mainBody.gravParameter / (100000 + vessel.mainBody.Radius)) - vessel.orbit.getOrbitalVelocityAtUT (ut).magnitude;
Vector3d nodeDV = new Vector3d (0, 0, deltaV);
base.Autopilot.ManeuverNode = vessel.patchedConicSolver.AddManeuverNode (ut);
base.Autopilot.ManeuverNode.OnGizmoUpdated (nodeDV, ut);
base.Autopilot.AutopilotController.Disable ();
base.Autopilot.AutopilotController.AutoStagingController.Disable ();
attitudeController.Attitude = Attitude.None;
}
}
public void OnFlyByWire(FlightCtrlState c)
{
foreach (LockableControl control in controls)
{
control.OnFlyByWire(ref c);
}
}
public void DriveAutoland(FlightCtrlState s)
{
if (!part.vessel.Landed)
{
Vector3d runwayStart = RunwayStart();
if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0)
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Gear, true);
loweredGear = true;
}
Vector3d vectorToWaypoint = ILSAimDirection();
double headingToWaypoint = vesselState.HeadingFromDirection(vectorToWaypoint);
Vector3d vectorToRunway = runwayStart - vesselState.CoM;
double verticalDistanceToRunway = Vector3d.Dot(vectorToRunway, vesselState.up) + (vesselState.altitudeTrue - vesselState.altitudeBottom);
double horizontalDistanceToRunway = Math.Sqrt(vectorToRunway.sqrMagnitude - verticalDistanceToRunway * verticalDistanceToRunway);
double flightPathAngleToRunway = 180 / Math.PI * Math.Atan2(verticalDistanceToRunway, horizontalDistanceToRunway);
double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F);
AimVelocityVector(desiredFPA, headingToWaypoint);
}
else
{
//keep the plane aligned with the runway:
Vector3d runwayDir = runway.End(vesselState.CoM) - runway.Start(vesselState.CoM);
if (Vector3d.Dot(runwayDir, vesselState.forward) < 0) runwayDir *= -1;
double runwayHeading = 180 / Math.PI * Math.Atan2(Vector3d.Dot(runwayDir, vesselState.east), Vector3d.Dot(runwayDir, vesselState.north));
core.attitude.attitudeTo(runwayHeading, 0, 0, this);
}
}
public void OnFlyByWire(FlightCtrlState c)
{
foreach (var control in controls)
{
control.OnFlyByWire(ref c);
}
}
public override void drive(FlightCtrlState s)
{
if (autoLand)
{
if (!part.vessel.Landed)
{
if (!loweredGear && (vesselState.CoM - runwayStart).magnitude < 1000.0)
{
part.vessel.rootPart.SendEvent("LowerLandingGear");
loweredGear = true;
}
Vector3d vectorToWaypoint = waypoint.transform.position - vesselState.CoM;
double headingToWaypoint = 180 / Math.PI * Math.Atan2(Vector3d.Dot(vectorToWaypoint, vesselState.east), Vector3d.Dot(vectorToWaypoint, vesselState.north));
double desiredFPA = Mathf.Clamp((float)(flightPathAngleToRunway + 3 * (flightPathAngleToRunway + glideslope)), -20.0F, 0.0F);
aimVelocityVector(desiredFPA, headingToWaypoint);
}
else
{
//keep the plane aligned with the runway:
Vector3d target = runwayEnd;
if (Vector3d.Dot(target - vesselState.CoM, vesselState.forward) < 0) target = runwayStart;
core.attitudeTo((target- vesselState.CoM).normalized, MechJebCore.AttitudeReference.INERTIAL, this);
}
}
else if (holdHeadingAndAlt)
{
double targetClimbRate = (targetAltitude - vesselState.altitudeASL) / 30.0;
double targetFlightPathAngle = 180 / Math.PI * Math.Asin(Mathf.Clamp((float)(targetClimbRate / vesselState.speedSurface), (float)Math.Sin(-Math.PI / 9), (float)Math.Sin(Math.PI / 9)));
aimVelocityVector(targetFlightPathAngle, targetHeading);
}
}
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 bool Execute(FlightComputer f, FlightCtrlState fcs)
{
if (mAbort)
{
fcs.mainThrottle = 0.0f;
return true;
}
if (Duration > 0)
{
fcs.mainThrottle = Throttle;
Duration -= TimeWarp.deltaTime;
}
else if (DeltaV > 0)
{
fcs.mainThrottle = Throttle;
DeltaV -= (Throttle * FlightCore.GetTotalThrust(f.Vessel) / f.Vessel.GetTotalMass()) * TimeWarp.deltaTime;
}
else
{
fcs.mainThrottle = 0.0f;
return true;
}
return false;
}
void SetAcceleration(float accel, FlightCtrlState s)
{
float gravAccel = GravAccel();
float requestEngineAccel = accel - gravAccel;
possibleAccel = gravAccel;
float dragAccel = 0;
float engineAccel = MaxEngineAccel(requestEngineAccel, out dragAccel);
if(engineAccel == 0)
{
s.mainThrottle = 0;
return;
}
requestEngineAccel = Mathf.Clamp(requestEngineAccel, -engineAccel, engineAccel);
float requestThrottle = (requestEngineAccel - dragAccel) / engineAccel;
s.mainThrottle = Mathf.Clamp01(requestThrottle);
//use brakes if overspeeding too much
if(requestThrottle < -0.5f)
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
}
else
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
}
}
public void postAutoPilotUpdate(FlightCtrlState state)
{
if (vesselRef.HoldPhysics)
return;
vesselSSAS.SurfaceSAS(state);
vesselAsst.vesselController(state);
}
public void ButtonPressedCallback(IController controller, int button, FlightCtrlState state)
{
if(!HighLogic.LoadedSceneIsFlight)
{
return;
}
var config = m_Configuration.GetConfigurationByIController(controller);
Bitset mask = controller.GetButtonsMask();
if (config.evaluatedDiscreteActionMasks.Contains(mask))
{
return;
}
List<DiscreteAction> actions = config.GetCurrentPreset().GetDiscreteBinding(mask);
if(actions != null)
{
foreach (DiscreteAction action in actions)
{
m_FlightManager.EvaluateDiscreteAction(config, action, state);
config.evaluatedDiscreteActionMasks.Add(mask);
}
}
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (!core.landing.PredictionReady)
return this;
// If the atomospheric drag is at least 100mm/s2 then start trying to target the overshoot using the parachutes
if (core.landing.deployChutes)
{
if (core.landing.ParachutesDeployable())
{
core.landing.ControlParachutes();
}
}
double currentError = Vector3d.Distance(core.target.GetPositionTargetPosition(), core.landing.LandingSite);
if (currentError < 150)
{
core.thrust.targetThrottle = 0;
core.rcs.enabled = core.landing.rcsAdjustment;
return new CoastToDeceleration(core);
}
// If a parachute has already been deployed then we will not be able to control attitude anyway, so move back to the coast to deceleration step.
if (vesselState.parachuteDeployed)
{
core.thrust.targetThrottle = 0;
return new CoastToDeceleration(core);
}
// We are not in .90 anymore. Turning while under drag is a bad idea
if (vesselState.drag > 0.1)
{
return new CoastToDeceleration(core);
}
Vector3d deltaV = core.landing.ComputeCourseCorrection(true);
status = "Performing course correction of about " + deltaV.magnitude.ToString("F1") + " m/s";
core.attitude.attitudeTo(deltaV.normalized, AttitudeReference.INERTIAL, core.landing);
if (core.attitude.attitudeAngleFromTarget() < 2)
courseCorrectionBurning = true;
else if (core.attitude.attitudeAngleFromTarget() > 30)
courseCorrectionBurning = false;
if (courseCorrectionBurning)
{
const double timeConstant = 2.0;
core.thrust.ThrustForDV(deltaV.magnitude, timeConstant);
}
else
{
core.thrust.targetThrottle = 0;
}
return this;
}
public override void Drive(FlightCtrlState s)
{
setPIDParameters();
// Removed the gravity since it also affect the target and we don't know the target pos here.
// Since the difference is negligable for docking it's removed
// TODO : add it back once we use the RCS Controler for other use than docking
Vector3d worldVelocityDelta = vessel.obt_velocity - targetVelocity;
//worldVelocityDelta += TimeWarp.fixedDeltaTime * vesselState.gravityForce; //account for one frame's worth of gravity
//worldVelocityDelta -= TimeWarp.fixedDeltaTime * gravityForce = FlightGlobals.getGeeForceAtPosition( Here be the target position ); ; //account for one frame's worth of gravity
// We work in local vessel coordinate
Vector3d velocityDelta = Quaternion.Inverse(vessel.GetTransform().rotation) * worldVelocityDelta;
if (!conserveFuel || (velocityDelta.magnitude > conserveThreshold))
{
if (!vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, true);
}
Vector3d rcs = new Vector3d();
foreach (Vector6.Direction dir in Enum.GetValues(typeof(Vector6.Direction)))
{
double dirDv = Vector3d.Dot(velocityDelta, Vector6.directions[dir]);
double dirAvail = vesselState.rcsThrustAvailable[dir];
if (dirAvail > 0 && Math.Abs(dirDv) > 0.001)
{
double dirAction = dirDv / (dirAvail * TimeWarp.fixedDeltaTime / vesselState.mass);
if (dirAction > 0)
{
rcs += Vector6.directions[dir] * dirAction;
}
}
}
Vector3d omega = Quaternion.Inverse(vessel.GetTransform().rotation) * (vessel.acceleration - vesselState.gravityForce);
rcs = pid.Compute(rcs, omega);
// Disabled the low pass filter for now. Was doing more harm than good
//rcs = lastAct + (rcs - lastAct) * (1 / ((Tf / TimeWarp.fixedDeltaTime) + 1));
lastAct = rcs;
s.X = Mathf.Clamp((float)rcs.x, -1, 1);
s.Y = Mathf.Clamp((float)rcs.z, -1, 1); //note that z and
s.Z = Mathf.Clamp((float)rcs.y, -1, 1); //y must be swapped
}
else if (conserveFuel)
{
if (vessel.ActionGroups[KSPActionGroup.RCS])
{
vessel.ActionGroups.SetGroup(KSPActionGroup.RCS, false);
}
}
base.Drive(s);
}
public void HoldOrientation(FlightCtrlState s, Vessel v, Quaternion orientation, bool roll)
{
mVesselState.Update(v);
// Used in the killRot activation calculation and drive_limit calculation
double precision = Math.Max(0.5, Math.Min(10.0, (Math.Min(mVesselState.torqueAvailable.x, mVesselState.torqueAvailable.z) + mVesselState.torqueThrustPYAvailable * s.mainThrottle) * 20.0 / mVesselState.MoI.magnitude));
// Reset the PID controller during roll to keep pitch and yaw errors
// from accumulating on the wrong axis.
double rollDelta = Mathf.Abs((float)(mVesselState.vesselRoll - lastResetRoll));
if (rollDelta > 180)
rollDelta = 360 - rollDelta;
if (rollDelta > 5) {
mPid.Reset();
lastResetRoll = mVesselState.vesselRoll;
}
// Direction we want to be facing
Quaternion delta = Quaternion.Inverse(Quaternion.Euler(90, 0, 0) * Quaternion.Inverse(v.ReferenceTransform.rotation) * orientation);
Vector3d deltaEuler = new Vector3d(
(delta.eulerAngles.x > 180) ? (delta.eulerAngles.x - 360.0F) : delta.eulerAngles.x,
-((delta.eulerAngles.y > 180) ? (delta.eulerAngles.y - 360.0F) : delta.eulerAngles.y),
(delta.eulerAngles.z > 180) ? (delta.eulerAngles.z - 360.0F) : delta.eulerAngles.z
);
Vector3d torque = new Vector3d(
mVesselState.torqueAvailable.x + mVesselState.torqueThrustPYAvailable * s.mainThrottle,
mVesselState.torqueAvailable.y,
mVesselState.torqueAvailable.z + mVesselState.torqueThrustPYAvailable * s.mainThrottle
);
Vector3d inertia = Vector3d.Scale(
mVesselState.angularMomentum.Sign(),
Vector3d.Scale(
Vector3d.Scale(mVesselState.angularMomentum, mVesselState.angularMomentum),
Vector3d.Scale(torque, mVesselState.MoI).Invert()
)
);
Vector3d err = deltaEuler * Math.PI / 180.0F;
err += inertia.Reorder(132);
err.Scale(Vector3d.Scale(mVesselState.MoI, torque.Invert()).Reorder(132));
Vector3d act = mPid.Compute(err);
float drive_limit = Mathf.Clamp01((float)(err.magnitude * drive_factor / precision));
act.x = Mathf.Clamp((float)act.x, drive_limit * -1, drive_limit);
act.y = Mathf.Clamp((float)act.y, drive_limit * -1, drive_limit);
act.z = Mathf.Clamp((float)act.z, drive_limit * -1, drive_limit);
act = lastAct + (act - lastAct) * (TimeWarp.fixedDeltaTime / Tf);
SetFlightCtrlState(act, deltaEuler, s, v, precision, drive_limit);
act = new Vector3d(s.pitch, s.yaw, s.roll);
lastAct = act;
stress = Math.Abs(act.x) + Math.Abs(act.y) + Math.Abs(act.z);
}
public void OnFlyByWire(FlightCtrlState fcs)
{
if (mEnabled) {
UpdateMemoryMap(fcs);
Tick();
Actuate(fcs);
}
}
protected override void onCtrlUpd(FlightCtrlState s)
{
var state = Utilities.CopyFlightCtrlState(s);
if (this.staticPressureAtm <= double.Epsilon)
{
state.pitch = state.roll = state.yaw = 0;
}
base.onCtrlUpd(state);
}
/*
public override void OnUpdate()
{
// Make thruster exhaust onscreen correspond to actual thrust.
if (smartTranslation && throttles != null)
{
for (int i = 0; i < throttles.Length; i++)
{
// 'throttles' and 'thrusters' are guaranteed to be of the
// same length.
float throttle = (float)throttles[i];
var tfx = thrusters[i].partModule.thrusterFX;
for (int j = 0; j < tfx.Count; j++)
{
tfx[j].Power *= throttle;
}
}
}
base.OnUpdate();
}
*/
public override void Drive(FlightCtrlState s)
{
if (smartTranslation)
{
AdjustRCSThrottles(s);
}
base.Drive(s);
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (deorbitBurnTriggered && core.attitude.attitudeAngleFromTarget() < 5)
core.thrust.targetThrottle = 1.0F;
else
core.thrust.targetThrottle = 0;
return this;
}
protected override void onCtrlUpd(FlightCtrlState s)
{
var state = Utilities.CopyFlightCtrlState (s);
state.mainThrottle = this.engineEnabled ? EngineCommander.UpdateThrust(state.mainThrottle, this) : 0;
if (state.mainThrottle == 0) {
state.pitch = state.roll = state.yaw = 0;
}
base.onCtrlUpd (state);
}
private void OnFlyByWire(FlightCtrlState c)
{
foreach (var param in flightParameters.Values)
{
if (param.Enabled)
{
param.OnFlyByWire(ref c);
}
}
}
public override bool Execute(FlightComputer f, FlightCtrlState fcs)
{
if (mAbort) {
fcs.wheelThrottle = 0.0f;
f.Vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
return true;
}
return f.mRoverComputer.Drive(this, fcs);
}
// Process keyboard input.
protected override void onCtrlUpd( FlightCtrlState s )
{
rotateRoll = -s.roll;
rotatePitch = -s.pitch;
rotateYaw = -s.yaw;
moveRight = -s.X;
moveForward = -s.Y;
moveUp = -s.Z;
base.onCtrlUpd( s );
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (planeChangeTriggered && core.attitude.attitudeAngleFromTarget() < 2)
{
core.thrust.targetThrottle = Mathf.Clamp01((float)(planeChangeDVLeft / (2 * core.vesselState.maxThrustAccel)));
}
else
{
core.thrust.targetThrottle = 0;
}
return this;
}
protected override void onCtrlUpd(FlightCtrlState s)
{
var state = Utilities.CopyFlightCtrlState(s);
if (!EngineEnabled)
{
state.mainThrottle = 0;
}
if (state.mainThrottle == 0)
{
state.pitch = state.roll = state.yaw = 0;
}
base.onCtrlUpd(state);
}
public override AutopilotStep Drive(FlightCtrlState s)
{
if (deorbitBurnTriggered && core.attitude.attitudeAngleFromTarget() < 5)
{
core.thrust.targetThrottle = Mathf.Clamp01((float)lowDeorbitBurnMaxThrottle);
}
else
{
core.thrust.targetThrottle = 0;
}
return this;
}
public override void Drive(FlightCtrlState s)
{
switch (mode)
{
case Mode.AUTOLAND:
DriveAutoland(s);
break;
case Mode.HOLD:
DriveHeadingAndAltitudeHold(s);
break;
}
}
public override void Drive(FlightCtrlState s)
{
if (current_step != null)
{
try
{
current_step = current_step.Drive(s);
}
catch (Exception ex)
{
Debug.LogException(ex);
}
}
}
//save the flight control state entered at a given time
public void push(FlightCtrlState state, double time)
{
TimedCtrlState savedCopy = new TimedCtrlState();
savedCopy.flightCtrlState = new FlightCtrlState();
savedCopy.flightCtrlState.CopyFrom(state);
savedCopy.ActTime = time;
states.Enqueue(savedCopy);
// this saves the current attitude control values to a sepperate controlstate, that way attitude control is allways accesible without delay
pitch = state.pitch;
roll = state.roll;
yaw = state.yaw;
}
protected override void onDrive(FlightCtrlState s)
{
if(_vessel == null)
return;
game.FlightControl control = new game.FlightControl();
control.update(s);
_vessel.updateState(this.vessel);
_protocol.requestControl(_vessel, control);
control.apply(s);
}
void DriveRetractSolarPanels(FlightCtrlState s)
{
if (autoThrottle)
{
core.thrust.targetThrottle = 0.0f;
}
if (timedLaunch && tMinus > 10.0)
{
status = "Awaiting liftoff";
return;
}
status = "Retracting solar panels";
core.solarpanel.RetractAll();
if (core.solarpanel.AllRetracted())
{
mode = AscentMode.VERTICAL_ASCENT;
}
}
private void onFlyByWire(FlightCtrlState s)
{
if (!checkControlledVessel() || this != masterModule())
{
return;
}
if (!isActive)
{
return;
}
onDrive(s);
checkFlightCtrlState(s);
if (vessel == FlightGlobals.ActiveVessel)
{
FlightInputHandler.state.mainThrottle = s.mainThrottle;
}
}
private bool Turn(RemoteTech.FlightComputer.Commands.DriveCommand dc, FlightCtrlState fs)
{
Delta = Math.Abs(Quaternion.Angle(mRoverRot, mVessel.ReferenceTransform.rotation));
DeltaT = Delta / mVessel.GetComponent <Rigidbody>().angularVelocity.magnitude;
if (Delta < dc.target)
{
fs.wheelThrottle = (float)throttlePID.Update(RoverSpeed, dc.speed, -1.0, 1.0);
fs.wheelSteer = dc.steering;
return(false);
}
else
{
fs.wheelThrottle = 0;
fs.wheelSteer = 0;
mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
dc.mode = RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Off;
return(true);
}
}
private void Drive(FlightCtrlState s)
{
if (this == vessel.GetMasterMechJeb())
{
foreach (ComputerModule module in GetComputerModules <ComputerModule>())
{
try
{
if (module.enabled)
{
module.Drive(s);
}
}
catch (Exception e)
{
Debug.LogError("MechJeb module " + module.GetType().Name + " threw an exception in Drive: " + e);
}
}
}
}
private void OnFlyByWire(FlightCtrlState st)
{
if (neutral.IsStale)
{
if (neutral.FlushValue)
{
st.Neutralize();
}
}
if (resetTrim.IsStale)
{
if (resetTrim.FlushValue)
{
st.ResetTrim();
}
}
PushNewSetting(ref st);
}
private void UpdateControl(FlightCtrlState c)
{
/* TODO: static engine torque and/or differential throttle */
for (int i = 0; i < 3; i++)
{
double clamp = Math.Max(Math.Abs(Actuation[i]), 0.005) * 2;
Actuation[i] = TargetTorque[i] / ControlTorque[i];
if (Math.Abs(Actuation[i]) < EPSILON)
{
Actuation[i] = 0;
}
Actuation[i] = Math.Max(Math.Min(Actuation[i], clamp), -clamp);
}
c.pitch = (float)Actuation.x;
c.roll = (float)Actuation.y;
c.yaw = (float)Actuation.z;
}
void AirspeedControl(FlightCtrlState s)
{
if (targetSpeed == 0)
{
if (useBrakes)
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
}
s.mainThrottle = 0;
return;
}
float currentSpeed = (float)vessel.srfSpeed;
float speedError = targetSpeed - currentSpeed;
float setAccel = speedError * throttleFactor;
SetAcceleration(setAccel, s);
}
public void RoverControl(FlightCtrlState s)
{
Vector3 killVector = leader.vessel.GetObtVelocity() - this.vessel.GetObtVelocity();
Quaternion rotAdjust = Quaternion.Inverse(vessel.ReferenceTransform.rotation);
killVector = rotAdjust * killVector;
float wheelThrottleFactor = 7;
float wheelSteerFactor = 0.3f;
if (Mathf.Abs(killVector.y) > Mathf.Abs(killVector.z))
{
s.wheelThrottle = Mathf.Clamp(wheelThrottleFactor * killVector.y, -1, 1);
}
else
{
s.wheelThrottle = Mathf.Clamp(wheelThrottleFactor * killVector.z, -1, 1);
}
s.wheelSteer = Mathf.Clamp(-wheelSteerFactor * killVector.x, -1, 1);
}
private void DriveGuidance(FlightCtrlState s)
{
if (core.guidance.status == PVGStatus.FINISHED)
{
mode = AscentMode.EXIT;
return;
}
if (!core.guidance.isStable())
{
double pitch = Math.Min(Math.Min(90, srfvelPitch()), vesselState.vesselPitch);
attitudeTo(pitch, srfvelHeading());
status = "WARNING: Unstable Guidance";
}
else
{
status = "Stable Guidance";
attitudeTo(core.guidance.pitch, core.guidance.heading);
}
}
private void OnFlyByWire(FlightCtrlState st)
{
Log.Info("OnFlyByWire, tic: " + _tick);
switch (_tick)
{
case 7:
case 6:
case 5:
case 4:
case 3:
case 2:
case 1:
st.mainThrottle = ConfigInfo.Instance.VesselOptions[ModuleNASACountdown.CraftName(FlightGlobals.ActiveVessel)].defaultInitialThrottle;
break;
case 0:
//st.mainThrottle = 1f;
Log.Info("OnFlyByWire, before GravityTurnAPI.Launch");
if (GravityTurnAPI.Launch()) // If GravityTurn is available
{
break;
}
st.mainThrottle = ConfigInfo.Instance.VesselOptions[ModuleNASACountdown.CraftName(FlightGlobals.ActiveVessel)].defaultThrottle;
if (ConfigInfo.Instance.VesselOptions[ModuleNASACountdown.CraftName(FlightGlobals.ActiveVessel)].enableSAS)
{
FlightGlobals.ActiveVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
}
break;
default:
if (ConfigInfo.Instance.VesselOptions[ModuleNASACountdown.CraftName(FlightGlobals.ActiveVessel)].enableSAS)
{
FlightGlobals.ActiveVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, true);
}
//st.mainThrottle = 0f;
st.mainThrottle = ConfigInfo.Instance.VesselOptions[ModuleNASACountdown.CraftName(FlightGlobals.ActiveVessel)].defaultThrottle;
break;
}
}
public void GuidanceSteer(FlightCtrlState s)
{
if (guidanceActive && MissileReferenceTransform != null && _velocityTransform != null)
{
Vector3 newTargetPosition = new Vector3();
if (_guidanceIndex == 1)
{
newTargetPosition = AAMGuidance();
CheckMiss(newTargetPosition);
}
else if (_guidanceIndex == 2)
{
newTargetPosition = AGMGuidance();
}
else if (_guidanceIndex == 3)
{
newTargetPosition = CruiseGuidance();
}
else if (_guidanceIndex == 4)
{
newTargetPosition = BallisticGuidance();
}
//Updating aero surfaces
if (TimeIndex > dropTime + 0.5f)
{
_velocityTransform.rotation = Quaternion.LookRotation(vessel.srf_velocity, -vessel.transform.forward);
var targetDirection = _velocityTransform.InverseTransformPoint(newTargetPosition).normalized;
targetDirection = Vector3.RotateTowards(Vector3.forward, targetDirection, 15 * Mathf.Deg2Rad, 0);
var localAngVel = vessel.angularVelocity;
var steerYaw = SteerMult * targetDirection.x - SteerDamping * -localAngVel.z;
var steerPitch = SteerMult * targetDirection.y - SteerDamping * -localAngVel.x;
s.yaw = Mathf.Clamp(steerYaw, -MaxSteer, MaxSteer);
s.pitch = Mathf.Clamp(steerPitch, -MaxSteer, MaxSteer);
}
s.mainThrottle = 1;
}
}
void DrivePrelaunch(FlightCtrlState s)
{
if (vessel.LiftedOff() && !vessel.Landed)
{
status = Localizer.Format("#MechJeb_Ascent_status4");//"Vessel is not landed, skipping pre-launch"
mode = AscentMode.ASCEND;
return;
}
if (autoThrottle)
{
Debug.Log("prelaunch killing throttle");
core.thrust.ThrustOff();
}
core.attitude.AxisControl(false, false, false);
if (timedLaunch && tMinus > 10.0)
{
status = Localizer.Format("#MechJeb_Ascent_status1");//"Pre Launch"
return;
}
if (autodeploySolarPanels && mainBody.atmosphere)
{
core.solarpanel.RetractAll();
if (core.solarpanel.AllRetracted())
{
Debug.Log("Prelaunch -> Ascend");
mode = AscentMode.ASCEND;
}
else
{
status = Localizer.Format("#MechJeb_Ascent_status5");//"Retracting solar panels"
}
}
else
{
mode = AscentMode.ASCEND;
}
}
void DriveCoastToApoapsis(FlightCtrlState s)
{
core.thrust.targetThrottle = 0;
double circularSpeed = OrbitalManeuverCalculator.CircularOrbitSpeed(mainBody, orbit.ApR);
double apoapsisSpeed = orbit.SwappedOrbitalVelocityAtUT(orbit.NextApoapsisTime(vesselState.time)).magnitude;
double circularizeBurnTime = (circularSpeed - apoapsisSpeed) / vesselState.limitedMaxThrustAccel;
if (vesselState.altitudeASL > mainBody.RealMaxAtmosphereAltitude())
{
mode = AscentMode.EXIT;
core.warp.MinimumWarp();
return;
}
//if our apoapsis has fallen too far, resume the gravity turn
if (orbit.ApA < autopilot.desiredOrbitAltitude - 1000.0)
{
mode = AscentMode.GRAVITY_TURN;
core.warp.MinimumWarp();
return;
}
core.thrust.targetThrottle = 0;
// follow surface velocity to reduce flipping
attitudeTo(srfvelPitch());
if (autopilot.autoThrottle && orbit.ApA < autopilot.desiredOrbitAltitude)
{
core.thrust.targetThrottle = ThrottleToRaiseApoapsis(orbit.ApR, autopilot.desiredOrbitAltitude + mainBody.Radius);
}
if (core.node.autowarp)
{
//warp at x2 physical warp:
core.warp.WarpPhysicsAtRate(2);
}
status = "Coasting to edge of atmosphere";
}
/// <summary>
/// Here we copy the flight state we received and apply to the specific vessel.
/// This method is called by ksp as it's a delegate. It's called on every FixedUpdate
/// </summary>
public void LunaOnVesselFlyByWire(Guid id, FlightCtrlState st)
{
if (!Enabled || !FlightStateSystemReady)
{
return;
}
if (FlightStatesDictionary.TryGetValue(id, out var value))
{
if (VesselCommon.IsSpectating)
{
st.CopyFrom(value.GetInterpolatedValue(st));
}
else
{
//If we are close to a vessel and we both are in space don't copy the
//input controls as then the vessel jitters, specially if the other player has SAS on
if (FlightGlobals.ActiveVessel?.situation > Vessel.Situations.FLYING)
{
var interpolatedState = value.GetInterpolatedValue(st);
st.mainThrottle = interpolatedState.mainThrottle;
st.gearDown = interpolatedState.gearDown;
st.gearUp = interpolatedState.gearUp;
st.headlight = interpolatedState.headlight;
st.killRot = interpolatedState.killRot;
}
else
{
st.CopyFrom(value.GetInterpolatedValue(st));
}
}
}
else
{
var vessel = FlightGlobals.FindVessel(id);
if (vessel != null && !FlightGlobals.FindVessel(id).packed)
{
AddVesselToSystem(vessel);
}
}
}
private void ProcessInput()
{
Verb verb = Verb.Set;
Address address = Address.Led0;
uint data = 0;
while (ReadCommand(ref verb, ref address, ref data))
{
switch (verb)
{
case Verb.Set:
switch (address)
{
case Address.Autopilot:
FlightGlobals.ActiveVessel.Autopilot.SetMode((VesselAutopilot.AutopilotMode)data);
break;
case Address.SAS:
FlightGlobals.ActiveVessel.ActionGroups.SetGroup(KSPActionGroup.SAS, (data & 1) == 1 ? true : false);
break;
case Address.RCS:
FlightGlobals.ActiveVessel.ActionGroups.SetGroup(KSPActionGroup.RCS, (data & 1) == 1 ? true : false);
break;
case Address.Throttle:
FlightCtrlState ctrlState = new FlightCtrlState();
FlightGlobals.ActiveVessel.GetControlState(ctrlState);
ctrlState.mainThrottle = data / 1024.0f;
FlightGlobals.ActiveVessel.SetControlState(ctrlState);
break;
}
break;
case Verb.Init:
initialized = false;
WriteCommand(Verb.InitAck, Address.Led0, 0);
break;
}
}
}
public override void drive(FlightCtrlState s)
{
if (abort != AbortStage.OFF)
{
switch (abort)
{
case AbortStage.THRUSTOFF:
FlightInputHandler.SetNeutralControls();
s.mainThrottle = 0;
abort = AbortStage.DECOUPLE;
break;
case AbortStage.DECOUPLE:
recursiveDecouple();
abort = AbortStage.BURNUP;
burnUpTime = Planetarium.GetUniversalTime();
break;
case AbortStage.BURNUP:
if ((Planetarium.GetUniversalTime() - burnUpTime < 2) || (vesselState.speedVertical < 10))
{
core.tmode = MechJebCore.TMode.DIRECT;
core.attitudeTo(Vector3d.up, MechJebCore.AttitudeReference.SURFACE_NORTH, this);
double int_error = Math.Abs(Vector3d.Angle(vesselState.up, vesselState.forward));
core.trans_spd_act = (int_error < 90) ? 100 : 0;
}
else
{
abort = AbortStage.LAND;
}
break;
case AbortStage.LAND:
core.controlRelease(this);
core.landActivate(this);
abort = AbortStage.OFF;
break;
}
}
base.drive(s);
}
private void DriveGravityTurn(FlightCtrlState s)
{
if (autopilot.autoThrottle)
{
core.thrust.targetThrottle = 1.0F;
}
if (autopilot.MET < pitchEndTime)
{
/* this can happen when users update the endtime box */
mode = AscentMode.INITIATE_TURN;
return;
}
if (stages[0].h >= h_burnout)
{
status = "Angular momentum target achieved";
core.thrust.targetThrottle = 0.0F;
mode = AscentMode.EXIT;
return;
}
if (saneGuidance && guidanceEnabled)
{
if (terminalGuidance)
{
status = "Locked Terminal Guidance";
}
else
{
status = "Stable PEG Guidance";
}
attitudeTo(guidancePitch);
}
else
{
// srfvelPitch == zero AoA
status = "Unguided Gravity Turn";
attitudeTo(Math.Min(90, srfvelPitch() + pitchBias));
}
}
public void Fly(FlightCtrlState state)
{
if ((vessel == null) || (vessel.checkLanded() && checkingLanded))
{
ScreenMessages.PostScreenMessage("Landed!", 3.0f, ScreenMessageStyle.UPPER_CENTER);
DisableLand();
// Shut-off throttle
FlightCtrlState ctrl = new FlightCtrlState();
vessel.GetControlState(ctrl);
ctrl.mainThrottle = 0;
// Necessary on Realism Overhaul to shutdown engine as at throttle=0 the engine may still have
// a lot of thrust
if (_keepIgnited)
{
ShutdownAllEngines();
}
autoMode = AutoMode.Off;
return;
}
// Only start checking if landed when taken off
if (!vessel.checkLanded())
{
checkingLanded = true;
}
Vector3 r = vessel.GetWorldPos3D();
Vector3 v = vessel.GetSrfVelocity();
Vector3 tr = _transform.InverseTransformPoint(r);
Vector3 tv = _transform.InverseTransformVector(v);
float g = (float)FlightGlobals.getGeeForceAtPosition(r).magnitude;
Vector3d dr, dv, da;
double desired_t; // closest time in trajectory (desired)
_traj.FindClosest(tr, tv, out dr, out dv, out da, out desired_t, 0.5f, 0.5f);
// Uses transformed positions and vectors
// sets throttle and desired attitude based on targets
// F is the inital force (acceleration) vector
AutopilotStepToTarget(state, tr, tv, dr, dv, da, g);
}
public override bool DriveAscent(FlightCtrlState s)
{
setTarget();
core.guidance.AssertStart(allow_execution: true);
switch (mode)
{
case AscentMode.VERTICAL_ASCENT:
DriveVerticalAscent(s);
break;
case AscentMode.INITIATE_TURN:
DriveInitiateTurn(s);
break;
case AscentMode.GUIDANCE:
DriveGuidance(s);
break;
}
return(mode != AscentMode.EXIT);
}
private void HoldAltitude(FlightCtrlState fs)
{
const double damping = 1000.0f;
Vessel v = mAttachedVessel;
double target_height = mCommand.AttitudeCommand.Altitude;
float target_pitch = (float)(Math.Atan2(target_height - v.orbit.ApA, damping) / Math.PI * 180.0f);
Vector3 up = (v.mainBody.position - v.CoM);
Vector3 forward = Vector3.Exclude(up,
v.mainBody.position + v.mainBody.transform.up * (float)v.mainBody.Radius - v.CoM
);
Vector3.OrthoNormalize(ref forward, ref up);
Vector3 direction = Vector3.Exclude(up, v.obt_velocity).normalized;
float target_heading = Vector3.Angle(forward, direction);
Quaternion rotationReference = Quaternion.LookRotation(forward, up);
RTUtil.Log("Pitch: {0}, Heading: {1}", target_pitch, target_heading);
HoldOrientation(fs, rotationReference *
Quaternion.Euler(new Vector3(target_pitch, target_heading, 0)));
}
public override bool DriveAscent(FlightCtrlState s)
{
switch (mode)
{
case AscentMode.VERTICAL_ASCENT:
DriveVerticalAscent(s);
break;
case AscentMode.GRAVITY_TURN:
DriveGravityTurn(s);
break;
case AscentMode.COAST_TO_APOAPSIS:
DriveCoastToApoapsis(s);
break;
case AscentMode.EXIT:
return(false);
}
return(true);
}
public static void LerpUnclamped(this FlightCtrlState fs, FlightCtrlState from, FlightCtrlState to, float lerpPercentage)
{
fs.X = LunaMath.LerpUnclamped(from.X, to.X, lerpPercentage);
fs.Y = LunaMath.LerpUnclamped(from.Y, to.Y, lerpPercentage);
fs.Z = LunaMath.LerpUnclamped(from.Z, to.Z, lerpPercentage);
fs.pitch = LunaMath.LerpUnclamped(from.pitch, to.pitch, lerpPercentage);
fs.pitchTrim = LunaMath.LerpUnclamped(from.pitchTrim, to.pitchTrim, lerpPercentage);
fs.roll = LunaMath.LerpUnclamped(from.roll, to.roll, lerpPercentage);
fs.rollTrim = LunaMath.LerpUnclamped(from.rollTrim, to.rollTrim, lerpPercentage);
fs.yaw = LunaMath.LerpUnclamped(from.yaw, to.yaw, lerpPercentage);
fs.yawTrim = LunaMath.LerpUnclamped(from.yawTrim, to.yawTrim, lerpPercentage);
fs.mainThrottle = LunaMath.LerpUnclamped(from.mainThrottle, to.mainThrottle, lerpPercentage);
fs.wheelSteer = LunaMath.LerpUnclamped(from.wheelSteer, to.wheelSteer, lerpPercentage);
fs.wheelSteerTrim = LunaMath.LerpUnclamped(from.wheelSteerTrim, to.wheelSteerTrim, lerpPercentage);
fs.wheelThrottle = LunaMath.LerpUnclamped(from.wheelThrottle, to.wheelThrottle, lerpPercentage);
fs.wheelThrottleTrim = LunaMath.LerpUnclamped(from.wheelThrottleTrim, to.wheelThrottleTrim, lerpPercentage);
fs.gearDown = LunaMath.Lerp(from.gearDown, to.gearDown, lerpPercentage);
fs.gearUp = LunaMath.Lerp(from.gearUp, to.gearUp, lerpPercentage);
fs.headlight = LunaMath.Lerp(from.headlight, to.headlight, lerpPercentage);
fs.killRot = LunaMath.Lerp(from.killRot, to.killRot, lerpPercentage);
}
void DriveUntargetedDeorbit(FlightCtrlState s)
{
if (orbit.PeA < -0.1 * mainBody.Radius)
{
core.thrust.targetThrottle = 0;
landStep = LandStep.FINAL_DESCENT;
return;
}
core.attitude.attitudeTo(Vector3d.back, AttitudeReference.ORBIT_HORIZONTAL, this);
if (core.attitude.attitudeAngleFromTarget() < 5)
{
core.thrust.targetThrottle = 1;
}
else
{
core.thrust.targetThrottle = 0;
}
status = "Doing deorbit burn.";
}
void IFlightControlParameter.UpdateAutopilot(FlightCtrlState c)
{
if (!Enabled)
{
return;
}
if (!(controlShared.Vessel.horizontalSrfSpeed > 0.1f))
{
return;
}
if (Mathf.Abs(VesselUtils.AngleDelta(VesselUtils.GetHeading(controlShared.Vessel), VesselUtils.GetVelocityHeading(controlShared.Vessel))) <= 90)
{
c.wheelSteer = Mathf.Clamp(Value / -10, -1, 1);
}
else
{
c.wheelSteer = -Mathf.Clamp(Value / -10, -1, 1);
}
}
public static void CopyFrom(this FlightCtrlState fs, VesselFlightStateMsgData msgData)
{
fs.mainThrottle = msgData.MainThrottle;
fs.wheelThrottleTrim = msgData.WheelThrottleTrim;
fs.X = msgData.X;
fs.Y = msgData.Y;
fs.Z = msgData.Z;
fs.killRot = msgData.KillRot;
fs.gearUp = msgData.GearUp;
fs.gearDown = msgData.GearDown;
fs.headlight = msgData.Headlight;
fs.wheelThrottle = msgData.WheelThrottle;
fs.roll = msgData.Roll;
fs.yaw = msgData.Yaw;
fs.pitch = msgData.Pitch;
fs.rollTrim = msgData.RollTrim;
fs.yawTrim = msgData.YawTrim;
fs.pitchTrim = msgData.PitchTrim;
fs.wheelSteer = msgData.WheelSteer;
fs.wheelSteerTrim = msgData.WheelSteerTrim;
}
public override void Drive(FlightCtrlState s)
{
switch (mode)
{
case AscentMode.VERTICAL_ASCENT:
DriveVerticalAscent(s);
break;
case AscentMode.GRAVITY_TURN:
DriveGravityTurn(s);
break;
case AscentMode.COAST_TO_APOAPSIS:
DriveCoastToApoapsis(s);
break;
case AscentMode.CIRCULARIZE:
DriveCircularizationBurn(s);
break;
}
}
public void Update(FlightCtrlState c)
{
//if (internalVessel == null)
//{
// this.DisableControl();
//}
if (!Enabled)
{
return; // skip update if not enabled
}
if (targetDirection == null)
{
return;
}
sw.Reset();
sw.Start();
if (internalVessel.situation == Vessel.Situations.PRELAUNCH)
{
ResetIs();
}
//if (sessionTime > lastSessionTime)
//{
//}
if (internalVessel.ActionGroups[KSPActionGroup.SAS])
{
UpdateStateVectors();
UpdateControl(c);
}
else
{
UpdateStateVectors();
UpdateControlParts();
UpdateTorque();
UpdatePredictionPI();
UpdateControl(c);
}
sw.Stop();
AverageDuration.Update((double)sw.ElapsedTicks / (double)System.TimeSpan.TicksPerMillisecond);
}
public override bool DriveAscent(FlightCtrlState s)
{
stats.RequestUpdate(this, true);
stats.liveSLT = true; /* yes, this disables the button, yes, it is important we do this */
if (last_edit_num_stages != edit_num_stages)
{
SetNumStages(edit_num_stages);
last_edit_num_stages = edit_num_stages;
}
UpdateRocketStats();
if (last_time != 0.0D)
{
converge(vesselState.time - last_time);
}
else
{
converge(0);
}
last_time = vesselState.time;
switch (mode)
{
case AscentMode.VERTICAL_ASCENT:
DriveVerticalAscent(s);
break;
case AscentMode.INITIATE_TURN:
DriveInitiateTurn(s);
break;
case AscentMode.GRAVITY_TURN:
DriveGravityTurn(s);
break;
}
return(mode != AscentMode.EXIT);
}
void DriveVerticalAscent(FlightCtrlState s)
{
if (timedLaunch)
{
status = "Awaiting liftoff";
return;
}
if (!ascentPath.IsVerticalAscent(vesselState.altitudeASL, vesselState.speedSurface))
{
mode = AscentMode.GRAVITY_TURN;
}
if (autoThrottle && orbit.ApA > desiredOrbitAltitude)
{
mode = AscentMode.COAST_TO_APOAPSIS;
}
//during the vertical ascent we just thrust straight up at max throttle
if (forceRoll && vesselState.altitudeTrue > 50)
{ // pre-align roll unless correctiveSteering is active as it would just interfere with that
core.attitude.attitudeTo(90 - desiredInclination, 90, verticalRoll, this);
}
else
{
core.attitude.attitudeTo(Vector3d.up, AttitudeReference.SURFACE_NORTH, this);
}
if (autoThrottle)
{
core.thrust.targetThrottle = 1.0F;
}
if (!vessel.LiftedOff())
{
status = "Awaiting liftoff";
}
else
{
status = "Vertical ascent";
}
}
void SetAcceleration(float accel, FlightCtrlState s)
{
float gravAccel = GravAccel();
float requestEngineAccel = accel - gravAccel;
possibleAccel = 0; //gravAccel;
float dragAccel = 0;
float engineAccel = MaxEngineAccel(requestEngineAccel, out dragAccel);
if (throttleOverride >= 0)
{
s.mainThrottle = throttleOverride;
return;
}
if (engineAccel == 0)
{
s.mainThrottle = accel > 0 ? 1 : 0;
return;
}
requestEngineAccel = Mathf.Clamp(requestEngineAccel, -engineAccel, engineAccel);
float requestThrottle = (requestEngineAccel - dragAccel) / engineAccel;
s.mainThrottle = Mathf.Clamp01(requestThrottle);
//use brakes if overspeeding too much
if (useBrakes)
{
if (requestThrottle < -0.5f)
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, true);
}
else
{
vessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
}
}
}
