public void OnFixedUpdate()
{
if (Vessel != null)
{
UpdateStateVectors(Vessel, Target);
SteeringHelper.AnalyzeParts(Vessel);
Vector3 Torque = SteeringHelper.TorqueAvailable;
var CoM = Vessel.CoM;
var MoI = Vessel.MOI;
phiTotal = calculatePhiTotal();
phiVector = calculatePhiVector();//deviation errors from orientation target
for (int i = 0; i < 3; i++)
{
MaxOmega[i] = Mathf.Max(Torque[i] / MoI[i], 0.0001f);
}
TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]);
TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]);
TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]);
if (Math.Abs(phiTotal) > RollControlRange)
{
TargetOmega[1] = 0;
rollRatePI.ResetI();
}
TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], Vessel.MOI[0], Torque[0]);
TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]);
TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]);
}
}
public void OnFixedUpdate()
{
if (Vessel != null)
{
UpdateStateVectors(Vessel, Target);
Vector3 Torque = SteeringHelper.GetVesselTorque(Vessel);
var CoM = Vessel.CoM;
var MoI = Vessel.MOI;
phiTotal = PhiTotal();
phiVector = PhiVector();
for (int i = 0; i < 3; i++)
{
MaxOmega[i] = Torque[i] * MaxStoppingTime / MoI[i];
}
TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]);
TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]);
TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]);
if (Math.Abs(phiTotal) > RollControlRange)
{
TargetOmega[1] = 0;
rollRatePI.ResetI();
}
TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], Vessel.MOI[0], Torque[0]);
TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]);
TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]);
}
}
public void InitMode(RemoteTech.FlightComputer.Commands.DriveCommand dc)
{
if (mVessel == null)
{
RTLog.Verbose("Vessel is null!");
return;
}
ForwardAxis = Vector3.zero;
mRoverAlt = (float)mVessel.altitude;
mRoverLat = (float)mVessel.latitude;
mRoverLon = (float)mVessel.longitude;
Delta = 0;
DeltaT = 0;
/* Explanation on targetRotation
* Quaternion.Euler(x,y,z) - Returns a rotation that rotates z degrees around the z axis,
* x degrees around the x axis, and y degrees around the y axis
* in that order.
*
* Unity Q.Euler(0,0,0) isn't matched to KSP's rotation "(0,0,0)" (-90, varying UP-axis, 90) so need to
* match the target rotation to the KSP's rotation.
*
* Rover-specific rotation is Forward (y) to HDG 0, Up (x) to North and
* Right (z) to East.
*/
const float KSPRotXAxis = -90f, KSPRotZAxis = 90f;
double AngleFromUpAxis = Mathf.Rad2Deg * Math.Atan(-mVessel.upAxis.z / -mVessel.upAxis.x);
float KSPRotYAxis = (float)-AngleFromUpAxis;
switch (dc.mode)
{
case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Turn:
mRoverRot = mVessel.ReferenceTransform.rotation;
break;
case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Distance:
targetRotation = Quaternion.Euler(KSPRotXAxis, KSPRotYAxis, KSPRotZAxis);
break;
case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.DistanceHeading:
targetRotation = Quaternion.Euler(KSPRotXAxis - dc.target2, KSPRotYAxis, KSPRotZAxis);
break;
case RemoteTech.FlightComputer.Commands.DriveCommand.DriveMode.Coord:
mTargetLat = dc.target;
mTargetLon = dc.target2;
targetRotation = Quaternion.Euler(KSPRotXAxis - TargetHDG, KSPRotYAxis, KSPRotZAxis);
break;
}
pidController.setPIDParameters(Kp, Ki, Kd);
throttlePID.ResetI();
steerPID.ResetI();
mVessel.ActionGroups.SetGroup(KSPActionGroup.Brakes, false);
}
public void Reset()
{
pitchPI.ResetI();
yawPI.ResetI();
rollPI.ResetI();
pitchRatePI.ResetI();
yawRatePI.ResetI();
rollRatePI.ResetI();
}
public void OnFixedUpdate()
{
if (Vessel != null)
{
UpdateStateVectors(Vessel, Target);
SteeringHelper.AnalyzeParts(Vessel);
Vector3 Torque = SteeringHelper.TorqueAvailable;
var CoM = Vessel.CoM;
var MoI = Vessel.MOI;
phiTotal = calculatePhiTotal();
phiVector = calculatePhiVector();//deviation errors from orientation target
for (int i = 0; i < 3; i++)
{
//Edge case: Very low (torque/MoI) (like 0.0078!) rate so need to rise max acceleration artifically
StoppingTime = (OmegaThreshold <= (Torque[i] / MoI[i])) ?
1.0f :
(float)RTUtil.Clamp((1.0 / (Torque[i] / MoI[i])) * (Math.Abs(phiVector[i]) - Phi1FStoppingTime), 1.0, MaxStoppingTime);
MaxOmega[i] = Mathf.Max((Torque[i] * StoppingTime) / MoI[i], 0.0001f);
}
TargetOmega[0] = pitchRatePI.Update(-phiVector[0], 0, MaxOmega[0]);
TargetOmega[1] = rollRatePI.Update(-phiVector[1], 0, MaxOmega[1]);
TargetOmega[2] = yawRatePI.Update(-phiVector[2], 0, MaxOmega[2]);
if (Math.Abs(phiTotal) > RollControlRange)
{
TargetOmega[1] = 0;
rollRatePI.ResetI();
}
TargetTorque[0] = pitchPI.Update(Omega[0], TargetOmega[0], Vessel.MOI[0], Torque[0]);
TargetTorque[1] = rollPI.Update(Omega[1], TargetOmega[1], Vessel.MOI[1], Torque[1]);
TargetTorque[2] = yawPI.Update(Omega[2], TargetOmega[2], Vessel.MOI[2], Torque[2]);
}
}
