- * 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); } } diff --git a/src/main/java/dev/lumareth/improvedAerodynamics/flight/AutoTrim.java b/src/main/java/dev/lumareth/improvedAerodynamics/flight/AutoTrim.java index 5b6a523..ee5830e 100644 --- a/src/main/java/dev/lumareth/improvedAerodynamics/flight/AutoTrim.java +++ b/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. *
- * 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. *
- * 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. + *
+ * 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; } diff --git a/src/main/resources/assets/improved_aerodynamics/lang/de_de.json b/src/main/resources/assets/improved_aerodynamics/lang/de_de.json index 202f1db..23606b1 100644 --- a/src/main/resources/assets/improved_aerodynamics/lang/de_de.json +++ b/src/main/resources/assets/improved_aerodynamics/lang/de_de.json @@ -1,8 +1,8 @@ { "key.categories.improved_aerodynamics": "Improved Aerodynamics", "key.improved_aerodynamics.toggle_auto_trim": "Automatische Trimmung umschalten", - "message.improved_aerodynamics.auto_trim.on": "Automatische Trimmung an", - "message.improved_aerodynamics.auto_trim.off": "Automatische Trimmung aus", + "message.improved_aerodynamics.auto_trim.on": "Flugcomputer an (automatische Trimmung)", + "message.improved_aerodynamics.auto_trim.off": "Flugcomputer aus (automatische Trimmung)", "message.improved_aerodynamics.auto_trim.unavailable": "Automatische Trimmung an, sie braucht aber physicsRateAerodynamics in der Serverkonfiguration", "message.improved_aerodynamics.auto_trim.no_seat": "Setz dich in einen Warium-Steuersitz, um die automatische Trimmung zu nutzen", "message.improved_aerodynamics.auto_trim.no_node": "Dieser Sitz ist mit keinem Vehicle Control Node verbunden", diff --git a/src/main/resources/assets/improved_aerodynamics/lang/en_us.json b/src/main/resources/assets/improved_aerodynamics/lang/en_us.json index 9d4d32d..a1f1af9 100644 --- a/src/main/resources/assets/improved_aerodynamics/lang/en_us.json +++ b/src/main/resources/assets/improved_aerodynamics/lang/en_us.json @@ -1,8 +1,8 @@ { "key.categories.improved_aerodynamics": "Improved Aerodynamics", "key.improved_aerodynamics.toggle_auto_trim": "Toggle auto trim", - "message.improved_aerodynamics.auto_trim.on": "Auto trim on", - "message.improved_aerodynamics.auto_trim.off": "Auto trim off", + "message.improved_aerodynamics.auto_trim.on": "Flight computer on (auto trim)", + "message.improved_aerodynamics.auto_trim.off": "Flight computer off (auto trim)", "message.improved_aerodynamics.auto_trim.unavailable": "Auto trim on, but it needs physicsRateAerodynamics enabled in the server config", "message.improved_aerodynamics.auto_trim.no_seat": "Sit in a Warium control seat to use auto trim", "message.improved_aerodynamics.auto_trim.no_node": "This seat is not linked to a vehicle control node",