Turn auto trim into a fly-by-wire flight computer

Pitch input becomes a rate command; the computer measures the pitching torque
without its own surfaces (airframe stability, thrust line, weight offset) and
cancels it with dynamic inversion, so relaxed-stability craft stay controllable.
Replaces trimAuthority with flightComputer* config values.

Co-Authored-By: Claude Sonnet 5.5 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_01YVREx3uo7nz1FEhzveWTUh

src/main/java/dev/lumareth/improvedAerodynamics/flight/AeroState.java

@@ -19,7 +19,7 @@ import java.util.concurrent.ConcurrentHashMap;
  * to; at the end of the tick that set is handed to the physics thread, which computes the forces from the ship's
  * motion in each substep until the next tick arrives.
  * <p>
- * The physics side also smooths pilot input, runs auto trim, and integrates damping implicitly so stiff damping
+ * The physics side also smooths pilot input, runs the flight computer (auto trim), and integrates damping implicitly so stiff damping
  * cannot overshoot.
  */
 public final class AeroState implements PhysicsBridge.Publishable {
@@ -55,6 +55,8 @@ public final class AeroState implements PhysicsBridge.Publishable {
         double roll;
         double trimIntegral;
         double trimOutput;
+        // Total pitch input the flight computer applied last substep, pilot and trim together
+        double computerOutput;
         // Torque (ship axes) one unit of pitch input on this node's pitch surfaces makes, measured every substep
         final Vector3d pitchResponse = new Vector3d();
         final Vector3d pitchResponseBuilding = new Vector3d();
@@ -79,8 +81,9 @@ public final class AeroState implements PhysicsBridge.Publishable {
             yaw = data.yaw();
             roll = data.roll();
             trimIntegral = data.autoTrim() ? data.storedTrim() : 0.0;
-            trimOutput = trimIntegral;
+            trimOutput = data.autoTrim() ? 0.0 : trimIntegral;
             savedTrim = trimIntegral;
+            computerOutput = pitch + trimOutput;
         }
     }
 
@@ -313,6 +316,8 @@ public final class AeroState implements PhysicsBridge.Publishable {
     private void updateNodes(Frame frame, Vector3dc worldVelocity, double altitude, Quaterniondc rotation, Matrix3dc inertia,
                              double[] w, double dt, boolean improved) {
         double step = Config.CONTROL_SMOOTHING.get() ? INPUT_RATE_PER_TICK * clock.weight() : Double.POSITIVE_INFINITY;
+        // Torque of the last substep, in ship axes, which the flight computer separates from its own surfaces' share
+        double[] torque = {wrench.totalTorque(0), wrench.totalTorque(1), wrench.totalTorque(2)};
         for (NodeData node : frame.nodes.values()) {
             NodeState state = nodeStates.get(node.key());
             if (state == null) {
@@ -323,7 +328,7 @@ public final class AeroState implements PhysicsBridge.Publishable {
             state.pitch = moveTowards(state.pitch, effective.pitch(), step);
             state.yaw = moveTowards(state.yaw, effective.yaw(), step);
             state.roll = moveTowards(state.roll, effective.roll(), step);
-            AutoTrim.update(state, effective, worldVelocity, altitude, rotation, inertia, w, dt, improved && wingStalled);
+            AutoTrim.update(state, effective, worldVelocity, altitude, rotation, inertia, w, torque, dt, improved && wingStalled);
         }
     }
 

src/main/java/dev/lumareth/improvedAerodynamics/flight/AutoTrim.java

@@ -7,19 +7,28 @@ import org.joml.Vector3d;
 import org.joml.Vector3dc;
 
 /**
- * Auto trim for the pitch control surfaces of one control node: while the pilot gives no pitch input it holds the
- * altitude the craft had when the pilot let go, cancelling whatever pitching moment the craft has (off-center weight,
- * thrust line, speed changes).
+ * The flight computer of one control node, switched on and off with the auto trim key. It is a fly-by-wire pitch
+ * system: the pilot's pitch input is a pitch rate command, not a surface deflection, and the computer moves the
+ * surfaces to make the craft follow it, whatever the airframe does on its own.
  * <p>
- * Three nested loops: altitude error sets a gentle climb rate, climb rate error sets a nose attitude (never more than
- * {@link #MAX_ATTITUDE} from level), attitude error sets a pitch rate, and a PI controller moves the trim to reach
- * that rate. Bounding the attitude matters: in Warium's physics the flight path follows the nose only slowly, so
- * steering the flight path directly with pitch rate keeps rotating the nose until the craft dives.
+ * Every physics substep it measures the pitching torque the craft makes with the pitch surfaces taken out
+ * ({@code external}: weight offset, thrust line, and above all the airframe's own stability or instability), and the
+ * torque one unit of pitch input makes ({@code response}). It then picks the input that makes the craft's total
+ * pitch torque equal the angular acceleration it wants, {@code rateGain * (rate command - pitch rate)} plus a small
+ * integral for what the model gets wrong. That cancels a craft that is statically unstable, or trimmed badly, as
+ * fast as the surfaces can move, so a jet built with relaxed stability can be flown like a stable one.
  * <p>
- * Attitude is measured from the ship's own up axis, so it assumes the craft is built level, as Warium craft are. The
- * trim's effect is measured every substep (torque per unit of pitch input), which gives the pitch axis and lets the
- * inner gains be expressed as angular acceleration, so the same tuning works for any craft, speed and altitude.
- * The trim holds still while the pilot pitches, when too slow, and when wings stall, so it never fights maneuvers.
+ * Without pilot input the rate command comes from three nested loops that hold the altitude the craft had when the
+ * pilot let go: altitude error sets a gentle climb rate, climb rate error sets a nose attitude (never more than
+ * {@link #MAX_ATTITUDE} from level), attitude error sets a pitch rate. Bounding the attitude matters: in Warium's
+ * physics the flight path follows the nose only slowly, so steering the flight path directly with pitch rate keeps
+ * rotating the nose until the craft dives.
+ * <p>
+ * Attitude is measured from the ship's own up axis, so it assumes the craft is built level, as Warium craft are.
+ * Torques are measured per unit of input, so the same tuning works for any craft, speed and altitude. Below the
+ * minimum speed the computer holds its output, the surfaces have no authority there anyway. Stalled wings cut the
+ * pilot's rate command and the altitude hold, so the computer only keeps the nose where it is. While cursor aim flies
+ * the node, that is in charge instead.
  */
 final class AutoTrim {
     // Climb rate (blocks/s) per block of altitude error, and its limit
@@ -32,12 +41,16 @@ final class AutoTrim {
     // Pitch rate (rad/s) per radian of attitude error, and its limit
     private static final double ATTITUDE_GAIN = 1.0;
     private static final double MAX_PITCH_RATE = 0.15;
-    // Angular acceleration per rad/s of pitch rate error, and integral gain
-    private static final double PROPORTIONAL_GAIN = 3.0;
-    private static final double INTEGRAL_GAIN = 1.5;
-    // Fastest the trim moves, in input per second
+    // Slow trim movement when the computer is switched off
     private static final double TRIM_RATE = 1.0;
     private static final double PILOT_DEADBAND = 0.01;
+    // The computer assumes the surfaces do this much of what was measured, so a response measured too high (stalled
+    // surfaces, clamped inputs) makes it command a little too much rather than too little, which stays stable
+    private static final double RESPONSE_MARGIN = 0.7;
+    // Pitch rate command while stalled, as a fraction of what the pilot asked
+    private static final double STALLED_COMMAND = 0.3;
+    // The integral may make up for this much rate error worth of acceleration
+    private static final double MAX_ERROR_FOR_INTEGRAL = 0.5;
 
     private AutoTrim() {
     }
@@ -49,57 +62,83 @@ final class AutoTrim {
     }
 
     static void update(AeroState.NodeState state, NodeData node, Vector3dc worldVelocity, double altitude,
-                       Quaterniondc rotation, Matrix3dc inertia, double[] w, double dt, boolean stalled) {
+                       Quaterniondc rotation, Matrix3dc inertia, double[] w, double[] torque, double dt, boolean stalled) {
         Targets targets = state.trimTargets;
+        boolean aimed = CursorAimRegistry.isActive(node.key());
         if (!node.autoTrim()) {
             state.trimIntegral = 0.0;
             state.trimOutput = AeroState.moveTowards(state.trimOutput, 0.0, TRIM_RATE * dt);
             state.savedTrim = 0.0;
+            state.computerOutput = state.pitch + state.trimOutput;
             targets.altitude = Double.NaN;
             targets.attitudeIntegral = 0.0;
             return;
         }
-        double authority = Config.TRIM_AUTHORITY.get();
         double response = state.pitchResponse.length();
         double speed = worldVelocity.length();
-        boolean hold = Math.abs(node.pitch()) > PILOT_DEADBAND || speed < Config.TRIM_MIN_SPEED.get() || stalled
-                || response < 1.0E-6;
-        if (hold || Double.isNaN(targets.altitude)) {
+        if (aimed || speed < Config.TRIM_MIN_SPEED.get() || response < 1.0E-6) {
+            // Cursor aim, no airflow or no pitch surfaces: the computer holds what it has
+            targets.altitude = Double.NaN;
+            targets.attitudeIntegral = 0.0;
+            state.computerOutput = state.pitch + state.trimOutput;
+            state.savedTrim = state.trimIntegral;
+            return;
+        }
+
+        // Pitch axis in ship axes, oriented so that positive rotation is what more pitch input does
+        Vector3d axis = state.pitchResponse.div(response, new Vector3d());
+        double axisInertia = axis.dot(inertia.transform(axis, new Vector3d()));
+        double pitchRate = axis.x * w[0] + axis.y * w[1] + axis.z * w[2];
+
+        // Does rotating about that axis raise the nose? Sign of the vertical part of axis x flight direction
+        Vector3d worldAxis = rotation.transform(axis, new Vector3d());
+        Vector3d direction = worldVelocity.normalize(new Vector3d());
+        double noseUp = Math.signum(worldAxis.cross(direction, new Vector3d()).y);
+        double attitude = attitude(axis, rotation);
+
+        double pilot = node.pitch();
+        boolean pilotActive = Math.abs(pilot) > PILOT_DEADBAND;
+        double rateCommand;
+        if (pilotActive || stalled || noseUp == 0.0 || Double.isNaN(attitude)) {
             // Hold whatever altitude the craft is at once the pilot lets go
             targets.altitude = altitude;
             targets.attitudeIntegral = 0.0;
-        }
-
-        double target = state.trimIntegral;
-        if (!hold) {
-            // Pitch axis in ship axes, oriented so that positive rotation is what more pitch input does
-            Vector3d axis = state.pitchResponse.div(response, new Vector3d());
-            double axisInertia = axis.dot(inertia.transform(axis, new Vector3d()));
-            double pitchRate = axis.x * w[0] + axis.y * w[1] + axis.z * w[2];
-
-            // Does rotating about that axis raise the nose? Sign of the vertical part of axis x flight direction
-            Vector3d worldAxis = rotation.transform(axis, new Vector3d());
-            Vector3d direction = worldVelocity.normalize(new Vector3d());
-            double noseUp = Math.signum(worldAxis.cross(direction, new Vector3d()).y);
-            double attitude = attitude(axis, rotation);
-            if (noseUp == 0.0 || Double.isNaN(attitude)) {
-                return;
+            rateCommand = pilotActive
+                    ? pilot * Math.toRadians(Config.FBW_STICK_RATE_DEGREES.get()) * (stalled ? STALLED_COMMAND : 1.0)
+                    : 0.0;
+        } else {
+            if (Double.isNaN(targets.altitude)) {
+                targets.altitude = altitude;
             }
-
             double climbTarget = clamp(ALTITUDE_GAIN * (targets.altitude - altitude), MAX_CLIMB_RATE);
             double climbError = climbTarget - worldVelocity.y();
             targets.attitudeIntegral = clamp(targets.attitudeIntegral + CLIMB_INTEGRAL_GAIN * climbError * dt, MAX_ATTITUDE);
             // Nose elevation wanted, turned into rotation about the axis
             double attitudeTarget = noseUp * clamp(targets.attitudeIntegral + CLIMB_GAIN * climbError, MAX_ATTITUDE);
-            double rateTarget = clamp(ATTITUDE_GAIN * (attitudeTarget - attitude), MAX_PITCH_RATE);
-
-            double error = rateTarget - pitchRate;
-            // Pitch input needed per rad/s² of angular acceleration
-            double gain = axisInertia / response;
-            state.trimIntegral = clamp(state.trimIntegral + INTEGRAL_GAIN * error * dt * gain, authority);
-            target = clamp(state.trimIntegral + PROPORTIONAL_GAIN * error * gain, authority);
+            rateCommand = clamp(ATTITUDE_GAIN * (attitudeTarget - attitude), MAX_PITCH_RATE);
         }
-        state.trimOutput = AeroState.moveTowards(state.trimOutput, target, TRIM_RATE * dt);
+
+        // Pitching torque about the axis the pitch surfaces are not making: total of the last substep minus what the
+        // input of that substep made
+        double total = axis.x * torque[0] + axis.y * torque[1] + axis.z * torque[2];
+        double external = total - response * state.computerOutput;
+
+        double authority = Config.FBW_AUTHORITY.get();
+        double rateError = rateCommand - pitchRate;
+        double acceleration = Config.FBW_RATE_GAIN.get() * rateError + state.trimIntegral;
+        double wanted = (axisInertia * acceleration - external) / (response * RESPONSE_MARGIN);
+        boolean saturated = Math.abs(wanted) > authority;
+        wanted = clamp(wanted, authority);
+        if (!saturated) {
+            // Integral of the acceleration, bounded so it cannot run away if the model is far off
+            double limit = Config.FBW_RATE_GAIN.get() * MAX_ERROR_FOR_INTEGRAL;
+            state.trimIntegral = clamp(state.trimIntegral + Config.FBW_INTEGRAL_GAIN.get() * rateError * dt, limit);
+        }
+
+        double applied = AeroState.moveTowards(state.computerOutput, wanted, Config.FBW_ACTUATOR_RATE.get() * dt);
+        state.computerOutput = applied;
+        // The surfaces get the pilot's smoothed input plus the trim, so the trim is what makes up the difference
+        state.trimOutput = applied - state.pitch;
         state.savedTrim = state.trimIntegral;
     }