richard
4b7fb0e1e8
- Route Warium and Futurism force queues through smoothed per-substep delivery - Evaluate wing, stabilizer, control surface and air brake forces every physics substep - Add optional improved flight model with angle-of-attack lift and stalls - Add auto trim toggled by a client key and a server packet - Add config options, translations, and mixin plugin version gating - Bump mod version to 0.2.0 and bundle Futurism 0.0.4 as a compile-only dependency
75 lines
3.8 KiB
Java
75 lines
3.8 KiB
Java
package dev.lumareth.improvedAerodynamics.flight;
|
|
|
|
import dev.lumareth.improvedAerodynamics.Config;
|
|
import org.joml.Matrix3dc;
|
|
import org.joml.Quaterniondc;
|
|
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 adds a
|
|
* pitch offset that brings the flight path back to level and then holds it there, cancelling whatever pitching
|
|
* moment the craft has (off-center weight, thrust line, speed changes).
|
|
* <p>
|
|
* Two loops: the flight path angle sets a gentle target pitch rate, and a PI controller moves the trim to reach it.
|
|
* The trim's effect is measured every substep (torque per unit of pitch input), which gives the pitch axis and lets
|
|
* the 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.
|
|
*/
|
|
final class AutoTrim {
|
|
// Target pitch rate per radian of flight path angle, and its limit (rad/s)
|
|
private static final double PATH_GAIN = 0.8;
|
|
private static final double MAX_PITCH_RATE = 0.1;
|
|
// 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
|
|
private static final double TRIM_RATE = 1.0;
|
|
private static final double PILOT_DEADBAND = 0.01;
|
|
|
|
private AutoTrim() {
|
|
}
|
|
|
|
static void update(AeroState.NodeState state, NodeData node, Vector3dc worldVelocity, Quaterniondc rotation,
|
|
Matrix3dc inertia, double[] w, double dt, boolean stalled) {
|
|
if (!node.autoTrim()) {
|
|
state.trimIntegral = 0.0;
|
|
state.trimOutput = AeroState.moveTowards(state.trimOutput, 0.0, TRIM_RATE * dt);
|
|
state.savedTrim = 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;
|
|
|
|
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 flight path? Sign of the vertical part of axis x 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 pathAngle = Math.asin(Math.max(-1.0, Math.min(1.0, direction.y)));
|
|
|
|
double targetRate = Math.max(-MAX_PITCH_RATE, Math.min(MAX_PITCH_RATE, -PATH_GAIN * pathAngle * noseUp));
|
|
double error = targetRate - 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);
|
|
}
|
|
state.trimOutput = AeroState.moveTowards(state.trimOutput, target, TRIM_RATE * dt);
|
|
state.savedTrim = state.trimIntegral;
|
|
}
|
|
|
|
private static double clamp(double value, double limit) {
|
|
return Math.max(-limit, Math.min(limit, value));
|
|
}
|
|
}
|