From 89991c248a403b5f2c3e971479ee1e36a122b109 Mon Sep 17 00:00:00 2001 From: cs-powell <142438185+cs-powell@users.noreply.github.com> Date: Mon, 31 Mar 2025 23:33:05 -0400 Subject: [PATCH] QUATERNION ORIENTIATION IS WORKING!! --- Python3/src/cognitiveModel.py | 6 - Python3/src/testPlatform.py | 290 ++++++++++++++++++++-------------- Python3/src/xpc/__init__.py | 2 +- 3 files changed, 169 insertions(+), 129 deletions(-) diff --git a/Python3/src/cognitiveModel.py b/Python3/src/cognitiveModel.py index f36b523..2605432 100644 --- a/Python3/src/cognitiveModel.py +++ b/Python3/src/cognitiveModel.py @@ -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): diff --git a/Python3/src/testPlatform.py b/Python3/src/testPlatform.py index 459b0aa..0289776 100644 --- a/Python3/src/testPlatform.py +++ b/Python3/src/testPlatform.py @@ -1,155 +1,195 @@ 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 eulerToQuat(psiInput,thetaInput,phiInput): + 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) -def runExperiment(title): + quat = [q0/e,q1/e,q2/e,q3/e] + return quat + +def experimentSetUp(client,altitudeInput): + print("Entered: EXPERIMENTSETUP") + if(True): + #Location: + groundLevel = 5434 + 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 + 3- Speeds: [3, V-indicated, 3, 2, 2, -998, VindMPH, V trueMPHas, vtrue MPHgs],\ + 16 - Angular Velocities + 17 - PitchRoll and Headings: [17, Pitch, Roll, Heading True, -998, -998, -998, -998, -998],\ + 18 - Angle of Attack + 20 - Latitude and Longitude + """ + kias = -998 + keas = -998 + ktas = -998 + ktgs = -998 + mph = -998 + mphas = 80 + mphgs = 80 + + print("Setting orientation for test") + # client.sendDREF("sim/operation/override/override_planepath",1) + # client.pauseSim(True) + + # client.sendDREF("sim/flightmodel/position/local_y",2000) + # client.sendDREFs(["sim/flightmodel/position/P","sim/flightmodel/position/Q","sim/flightmodel/position/R"],[0,0,0]) + # client.sendDREF("sim/flightmodel/position/q",[0.3, 0, 0, 0.6]) + + # data = [\ + # [3, -998, -998, -998, -998, -998, -998, mphas, mphgs],\ + # [16, 0, 0, 0, -998, -998, -998, -998, -998],\ + # [17, 0, 0, 0, -998, -998, -998, -998, -998],\ + # [18, -998, -998, -998, -998, -998, -998, -998, -998],\ + # [19, -998, -998, -998, -998, -998, -998, -998, -998],\ + # testLocation\ + # ] + # client.sendDATA(data) + # client.sendDREF("sim/operation/override/override_planepath",0) + + # Set control surfaces and throttle of the player aircraft using sendCTRL + # 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 - while(difference < 3 and experimentLive): + timeoutLimit = 1 + newExperiment = experimentStart + while(difference < timeoutLimit 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) - - 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) - - - # # 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) - - groundLevel = 5434 - offset = 4000 - altitudeFEET = groundLevel + offset - altitudeMETERS = altitudeFEET/3.281 - altitude = altitudeMETERS - """ - Set orientation and Position - 1 - Time - 3- Speeds: [3, V-indicated, 3, 2, 2, -998, VindMPH, V trueMPHas, vtrue MPHgs],\ - 16 - Angular Velocities - 17 - PitchRoll and Headings: [17, Pitch, Roll, Heading True, -998, -998, -998, -998, -998],\ - 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 - ktgs = -998 - mph = -998 - mphas = 80 - mphgs = 80 - - print("Setting orientation for test") - # client.sendDREF("sim/operation/override/override_planepath",1) - # client.pauseSim(True) - - # client.sendDREF("sim/flightmodel/position/local_y",2000) - # client.sendDREFs(["sim/flightmodel/position/P","sim/flightmodel/position/Q","sim/flightmodel/position/R"],[0,0,0]) - # client.sendDREF("sim/flightmodel/position/q",[0.3, 0, 0, 0.6]) - - # data = [\ - # [3, -998, -998, -998, -998, -998, -998, mphas, mphgs],\ - # [16, 0, 0, 0, -998, -998, -998, -998, -998],\ - # [17, 0, 0, 0, -998, -998, -998, -998, -998],\ - # [18, -998, -998, -998, -998, -998, -998, -998, -998],\ - # [19, -998, -998, -998, -998, -998, -998, -998, -998],\ - # testLocation\ - # ] - # 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) + cogModel = AircraftLandingModel(client,printFlag) + experimentSetUp(cogModel.client,3000) + cogModel.client.pauseSim(False) count = 0 innercount = 0 clockStart = time.time() retry = 0 - while(cogModel.simulationStatus()): - # print("Start of innerwhile loop") + newExperment = False + while(cogModel.simulationStatus()): 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") diff --git a/Python3/src/xpc/__init__.py b/Python3/src/xpc/__init__.py index 7851450..0a8fcc1 100644 --- a/Python3/src/xpc/__init__.py +++ b/Python3/src/xpc/__init__.py @@ -311,7 +311,7 @@ class XPlaneConnect(object): raise ValueError("value must have less than 256 items.") fmt = "