new heading tracking via longitude complete

Python3/src/cognitiveModel.py

@@ -4,6 +4,7 @@ import xpc
 import math
 # import geopy
 from geographiclib.geodesic import Geodesic as geo
+from rich import print
 
 
 
@@ -12,6 +13,7 @@ class scaleFactor():
     SCALEYOKEPULL = 10
     SCALEYOKESTEER = 10
     SCALERUDDER = 10
+    SCALELATITUDERUDDER = 0.02
     SCALETHROTTLE = 1000
 
 ###Define variables/parameters for aircraft class/category : Wisdom of Raju 
@@ -22,13 +24,15 @@ class AircraftLandingModel(pyactr.ACTRModel):
         self.inProgress = True
         self.printControlsFlag = printFlag
         self.targetLat = 39.895791
-        self.targetLong = -104.696085
+        self.targetLong = -104.696014
         """
         Setting DREF variables and loading into drefs array
         """
         airspeedDREF = "sim/cockpit2/gauges/indicators/airspeed_kts_pilot"
         rollDREF = "sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot"
         magneticHeadingDREF = "sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_pilot"
+        latitudeDREF = "sim/flightmodel/position/latitude" ## Lat 
+        longitudeDREF = "sim/flightmodel/position/longitude" ##Long
         verticalSpeedDREF = "sim/flightmodel/position/vh_ind_fpm"
         altitudeAGLDREF = "sim/flightmodel/position/y_agl"
         pitchDREF = "sim/flightmodel/position/true_theta"
@@ -41,6 +45,8 @@ class AircraftLandingModel(pyactr.ACTRModel):
             "airspeed" : airspeedDREF,
             "roll" : rollDREF,
             "heading" : magneticHeadingDREF,
+            "latitude": latitudeDREF,
+            "longitude": longitudeDREF,
             "vertical speed" : verticalSpeedDREF,
             "altitude": altitudeAGLDREF,
             "pitch" : pitchDREF,
@@ -55,6 +61,8 @@ class AircraftLandingModel(pyactr.ACTRModel):
         airspeed = self.client.getDREF("sim/cockpit2/gauges/indicators/airspeed_kts_pilot")
         roll = self.client.getDREF("sim/cockpit2/gauges/indicators/roll_AHARS_deg_pilot")
         heading = self.client.getDREF("sim/cockpit2/gauges/indicators/heading_AHARS_deg_mag_pilot")
+        latitude = self.client.getDREF("sim/flightmodel/position/latitude") ##Current Lat 
+        longitude = self.client.getDREF("sim/flightmodel/position/longitude") ##Current Long
         descent_rate = self.client.getDREF("sim/flightmodel/position/vh_ind_fpm")
         altitude = self.client.getDREF("sim/flightmodel/position/y_agl")
         pitch = self.client.getDREF("sim/flightmodel/position/true_theta")
@@ -67,6 +75,8 @@ class AircraftLandingModel(pyactr.ACTRModel):
         self.airspeed = airspeed[0]
         self.roll = roll[0]
         self.heading = HARDCODE_HEADING
+        self.latitude = latitude[0]
+        self.longitude = longitude[0]
         self.descent_rate = descent_rate[0]
         self.altitude = altitude[0]
         self.pitch = pitch[0]
@@ -79,6 +89,8 @@ class AircraftLandingModel(pyactr.ACTRModel):
             "airspeed" : self.airspeed,
             "roll" : self.roll,
             "heading" : self.heading,
+            "latitude": self.latitude,
+            "longitude": self.longitude,
             "vertical speed" : self.descent_rate,
             "altitude": self.altitude,
             "pitch" : self.pitch,
@@ -93,10 +105,12 @@ class AircraftLandingModel(pyactr.ACTRModel):
         self.target_airspeed = 80
         self.target_roll = 0
         self.target_heading = self.heading #Track heading from initialization[DEPRECATED]
+        self.target_Lat = self.latitude #Track Lat
+        self.target_Long = self.longitude #Track Long
         self.target_descent_rate = 500
         self.target_altitude = -998
         self.target_pitch = 20
-        self.targets = [self.target_airspeed,self.target_roll,self.target_heading,self.target_descent_rate,self.target_altitude,self.target_pitch]
+        self.targets = [self.target_airspeed,self.target_roll,self.target_heading,self.target_Lat,self.target_Long,self.target_descent_rate,self.target_altitude,self.target_pitch]
         
 
         #State Flags (Boolean)  & Current State (Integer)
@@ -122,6 +136,8 @@ class AircraftLandingModel(pyactr.ACTRModel):
         self.integral_airspeed = 0
         self.integral_roll = 0
         self.integral_heading = 0
+        self.integral_Latitude = 0
+        self.integral_Longitude = 0
         self.integral_descent_rate = 0
 
         #Flare Specific Parameters
@@ -159,7 +175,7 @@ class AircraftLandingModel(pyactr.ACTRModel):
     def get_bearing(self,lat1, lat2, long1, long2):
         brng = geo.WGS84.Inverse(lat1, long1, lat2, long2)['azi1']
         self.target_heading = brng
-        print("TARGET BEARING: " + str(brng))
+        # print("TARGET BEARING: " + str(brng))
 
     def getAndLoadDREFS(self):
         results = self.client.getDREFs(self.sources.values())
@@ -171,6 +187,7 @@ class AircraftLandingModel(pyactr.ACTRModel):
         lat = self.client.getDREF("sim/flightmodel/position/latitude") ##Current Lat 
         long = self.client.getDREF("sim/flightmodel/position/longitude") ##Current Long
         self.get_bearing(lat[0],self.targetLat,long[0],self.targetLong)
+        
 
         # print("getDrefs: " + str(results[1][0]))
         # print("current destination: " + str(self.destinations["airspeed"]))
@@ -193,11 +210,36 @@ class AircraftLandingModel(pyactr.ACTRModel):
             # print("*Parameter,Target,Current,Yoke Steer:     " + "Roll, " + str(self.target_roll) + "," +  str(self.destinations["roll"])+ "," + str(yokeSteer))
             # print("*Parameter,Target,Current,Rudder:         " + "Heading, " + str(self.target_heading) + "," +  str(self.heading)+ "," + str(rudder))
             # print("*Parameter,Target,Current,Throttle:       " + "Descent Rate, " + str(self.target_descent_rate) + "," +  str(self.descent_rate)+ "," + str(throttle))
-            parameter = ["Airspeed","Roll","Heading","Descent Rate","Altitude","Flare: Pitch", "Brakes: Wheel Speed", "Brakes: Wheel Weight"]
+            parameter = ["Airspeed","Roll","Heading","Longitude","Descent Rate","Altitude","Flare: Pitch", "Brakes: Wheel Speed", "Brakes: Wheel Weight"]
-            target =  [str(round(self.target_airspeed)),str(round(self.target_roll)),str(round(self.target_heading,3)),str(round(self.target_descent_rate,3)),str(round(self.altitude,3)),str(round(self.target_pitch,3)),0, 0]
+            target =  [str(round(self.target_airspeed)),
-            current = [str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),str(round(self.dictionaryAccess(self.destinations,"roll"),3)),str(round(self.dictionaryAccess(self.destinations,"heading"),3)),str(round(self.dictionaryAccess(self.destinations,"vertical speed"),3)),
+                       str(round(self.target_roll)),
-                       str(round(self.dictionaryAccess(self.destinations,"altitude"),3)),str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),str(round(self.dictionaryAccess(self.destinations,"wheelSpeed"),3)),str(round(self.dictionaryAccess(self.destinations,"wheelWeight"),3))]
+                       str(round(self.target_heading,3)),
-            controlVal = [str(round(yokePull,3)),str(round(yokeSteer,3)),str(round(rudder,3)),str(round(throttle,3)),str(round(self.altitude,3)),str(self.dictionaryAccess(self.phaseFlags,"flare")),str(round(self.dictionaryAccess(self.destinations,"brakes"),3)),str(round(self.dictionaryAccess(self.destinations,"brakes"),3))]
+                       str(round(self.target_Long,6)),
+                       str(round(self.target_descent_rate,3)),
+                       str(round(self.altitude,3)),
+                       str(round(self.target_pitch,3)),
+                       0, 
+                       0]
+            
+            current = [str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"roll"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"heading"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"longitude"),6)),
+
+                       str(round(self.dictionaryAccess(self.destinations,"vertical speed"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"altitude"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"pitch"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"wheelSpeed"),3)),
+                       str(round(self.dictionaryAccess(self.destinations,"wheelWeight"),3))]
+            controlVal = [str(round(yokePull,3)),
+                          str(round(yokeSteer,3)),
+                          str(round(rudder,3)),
+                          str(round(rudder,3)), 
+                          str(round(throttle,3)),
+                          str(round(self.altitude,3)),
+                          str(self.dictionaryAccess(self.phaseFlags,"flare")),
+                          str(round(self.dictionaryAccess(self.destinations,"brakes"),3)),
+                          str(round(self.dictionaryAccess(self.destinations,"brakes"),3))]
 
             header_row = "{:<20} {:<20} {:<20} {:>10}"
             headers = "Parameter Target Current Control_Value".split()
@@ -281,7 +323,18 @@ class AircraftLandingModel(pyactr.ACTRModel):
             #                                                                  scaleFactor.SCALEYOKEPULL)
 
         yoke_steer, self.integral_roll = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"roll"), self.target_roll, self.integral_roll,scaleFactor.SCALEYOKESTEER)
-        rudder, self.integral_heading = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"heading"), self.target_heading, self.integral_heading,scaleFactor.SCALERUDDER)
+
+        """
+        RUDDER CONTROLS
+        """
+        #ORIGINAL
+        # rudder, self.integral_heading = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"heading"), self.target_heading, self.integral_heading,scaleFactor.SCALERUDDER) 
+        #lATITUDE/LONGITUDE
+        rudder, self.integral_Longitude = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"longitude"), self.target_Long, self.integral_Longitude,scaleFactor.SCALELATITUDERUDDER)
+
+        
+        
+        
         throttle, self.integral_descent_rate = self.proportionalIntegralControl(0,self.dictionaryAccess(self.destinations,"vertical speed"), self.target_descent_rate, self.integral_descent_rate,scaleFactor.SCALETHROTTLE)
         ### 1. For Calculated Yoke and Throttle Values 
         #Invert Throttle Control & divide by 5 to scale
@@ -323,7 +376,7 @@ class AircraftLandingModel(pyactr.ACTRModel):
  
         ##Method 2: Same Control Statements with Change in Parameter to decided pitch from Airspeed ---> Local Pitch Relative to the Horizon
 
-
+        rudder = rudder * -1
         #Switch Target for Pitch to Local Pitch Axis (ex. +10 Degrees nose up)
         if(self.printControlsFlag):
             self.printControls(1,0,yoke_pull,yoke_steer,rudder,throttle) #PRINT CONTROLS 
@@ -383,7 +436,7 @@ class AircraftLandingModel(pyactr.ACTRModel):
            and self.dictionaryAccess(self.destinations,"brakes") == 1):            
             self.inProgress = False
     
-    def simulationStatus(self):
+    def getSimulationStatus(self):
         return self.inProgress
         
 

Python3/src/experiments/weather_files/weather.csv

@@ -0,0 +1,8 @@
+Experiment Number,Starting Altitude (AGL),Wind Layer Altitude,Wind Direction,Wind Speed
+1,2000,0,179,0
+2,2000,0,179,5
+3,2000,0,179,10
+4,2000,0,179,15
+5,2000,0,179,20
+6,2000,0,179,25
+7,2000,0,179,30

Python3/src/experiments/weather_files/weather_2.csv

@@ -1,5 +0,0 @@
-Wind,2,3,4
-Altitudes,3,4,5
-Turtles,4,5,6
-Ducks,5,6,7
-Penguins,6,7,8

Python3/src/testPlatform.py

@@ -1,5 +1,9 @@
+
+
 import datetime
 from math import cos, pi, sin, sqrt
+
+
 import os
 import time
 from time import sleep
@@ -7,8 +11,12 @@ import xpc
 from cognitiveModel import AircraftLandingModel
 import shutil
 import csv
+from enum import Enum
 
 
+class messageType(Enum):
+    REGULAR = 1
+    ERROR = 2
 
 def eulerToQuat(psiInput,thetaInput,phiInput):
     
@@ -24,28 +32,25 @@ def eulerToQuat(psiInput,thetaInput,phiInput):
     quat = [q0/e,q1/e,q2/e,q3/e]
     return quat
 
-
+def loadFile():
-def loadFile(index):
+    filename = "Python3/src/experiments/weather_files/weather.csv"
-    filename = "Python3/src/experiments/weather_files/weather_" + str(index) + ".csv"
     with open(filename,"r") as f:
         matrix = list(csv.reader(f,delimiter=','))
         print(matrix)
         return matrix
+    
 
 
 def selectWeather(matrix,experimentNumber):
     return matrix[experimentNumber]
 
-
+def experimentSetUp(client,currentConditions,newExperiment):
-
-def experimentSetUp(client,altitudeInput,newExperiment):
-    loadFile(2)
     # input("Check The Loaded File Now")
     print("Entered: EXPERIMENTSETUP")
     if(newExperiment):
         #Location:
         groundLevel = 5434
-        offset = altitudeInput
+        offset = float(currentConditions[1])
         altitudeFEET = groundLevel + offset
         altitudeMETERS = altitudeFEET/3.281
         altitude = altitudeMETERS
@@ -58,6 +63,7 @@ def experimentSetUp(client,altitudeInput,newExperiment):
         client.sendDATA(data)
 
 
+
         print("Zero velocities")
         x = "sim/flightmodel/position/local_vx"
         y = "sim/flightmodel/position/local_vy"
@@ -104,19 +110,18 @@ def experimentSetUp(client,altitudeInput,newExperiment):
             # result = client.getDREF(windLayer)
             # print("Wind:" + str(result))
         print("Setting Wind Layers")
-        client.sendDREF(windLayer,6000)
+        client.sendDREF(windLayer,float(currentConditions[2]))
-        client.sendDREF(windLayer2,15000)
+        # client.sendDREF(windLayer2,15000)
-        client.sendDREF(windLayer3,15000)
+        # client.sendDREF(windLayer3,15000)
         print("Setting Wind Direction and Speed")
-        client.sendDREF(windDirection,170)
-        print("Set 1")
 
-        client.sendDREF(windSpeed,40)
+        client.sendDREF(windDirection,float(currentConditions[3]))
+        print("Set 1")
+        client.sendDREF(windSpeed,float(currentConditions[4]))
         print("Set 2")
 
 
         # client.sendDREF(turbulence,turbulencePercentage)
-
         # preset = "sim/weather/region/weather_preset"
         # value = 8
         # client.sendDREF(preset,value)
@@ -139,121 +144,133 @@ def experimentSetUp(client,altitudeInput,newExperiment):
             18 - Angle of Attack
             20 - Latitude and Longitude
         """
-        kias = -998
+        message = "Conditions are set as\n" \
-        keas = -998
+        "Starting Altitude: {} \n" \
-        ktas = -998
+        "Layer Altitude: {} \n" \
-        ktgs = -998
+        "Wind Direction: {} \n" \
-        mph = -998
+        "Wind Speed: {} \n".format(currentConditions[1],currentConditions[2],currentConditions[3],currentConditions[4])
-        mphas = 80
+        specialPrint(message,False,messageType.REGULAR)        
-        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):
+def runExperiment(title,currentConditions,allowPrinting,isNewExperiment):
-    print("Model Test: Xplane setting up connection")
+    specialPrint("New Experiment\nSetting Up the Simulation",False,messageType.REGULAR)
-    print("Setting up simulation")
     startTime = 0
     endTime = 0
-    difference = endTime - startTime
+    timeElapsed = endTime - startTime
-    experimentLive = True
+    experimentInProgress = True
     timeoutLimit = 10
-    newExperiment =  experimentStart
+    newExperiment =  isNewExperiment
-    while(difference < timeoutLimit and experimentLive):
+
-        print("Time Elapsed: -----> " + str(difference))
+    while(timeElapsed <= timeoutLimit and experimentInProgress):
+        # print("Time Elapsed: -----> " + str(timeElapsed))
         try:
             with xpc.XPlaneConnect() as client:
-                # Verify connection
+                """ 
+                Test Connection
+                """
                 client.getDREF("sim/test/test_float")
-                cogModel = AircraftLandingModel(client,printFlag)
+                """
-                experimentSetUp(cogModel.client,2000,newExperiment)
+                Setup Model
+                """
+                cogModel = AircraftLandingModel(client,allowPrinting)
+                """
+                Set Simulator Conditions
+                """
+                experimentSetUp(cogModel.client,currentConditions,newExperiment)
+                """
+                """
                 cogModel.client.pauseSim(False)
-                count = 0
-                innercount = 0
-                clockStart = time.time()
-                retry = 0
                 newExperiment = False
-                while(cogModel.simulationStatus()):  
+                
-                    clockStart = time.time() #START TIMER
+                """
-                    #Run Model
+                Single Experiment Loop
-                    cogModel.update_aircraft_state()
+                """
+                while(experimentInProgress):
+                    cogModel.update_aircraft_state() 
                     cogModel.update_controls_simultaneously()
-                    client.pauseSim(False) #Unpause Simulator
+                    client.pauseSim(False)          #Unpause Simulator
-                    clockEnd = time.time() # STOP TIMER
+                    sleep(0.05)                     # Let Simulator Run 50 Milliseconds
-                    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()
+                    experimentInProgress = cogModel.getSimulationStatus()
         except:
-            print("except detected")
             endTime = time.time()
-            print("Start Time:" + str(startTime))
+            timeElapsed = endTime - startTime
-            print("End Time:" + str(endTime))
+            message = "Except detected\nStart Time: {a}\nEnd Time: {b}\n" \
-            difference = endTime - startTime
+            "Time Elapsed: -----> {c} ".format(a= startTime, b=endTime,c=timeElapsed)
+            specialPrint(message,False,messageType.ERROR)
             continue
-    if(difference >= timeoutLimit):
+
-        print("Timeout[" + str(difference) +"]:"+"Error, please run test again")
+    """
+    Parse End Condition: Succesful Run or Timeout-Induced End
+    """
+    if(timeElapsed >= timeoutLimit):
+        print("Timeout[" + str(timeElapsed) +"]:"+"Error, please run test again")
     else: 
         print("Model has finished running")
 
-    #Copy data.txt to the cloudddddd using python magic and accurate filepaths
+
+    """
+    Ask Experimenter if they would like to exit experiment battery early 
+    """
+    exitDecision = input("Press 'y' or any key to continue, press 'n' to exit...")
+    if(exitDecision == "n"):
+        exitExperimentLoop = True
+    else:
+        exitExperimentLoop = False
+    return exitExperimentLoop
+
+def setUp():
+    open("/Users/flyingtopher/X-Plane 11/Data.txt", 'w')
+    specialPrint("Data Collection File Ready",False,messageType.REGULAR)
+
+def cleanUp(title):
     now = datetime.datetime
     shutil.copy("/Users/flyingtopher/X-Plane 11/Data.txt", "/Users/flyingtopher/Desktop/Test Destination/" + title + "_" + str(now.now()) + "_" + ".txt")
-    # 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")
-    # exit = input("Press 'y' or any key to continue, press 'n' to exit...")
-    exit = False
-    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():
-    ##Store Experiment Battery
-    ##Different paramters on every run of the model
-    ## Nested loops: 
-        ##wind conditions, pilot conditions
-    # title = input("Please Enter Experiment Set Title, leave blank for trial runs")
-    title = 'test'
-    count = 0
-    while(count<7):
-        # input("Press Enter to Start Experiment #" + str(count) + ": ")
-        print("Data File Reset")
-        f = open("/Users/flyingtopher/X-Plane 11/Data.txt", 'w')
-        exit = runExperiment(title,False,True)
-        if(exit):
-            break
-        count+=1
 
-    print("Experiment Battery Complete")
+    specialPrint("Data File Ready",False,messageType.REGULAR)
+
+def specialPrint(text, inputRequested,type): 
+    if(type == messageType.REGULAR): 
+        print("-" * 81)
+        print(text)
+        print("-" * 81 + "\n")
+        if(inputRequested):
+            inputReceived = input()
+            return inputReceived
+    if(type == messageType.ERROR):
+        print("*" * 81)
+        print(text)
+        print("*" * 81 + "\n")
+
+
+def ex():
+    """
+    One Time experimental setup
+    """
+    title = specialPrint("Please Enter Experiment Set Title, leave blank for trial runs\n", True, messageType.REGULAR)
+    experimentConditionMatrix = loadFile()
+    allowPrinting = True
+    isNewExperiment = True
+    experimentCount = 0
+    """
+    Experiment Loop
+    """
+    while(experimentCount<len(experimentConditionMatrix)):
+        setUp()
+        currentConditions = experimentConditionMatrix[experimentCount+1]
+        exitExperimentLoop = runExperiment(title,currentConditions,allowPrinting,isNewExperiment)
+        if(exitExperimentLoop):
+            break
+        experimentCount+=1
+        cleanUp(title)
+    """
+    End of Experiments
+    """
+    specialPrint("Experiment Battery Complete", False,messageType.REGULAR)
+
+
 
 if __name__ == "__main__":
     ex()