388 lines
18 KiB
Python
388 lines
18 KiB
Python
import pyactr
|
|
# from XPlaneConnect import *
|
|
import xpc
|
|
import math
|
|
# Initialize XPlaneConnect client
|
|
class scaleFactor():
|
|
SCALEYOKEPULL = 10
|
|
SCALEYOKESTEER = 10
|
|
SCALERUDDER = 10
|
|
SCALETHROTTLE = 1000
|
|
|
|
###Define variables/parameters for aircraft class/category : Wisdom of Raju
|
|
class AircraftLandingModel(pyactr.ACTRModel):
|
|
def __init__(self,client,printFlag):
|
|
super().__init__()
|
|
self.client = client
|
|
self.inProgress = True
|
|
self.printControlsFlag = printFlag
|
|
|
|
"""
|
|
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"
|
|
verticalSpeedDREF = "sim/flightmodel/position/vh_ind_fpm"
|
|
altitudeAGLDREF = "sim/flightmodel/position/y_agl"
|
|
pitchDREF = "sim/flightmodel/position/true_theta"
|
|
brakeDREF = "sim/cockpit2/controls/parking_brake_ratio"
|
|
wheelSpeedDREF = "sim/flightmodel2/gear/tire_rotation_speed_rad_sec"
|
|
wheelWeightDREF = "sim/flightmodel/parts/tire_vrt_def_veh"
|
|
# self.sources = [airspeedDREF,rollDREF,magneticHeadingDREF,verticalSpeedDREF,altitudeAGLDREF,pitchDREF,brakeDREF,wheelSpeedDREF,wheelWeightDREF]
|
|
|
|
self.sources = {
|
|
"airspeed" : airspeedDREF,
|
|
"roll" : rollDREF,
|
|
"heading" : magneticHeadingDREF,
|
|
"vertical speed" : verticalSpeedDREF,
|
|
"altitude": altitudeAGLDREF,
|
|
"pitch" : pitchDREF,
|
|
"brakes": brakeDREF,
|
|
"wheelSpeed": wheelSpeedDREF,
|
|
"wheelWeight": wheelWeightDREF
|
|
}
|
|
|
|
"""
|
|
Initial Initialization of destination Variables and loading into destinations array
|
|
"""
|
|
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")
|
|
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")
|
|
brake = self.client.getDREF("sim/cockpit2/controls/parking_brake_ratio")
|
|
wheelS = self.client.getDREF("sim/flightmodel2/gear/tire_rotation_speed_rad_sec")
|
|
wheelW = self.client.getDREF("sim/flightmodel/parts/tire_vrt_def_veh")
|
|
|
|
|
|
self.airspeed = airspeed[0]
|
|
self.roll = roll[0]
|
|
self.heading = heading[0]
|
|
self.descent_rate = descent_rate[0]
|
|
self.altitude = altitude[0]
|
|
self.pitch = pitch[0]
|
|
self.brakes = brake[0]
|
|
self.wheelSpeed = wheelS[0]
|
|
self.wheelWeight = wheelW[0]
|
|
# self.destinations = [self.airspeed,self.roll,self.heading,self.descent_rate,self.altitude,self.pitch,self.brakes,self.wheelSpeed,self.wheelWeight]
|
|
|
|
self.destinations = {
|
|
"airspeed" : self.airspeed,
|
|
"roll" : self.roll,
|
|
"heading" : self.heading,
|
|
"vertical speed" : self.descent_rate,
|
|
"altitude": self.altitude,
|
|
"pitch" : self.pitch,
|
|
"brakes": self.brakes,
|
|
"wheelSpeed": self.wheelSpeed,
|
|
"wheelWeight": self.wheelWeight
|
|
}
|
|
|
|
"""
|
|
Initial Initialization of target Values
|
|
"""
|
|
self.target_airspeed = 80
|
|
self.target_roll = 0
|
|
self.target_heading = self.heading #Track heading from initialization
|
|
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]
|
|
|
|
|
|
#State Flags (Boolean) & Current State (Integer)
|
|
self.descent = False
|
|
self.flare = False
|
|
self.rollOut = False
|
|
self.currentState = 0
|
|
# self.stateFlags = [self.descent,self.flare,self.rollOut]
|
|
self.phaseFlags = {
|
|
"descent" : self.descent,
|
|
"flare" : self.flare,
|
|
"roll out" : self.rollOut
|
|
}
|
|
|
|
|
|
# Declare the state for previous values
|
|
self.previous_airspeed = None
|
|
self.previous_roll = None
|
|
self.previous_heading = None
|
|
self.previous_descent_rate = None
|
|
|
|
# Initialize the integral errors for each parameter
|
|
self.integral_airspeed = 0
|
|
self.integral_roll = 0
|
|
self.integral_heading = 0
|
|
self.integral_descent_rate = 0
|
|
|
|
#Flare Specific Parameters
|
|
self.integral_pitch = 0
|
|
|
|
# Integral gains (tune these values for performance)
|
|
self.Kp = 0.1 # Proportional gain
|
|
self.Ki = 0.01 # Integral gain
|
|
# self.Ki = 2 # Integral gain
|
|
|
|
|
|
"""
|
|
Variable Atlas Schema: source => destination => target
|
|
"""
|
|
# self.variableAtlas = list(xrange(1000))
|
|
# idx = 0
|
|
# while(idx < len(self.sources) - 1):
|
|
# self.variableAtlas[0] = [self.sources[idx],self.destinations[idx],self.targets[idx]]
|
|
# idx += 1
|
|
|
|
def dictionaryAccess(self,dictionary,key):
|
|
# print("dictionary access for: " + str(key))
|
|
result = dictionary[key]
|
|
if isinstance(result, tuple):
|
|
return result[0]
|
|
else:
|
|
return result
|
|
|
|
|
|
|
|
|
|
def reassignClient(self,newClient):
|
|
self.client = newClient
|
|
|
|
def getAndLoadDREFS(self):
|
|
results = self.client.getDREFs(self.sources.values())
|
|
idx = 0
|
|
for key in self.sources:
|
|
self.destinations[key] = results[idx]
|
|
idx+=1
|
|
# print("getDrefs: " + str(results[1][0]))
|
|
# print("current destination: " + str(self.destinations["airspeed"]))
|
|
# print("current main Airspeed: " + str(self.airspeed))
|
|
|
|
# while(idx < len(results) - 2):
|
|
# self.destinations[idx] = results[idx][0]
|
|
# if(idx == 1):
|
|
# print("getDrefs: " + str(results[idx][0]))
|
|
# print("current destination: " + str(self.destinations[idx]))
|
|
# print("current main Airspeed: " + str(self.airspeed))
|
|
# idx += 1
|
|
|
|
|
|
def printControls(self,calculated,errors,yokePull,yokeSteer,rudder,throttle):
|
|
# print("In print controls")
|
|
if(calculated == 1):
|
|
# print("* Calculated Controls *")
|
|
# print("*Parameter,Target,Current,Yoke Pull: " + "Airspeed, " + str(self.target_airspeed) + "," + str(self.airspeed)+ "," + str(yokePull))
|
|
# 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"]
|
|
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]
|
|
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.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(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()
|
|
row = "{:<20} {:<20} {:<20} {:>10}"
|
|
print("\n" + header_row.format(*headers))
|
|
print("-" * 81)
|
|
for parameter, target, current, controlVal in zip(parameter, target, current, controlVal):
|
|
print(row.format(parameter, target, current, controlVal))
|
|
|
|
|
|
def printVariables(self,errors,target,current,error,param1,param2):
|
|
if(errors == 1):
|
|
targetF = [str(round(target,3))]
|
|
currentF = [str(round(current,3))]
|
|
errorF = [str(round(error,3))]
|
|
param1F = [str(round(param1,3))]
|
|
param2F = [str(round(param2,3))]
|
|
row = "{:<20} {:<20} {:<20} {:>10} {:>7.2f}"
|
|
header_row = "{:<20} {:<20} {:<20} {:>10} {:>7}"
|
|
headers = "Target Current Error (self.Kp*error) (self.Ki*integral_error)".split()
|
|
print("\n" +header_row.format(*headers))
|
|
print("-" * 81)
|
|
# print(print(row.format(first_, last_, major_, credits_, gpa_)))
|
|
for targetF, currentF, errorF, param1F, param2F in zip(targetF, currentF, errorF, param1F, param2F):
|
|
print(row.format(target, current, error, param1, param2))
|
|
|
|
# print("* Target, Current, Error, param1, param2 \n" +
|
|
# "* ______ _______ ______ _______ ______\n " +
|
|
# str(round(target,2)) + "\n " + str(round(current,2)) +
|
|
# "\n " + str(round(error,2)) +
|
|
# "\n " + str(round(param1,2)) +
|
|
# "\n " + str(round(param2,2)))
|
|
|
|
|
|
|
|
def proportionalIntegralControl(self,print, current, target, integral_error,scalingFactor):
|
|
"""
|
|
Proportional-Integral control rule implementation for multiple parameters.
|
|
"""
|
|
# Calculate the error (current - target)
|
|
error = target - current
|
|
# error = current - target
|
|
# print("Error: " + str(error))
|
|
# Update the integral of the error
|
|
integral_error += error
|
|
# print("Integral_error: " + str(integral_error))
|
|
|
|
# Calculate the control value using the PI formula
|
|
control_value = (self.Kp * error) + (self.Ki * integral_error)
|
|
|
|
###Transformations:
|
|
#Simple Sigmoid:
|
|
control_value = (2 / (1 + math.exp(-(control_value/scalingFactor)))) - 1
|
|
|
|
# self.printVariables(print,target,current,error,(self.Kp * error),(self.Ki * integral_error))
|
|
return control_value, integral_error # Return control value and updated integral error
|
|
|
|
|
|
def update_controls_simultaneously(self):
|
|
"""
|
|
Update all controls at the same time by calculating control values for each parameter.
|
|
"""
|
|
# print("In update controls")
|
|
# print("Entered Update Controls Simultaneously")
|
|
# Compute control values for all parameters (yoke pull, yoke steer, rudder, throttle)
|
|
if(self.dictionaryAccess(self.phaseFlags,"flare")):
|
|
yoke_pull, self.integral_airspeed = self.proportionalIntegralControl(1,self.dictionaryAccess(self.destinations,"pitch"),
|
|
self.target_pitch,
|
|
self.integral_pitch,
|
|
scaleFactor.SCALEYOKEPULL)
|
|
|
|
if(self.dictionaryAccess(self.phaseFlags,"flare") == False):
|
|
self.target_pitch = 10
|
|
yoke_pull, self.integral_airspeed = self.proportionalIntegralControl(1,self.dictionaryAccess(self.destinations,"pitch"),
|
|
self.target_pitch,
|
|
self.integral_pitch,
|
|
scaleFactor.SCALEYOKEPULL)
|
|
# yoke_pull, self.integral_airspeed = self.proportionalIntegralControl(1,self.airspeed,
|
|
# self.target_airspeed,
|
|
# self.integral_airspeed,
|
|
# 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)
|
|
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
|
|
|
|
throttle = -throttle
|
|
throttle = throttle/5
|
|
#Invert Yoke Pull & divide by 5 to scale
|
|
yoke_pull = yoke_pull/5
|
|
## 2. For Constant Yoke and Throttle Values
|
|
# Constant yoke "back pressure" equal to 20% of total travel distance
|
|
if(self.dictionaryAccess(self.phaseFlags,"flare") == False):
|
|
yoke_pull = yoke_pull * 20
|
|
# yoke_pull = 0.23
|
|
throttle = 0.28
|
|
if(self.dictionaryAccess(self.phaseFlags,"flare") == True):
|
|
# yoke_pull = -yoke_pull
|
|
yoke_pull = yoke_pull * 20
|
|
throttle = 0
|
|
# Constant throttle setting below the threshold needed to maintain straight and level flight
|
|
|
|
## Method 1:
|
|
# if(self.altitude < 350 and self.airspeed > 175): ## Integrate using the control equations;; A goal state update
|
|
# throttle = 0.1
|
|
# yoke_pull = 0.4
|
|
|
|
|
|
# if(self.altitude < 300 and self.airspeed > 170): ## Integrate using the control equations;; A goal state update
|
|
# throttle = 0.05
|
|
# yoke_pull = 0.6
|
|
|
|
# if(self.altitude < 250 and self.airspeed > 160): ## Integrate using the control equations;; A goal state update
|
|
# throttle = 0
|
|
# yoke_pull = 0.8
|
|
|
|
|
|
# if(self.altitude < 250 and self.airspeed > 160): ## Integrate using the control equations;; A goal state update
|
|
# throttle = 0
|
|
# yoke_pull = 0.8
|
|
|
|
##Method 2: Same Control Statements with Change in Parameter to decided pitch from Airspeed ---> Local Pitch Relative to the Horizon
|
|
|
|
|
|
#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
|
|
# Send all controls simultaneously to X-Plane
|
|
self.send_controls_to_xplane(yoke_pull, yoke_steer, rudder, throttle)
|
|
|
|
|
|
def send_controls_to_xplane(self, yoke_pull, yoke_steer, rudder, throttle):
|
|
"""
|
|
Sends all control inputs to X-Plane using XPlaneConnect
|
|
"""
|
|
# Send yoke pull, yoke steer, rudder, and throttle simultaneously
|
|
# print("Yoke Pull:" + str(yoke_pull))
|
|
|
|
#Set the Trim to a value that allows the aircraft to osscilate around the target airspeed
|
|
if(self.dictionaryAccess(self.phaseFlags,"flare") == False):
|
|
trimdref = "sim/flightmodel/controls/elv_trim"
|
|
trim = -0.3
|
|
self.client.sendDREF(trimdref,trim)
|
|
|
|
if(self.dictionaryAccess(self.phaseFlags,"flare")):
|
|
trimdref = "sim/flightmodel/controls/elv_trim"
|
|
trim = 0
|
|
self.client.sendDREF(trimdref,trim)
|
|
|
|
if(self.dictionaryAccess(self.phaseFlags,"roll out")):
|
|
#Cut the Throttle
|
|
throttle = 0
|
|
#Release Yoke Back Pressure (Pitch Up Pressure from the flare maneuver)
|
|
yoke_pull = 0
|
|
#Hit the Brakes
|
|
brakedref = "sim/cockpit2/controls/parking_brake_ratio"
|
|
brake = 1
|
|
self.client.sendDREF(brakedref,brake)
|
|
|
|
self.client.sendCTRL([yoke_pull, yoke_steer, rudder, throttle, -998, -998]) # Control inputs: [yoke_pull, yoke_steer, rudder, throttle]
|
|
|
|
def conditionChecks(self):
|
|
|
|
if(self.dictionaryAccess(self.destinations,"wheelWeight") > 0.01
|
|
and self.dictionaryAccess(self.destinations,"wheelSpeed") > 1):
|
|
#Two Parameters to Confirm Touchdown and wheel contact
|
|
# "sim/flightmodel/parts/tire_vrt_def_veh" #Gear Strut Deflection (Weight on wheels)
|
|
# "sim/flightmodel2/gear/tire_rotation_rate_rad_sec" #Tire Rotation Rate
|
|
self.phaseFlags["roll out"] = True
|
|
print("Hit the brakes")
|
|
|
|
if(self.dictionaryAccess(self.destinations,"altitude") <= 20
|
|
and self.dictionaryAccess(self.phaseFlags,"flare") == False):
|
|
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
|
|
and self.dictionaryAccess(self.destinations,"airspeed") < 2
|
|
and self.dictionaryAccess(self.destinations,"brakes") == 1):
|
|
self.inProgress = False
|
|
|
|
def simulationStatus(self):
|
|
return self.inProgress
|
|
|
|
|
|
# Update the model's DM based on X-Plane data
|
|
def update_aircraft_state(self):
|
|
"""
|
|
Faster Method
|
|
"""
|
|
self.getAndLoadDREFS()
|
|
print("midpoint")
|
|
self.conditionChecks()
|
|
|
|
|
|
|
|
|
|
|
|
# def logData(self):
|