FlightSim-Streaming/XPlaneConnectCSP
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

QUATERNION ORIENTIATION IS WORKING!!

Browse Source
This commit is contained in:
cs-powell
2025-03-31 23:33:05 -04:00
parent ff32f69292
commit 89991c248a
3 changed files with 169 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Python3/src/cognitiveModel.py
View File

@@ -359,7 +359,6 @@ class AircraftLandingModel(pyactr.ACTRModel):
self.phaseFlags["flare"] = True
self.Ki = 0.01 ## Increase Control Authority to compensate for decreasing airspeed
print("Altitude < 500; Flare Set True")
print("*******FLAG*******")
if(self.dictionaryAccess(self.destinations,"wheelWeight") > 0.01
and self.dictionaryAccess(self.destinations,"wheelSpeed") < 1
@@ -377,11 +376,6 @@ class AircraftLandingModel(pyactr.ACTRModel):
Faster Method
"""
self.getAndLoadDREFS()
print("midpoint")
self.conditionChecks()
# def logData(self):

222
Python3/src/testPlatform.py
View File

@@ -1,86 +1,118 @@
import datetime
from math import cos, pi, sin, sqrt
import os
import time
from time import sleep
import xpc
from cognitiveModel import AircraftLandingModel
import shutil
def experimentSetUp(client):
def runExperiment(title):
def eulerToQuat(psiInput,thetaInput,phiInput):
print("Model Test: Xplane setting up connection")
print("Setting up simulation")
startTime = 0
endTime = 0
difference = endTime - startTime
experimentLive = True
while(difference < 3 and experimentLive):
print("Time Elapsed: -----> " + str(difference))
try:
with xpc.XPlaneConnect() as client:
# Verify connection
# try:
# If X-Plane does not respond to the request, a timeout error
# will beff raised.
client.getDREF("sim/test/test_float")
# except:
# print("Error establishing connection to X-Plane.")
# print("Exiting...")
# return
cogModel = AircraftLandingModel(client,True)
psi = float(pi / 360 * psiInput)
theta = float(pi / 360 * thetaInput)
phi = float(pi / 360 * phiInput)
q0 = cos(psi) * cos(theta) * cos(phi) + sin(psi) * sin(theta) * sin(phi)
q1 = cos(psi) * cos(theta) * sin(phi) - sin(psi) * sin(theta) * cos(phi)
q2 = cos(psi) * sin(theta) * cos(phi) + sin(psi) * cos(theta) * sin(phi)
q3 = -cos(psi) * sin(theta) * sin(phi) + sin(psi) * cos(theta) * cos(phi)
e = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
experimentSetUp(cogModel.client)
# # Set position of the player aircraft
# print("Setting position")
# # Lat Lon Alt Pitch Roll Yaw Gear
# posi = [37.524, -122.06899, 2500, 0, 0, 0, 1]
# client.sendPOSI(posi)
# # Set position of a non-player aircraft
# print("Setting NPC position")
# # Lat Lon Alt Pitch Roll Yaw Gear
# posi = [37.52465, -122.06899, 2500, 0, 20, 0, 1]
# client.sendPOSI(posi, 1)
# # Set angle of attack, velocity, and orientation using the DATA command
# print("Setting orientation")
# data = [\
# [18, 0, -998, 0, -998, -998, -998, -998, -998],\
# [ 3, 130, 130, 130, 130, -998, -998, -998, -998],\
# [16, 0, 0, 0, -998, -998, -998, -998, -998]\
# ]
# client.sendDATA(data)
# client.pauseSim(True)
# # Set position of the player aircraft
# print("Setting Experiment Position: Denver Airport Runway")
# # Lat Lon Alt Pitch Roll Yaw Gear
# posi = [39.945, -104.70, 2500, 0, 0, 0, 1] #3NM Approach for Denver Intl Runway 16R (16,000 feet)
# client.sendPOSI(posi)
# quat = (1.0,0.0,0.0,173.0)
# quatDref = "sim/flightmodel/position/q"
# client.sendDREF(quatDref,quat)
# speed = (1.0,0.0,0.0,173.0)
# client.sendDREF("sim/flightmodel/position/indicated_airspeed",speed)
# client.pauseSim(False)
quat = [q0/e,q1/e,q2/e,q3/e]
return quat
# # Set angle of attack, velocity, and orientation using the DATA command
# print("Setting orientation")
# data = [\
# [18, 0, -998, 0, -998, -998, -998, -998, -998],\
# [ 3, 130, 130, 130, 130, -998, -998, -998, -998],\
# [16, 0, 0, 0, -998, -998, -998, -998, -998]\
# ]
# client.sendDATA(data)
def experimentSetUp(client,altitudeInput):
print("Entered: EXPERIMENTSETUP")
if(True):
#Location:
groundLevel = 5434
offset = 4000
offset = altitudeInput
altitudeFEET = groundLevel + offset
altitudeMETERS = altitudeFEET/3.281
altitude = altitudeMETERS
print(str(altitude))
location1 = [20, -998, 39.96239, -104.69713, altitude, -998, -998, -998, -998]
# testLocation = [20, -998, 27.20579, -80.08621, altitude, -998, -998, -998, -998] # 27.20579°N/80.08621°W
data = [
location1\
]
client.sendDATA(data)
print("Zero velocities")
x = "sim/flightmodel/position/local_vx"
y = "sim/flightmodel/position/local_vy"
z = "sim/flightmodel/position/local_vz"
client.sendDREF(x,0)
client.sendDREF(y,0)
client.sendDREF(z,0)
print("Zero rotation")
p = "sim/flightmodel/position/P"
q = "sim/flightmodel/position/Q"
r = "sim/flightmodel/position/R"
client.sendDREF(p,0)
client.sendDREF(q,0)
client.sendDREF(r,0)
print("set heading")
# client.pauseSim(True)
orient = "sim/flightmodel/position/q"
# orientCommand = [1,0.0,0.0,0.0]
pitch = client.getDREF("sim/flightmodel/position/true_theta")
roll = client.getDREF("sim/flightmodel/position/true_phi")
orientCommand = eulerToQuat(179,0,0) # heading, pitch,Roll
orientTest = [1.0,1.0,1.0,1.0] # heading, pitch,Roll
print("ORIENT TO: " + str(orientCommand))
client.sendDREF(orient,orientCommand)
orientResult = client.getDREF(orient)
print(str(orientResult))
# client.pauseSim(False)
#Weather:
windLayer = "sim/weather/wind_altitude_msl_m[0]"
windLayer2 = "sim/weather/wind_altitude_msl_m[1]"
windLayer3 = "sim/weather/wind_altitude_msl_m[2]"
windDirection = "sim/weather/wind_direction_degt[0]"
windSpeed = "sim/weather/wind_speed_kt[0]"
# windDirections = [50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0]
# client.sendDREF(windDirection,windDirections)
# winds = [50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0,50.0]
# client.sendDREF(windSpeed,winds)
# result = client.getDREF(windLayer)
# print("Wind:" + str(result))
print("Setting Wind Layers")
client.sendDREF(windLayer,6000)
client.sendDREF(windLayer2,15000)
client.sendDREF(windLayer3,15000)
print("Setting Wind Direction and Speed")
client.sendDREF(windDirection,170)
print("Set 1")
client.sendDREF(windSpeed,0)
print("Set 2")
# client.sendDREF(turbulence,turbulencePercentage)
# preset = "sim/weather/region/weather_preset"
# value = 8
# client.sendDREF(preset,value)
client.pauseSim(True)
input("Press Enter to finish setting up Simulation")
client.pauseSim(False)
print("Setting initial velocity")
zInit = "sim/flightmodel/position/local_vx"
client.sendDREF(zInit, 0)
print("setup complete")
"""
Set orientation and Position
1 - Time
@@ -90,9 +122,6 @@ def runExperiment(title):
18 - Angle of Attack
20 - Latitude and Longitude
"""
location1 = [20, groundLevel + 3000, 39.96239, -104.69713, -998, -998, -998, -998, -998]
testLocation = [20, -998, 27.20579, -80.08621, altitude, -998, -998, -998, -998] # 27.20579°N/80.08621°W
kias = -998
keas = -998
ktas = -998
@@ -120,36 +149,47 @@ def runExperiment(title):
# client.sendDATA(data)
# client.sendDREF("sim/operation/override/override_planepath",0)
client.pauseSim(False)
# Set control surfaces and throttle of the player aircraft using sendCTRL
print("Setting controls")
ctrl = [0.0, 0.0, 0.0, 0.0]
client.sendCTRL(ctrl)
print("past setting controls")
# Pitch, Roll, Rudder, Throttle
# Pause the sim
# client.pauseSim(False)
# print("Setting controls")
# ctrl = [0.0, 0.0, 0.0, 0.0]
# cogModel.client.sendCTRL(ctrl)
# print("past setting controls")
else:
print("Experiment currently in progress, not resetting position and environmental conditions")
def runExperiment(title,printFlag,experimentStart):
print("Model Test: Xplane setting up connection")
print("Setting up simulation")
startTime = 0
endTime = 0
difference = endTime - startTime
experimentLive = True
timeoutLimit = 1
newExperiment = experimentStart
while(difference < timeoutLimit and experimentLive):
print("Time Elapsed: -----> " + str(difference))
try:
with xpc.XPlaneConnect() as client:
# Verify connection
client.getDREF("sim/test/test_float")
cogModel = AircraftLandingModel(client,printFlag)
experimentSetUp(cogModel.client,3000)
cogModel.client.pauseSim(False)
count = 0
innercount = 0
clockStart = time.time()
retry = 0
newExperment = False
while(cogModel.simulationStatus()):
# print("Start of innerwhile loop")
clockStart = time.time() #START TIMER
# client.pauseSim(True) # Pause Simulator
#Run Model
# print("----------------> 1 <")
cogModel.update_aircraft_state()
# print("----------------> 2 <")
cogModel.update_controls_simultaneously()
client.pauseSim(False) #Unpause Simulator
clockEnd = time.time() # STOP TIMER
count+=1
# print("Clock Time: " + str(clockEnd - clockStart)) ## LOG TIME TAKEN
sleep(0.05) # LET Simulator Run 50 Milliseconds
startTime = time.time()
experimentLive = cogModel.simulationStatus()
except:
@@ -159,8 +199,7 @@ def runExperiment(title):
print("End Time:" + str(endTime))
difference = endTime - startTime
continue
if(difference >= 3):
if(difference >= timeoutLimit):
print("Timeout[" + str(difference) +"]:"+"Error, please run test again")
else:
print("Model has finished running")
@@ -171,7 +210,12 @@ def runExperiment(title):
# print("CLEAN UP: Data File Copied and saved")
# os.remove("/Users/flyingtopher/X-Plane 11/Data.txt")
print("CLEAN UP: Data File Deleted and Reset")
input("Press any key to exit...")
exit = input("Press 'y' or any key to continue, press 'n' to exit...")
if(exit == "n"):
exit = True
else:
exit = False
return exit
##Reset the sim with the keyboard shortcut (wrapper around model that waits for reconnection)
def ex():
@@ -185,7 +229,9 @@ def ex():
input("Press Enter to Start Experiment #" + str(count) + ": ")
print("Data File Reset")
f = open("/Users/flyingtopher/X-Plane 11/Data.txt", 'w')
runExperiment(title)
exit = runExperiment(title,False,True)
if(exit):
break
count+=1
print("Experiment Battery Complete")

2
Python3/src/xpc/__init__.py
View File

@@ -311,7 +311,7 @@ class XPlaneConnect(object):
raise ValueError("value must have less than 256 items.")
fmt = "<B{0:d}sB{1:d}f".format(len(dref), len(value))
# buffer += struct.pack(fmt.encode(), len(dref), dref.encode(), len(value), value)
buffer += struct.pack(fmt.encode(), len(dref), dref.encode(), 2, *value)
buffer += struct.pack(fmt.encode(), len(dref), dref.encode(), len(value), *value)
else:
fmt = "<B{0:d}sBf".format(len(dref))