private void dockLogic(float dt)
{
// calculate the dock attitude based on the two docking ports
Quaternion dockAttitude = getDockAttitude();
// nominal (no rotation) dock attitude
Vector3 forward = new Vector3(0, 1, 0);
Vector3 up = new Vector3(0, 0, 1);
// rotate the nominal vectors by the dock attitude quaternion
forward = dockAttitude * forward;
up = dockAttitude * up;
// transform the vectors into the inertial frame
forward = flightData.targetPart.transform.TransformDirection(forward);
up = flightData.targetPart.transform.TransformDirection(up);
// check to see if we are in the approach corridor
dockDeviationAngle = getDeviationAngle();
// maintain docking attitude
if(dockMode == DockMode.ATTITUDE) {
// command alignment
attActive = true;
attUpActive = true;
attCommand = forward;
attUpCommand = up;
attTransform = flightData.vesselPart.transform;
}
// fully autonomous docking
else if(dockMode == DockMode.AUTO) {
// get the target's dock rotation
Quaternion targetDockRotation = getDockingPortRotation(flightData.targetPart);
// handle states
if(dockState == DockState.ORIENT) {
// command alignment
attActive = true;
attUpActive = true;
attCommand = forward;
attUpCommand = up;
attTransform = flightData.vesselPart.transform;
// next state?
if(Util.maxElement(pyrError) < dockPyrTransitionMargin) {
dockStateTimer += dt;
if(dockStateTimer > dockStateTransitionDelay) {
if (dockDeviationAngle < 100) {
dockState = DockState.ENTRY;
dockDistance = Mathf.Clamp(Vector3.Distance(flightData.vesselPart.transform.position, flightData.targetPart.transform.position), 5f, 50f);
}
dockStateTimer = 0;
}
} else {
dockStateTimer = 0;
}
}
else if(dockState == DockState.ENTRY) {
// move to entry
// rotate the position command by the target dock rotation
rposActive = true;
rposCommand = targetDockRotation * (dockEntryPoint / 50f) * dockDistance;
rposTransform = flightData.targetPart.transform;
rposOffset = flightData.vesselPart.transform.position - flightData.vessel.ReferenceTransform.position;
// maintain alignment
attActive = true;
attUpActive = true;
attCommand = forward;
attUpCommand = up;
attTransform = flightData.vesselPart.transform;
// next state?
if(Util.maxElement(rposError) < dockPosTransitionMargin &&
Util.maxElement(pyrError) < dockPyrTransitionMargin) {
dockStateTimer += dt;
if(dockStateTimer > dockStateTransitionDelay) {
dockState = DockState.APPROACH;
dockStateTimer = 0;
}
} else {
dockStateTimer = 0;
}
}
else if(dockState == DockState.APPROACH) {
// approach
rposActive = true;
rposCommand = Vector3.zero;
rposTransform = flightData.targetPart.transform;
rposOffset = flightData.vesselPart.transform.position - flightData.vessel.ReferenceTransform.position;
// maintain alignment
attActive = true;
attUpActive = true;
attCommand = forward;
attUpCommand = up;
attTransform = flightData.vesselPart.transform;
// abort?
if(dockDeviationAngle > dockAbortDeviation) {
dockState = DockState.ABORT;
dockAbort = DockAbort.DEVIATION;
dockStateTimer = 0;
}
if(Util.maxElement(pyrError) > dockAbortAttitude) {
dockState = DockState.ABORT;
dockAbort = DockAbort.ATTITUDE;
dockStateTimer = 0;
}
/*if (relPos.y < -dockAbortLatchMiss) {
//dockState = DockState.ABORT;
//dockAbort = DockAbort.LATCH;
//dockStateTimer = 0;
}*/
// limit approach velocity
rvelLimitMax = true;
float dist = rposError.magnitude;
if(dist < 2.5f) {
rvelLimitMin = true;
rvelLimit = 0.1f;
}
else if(dist < 5.0f) {
rvelLimit = 0.25f;
}
else if(dist < 10.0f) {
rvelLimit = 0.5f;
} else if(dist < 0.95f && rvelLimitMin) {
requestCommand(Command.OFF);
}
else {
rvelLimit = 1.0f;
}
}
/*else if(dockState == DockState.DOCKED) {
// undocked?
if (((ORDA_dock)flightData.vesselPart).isDocked () == false) {
dockState = DockState.DEPART;
dockStateTimer = 0;
}
if(command != Command.OFF) {
print ("dockLogic: command to OFF");
requestCommand(Command.OFF);
}
}
else if(dockState == DockState.DEPART) {
// free drift a bit first
if(distance > 2.5f) {
// minimize rotation
avelActive = true;
avelCommand = Vector3.zero;
// move to entry
rposActive = true;
rposDock = true;
rposCommand = dockEntryPoint;
}
}*/
else if(dockState == DockState.ABORT) {
// minimize rotation
avelActive = true;
avelCommand = Vector3.zero;
// move to entry
rposActive = true;
rposCommand = dockEntryPoint;
rposTransform = flightData.targetPart.transform;
}
}
}
public void restoreConfiguration(GNCconfig config)
{
// should be good, might need to add some checks
command = config.command;
rateMode = config.rateMode;
attMode = config.attMode;
eacMode = config.eacMode;
posMode = config.posMode;
dockMode = config.dockMode;
dockState = config.dockState;
dockAbort = config.dockAbort;
userRateSetting = config.userRateSetting.toVector3 ();
userAttSetting = config.userAttSetting.toVector3 ();
userAttUpSetting = config.userAttUpSetting.toVector3 ();
userPosSetting = config.userPosSetting.toVector3 ();
Kp_AngVel = config.Kp_AngVel;
Kp_AngAcc = config.Kp_AngAcc;
Kp_Vel = config.Kp_Vel;
Kp_Acc = config.Kp_Acc;
eacPulseLength = config.eacPulseLength;
eacPulseLevel = config.eacPulseLevel;
eacRate = config.eacRate;
dockRollAdjust = config.dockRollAdjust;
networkPropulsion = config.networkPropulsion;
// ...
if (command == Command.EAC && eacMode == EACMode.RATE_ATT) {
eacAttInvalid = true;
}
}
public void requestDockEngage()
{
if(command != Command.DOCK)
return;
if(dockMode != DockMode.AUTO)
return;
dockState = DockState.ORIENT;
dockAbort = DockAbort.UNKNOWN;
dockStateTimer = 0;
}
public void requestDockMode(DockMode m)
{
if (command != Command.DOCK)
return;
if (dockMode == m)
dockMode = DockMode.IDLE;
else
dockMode = m;
dockState = DockState.IDLE;
dockAbort = DockAbort.UNKNOWN;
// check dockRollAdjust and adjust if it does not match
if (dockMode != DockMode.IDLE && flightData.targetPart != null) {
/*float rollAngle = 0;
bool restrictedRoll = flightData.targetPart.getRingRestrictions(out rollAngle);
if(restrictedRoll && rollAngle != 0) {
if((Mathf.Abs(dockRollAdjust) % rollAngle) > 1 || Mathf.Abs (dockRollAdjust) < 1) {
dockRollAdjust = rollAngle;
print ("adjusted dockRollAdjust to " + dockRollAdjust.ToString("F1"));
}
}*/
}
}
public void getDockState(out DockState outDockState,
out DockAbort outDockAbort)
{
outDockState = dockState;
outDockAbort = dockAbort;
}
