QUATERNION ORIENTIATION IS WORKING!!

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):

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:
+                cogModel = AircraftLandingModel(client,printFlag)
-                #     print("Error establishing connection to X-Plane.")
+                experimentSetUp(cogModel.client,3000)
-                #     print("Exiting...")
+                cogModel.client.pauseSim(False)
-                #     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)
                 count = 0
                 innercount = 0
                 clockStart = time.time()
                 retry = 0
-                while(cogModel.simulationStatus()):
+                newExperment = False
-                    # print("Start of innerwhile loop")
+                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 >= timeoutLimit):
-    if(difference >= 3):
         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")
 

Python3/src/xpc/__init__.py

@@ -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))