103 lines
4.7 KiB
C
103 lines
4.7 KiB
C
// Candidate 100m field experiment only. BrakingReserve is an admission
|
|
// hypothesis, not measured stopping performance or an emergency-landing promise.
|
|
class ERR_AircraftFieldRules
|
|
{
|
|
static bool Length(float distance) { return distance >= 99.95 && distance <= 100.05; }
|
|
static bool FiniteVector(vector value)
|
|
{
|
|
return Math.AbsFloat(value[0]) <= 1000000 && Math.AbsFloat(value[1]) <= 1000000 && Math.AbsFloat(value[2]) <= 1000000;
|
|
}
|
|
static float HorizontalSpeed(vector velocity)
|
|
{
|
|
velocity[1] = 0;
|
|
return velocity.Length();
|
|
}
|
|
static bool SpeedAllowed(vector velocity) { return FiniteVector(velocity) && HorizontalSpeed(velocity) <= 2.5; }
|
|
static bool ValidStep(float dt) { return dt > 0 && dt <= 0.5; }
|
|
static float RampSpeed(float current, float target, float dt)
|
|
{
|
|
if (!(current >= 0 && current <= 2)) return 0;
|
|
if (!(target >= 0 && target <= 2) || !ValidStep(dt)) return current;
|
|
return current + Math.Clamp(target - current, -0.2 * dt, 0.2 * dt);
|
|
}
|
|
static vector CapHorizontal(vector value, float limit)
|
|
{
|
|
value[1] = 0;
|
|
float magnitude = value.Length();
|
|
if (magnitude > limit) value = value * (limit / magnitude);
|
|
return value;
|
|
}
|
|
// Slew the complete horizontal vector: changing bearing cannot bypass the
|
|
// acceleration velocityReference cap through an instantaneous sign/direction change.
|
|
static vector RampReference(vector current, vector desired, float dt)
|
|
{
|
|
if (!FiniteVector(current)) return "0 0 0";
|
|
current = CapHorizontal(current, 2);
|
|
if (!FiniteVector(desired) || !ValidStep(dt)) return current;
|
|
desired = CapHorizontal(desired, 2);
|
|
return CapHorizontal(current + CapHorizontal(desired - current, 0.2 * dt), 2);
|
|
}
|
|
// Integral units are (m/s)*seconds; nextIntegral is caller-owned state.
|
|
// Horizontal demand is world-space; the exact-owner controller converts it
|
|
// to the native airframe basis. The caller resets state at phase transitions.
|
|
static vector VelocityDemand(vector velocityReference, vector velocity, vector integral, float dt, out vector nextIntegral)
|
|
{
|
|
nextIntegral = "0 0 0";
|
|
if (!FiniteVector(velocityReference) || !FiniteVector(velocity) || !FiniteVector(integral) || !ValidStep(dt)) return "0 0 0";
|
|
velocityReference = CapHorizontal(velocityReference, 2);
|
|
velocity[1] = 0;
|
|
integral = CapHorizontal(integral, 4);
|
|
vector error = velocityReference - velocity;
|
|
vector candidate = CapHorizontal(integral + error * dt, 4);
|
|
vector proposed = error * 0.10 + candidate * 0.02;
|
|
// Freeze only integration which pushes an already saturated vector outward.
|
|
// Opposing error can unwind stored demand while the output remains capped.
|
|
nextIntegral = candidate;
|
|
if (proposed.Length() > 0.08 && vector.Dot(proposed, error) > 0) nextIntegral = integral;
|
|
return CapHorizontal(error * 0.10 + nextIntegral * 0.02, 0.08);
|
|
}
|
|
static float BrakingReserve(float speed)
|
|
{
|
|
if (!(speed >= 0 && speed <= 2.5)) return 1000000;
|
|
return speed * speed / 0.30 + speed + 3;
|
|
}
|
|
static bool ShouldBrake(float remaining, float speed)
|
|
{
|
|
if (!(remaining >= 0 && remaining <= 120) || !(speed >= 0 && speed <= 2.5)) return true;
|
|
// The candidate reserve uses actual speed; phase2 immediately targets rest.
|
|
// This predicate does not assert measured native stopping performance.
|
|
return remaining <= BrakingReserve(speed);
|
|
}
|
|
// Admit only the full experimental lease, with <=1s dispatch allowance.
|
|
// Never recompute a deadline from these checks or renew the original lease.
|
|
static bool LeaseAdmitted(float now, float deadline)
|
|
{
|
|
return now >= 0 && deadline - now >= 239000 && deadline - now <= 240000;
|
|
}
|
|
static bool AbortDue(float now, float deadline)
|
|
{
|
|
if (!(now >= 0 && deadline > now && deadline - now <= 240000)) return true;
|
|
return deadline - now <= 85000;
|
|
}
|
|
// 'since' is the caller's continuous low-speed observation start, in ms.
|
|
// The caller clears it whenever actual horizontal speed is >=0.25m/s.
|
|
static bool LowSpeedHeld(float speed, float since, float now)
|
|
{
|
|
return speed >= 0 && speed < 0.25 && since >= 0 && now >= since && now - since >= 2000;
|
|
}
|
|
static vector Nearest(vector start, vector end, vector point)
|
|
{
|
|
vector leg = end - start; leg[1] = 0;
|
|
vector offset = point - start; offset[1] = 0;
|
|
float squared = vector.Dot(leg, leg);
|
|
if (!(squared > 0)) return start;
|
|
return start + leg * Math.Clamp(vector.Dot(offset, leg) / squared, 0, 1);
|
|
}
|
|
static bool Contains(vector start, vector end, vector point, float upright)
|
|
{
|
|
if (!FiniteVector(start) || !FiniteVector(end) || !FiniteVector(point) || !Length(vector.DistanceXZ(start, end))) return false;
|
|
float height = point[1] - start[1];
|
|
return height >= -3 && height <= 18 && upright >= 0.85 && upright <= 1.01 && vector.DistanceXZ(point, Nearest(start, end, point)) < 8;
|
|
}
|
|
}
|