def SetGuidanceParams(self,params):
guidParams = GuidanceParam()
guidParams.defaultWpSpeed = params['DEF_WP_SPEED']
guidParams.captureRadiusScaling = params['CAP_R_SCALING']
guidParams.guidanceRadiusScaling = params['GUID_R_SCALING']
guidParams.xtrkDev = params['XTRKDEV']
guidParams.climbFpAngle = params['CLIMB_ANGLE']
guidParams.climbAngleVRange = params['CLIMB_ANGLE_VR']
guidParams.climbAngleHRange = params['CLIMB_ANGLE_HR']
guidParams.climbRateGain = params['CLIMB_RATE_GAIN']
guidParams.maxClimbRate = params['MAX_CLIMB_RATE']
guidParams.minClimbRate = params['MIN_CLIMB_RATE']
guidParams.maxCap = params['MAX_CAP']
guidParams.minCap = params['MIN_CAP']
guidParams.maxSpeed = params['MAX_GS'] * 0.5
guidParams.minSpeed = params['MIN_GS'] * 0.5
guidParams.yawForward = True if params['YAW_FORWARD'] == 1 else False
self.defaultWPSpeed = guidParams.defaultWpSpeed
self.Guidance.SetGuidanceParams(guidParams)
def __init__(self, initialPos, simtype="UAS_ROTOR", vehicleID = 0,
fasttime = True, verbose=0,callsign = "SPEEDBIRD",
monitor="DAIDALUS",
daaConfig="data/DaidalusQuadConfig.txt"):
self.fasttime = fasttime
self.callsign = callsign
self.verbose = verbose
self.home_pos = [initialPos[0], initialPos[1], initialPos[2]]
self.traffic = []
if simtype == "UAM_VTOL":
self.ownship = VehicleSim(vehicleID,0.0,0.0,0.0,0.0,0.0,0.0)
else:
from quadsim import QuadSim
self.ownship = QuadSim()
self.vehicleID = vehicleID
self.Cog = Cognition(callsign)
self.Guidance = Guidance(GuidanceParam())
self.Geofence = GeofenceMonitor([3,2,2,20,20])
self.Trajectory = Trajectory(callsign)
self.tfMonitor = TrafficMonitor(callsign,daaConfig,False,monitor)
self.Merger = Merger(callsign,vehicleID)
# Aircraft data
self.flightplan1 = []
self.etaFP1 = False
self.etaFP2 = False
self.flightplan2 = []
self.controlInput = [0.0,0.0,0.0]
self.fenceList = []
self.guidanceMode = GuidanceMode.NOOP
# Merger
self.arrTime = None
self.logLatency = 0
self.prevLogUpdate = 0
self.mergerLog = LogData()
self.position = self.home_pos
self.velocity = [0.0,0.0,0.0]
self.trkgsvs = [0.0,0.0,0.0]
self.localPos = []
self.ownshipLog = {"t": [], "position": [], "velocityNED": [], "positionNED": [],
"trkbands": [], "gsbands": [], "altbands": [], "vsbands": [],
"localPlans": [], "localFences": [], "commandedVelocityNED": []}
self.trafficLog = {}
self.emergbreak = False
if self.fasttime:
self.currTime = 0
else:
self.currTime = time.time()
self.numSecPlan = 0
self.plans = []
self.fences = []
self.mergeFixes = []
self.localPlans = []
self.localFences= []
self.localMergeFixes = []
self.daa_radius = 0
self.startSent = False
self.nextWP1 = 1
self.nextWP2 = 1
self.numFences = 0
self.resSpeed = 0
self.defaultWPSpeed = 1
self.missionComplete = False
self.fphases = -1
self.land = False
self.activePlan = "Plan0"
self.windFrom = 0
self.windSpeed = 0
def __init__(self, home_pos, callsign="SPEEDBIRD", vehicleID=0, verbose=1,
logRateHz=5, fasttime=True, simtype="UAS_ROTOR",
monitor="DAIDALUS", daaConfig="data/DaidalusQuadConfig.txt"):
"""
Initialize pycarous
:param fasttime: when fasttime is True, simulation will run in fasttime
:param simtype: string defining vehicle model: UAS_ROTOR, UAM_VTOL, ...
:param monitor: string defining DAA module: DAIDALUS, ACAS, ...
:param daaConfig: DAA module configuration file
Other parameters are defined in parent class, see IcarousInterface.py
"""
super().__init__(home_pos, callsign, vehicleID, verbose)
self.simType = "pycarous"
self.fasttime = fasttime
if self.fasttime:
self.currTime = 0
else:
self.currTime = time.time()
# Initialize vehicle sim
if simtype == "UAM_VTOL":
from vehiclesim import UamVtolSim
self.ownship = UamVtolSim(self.vehicleID, home_pos)
elif simtype == "UAS_ROTOR":
from vehiclesim import QuadSim
self.ownship = QuadSim(self.vehicleID, home_pos)
elif simtype == "UAM_SPQ":
from vehiclesim import SixPassengerQuadSim
self.ownship = SixPassengerQuadSim(self.vehicleID, home_pos)
# Initialize ICAROUS apps
self.Cog = Cognition(self.callsign)
self.Guidance = Guidance(GuidanceParam())
self.Geofence = GeofenceMonitor([3, 2, 2, 20, 20])
self.Trajectory = Trajectory(self.callsign)
self.tfMonitor = TrafficMonitor(self.callsign, daaConfig, False, monitor)
self.Merger = Merger(self.callsign, self.vehicleID)
# Merger data
self.arrTime = None
self.logLatency = 0
self.prevLogUpdate = 0
self.mergerLog = LogData()
self.localMergeFixes = []
# Fligh plan data
self.flightplan1 = []
self.flightplan2 = []
self.etaFP1 = False
self.etaFP2 = False
self.nextWP1 = 1
self.nextWP2 = 1
self.plans = []
self.localPlans = []
self.activePlan = "Plan0"
self.numSecPlan = 0
# Geofence data
self.fences = []
self.localFences= []
self.numFences = 0
# Other aircraft data
self.guidanceMode = GuidanceMode.NOOP
self.emergbreak = False
self.daa_radius = 0
self.turnRate = 0
self.fphases = -1
self.land = False
self.ditch = False
self.loopcount = 0
#teste
self.fp = []