public void EnableVertSpeedHold()
{
if (!enabled)
{
return;
}
VertSpeedHoldEnabled = true;
VertSpeedPIDController.Reset();
}
public void EnableAltitudeHold()
{
if (!enabled)
{
return;
}
AltitudeHoldEnabled = true;
if (VertSpeedHoldEnabled)
{
VertSpeedPIDController.Reset();
}
else
{
EnableVertSpeedHold();
}
}
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);
}
}
