588 lines
23 KiB
Python
588 lines
23 KiB
Python
import json
|
|
import math
|
|
import os
|
|
import sys
|
|
import uuid
|
|
|
|
|
|
FT_PER_M = 3.280839895
|
|
M_PER_FT = 1 / FT_PER_M
|
|
KTS_PER_MPS = 1.943844492
|
|
MPS_PER_FPS = 0.3048
|
|
DEFAULT_INTEGRATION_RATE_HZ = 120
|
|
|
|
|
|
def main():
|
|
request = json.load(sys.stdin)
|
|
operation = request.get("operation")
|
|
|
|
if operation == "trim":
|
|
response = trim_initial_state(request)
|
|
elif operation == "simulate":
|
|
response = simulate(request)
|
|
else:
|
|
raise RuntimeError(f"Unsupported JSBSim runner operation: {operation}")
|
|
|
|
json.dump(response, sys.stdout)
|
|
|
|
|
|
def create_fdm(aircraft_id, integration_rate_hz):
|
|
try:
|
|
import jsbsim
|
|
except ImportError as error:
|
|
raise RuntimeError(
|
|
"Python package 'jsbsim' is not installed. Install it with: pip install jsbsim"
|
|
) from error
|
|
|
|
root_dir = os.environ.get("JSBSIM_ROOT")
|
|
fdm = jsbsim.FGFDMExec(root_dir) if root_dir else jsbsim.FGFDMExec(None)
|
|
fdm.set_debug_level(0)
|
|
fdm.disable_output()
|
|
fdm.set_dt(1.0 / integration_rate_hz)
|
|
|
|
if aircraft_id == "f22cobra":
|
|
package_root = os.path.dirname(jsbsim.__file__)
|
|
service_root = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
|
|
fdm.set_aircraft_path(os.path.join(service_root, "aircraft"))
|
|
fdm.set_engine_path(os.path.join(package_root, "engine"))
|
|
fdm.set_systems_path(os.path.join(package_root, "systems"))
|
|
elif root_dir:
|
|
fdm.set_aircraft_path(os.path.join(root_dir, "aircraft"))
|
|
fdm.set_engine_path(os.path.join(root_dir, "engine"))
|
|
fdm.set_systems_path(os.path.join(root_dir, "systems"))
|
|
|
|
if not fdm.load_model(aircraft_id):
|
|
raise RuntimeError(f"JSBSim could not load aircraft model: {aircraft_id}")
|
|
|
|
return fdm
|
|
|
|
|
|
def trim_initial_state(request):
|
|
integration_rate_hz = integration_rate_from_request(request)
|
|
aircraft_id = normalize_aircraft_id(request.get("aircraftId"))
|
|
trim_request = request.get("trim", request)
|
|
fdm = create_fdm(aircraft_id, integration_rate_hz)
|
|
|
|
altitude_m = number_or_default(trim_request.get("altitudeM"), 1000)
|
|
speed_mps = number_or_default(trim_request.get("speedMps"), 70)
|
|
heading_deg = number_or_default(trim_request.get("headingDeg"), 0)
|
|
gamma_deg = number_or_default(trim_request.get("flightPathAngleDeg"), 0)
|
|
|
|
configure_initial_conditions(
|
|
fdm,
|
|
altitude_m=altitude_m,
|
|
speed_mps=speed_mps,
|
|
heading_deg=heading_deg,
|
|
gamma_deg=gamma_deg,
|
|
)
|
|
run_trim(fdm)
|
|
|
|
return {
|
|
"success": True,
|
|
"aircraftId": request.get("aircraftId"),
|
|
"state": read_state(fdm, fallback_position=[0, altitude_m, 0]),
|
|
}
|
|
|
|
|
|
def simulate(request):
|
|
sample_rate_hz = max(1, min(240, number_or_default(request.get("sampleRateHz"), 30)))
|
|
integration_rate_hz = integration_rate_from_request(request)
|
|
integration_dt = 1.0 / integration_rate_hz
|
|
duration_sec = max(0.1, min(3600, number_or_default(request.get("durationSec"), 10)))
|
|
aircraft_id = normalize_aircraft_id(request.get("aircraftId"))
|
|
maneuver = request.get("maneuver") or {"type": "straight", "parameters": {}}
|
|
parameters = maneuver.get("parameters") or {}
|
|
initial_state = request.get("initialState") or {}
|
|
|
|
speed_mps = number_or_default(initial_state.get("velocityMps"), 70)
|
|
heading_deg = number_or_default(initial_state.get("headingDeg"), 0)
|
|
start_position = vector_or_default(initial_state.get("position"), [0, 1000, 0])
|
|
gamma_deg = maneuver_gamma_deg(maneuver.get("type"), parameters)
|
|
|
|
fdm = create_fdm(aircraft_id, integration_rate_hz)
|
|
configure_initial_conditions(
|
|
fdm,
|
|
altitude_m=start_position[1],
|
|
speed_mps=speed_mps,
|
|
heading_deg=heading_deg,
|
|
gamma_deg=gamma_deg,
|
|
)
|
|
if should_trim(request, parameters):
|
|
run_trim(fdm)
|
|
|
|
total_samples = int(math.floor(duration_sec * sample_rate_hz)) + 1
|
|
samples = []
|
|
east = start_position[0]
|
|
north = start_position[2]
|
|
sim_time = 0.0
|
|
|
|
for index in range(total_samples):
|
|
target_t = min(index / sample_rate_hz, duration_sec)
|
|
|
|
while sim_time < target_t - 1e-9:
|
|
step_dt = min(integration_dt, target_t - sim_time)
|
|
fdm.set_dt(step_dt)
|
|
controls = controls_at_time(maneuver, sim_time, fdm)
|
|
apply_controls(fdm, controls)
|
|
fdm.run()
|
|
state = read_state(fdm, fallback_position=[east, start_position[1], north])
|
|
east += get_prop(fdm, "velocities/v-east-fps", 0) * MPS_PER_FPS * step_dt
|
|
north += get_prop(fdm, "velocities/v-north-fps", 0) * MPS_PER_FPS * step_dt
|
|
sim_time += step_dt
|
|
|
|
state = read_state(fdm, fallback_position=start_position if index == 0 else [east, start_position[1], north])
|
|
|
|
altitude_m = get_prop(fdm, "position/h-sl-ft", start_position[1] * FT_PER_M) * M_PER_FT
|
|
state["position"] = [round_float(east), round_float(altitude_m), round_float(north)]
|
|
state["t"] = round_float(target_t)
|
|
samples.append(state)
|
|
|
|
return {
|
|
"id": f"sim_{uuid.uuid4()}",
|
|
"aircraftId": request.get("aircraftId"),
|
|
"sampleRateHz": sample_rate_hz,
|
|
"integrationRateHz": integration_rate_hz,
|
|
"durationSec": duration_sec,
|
|
"coordinateFrame": "local-eun",
|
|
"headingConvention": "true heading: 0 deg north, clockwise positive, counter-clockwise negative",
|
|
"samples": samples,
|
|
}
|
|
|
|
|
|
def configure_initial_conditions(fdm, altitude_m, speed_mps, heading_deg, gamma_deg):
|
|
set_prop(fdm, "ic/h-sl-ft", altitude_m * FT_PER_M)
|
|
set_prop(fdm, "ic/vc-kts", speed_mps * KTS_PER_MPS)
|
|
set_prop(fdm, "ic/gamma-deg", gamma_deg)
|
|
set_prop(fdm, "ic/psi-true-deg", heading_deg)
|
|
set_prop(fdm, "ic/lat-geod-deg", 0)
|
|
set_prop(fdm, "ic/long-gc-deg", 0)
|
|
set_prop(fdm, "ic/terrain-elevation-ft", 0)
|
|
set_prop(fdm, "propulsion/set-running", -1)
|
|
if not fdm.run_ic():
|
|
raise RuntimeError("JSBSim run_ic() failed.")
|
|
set_airborne_configuration(fdm)
|
|
|
|
|
|
def set_airborne_configuration(fdm):
|
|
set_prop(fdm, "gear/gear-cmd-norm", 0)
|
|
set_prop(fdm, "gear/gear-pos-norm", 0)
|
|
set_prop(fdm, "fcs/flap-cmd-norm", 0)
|
|
set_prop(fdm, "fcs/flap-pos-norm", 0)
|
|
set_prop(fdm, "fcs/speedbrake-cmd-norm", 0)
|
|
set_prop(fdm, "fcs/speedbrake-pos-norm", 0)
|
|
|
|
|
|
def run_trim(fdm):
|
|
try:
|
|
fdm.do_trim(0)
|
|
except Exception as error:
|
|
raise RuntimeError(f"JSBSim longitudinal trim failed: {error}") from error
|
|
|
|
|
|
def read_state(fdm, fallback_position):
|
|
phi = get_prop(fdm, "attitude/phi-rad", 0)
|
|
theta = get_prop(fdm, "attitude/theta-rad", 0)
|
|
psi = get_prop(fdm, "attitude/psi-rad", 0)
|
|
heading_deg = normalize_heading(math.degrees(psi))
|
|
altitude_m = get_prop(fdm, "position/h-sl-ft", fallback_position[1] * FT_PER_M) * M_PER_FT
|
|
velocity_mps = get_prop(fdm, "velocities/vtrue-fps", 0) * MPS_PER_FPS
|
|
vertical_speed_mps = get_prop(fdm, "velocities/h-dot-fps", 0) * MPS_PER_FPS
|
|
pitch_rate_deg_s = math.degrees(get_prop(fdm, "velocities/q-rad_sec", 0))
|
|
roll_rate_deg_s = math.degrees(get_prop(fdm, "velocities/p-rad_sec", 0))
|
|
yaw_rate_deg_s = math.degrees(get_prop(fdm, "velocities/r-rad_sec", 0))
|
|
|
|
return {
|
|
"position": [fallback_position[0], round_float(altitude_m), fallback_position[2]],
|
|
"rotation": euler_to_enu_body_quat(phi, theta, psi),
|
|
"velocityMps": round_float(velocity_mps),
|
|
"headingDeg": round_float(heading_deg),
|
|
"pitchDeg": round_float(math.degrees(theta)),
|
|
"rollDeg": round_float(math.degrees(phi)),
|
|
"alphaDeg": round_float(math.degrees(get_prop(fdm, "aero/alpha-rad", 0))),
|
|
"betaDeg": round_float(math.degrees(get_prop(fdm, "aero/beta-rad", 0))),
|
|
"loadFactor": round_float(get_prop(fdm, "accelerations/n-pilot-z-norm", 1)),
|
|
"mach": round_float(get_prop(fdm, "velocities/mach", 0)),
|
|
"qbar": round_float(get_prop(fdm, "aero/qbar-psf", 0)),
|
|
"verticalSpeedMps": round_float(vertical_speed_mps),
|
|
"pitchRateDegS": round_float(pitch_rate_deg_s),
|
|
"rollRateDegS": round_float(roll_rate_deg_s),
|
|
"yawRateDegS": round_float(yaw_rate_deg_s),
|
|
"throttle": round_float(get_prop(fdm, "fcs/throttle-cmd-norm", 0)),
|
|
"controlInputs": {
|
|
"elevator": round_float(get_prop(fdm, "fcs/elevator-cmd-norm", 0)),
|
|
"aileron": round_float(get_prop(fdm, "fcs/aileron-cmd-norm", 0)),
|
|
"rudder": round_float(get_prop(fdm, "fcs/rudder-cmd-norm", 0)),
|
|
"throttle": round_float(get_prop(fdm, "fcs/throttle-cmd-norm", 0)),
|
|
"tvc": round_float(get_prop(fdm, "fcs/tvc-cmd-norm", 0)),
|
|
"speedbrake": round_float(get_prop(fdm, "fcs/speedbrake-cmd-norm", 0)),
|
|
},
|
|
"controlSurfaces": {
|
|
"elevatorDeg": round_float(math.degrees(get_prop(fdm, "fcs/elevator-pos-rad", 0))),
|
|
"aileronDeg": round_float(math.degrees(get_prop(fdm, "fcs/aileron-pos-rad", 0))),
|
|
"rudderDeg": round_float(math.degrees(get_prop(fdm, "fcs/rudder-pos-rad", 0))),
|
|
"tvcDeg": round_float(math.degrees(get_prop(fdm, "fcs/tvc-pos-rad", 0))),
|
|
"speedbrakeNorm": round_float(get_prop(fdm, "fcs/speedbrake-pos-norm", 0)),
|
|
},
|
|
"fcs": {
|
|
"tvcPosNorm": round_float(get_prop(fdm, "fcs/tvc-pos-norm", 0)),
|
|
"tvcInhibit": round_float(get_prop(fdm, "fcs/tvc-inhibit", 0)),
|
|
"thrustNorm": round_float(get_prop(fdm, "fcs/thrust-norm", 0)),
|
|
"throttleOverride": round_float(get_prop(fdm, "fcs/throttle-override", 0)),
|
|
"pitchRateCmd": round_float(get_prop(fdm, "fcs/pitch-rate-cmd", 0)),
|
|
},
|
|
}
|
|
|
|
|
|
def maneuver_gamma_deg(maneuver_type, parameters):
|
|
if maneuver_type == "climb":
|
|
return abs(number_or_default(parameters.get("flightPathAngleDeg"), 8))
|
|
if maneuver_type == "descent":
|
|
return -abs(number_or_default(parameters.get("flightPathAngleDeg"), 8))
|
|
if maneuver_type == "straight":
|
|
return number_or_default(parameters.get("flightPathAngleDeg"), 0)
|
|
if maneuver_type in ("control-script", "cobra"):
|
|
return number_or_default(parameters.get("flightPathAngleDeg"), 0)
|
|
raise RuntimeError(
|
|
f"JSBSim minimal runner supports straight/climb/descent/control-script/cobra only. Unsupported maneuver.type: {maneuver_type}"
|
|
)
|
|
|
|
|
|
def controls_at_time(maneuver, t, fdm=None):
|
|
maneuver_type = maneuver.get("type")
|
|
parameters = maneuver.get("parameters") or {}
|
|
|
|
if maneuver_type == "cobra":
|
|
if parameters.get("controlMode", "closed-loop") == "closed-loop" and fdm is not None:
|
|
return cobra_closed_loop_controls(parameters, t, fdm)
|
|
timeline = cobra_timeline(parameters)
|
|
elif maneuver_type == "control-script":
|
|
timeline = parameters.get("timeline")
|
|
else:
|
|
return {
|
|
"elevator": 0,
|
|
"aileron": 0,
|
|
"rudder": 0,
|
|
"throttle": number_or_default(parameters.get("throttle"), 0.82),
|
|
"tvc": 0,
|
|
"speedbrake": 0,
|
|
}
|
|
|
|
if not isinstance(timeline, list) or len(timeline) == 0:
|
|
return {"elevator": 0, "aileron": 0, "rudder": 0, "throttle": 0.82, "tvc": 0, "speedbrake": 0}
|
|
|
|
points = sorted(
|
|
[point for point in timeline if isinstance(point, dict)],
|
|
key=lambda point: number_or_default(point.get("t"), 0),
|
|
)
|
|
if not points:
|
|
return {"elevator": 0, "aileron": 0, "rudder": 0, "throttle": 0.82}
|
|
|
|
if t <= number_or_default(points[0].get("t"), 0):
|
|
return control_point(points[0])
|
|
|
|
for index in range(1, len(points)):
|
|
previous = points[index - 1]
|
|
current = points[index]
|
|
previous_t = number_or_default(previous.get("t"), 0)
|
|
current_t = number_or_default(current.get("t"), previous_t)
|
|
if t <= current_t:
|
|
amount = 0 if current_t <= previous_t else (t - previous_t) / (current_t - previous_t)
|
|
return interpolate_controls(control_point(previous), control_point(current), amount)
|
|
|
|
return control_point(points[-1])
|
|
|
|
|
|
def cobra_closed_loop_controls(parameters, t, fdm):
|
|
entry_delay = max(0, number_or_default(parameters.get("entryDelaySec"), 0.15))
|
|
pull_duration = max(0.1, number_or_default(parameters.get("pullDurationSec"), 0.7))
|
|
hold_duration = max(0, number_or_default(parameters.get("holdDurationSec"), 0))
|
|
recovery_duration = max(0.1, number_or_default(parameters.get("recoveryDurationSec"), 2.0))
|
|
target_alpha = clamp(number_or_default(parameters.get("targetAlphaDeg"), 80), 45, 130)
|
|
recovery_alpha = clamp(number_or_default(parameters.get("recoveryAlphaDeg"), 8), 0, 35)
|
|
pitch_rate_limit = max(20, number_or_default(parameters.get("pitchRateLimitDegS"), 80))
|
|
hold_pitch_rate = number_or_default(parameters.get("holdPitchRateDegS"), 0)
|
|
max_pull_elevator = clamp(number_or_default(parameters.get("maxElevatorCmd"), -0.45), -1, 0)
|
|
max_recovery_elevator = clamp(number_or_default(parameters.get("recoveryElevatorCmd"), 0.65), 0, 1)
|
|
pull_tvc = clamp(number_or_default(parameters.get("pullTvcCmd"), -0.05), -1, 0)
|
|
recovery_tvc = clamp(number_or_default(parameters.get("recoveryTvcCmd"), 0.85), -1, 1)
|
|
throttle = clamp(number_or_default(parameters.get("throttleCmd"), 1), 0, 1)
|
|
speedbrake = clamp(number_or_default(parameters.get("speedbrakeCmd"), 0), 0, 1)
|
|
|
|
alpha = math.degrees(get_prop(fdm, "aero/alpha-rad", 0))
|
|
pitch = math.degrees(get_prop(fdm, "attitude/theta-rad", 0))
|
|
pitch_rate = math.degrees(get_prop(fdm, "velocities/q-rad_sec", 0))
|
|
speed_mps = get_prop(fdm, "velocities/vtrue-fps", 0) * MPS_PER_FPS
|
|
|
|
pull_end = entry_delay + pull_duration
|
|
hold_end = pull_end + hold_duration
|
|
recovery_end = hold_end + recovery_duration
|
|
|
|
if t < entry_delay:
|
|
return {
|
|
"elevator": 0,
|
|
"aileron": 0,
|
|
"rudder": 0,
|
|
"throttle": throttle,
|
|
"tvc": 0,
|
|
"speedbrake": 0,
|
|
}
|
|
|
|
if t < pull_end:
|
|
alpha_margin = target_alpha - alpha
|
|
alpha_scale = clamp(alpha_margin / 24, 0, 1)
|
|
rate_excess = max(0, pitch_rate - pitch_rate_limit)
|
|
rate_relief = clamp(rate_excess / pitch_rate_limit, 0, 1)
|
|
speed_relief = clamp((115 - speed_mps) / 45, 0, 0.35)
|
|
elevator = max_pull_elevator * alpha_scale + max_recovery_elevator * (rate_relief + speed_relief)
|
|
tvc = pull_tvc * alpha_scale
|
|
brake = speedbrake if alpha > 18 else 0
|
|
elif t < hold_end:
|
|
elevator = cobra_track_alpha(
|
|
target_alpha,
|
|
hold_pitch_rate,
|
|
alpha,
|
|
pitch_rate,
|
|
max_pull_elevator,
|
|
max_recovery_elevator,
|
|
)
|
|
tvc = pull_tvc * clamp((target_alpha - alpha) / 20, -0.4, 1)
|
|
brake = speedbrake
|
|
else:
|
|
recovery_amount = clamp((t - hold_end) / recovery_duration, 0, 1)
|
|
desired_alpha = target_alpha + (recovery_alpha - target_alpha) * smoothstep(recovery_amount)
|
|
desired_pitch_rate = 18 + (0 - 18) * recovery_amount
|
|
elevator = cobra_track_alpha(
|
|
desired_alpha,
|
|
desired_pitch_rate,
|
|
alpha,
|
|
pitch_rate,
|
|
max_pull_elevator * 0.55,
|
|
max_recovery_elevator,
|
|
)
|
|
if pitch > 70 and pitch_rate > 25:
|
|
elevator = max(elevator, max_recovery_elevator * 0.75)
|
|
tvc = recovery_tvc * clamp((alpha - desired_alpha) / 35, 0, 1)
|
|
brake = speedbrake if t < recovery_end and alpha > 35 else 0
|
|
|
|
return {
|
|
"elevator": clamp(elevator, -1, 1),
|
|
"aileron": 0,
|
|
"rudder": 0,
|
|
"throttle": throttle,
|
|
"tvc": clamp(tvc, -1, 1),
|
|
"speedbrake": brake,
|
|
}
|
|
|
|
|
|
def cobra_track_alpha(target_alpha, target_pitch_rate, alpha, pitch_rate, pull_limit, recovery_limit):
|
|
alpha_error = target_alpha - alpha
|
|
rate_error = pitch_rate - target_pitch_rate
|
|
elevator = -0.018 * alpha_error + 0.006 * rate_error
|
|
return clamp(elevator, pull_limit, recovery_limit)
|
|
|
|
|
|
def cobra_timeline(parameters):
|
|
pull_duration = max(0.1, number_or_default(parameters.get("pullDurationSec"), 0.9))
|
|
unload_duration = max(0.1, number_or_default(parameters.get("unloadDurationSec"), 0.7))
|
|
recovery_duration = max(0.1, number_or_default(parameters.get("recoveryDurationSec"), 2.0))
|
|
max_elevator = clamp(number_or_default(parameters.get("maxElevatorCmd"), -1.0), -1, 1)
|
|
recovery_elevator = clamp(number_or_default(parameters.get("recoveryElevatorCmd"), 0.35), -1, 1)
|
|
throttle = clamp(number_or_default(parameters.get("throttleCmd"), 1.0), 0, 1)
|
|
pull_tvc = clamp(number_or_default(parameters.get("pullTvcCmd"), 0), -1, 1)
|
|
recovery_tvc = clamp(number_or_default(parameters.get("recoveryTvcCmd"), 1), -1, 1)
|
|
speedbrake = clamp(number_or_default(parameters.get("speedbrakeCmd"), 0), 0, 1)
|
|
t0 = 0
|
|
t1 = t0 + 0.2
|
|
t2 = t1 + pull_duration
|
|
t3 = t2 + unload_duration
|
|
t4 = t3 + recovery_duration
|
|
|
|
return [
|
|
{"t": t0, "elevator": 0, "aileron": 0, "rudder": 0, "throttle": throttle, "tvc": 0, "speedbrake": 0},
|
|
{"t": t1, "elevator": max_elevator, "aileron": 0, "rudder": 0, "throttle": throttle, "tvc": pull_tvc, "speedbrake": speedbrake},
|
|
{"t": t2, "elevator": 0, "aileron": 0, "rudder": 0, "throttle": throttle, "tvc": pull_tvc, "speedbrake": speedbrake},
|
|
{"t": t3, "elevator": recovery_elevator, "aileron": 0, "rudder": 0, "throttle": throttle, "tvc": recovery_tvc, "speedbrake": speedbrake},
|
|
{"t": t4, "elevator": 0, "aileron": 0, "rudder": 0, "throttle": throttle, "tvc": 0, "speedbrake": 0},
|
|
]
|
|
|
|
|
|
def apply_controls(fdm, controls):
|
|
set_prop(fdm, "fcs/elevator-cmd-norm", clamp(controls.get("elevator"), -1, 1))
|
|
set_prop(fdm, "fcs/aileron-cmd-norm", clamp(controls.get("aileron"), -1, 1))
|
|
set_prop(fdm, "fcs/rudder-cmd-norm", clamp(controls.get("rudder"), -1, 1))
|
|
set_prop(fdm, "fcs/throttle-cmd-norm", clamp(controls.get("throttle"), 0, 1))
|
|
set_prop(fdm, "fcs/tvc-cmd-norm", clamp(controls.get("tvc"), -1, 1))
|
|
set_prop(fdm, "fcs/speedbrake-cmd-norm", clamp(controls.get("speedbrake"), 0, 1))
|
|
|
|
|
|
def control_point(point):
|
|
return {
|
|
"elevator": clamp(number_or_default(point.get("elevator"), 0), -1, 1),
|
|
"aileron": clamp(number_or_default(point.get("aileron"), 0), -1, 1),
|
|
"rudder": clamp(number_or_default(point.get("rudder"), 0), -1, 1),
|
|
"throttle": clamp(number_or_default(point.get("throttle"), 0.82), 0, 1),
|
|
"tvc": clamp(number_or_default(point.get("tvc"), 0), -1, 1),
|
|
"speedbrake": clamp(number_or_default(point.get("speedbrake"), 0), 0, 1),
|
|
}
|
|
|
|
|
|
def interpolate_controls(start, end, amount):
|
|
return {
|
|
"elevator": start["elevator"] + (end["elevator"] - start["elevator"]) * amount,
|
|
"aileron": start["aileron"] + (end["aileron"] - start["aileron"]) * amount,
|
|
"rudder": start["rudder"] + (end["rudder"] - start["rudder"]) * amount,
|
|
"throttle": start["throttle"] + (end["throttle"] - start["throttle"]) * amount,
|
|
"tvc": start["tvc"] + (end["tvc"] - start["tvc"]) * amount,
|
|
"speedbrake": start["speedbrake"] + (end["speedbrake"] - start["speedbrake"]) * amount,
|
|
}
|
|
|
|
|
|
def normalize_aircraft_id(aircraft_id):
|
|
if not aircraft_id:
|
|
return "c172p"
|
|
return aircraft_id.replace("-jsbsim", "")
|
|
|
|
|
|
def get_prop(fdm, name, fallback):
|
|
try:
|
|
return float(fdm.get_property_value(name))
|
|
except Exception:
|
|
try:
|
|
return float(fdm[name])
|
|
except Exception:
|
|
return fallback
|
|
|
|
|
|
def set_prop(fdm, name, value):
|
|
try:
|
|
fdm.set_property_value(name, value)
|
|
except Exception:
|
|
fdm[name] = value
|
|
|
|
|
|
def vector_or_default(value, fallback):
|
|
if not isinstance(value, list) or len(value) != 3:
|
|
return fallback
|
|
return [number_or_default(value[index], fallback[index]) for index in range(3)]
|
|
|
|
|
|
def number_or_default(value, fallback):
|
|
return value if isinstance(value, (int, float)) and math.isfinite(value) else fallback
|
|
|
|
|
|
def should_trim(request, parameters):
|
|
trim_value = request.get("trim", parameters.get("trim", True))
|
|
return trim_value is not False
|
|
|
|
|
|
def integration_rate_from_request(request):
|
|
return int(
|
|
max(
|
|
1,
|
|
min(
|
|
1000,
|
|
number_or_default(
|
|
request.get("integrationRateHz"),
|
|
DEFAULT_INTEGRATION_RATE_HZ,
|
|
),
|
|
),
|
|
)
|
|
)
|
|
|
|
|
|
def clamp(value, minimum, maximum):
|
|
return max(minimum, min(maximum, number_or_default(value, minimum)))
|
|
|
|
|
|
def smoothstep(value):
|
|
amount = clamp(value, 0, 1)
|
|
return amount * amount * (3 - 2 * amount)
|
|
|
|
|
|
def normalize_heading(value):
|
|
heading = value % 360
|
|
if heading > 180:
|
|
heading -= 360
|
|
if heading <= -180:
|
|
heading += 360
|
|
return heading
|
|
|
|
|
|
def euler_to_enu_body_quat(phi, theta, psi):
|
|
body_to_ned = body_to_ned_matrix(phi, theta, psi)
|
|
body_to_enu = [
|
|
[body_to_ned[1][0], body_to_ned[1][1], body_to_ned[1][2]],
|
|
[body_to_ned[0][0], body_to_ned[0][1], body_to_ned[0][2]],
|
|
[-body_to_ned[2][0], -body_to_ned[2][1], -body_to_ned[2][2]],
|
|
]
|
|
return matrix_to_quat(body_to_enu)
|
|
|
|
|
|
def body_to_ned_matrix(phi, theta, psi):
|
|
cphi = math.cos(phi)
|
|
sphi = math.sin(phi)
|
|
ctheta = math.cos(theta)
|
|
stheta = math.sin(theta)
|
|
cpsi = math.cos(psi)
|
|
spsi = math.sin(psi)
|
|
|
|
return [
|
|
[
|
|
ctheta * cpsi,
|
|
sphi * stheta * cpsi - cphi * spsi,
|
|
cphi * stheta * cpsi + sphi * spsi,
|
|
],
|
|
[
|
|
ctheta * spsi,
|
|
sphi * stheta * spsi + cphi * cpsi,
|
|
cphi * stheta * spsi - sphi * cpsi,
|
|
],
|
|
[-stheta, sphi * ctheta, cphi * ctheta],
|
|
]
|
|
|
|
|
|
def matrix_to_quat(matrix):
|
|
m00, m01, m02 = matrix[0]
|
|
m10, m11, m12 = matrix[1]
|
|
m20, m21, m22 = matrix[2]
|
|
trace = m00 + m11 + m22
|
|
|
|
if trace > 0:
|
|
scale = math.sqrt(trace + 1.0) * 2
|
|
w = 0.25 * scale
|
|
x = (m21 - m12) / scale
|
|
y = (m02 - m20) / scale
|
|
z = (m10 - m01) / scale
|
|
elif m00 > m11 and m00 > m22:
|
|
scale = math.sqrt(1.0 + m00 - m11 - m22) * 2
|
|
w = (m21 - m12) / scale
|
|
x = 0.25 * scale
|
|
y = (m01 + m10) / scale
|
|
z = (m02 + m20) / scale
|
|
elif m11 > m22:
|
|
scale = math.sqrt(1.0 + m11 - m00 - m22) * 2
|
|
w = (m02 - m20) / scale
|
|
x = (m01 + m10) / scale
|
|
y = 0.25 * scale
|
|
z = (m12 + m21) / scale
|
|
else:
|
|
scale = math.sqrt(1.0 + m22 - m00 - m11) * 2
|
|
w = (m10 - m01) / scale
|
|
x = (m02 + m20) / scale
|
|
y = (m12 + m21) / scale
|
|
z = 0.25 * scale
|
|
|
|
return [round_float(x), round_float(y), round_float(z), round_float(w)]
|
|
|
|
|
|
def round_float(value):
|
|
return round(value, 6)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
main()
|
|
except Exception as error:
|
|
print(str(error), file=sys.stderr)
|
|
sys.exit(1)
|