private void hdgModeChanged(HrztMode newMode, bool active, bool setTarget = true)
{
hdgBankCtrl.skipDerivative = true;
bankToYawCtrl.skipDerivative = true;
aileronCtrl.skipDerivative = true;
rudderCtrl.skipDerivative = true;
xtrkSpeedCtrl.skipDerivative = true;
if (!active)
{
InputLockManager.RemoveControlLock(yawLockID);
yawLockEngaged = false;
hdgBankCtrl.Clear();
bankToYawCtrl.Clear();
aileronCtrl.Clear();
rudderCtrl.Clear();
xtrkSpeedCtrl.Clear();
}
else
{
if (!yawLockEngaged)
{
InputLockManager.SetControlLock(ControlTypes.YAW, yawLockID);
yawLockEngaged = true;
}
APController ctrl;
switch (newMode)
{
case HrztMode.Course:
// we do not use hdgBankCtrl in course mode, but nice to make
// its set point match the current course I guess.
if (validFlightPlan())
{
// go direct to waypoint to avoid huge banks to get on course
flightPlan.DirectToWaypoint(this, flightPlan.next, vessel, vesselData);
hdgBankCtrl.SetPoint = flightPlan.CourseHeading();
var vC = flightPlan.courseStatus.vC;
// try to avoid super sharp course corrections
if (Double.IsNaN(vC))
{
// no hope, we have no current cross track velocity
xtrkSpeedCtrl.SetPoint = 0;
}
else
{
xtrkSpeedCtrl.SetPoint = vC;
xtrkSpeedCtrl.BumplessSetPoint = 0;
}
}
else
{
hdgBankCtrl.SetPoint = 0;
xtrkSpeedCtrl.SetPoint = 0;
}
break;
case HrztMode.Heading:
if (setTarget)
hdgBankCtrl.SetPoint = vesselData.heading;
targetHeading = hdgBankCtrl.SetPoint;
break;
case HrztMode.Bank:
ctrl = aileronCtrl;
if (setTarget)
ctrl.SetPoint = vesselData.bank;
targetHeading = ctrl.SetPoint;
break;
}
}
HrztActive = active;
CurrentHrztMode = newMode;
}
private void hdgModeChanged(HrztMode newMode, bool active, bool setTarget = true)
{
AsstList.HdgBank.GetAsst(this).skipDerivative = true;
AsstList.BankToYaw.GetAsst(this).skipDerivative = true;
AsstList.Aileron.GetAsst(this).skipDerivative = true;
AsstList.Rudder.GetAsst(this).skipDerivative = true;
if (!active)
{
InputLockManager.RemoveControlLock(yawLockID);
yawLockEngaged = false;
stopHdgShift = true;
headingEdit = true;
AsstList.HdgBank.GetAsst(this).Clear();
AsstList.BankToYaw.GetAsst(this).Clear();
AsstList.Aileron.GetAsst(this).Clear();
AsstList.Rudder.GetAsst(this).Clear();
}
else
{
if (!yawLockEngaged)
{
InputLockManager.SetControlLock(ControlTypes.YAW, yawLockID);
yawLockEngaged = true;
}
bPause = false;
switch (newMode)
{
case HrztMode.HeadingNum:
if (setTarget)
AsstList.HdgBank.GetAsst(this).UpdateSetpoint(vesModule.vesselData.heading);
targetHeading = AsstList.HdgBank.GetAsst(this).target_setpoint.ToString("0.00");
break;
case HrztMode.Heading:
if (setTarget)
StartCoroutine(shiftHeadingTarget(vesModule.vesselData.heading));
break;
case HrztMode.Bank:
if (setTarget)
AsstList.Aileron.GetAsst(this).UpdateSetpoint(vesModule.vesselData.bank);
targetHeading = AsstList.Aileron.GetAsst(this).target_setpoint.ToString("0.00");
break;
}
}
HrztActive = active;
CurrentHrztMode = newMode;
}
/// <summary>
///
/// </summary>
/// <param name="active">Sets the state of the heading control system. True = enabled</param>
/// <param name="setTarget">Whether to update the target value</param>
/// <param name="mode">Mode to use. Heading, bank, etc.</param>
/// <param name="target">The new target value</param>
public void SetHrzt(bool active, bool setTarget, HrztMode mode, double target)
{
if (setTarget)
{
switch (mode)
{
case HrztMode.Bank:
AsstList.Aileron.GetAsst(this).UpdateSetpoint(target.headingClamp(180), true, vesModule.vesselData.bank);
break;
case HrztMode.Heading:
StartCoroutine(shiftHeadingTarget(target.headingClamp(360)));
break;
case HrztMode.HeadingNum:
AsstList.HdgBank.GetAsst(this).UpdateSetpoint(target.headingClamp(360), true, vesModule.vesselData.heading);
break;
}
}
hdgModeChanged(mode, active, !setTarget);
}
public void SetHrzt(bool active, bool setTarget, HrztMode mode, double target)
{
if (active && setTarget)
{
switch (mode)
{
case HrztMode.Bank:
AsstList.Aileron.GetAsst(this).BumplessSetPoint = target;
break;
case HrztMode.Heading:
StartCoroutine(shiftHeadingTarget(target.headingClamp(360)));
break;
case HrztMode.HeadingNum:
AsstList.HdgBank.GetAsst(this).SetPoint = target.headingClamp(360);
break;
}
}
hdgModeChanged(mode, active, !setTarget);
}