Skip to content

Commit

Permalink
multiple Autopilot / Autoland fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@felixscheffer
felixscheffer committed Aug 11, 2020
1 parent 2d0ad46 commit 89a1dcc
Show file tree
Hide file tree
Showing 4 changed files with 41 additions and 35 deletions.
1 change: 1 addition & 0 deletions MechJeb2/MechJeb2.csproj
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@
<Compile Include="Maneuver\TimeSelector.cs" />
<Compile Include="Maneuver\TransferCalculator.cs" />
<Compile Include="MathExtensions.cs" />
<Compile Include="Matrix3x3d.cs" />
<Compile Include="MechJebAR202.cs" />
<Compile Include="MechJebCore.cs" />
<Compile Include="MechJebModuleAirplaneAutopilot.cs" />
Expand Down
68 changes: 35 additions & 33 deletions MechJeb2/MechJebModuleAirplaneAutopilot.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@ public class MechJebModuleAirplaneAutopilot : ComputerModule
public EditableDouble AccKp = 0.5, AccKi = 0.5, AccKd = 0.005;

[Persistent(pass = (int)Pass.Local)]
public EditableDouble PitKp = 2.0, PitKi = 0.6, PitKd = 0.65;
public EditableDouble PitKp = 2.0, PitKi = 1.0, PitKd = 0.005;

[Persistent (pass = (int)Pass.Local)]
public EditableDouble RolKp = 0.5, RolKi = 0.02, RolKd = 0.5;
Expand Down Expand Up @@ -92,6 +92,7 @@ public void EnableSpeedHold ()
{
if (!enabled)
return;

_spd = vesselState.speedSurface;
SpeedHoldEnabled = true;
core.thrust.users.Add (this);
Expand All @@ -102,6 +103,7 @@ public void DisableSpeedHold ()
{
if (!enabled)
return;

SpeedHoldEnabled = false;
core.thrust.users.Remove (this);
}
Expand All @@ -110,6 +112,7 @@ public void EnableVertSpeedHold ()
{
if (!enabled)
return;

VertSpeedHoldEnabled = true;
PitchPIDController.Reset();
initPitchController = true;
Expand All @@ -119,13 +122,15 @@ public void DisableVertSpeedHold ()
{
if (!enabled)
return;

VertSpeedHoldEnabled = false;
}

public void EnableAltitudeHold ()
{
if (!enabled)
return;

AltitudeHoldEnabled = true;
if (VertSpeedHoldEnabled) {
PitchPIDController.Reset();
Expand All @@ -138,26 +143,27 @@ public void DisableAltitudeHold ()
{
if (!enabled)
return;

AltitudeHoldEnabled = false;
DisableVertSpeedHold ();
}

private double convertAltitudeToVerticalSpeed (double deltaAltitude)
private double convertAltitudeToVerticalSpeed (double deltaAltitude, double reference = 5)
{
double reference = 5;
if (reference < 2)
reference = 2;

if (deltaAltitude > reference || deltaAltitude < -reference)
return deltaAltitude / reference;
else if (deltaAltitude > 0.1)
return deltaAltitude * deltaAltitude / (reference * reference);
else if (deltaAltitude > -0.1)
return 0;
return Math.Max(0.05, deltaAltitude * deltaAltitude / (reference * reference));
else if (deltaAltitude < -0.1)
return Math.Min(-0.05, -deltaAltitude * deltaAltitude / (reference * reference));
else
return -deltaAltitude * deltaAltitude / (reference * reference);
return 0;
}

private void UpdatePID()
private void UpdatePID ()
{
AccelerationPIDController.Kp = AccKp;
AccelerationPIDController.Ki = AccKi;
Expand All @@ -173,40 +179,30 @@ private void UpdatePID()
YawPIDController.Kd = YawKd;
}

private void SynchronizePIDsWithSAS(FlightCtrlState s)
private void SynchronizePIDs (FlightCtrlState s)
{
var isSASenabled = part.vessel.ActionGroups[KSPActionGroup.SAS];

if (isSASenabled)
// synchronize PID controllers
if (initPitchController)
{
// synchronize PID controllers with SAS
if (initPitchController && VertSpeedHoldEnabled)
{
PitchPIDController.intAccum = s.pitch * 100 / PitchPIDController.Ki;
}

if (initRollController && RollHoldEnabled)
{
RollPIDController.intAccum = s.roll * 100 / RollPIDController.Ki;
}
PitchPIDController.intAccum = s.pitch * 100 / PitchPIDController.Ki;
}

if (initYawController && HeadingHoldEnabled)
{
YawPIDController.intAccum = s.yaw * 100 / YawPIDController.Ki;
}
if (initRollController)
{
RollPIDController.intAccum = s.roll * 100 / RollPIDController.Ki;
}

if (VertSpeedHoldEnabled && RollHoldEnabled && HeadingHoldEnabled)
{
part.vessel.ActionGroups.SetGroup(KSPActionGroup.SAS, false);
}
if (initYawController)
{
YawPIDController.intAccum = s.yaw * 100 / YawPIDController.Ki;
}

initPitchController = false;
initRollController = false;
initYawController = false;
}

private double computeYaw()
private double computeYaw ()
{
// probably not perfect, especially when the aircraft is turning
double path = vesselState.HeadingFromDirection(vesselState.surfaceVelocity);
Expand All @@ -219,7 +215,7 @@ private double computeYaw()
public override void Drive (FlightCtrlState s)
{
UpdatePID();
SynchronizePIDsWithSAS(s);
SynchronizePIDs(s);

//SpeedHold (set AccelerationTarget automatically to hold speed)
if (SpeedHoldEnabled) {
Expand All @@ -240,7 +236,13 @@ public override void Drive (FlightCtrlState s)

//AltitudeHold (set VertSpeed automatically to hold altitude)
if (AltitudeHoldEnabled) {
RealVertSpeedTarget = convertAltitudeToVerticalSpeed(AltitudeTarget - vesselState.altitudeASL);
// NOTE:
// There is about 0.4 between altitudeASL and the altitude that is displayed in KSP in the top center
// i.e. 3000.4m (altitudeASL) equals 3000m ingame.
// maybe there is something wrong with the way we calculate altitudeASL. Idk;
double deltaAltitude = AltitudeTarget + 0.4 - vesselState.altitudeASL;

RealVertSpeedTarget = convertAltitudeToVerticalSpeed(deltaAltitude, 10);
RealVertSpeedTarget = UtilMath.Clamp(RealVertSpeedTarget, -VertSpeedTarget, VertSpeedTarget);
}
else
Expand Down
5 changes: 4 additions & 1 deletion MechJeb2/MechJebModuleAirplaneGuidance.cs
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
using System;
using System.Linq;
using UnityEngine;
using KSP.Localization;
Expand Down Expand Up @@ -100,6 +101,7 @@ protected override void WindowGUI (int windowID)
if (GUILayout.Button("-", GUILayout.Width(18))) { VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true; }
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true),GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18))) { VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true; }
VertSpeedTargettmp = Math.Max(0, VertSpeedTargettmp);
GUILayout.Label ("m/s", GUILayout.ExpandWidth (true));
if (change || GUILayout.Button (Localizer.Format("#MechJeb_Aircraftauto_btnset2"), autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen)) {
autopilot.VertSpeedTarget = VertSpeedTargettmp;
Expand All @@ -112,7 +114,8 @@ protected override void WindowGUI (int windowID)
if (GUILayout.Button("-", GUILayout.Width(18))) { VertSpeedTargettmp.val -= (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true; }
VertSpeedTargettmp.text = GUILayout.TextField(VertSpeedTargettmp.text, GUILayout.ExpandWidth(true), GUILayout.Width(60));
if (GUILayout.Button("+", GUILayout.Width(18))) { VertSpeedTargettmp.val += (GameSettings.MODIFIER_KEY.GetKey() ? 5 : 1); change = true; }
GUILayout.Label("°", GUILayout.ExpandWidth(true));
VertSpeedTargettmp = Math.Max(0, VertSpeedTargettmp);
GUILayout.Label("m/s", GUILayout.ExpandWidth(true));
if (change || GUILayout.Button(Localizer.Format("#MechJeb_Aircraftauto_btnset6"), autopilot.VertSpeedTarget == VertSpeedTargettmp ? btWhite : btGreen)) {
autopilot.VertSpeedTarget = VertSpeedTargettmp;
}
Expand Down
2 changes: 1 addition & 1 deletion MechJeb2/MechJebModuleSpaceplaneAutopilot.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -340,7 +340,7 @@ public double GetAutolandTargetVerticalSpeed(Vector3d vectorToWaypoint)
Debug.Assert(vertSpeed < 0);

double latDist = LateralDistance(vesselState.CoM, runway.GetVectorToTouchdown());
Vector3d vectorToCorrectPointOnGlideslope = runway.GetPointOnGlideslope(glideslope, LateralDistance(vesselState.CoM, runway.GetVectorToTouchdown()));
Vector3d vectorToCorrectPointOnGlideslope = runway.GetPointOnGlideslope(glideslope, latDist);
double desiredAlt = GetAutolandTargetAltitude(vectorToCorrectPointOnGlideslope);
double deltaToCorrectAlt = desiredAlt - vesselState.altitudeASL;
double expPerMeter = (Math.Log(maximumVerticalSpeedCorrection + 1) - Math.Log(1)) / desiredAlt;
Expand Down

0 comments on commit 89a1dcc

Please sign in to comment.