selected VNAV works, managed - almost

Signed-off-by: Octol1ttle <l1ttleofficial@outlook.com>
Octol1ttle · Jun 20, 2024 · 7c9b216 · 7c9b216
1 parent 2b32702
commit 7c9b216
Show file tree

Hide file tree

Showing 11 changed files with 147 additions and 97 deletions.
diff --git a/src/main/java/ru/octol1ttle/flightassistant/alerts/impl/nav/MinimumsAlert.java b/src/main/java/ru/octol1ttle/flightassistant/alerts/impl/nav/MinimumsAlert.java
@@ -5,9 +5,9 @@
 import net.minecraft.text.Text;
 import org.jetbrains.annotations.NotNull;
 import ru.octol1ttle.flightassistant.DrawHelper;
-import ru.octol1ttle.flightassistant.alerts.impl.AlertSoundData;
 import ru.octol1ttle.flightassistant.alerts.api.BaseAlert;
 import ru.octol1ttle.flightassistant.alerts.api.IECAMAlert;
+import ru.octol1ttle.flightassistant.alerts.impl.AlertSoundData;
 import ru.octol1ttle.flightassistant.computers.impl.navigation.FlightPlanner;
 import ru.octol1ttle.flightassistant.config.FAConfig;
 import ru.octol1ttle.flightassistant.registries.ComputerRegistry;
@@ -38,7 +38,7 @@ public int render(TextRenderer textRenderer, DrawContext context, int x, int y,
             return 0;
         }
 
-        return DrawHelper.drawHighlightedText(textRenderer, context, Text.translatable("alerts.flightassistant.reached_minimums"), x, y,
+        return DrawHelper.drawHighlightedText(textRenderer, context, Text.translatable("alerts.flightassistant.gpws.reached_minimums"), x, y,
                 FAConfig.indicator().cautionColor, false);
     }
 }
diff --git a/.../java/ru/octol1ttle/flightassistant/alerts/impl/nav/gpws/UnsafeTerrainClearanceAlert.java b/.../java/ru/octol1ttle/flightassistant/alerts/impl/nav/gpws/UnsafeTerrainClearanceAlert.java
@@ -10,6 +10,7 @@
 import ru.octol1ttle.flightassistant.alerts.impl.AlertSoundData;
 import ru.octol1ttle.flightassistant.computers.impl.navigation.FlightPlanner;
 import ru.octol1ttle.flightassistant.computers.impl.safety.GroundProximityComputer;
+import ru.octol1ttle.flightassistant.computers.impl.safety.GroundProximityComputer.TerrainClearanceStatus;
 import ru.octol1ttle.flightassistant.config.FAConfig;
 import ru.octol1ttle.flightassistant.registries.ComputerRegistry;
 
@@ -19,7 +20,7 @@ public class UnsafeTerrainClearanceAlert extends BaseAlert implements ICenteredA
 
     @Override
     public boolean isTriggered() {
-        return gpws.landingClearanceStatus == GroundProximityComputer.LandingClearanceStatus.TOO_LOW;
+        return gpws.terrainClearanceStatus == TerrainClearanceStatus.TOO_LOW;
     }
 
     @Override

diff --git a/src/main/java/ru/octol1ttle/flightassistant/computers/impl/AirDataComputer.java b/src/main/java/ru/octol1ttle/flightassistant/computers/impl/AirDataComputer.java
@@ -114,7 +114,7 @@ private float computeGroundLevel() {
         if (!isCurrentChunkLoaded) {
             return groundLevel; // last known cache
         }
-        Vec3d ground = findGround(player().getBlockPos().mutableCopy());
+        Vec3d ground = findGround(position());
         return ground == null ? voidLevel() : (float) ground.getY();
     }
 
@@ -123,14 +123,14 @@ public boolean isGround(BlockPos pos) {
         return !block.isAir();
     }
 
-    public Vec3d findGround(BlockPos.Mutable from) {
+    public Vec3d findGround(Vec3d from) {
         if (!isChunkLoadedAt(from)) {
             return null;
         }
 
         BlockHitResult result = world().raycast(new RaycastContext(
-                position().offset(Direction.UP, 0.5),
-                position().withAxis(Direction.Axis.Y, voidLevel()),
+                from.offset(Direction.UP, 0.5),
+                from.withAxis(Direction.Axis.Y, voidLevel()),
                 RaycastContext.ShapeType.COLLIDER,
                 RaycastContext.FluidHandling.ANY,
                 player()
@@ -197,13 +197,12 @@ public World world() {
         return player().getWorld();
     }
 
-    public boolean isChunkLoadedAt(BlockPos pos){
+    public boolean isChunkLoadedAt(Vec3d pos) {
         return world().getChunkManager().isChunkLoaded(ChunkSectionPos.getSectionCoord(pos.getX()), ChunkSectionPos.getSectionCoord(pos.getZ()));
     }
 
-    private boolean isCurrentChunkLoaded(){
-        BlockPos pos = player().getBlockPos();
-        return isChunkLoadedAt(pos);
+    private boolean isCurrentChunkLoaded() {
+        return isChunkLoadedAt(position());
     }
 
     public static float validate(float f, float bounds) {

diff --git a/src/main/java/ru/octol1ttle/flightassistant/computers/impl/FlightPhaseComputer.java b/src/main/java/ru/octol1ttle/flightassistant/computers/impl/FlightPhaseComputer.java
@@ -3,80 +3,72 @@
 import net.minecraft.text.Text;
 import ru.octol1ttle.flightassistant.computers.api.ITickableComputer;
 import ru.octol1ttle.flightassistant.computers.impl.autoflight.AutoFlightController;
+import ru.octol1ttle.flightassistant.computers.impl.autoflight.ThrustController;
 import ru.octol1ttle.flightassistant.computers.impl.navigation.FlightPlanner;
 import ru.octol1ttle.flightassistant.registries.ComputerRegistry;
 
 public class FlightPhaseComputer implements ITickableComputer {
     private final AirDataComputer data = ComputerRegistry.resolve(AirDataComputer.class);
     private final AutoFlightController autoflight = ComputerRegistry.resolve(AutoFlightController.class);
     private final FlightPlanner plan = ComputerRegistry.resolve(FlightPlanner.class);
+    private final ThrustController thrust = ComputerRegistry.resolve(ThrustController.class);
     private Phase phase = Phase.UNKNOWN;
 
-    @SuppressWarnings("DataFlowIssue")
+
     @Override
     public void tick() {
-        if (data.player().isOnGround()) {
-            phase = Phase.ON_GROUND;
-        }
+        phase = computePhase();
+    }
 
-        if (!data.isFlying()) {
-            return;
+    private Phase computePhase() {
+        if (data.player().isOnGround() || !data.isFlying()) {
+            return Phase.ON_GROUND;
         }
 
         if (phase == Phase.ON_GROUND) {
-            phase = Phase.TAKEOFF;
+            return Phase.TAKEOFF;
         }
 
         if (phase == Phase.TAKEOFF && data.heightAboveGround() <= 15.0f) {
-            return;
+            return Phase.TAKEOFF;
         }
 
         Integer cruiseAltitude = plan.getCruiseAltitude();
         Integer targetAltitude = autoflight.getTargetAltitude();
         if (cruiseAltitude == null || targetAltitude == null) {
-            phase = Phase.UNKNOWN;
-            return;
+            return Phase.UNKNOWN;
         }
 
-        if (!isNearDestination()) {
-            if (data.altitude() - targetAltitude <= 5.0f) {
-                phase = Phase.CLIMB;
-            } else {
-                phase = Phase.DESCENT;
-            }
-
+        if (!plan.isOnApproach() && phase != Phase.GO_AROUND) {
             if (targetAltitude.equals(cruiseAltitude) && Math.abs(cruiseAltitude - data.altitude()) <= 5.0f) {
-                phase = Phase.CRUISE;
+                return Phase.CRUISE;
+            }
+            if (data.altitude() - targetAltitude <= 5.0f) {
+                return Phase.CLIMB;
             }
+            return Phase.DESCENT;
         }
 
         if (phase == Phase.GO_AROUND) {
-            if (plan.getDistanceToWaypoint() > 150.0f) {
-                phase = Phase.APPROACH;
+            Double distance = plan.getDistanceToWaypoint();
+            if (distance != null && distance > 150.0f && thrust.getTargetThrust() < 0.99f) {
+                return Phase.APPROACH;
             }
         } else {
-            if (plan.isOnApproach()) {
-                phase = Phase.APPROACH;
+            if (thrust.getTargetThrust() >= 0.99f) {
+                return Phase.GO_AROUND;
             }
 
-            if (phase == Phase.APPROACH && plan.autolandAllowed) {
-                phase = Phase.LAND;
-            }
+            return plan.autolandAllowed ? Phase.LAND : Phase.APPROACH;
         }
+
+        return phase;
     }
 
     public Phase get() {
         return this.phase;
     }
 
-    private boolean isAboutToLand() {
-        return phase == Phase.APPROACH || phase == Phase.LAND;
-    }
-
-    public boolean isNearDestination() {
-        return isAboutToLand() || phase == Phase.GO_AROUND;
-    }
-
     @Override
     public String getId() {
         return "flight_phase";

diff --git a/src/main/java/ru/octol1ttle/flightassistant/computers/impl/autoflight/AutopilotComputer.java b/src/main/java/ru/octol1ttle/flightassistant/computers/impl/autoflight/AutopilotComputer.java
@@ -17,6 +17,7 @@
 import ru.octol1ttle.flightassistant.computers.impl.AirDataComputer;
 import ru.octol1ttle.flightassistant.computers.impl.FlightPhaseComputer;
 import ru.octol1ttle.flightassistant.computers.impl.FlightPhaseComputer.Phase;
+import ru.octol1ttle.flightassistant.computers.impl.TimeComputer;
 import ru.octol1ttle.flightassistant.computers.impl.navigation.FlightPlanner;
 import ru.octol1ttle.flightassistant.registries.ComputerRegistry;
 
@@ -29,6 +30,7 @@ public class AutopilotComputer implements ITickableComputer, IAutopilotProvider,
     private final FlightPhaseComputer phase = ComputerRegistry.resolve(FlightPhaseComputer.class);
     private final FlightPlanner plan = ComputerRegistry.resolve(FlightPlanner.class);
     private final ThrustController thrust = ComputerRegistry.resolve(ThrustController.class);
+    private final TimeComputer time = ComputerRegistry.resolve(TimeComputer.class);
 
     public Text verticalMode;
     public Text lateralMode;
@@ -47,17 +49,23 @@ public void tick() {
         setTargetHeading(null, Text.empty());
 
         if (phase.get() == Phase.ON_GROUND) {
+            togaHeading = null;
             return;
         }
 
         if (phase.get() == Phase.TAKEOFF
-                || phase.get() == Phase.GO_AROUND && data.heightAboveGround() < 15.0f) {
+                || phase.get() == Phase.GO_AROUND) {
             if (togaHeading == null) {
                 togaHeading = data.heading();
             }
 
+            boolean useReducedThrust = thrust.getThrustHandler().isFireworkLike();
             setTargetThrust(1.0f, Text.translatable("mode.flightassistant.thrust.toga"));
-            setTargetPitch(55.0f, Text.translatable("mode.flightassistant.vert.climb.optimum"));
+            if (data.heightAboveGround() < 15.0f) {
+                setTargetPitch(useReducedThrust ? 55.0f : 35.0f, Text.translatable("mode.flightassistant.vert.climb.optimum"));
+            } else {
+                setTargetPitch(useReducedThrust ? 10.0f : 5.0f, Text.translatable("mode.flightassistant.vert.go_around"));
+            }
 
             String lat = phase.get() == Phase.GO_AROUND ? ".go_around" : ".takeoff";
             setTargetHeading(togaHeading, Text.translatable("mode.flightassistant.lat" + lat, togaHeading.intValue()));
@@ -96,27 +104,50 @@ private void tickSelectedAltitude(float diff, float speedAdjustment) {
         float abs = Math.abs(diff);
 
         float pitch;
-        // TODO: cant level off on the first try
         if (diff > 0) {
             if (useReducedThrust) {
                 setTargetThrust(THRUST_CLIMB_REDUCED, Text.translatable("mode.flightassistant.thrust.climb_reduced"));
-                pitch = 45.0f - 45.0f * (Math.max(40.0f - Math.max(abs - 5, 0), 0.0f) / 40.0f) + 10.0f;
+                pitch = 55.0f;
+                if (abs <= 200) {
+                    pitch -= 30.0f * MathHelper.clamp((200.0f - abs) / 100.0f, 0.0f, 1.0f);
+                }
+                if (abs <= 100) {
+                    pitch -= 15.0f * MathHelper.clamp((100.0f - abs) / 95.0f, 0.0f, 1.0f);
+                }
             } else {
                 setTargetThrust(THRUST_CLIMB, Text.translatable("mode.flightassistant.thrust.climb"));
-                pitch = 45.0f - 45.0f * (Math.max(50.0f - abs, 0.0f) / 50.0f) + 5.0f + speedAdjustment;
+                pitch = 35.0f + speedAdjustment;
+                if (abs <= 200) {
+                    pitch -= 25.0f * MathHelper.clamp((200.0f - abs) / 100.0f, 0.0f, 1.0f);
+                }
+                if (abs <= 100) {
+                    pitch -= (5.0f + speedAdjustment) * MathHelper.clamp((100.0f - abs) / 100.0f, 0.0f, 1.0f);
+                }
             }
             setTargetPitch(pitch, Text.translatable("mode.flightassistant.vert.climb.selected", autoflight.selectedAltitude));
         } else {
             setTargetThrust(0.0f, Text.translatable("mode.flightassistant.thrust.idle"));
             if (useReducedThrust) {
-                pitch = -25.0f + 25.0f * (Math.max(40.0f - Math.max(abs - 10, 0), 0.0f) / 40.0f);
+                pitch = -35.0f;
+                if (abs <= 100) {
+                    pitch += 15.0f * MathHelper.clamp((100.0f - abs) / 50.0f, 0.0f, 1.0f);
+                }
+                if (abs <= 50) {
+                    pitch += 10.0f * MathHelper.clamp((50.0f - abs) / 40.0f, 0.0f, 1.0f);
+                }
             } else {
-                pitch = -35.0f + 35.0f * (Math.max(50.0f - abs, 0.0f) / 50.0f) + 5.0f + speedAdjustment;
+                pitch = -35.0f + speedAdjustment;
+                if (abs <= 100) {
+                    pitch += 20.0f * MathHelper.clamp((100.0f - abs) / 50.0f, 0.0f, 1.0f);
+                }
+                if (abs <= 50) {
+                    pitch += (20.0f + speedAdjustment) * MathHelper.clamp((50.0f - abs) / 50.0f, 0.0f, 1.0f);
+                }
             }
             setTargetPitch(pitch, Text.translatable("mode.flightassistant.vert.descend.selected", autoflight.selectedAltitude));
         }
 
-        if (abs <= 5) {
+        if (abs <= 10.0f) {
             setTargetThrust(THRUST_CLIMB, Text.translatable("mode.flightassistant.thrust.climb"));
             setTargetPitch(pitch, Text.translatable("mode.flightassistant.vert.hold.selected", autoflight.selectedAltitude));
         }
@@ -127,26 +158,33 @@ private void tickManagedAltitude(Integer targetAltitude, float speedAdjustment,
         float diff = targetAltitude - data.altitude();
         double distance = Vector2d.distance(data.position().x, data.position().z, pos.x, pos.y);
         float degrees = FAMathHelper.toDegrees(MathHelper.atan2(diff, distance));
+
+        float pitch;
         if (diff > 0) {
-            float pitch;
             if (useReducedThrust) {
                 setTargetThrust(THRUST_CLIMB_REDUCED, Text.translatable("mode.flightassistant.thrust.climb_reduced"));
-                pitch = MathHelper.clamp(degrees, 7.5f, PitchController.CLIMB_PITCH);
+                pitch = MathHelper.clamp(degrees, 10.0f, PitchController.CLIMB_PITCH);
             } else {
                 setTargetThrust(THRUST_CLIMB, Text.translatable("mode.flightassistant.thrust.climb"));
-                pitch = Math.min(degrees, PitchController.CLIMB_PITCH) + speedAdjustment;
+                pitch = MathHelper.clamp(degrees + speedAdjustment, 5.0f, 35.0f);
             }
             setTargetPitch(pitch, Text.translatable("mode.flightassistant.vert.climb.managed", targetAltitude));
         } else {
             setTargetThrust(0.0f, Text.translatable("mode.flightassistant.thrust.idle"));
-            float pitch;
             if (useReducedThrust) {
                 pitch = MathHelper.clamp(degrees, PitchController.DESCEND_PITCH, PitchController.GLIDE_PITCH);
             } else {
-                pitch = MathHelper.clamp(degrees, PitchController.DESCEND_PITCH, 0.0f);
+                pitch = MathHelper.clamp(degrees + 5.0f + speedAdjustment, PitchController.DESCEND_PITCH, 5.0f);
             }
             setTargetPitch(pitch, Text.translatable("mode.flightassistant.vert.descend.managed", targetAltitude));
         }
+
+        if (Math.abs(diff) <= 10.0f) {
+            setTargetThrust(THRUST_CLIMB, Text.translatable("mode.flightassistant.thrust.climb"));
+            setTargetPitch(pitch, targetAltitude.equals(plan.getCruiseAltitude())
+                    ? Text.translatable("mode.flightassistant.vert.hold.cruise", targetAltitude)
+                    : Text.translatable("mode.flightassistant.vert.hold.selected", targetAltitude));
+        }
     }
 
     private void tickSpeed(Integer targetSpeed) {
@@ -156,11 +194,11 @@ private void tickSpeed(Integer targetSpeed) {
 
         float diff = targetSpeed - data.speed();
 
-        float thr = THRUST_CLIMB;
+        float thr = MathHelper.clamp(thrust.getTargetThrust() + diff * 0.01f * time.deltaTime, -0.1f, THRUST_CLIMB);
         if (thrust.getThrustHandler().isFireworkLike()) {
             thr = targetSpeed / FireworkController.FIREWORK_SPEED;
         }
-        setTargetThrust(thr * Math.min(diff * 0.1f, 1.0f), Text.translatable("mode.flightassistant.thrust.speed.selected", targetSpeed));
+        setTargetThrust(thr, Text.translatable("mode.flightassistant.thrust.speed.selected", targetSpeed));
     }
 
     private void tickLateral() {

diff --git a/src/main/java/ru/octol1ttle/flightassistant/computers/impl/navigation/FlightPlanner.java b/src/main/java/ru/octol1ttle/flightassistant/computers/impl/navigation/FlightPlanner.java
@@ -4,15 +4,14 @@
 import net.minecraft.text.Text;
 import net.minecraft.util.hit.BlockHitResult;
 import net.minecraft.util.hit.HitResult;
-import net.minecraft.util.math.BlockPos;
 import net.minecraft.util.math.MathHelper;
 import net.minecraft.util.math.Vec3d;
 import net.minecraft.world.RaycastContext;
 import org.jetbrains.annotations.Nullable;
 import org.joml.Vector2d;
 import ru.octol1ttle.flightassistant.FAMathHelper;
-import ru.octol1ttle.flightassistant.computers.impl.AirDataComputer;
 import ru.octol1ttle.flightassistant.computers.api.ITickableComputer;
+import ru.octol1ttle.flightassistant.computers.impl.AirDataComputer;
 import ru.octol1ttle.flightassistant.computers.impl.autoflight.PitchController;
 import ru.octol1ttle.flightassistant.registries.ComputerRegistry;
 
@@ -56,7 +55,7 @@ private boolean tickLanding(Vector2d target) {
             return false;
         }
 
-        Vec3d landPos = data.findGround(new BlockPos.Mutable(target.x, data.world().getTopY(), target.y));
+        Vec3d landPos = data.findGround(new Vec3d(target.x, data.world().getTopY(), target.y));
         if (landPos == null) {
             return false;
         }