def InputFlightplanFromFile(self,filename,eta=False,repair=False,startTimeShift=0):
import re
match = re.search('\.eutl',filename)
waypoints = []
if match is None:
fp = GetFlightplan(filename,self.defaultWPSpeed,eta)
waypoints = ConstructWaypointsFromList(fp,eta)
else:
wps,totalwps = GetEUTLPlanFromFile(filename,0,timeshift=startTimeShift)
for i in range(totalwps):
waypoints.append(wps[i])
eta = True
self.InputFlightplan(waypoints,eta,repair)
def InputFlightplan(self, fp, eta=False, repair=False):
waypoints = ConstructWaypointsFromList(fp, eta)
self.etaFP1 = eta
self.localPlans.append(self.GetLocalFlightPlan(waypoints))
self.Cog.InputFlightplanData("Plan0", waypoints, repair, self.turnRate)
self.Guidance.InputFlightplanData("Plan0", waypoints, repair,
self.turnRate)
self.Trajectory.InputFlightplanData("Plan0", waypoints, repair,
self.turnRate)
if repair:
waypoints = self.Guidance.GetKinematicPlan("Plan0")
self.plans.append(waypoints)
self.flightplan1 = waypoints
self.flightplan2 = []
# Set initial conditions of model based on flightplan waypoints
dist = distance(waypoints[1].latitude, waypoints[1].longitude,
waypoints[0].latitude, waypoints[0].longitude)
speed = dist / (waypoints[1].time - waypoints[0].time)
hdg = ComputeHeading(
[waypoints[0].latitude, waypoints[0].longitude, 0],
[waypoints[1].latitude, waypoints[1].longitude, 0])
vn, ve, vd = ConvertTrkGsVsToVned(hdg, speed, 0)
self.ownship.SetInitialConditions(z=waypoints[0].altitude,
vx=ve,
vy=vn)
def InputFlightplanFromFile(self, filename, eta=False, repair=False):
self.gs.loadWaypoint(filename)
time.sleep(1)
fp = GetFlightplan(filename, self.defaultWPSpeed, eta)
waypoints = ConstructWaypointsFromList(fp, eta)
self.flightplan1 = waypoints
self.plans.append(waypoints)
self.localPlans.append(self.GetLocalFlightPlan(waypoints))
def InputFlightplan(self, fp, eta=False, repair=False):
waypoints = ConstructWaypointsFromList(fp, eta)
self.flightplan1 = waypoints
self.plans.append(waypoints)
self.localPlans.append(self.GetLocalFlightPlan(waypoints))
self.gs.wploader.clear()
for wp in fp:
lat, lon, alt, wp_metric = wp
self.gs.wploader.add_latlonalt(lat, lon, alt)
self.gs.wploader.wp(-1).param4 = wp_metric
self.gs.send_all_waypoints()
time.sleep(1)
def InputFlightplan(self, fp, eta=False, repair=False):
waypoints = ConstructWaypointsFromList(fp, eta)
self.etaFP1 = eta
self.localPlans.append(self.GetLocalFlightPlan(waypoints))
self.Cog.InputFlightplanData("Plan0", waypoints, repair, self.turnRate)
self.Guidance.InputFlightplanData("Plan0", waypoints, repair,
self.turnRate)
self.Trajectory.InputFlightplanData("Plan0", waypoints, repair,
self.turnRate)
if repair:
waypoints = self.Guidance.GetKinematicPlan("Plan0")
self.plans.append(waypoints)
self.flightplan1 = waypoints
self.flightplan2 = []
def RunCognition(self):
self.Cog.InputVehicleState(self.position,self.trkgsvs,self.trkgsvs[0])
nextWP1 = self.nextWP1
if self.nextWP1 >= len(self.flightplan1):
nextWP1 = len(self.flightplan1) - 1
dist = distance(self.position[0],self.position[1],self.flightplan1[nextWP1].latitude,self.flightplan1[nextWP1].longitude)
if self.verbose > 1:
if self.activePlan == 'Plan0':
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP1,dist))
else:
print("%s: %s, Distance to wp %d: %f" %(self.callsign,self.activePlan,self.nextWP2,dist))
self.fphase = self.Cog.RunCognition(self.currTime)
if self.fphase == -2:
self.land = True
n, cmd = self.Cog.GetOutput()
if n > 0:
if cmd.commandType == CommandTypes.FP_CHANGE:
self.guidanceMode = GuidanceMode.FLIGHTPLAN
self.activePlan = cmd.commandU.fpChange.name.decode('utf-8')
nextWP = cmd.commandU.fpChange.wpIndex
eta = False
if self.activePlan == "Plan0":
self.flightplan2 = []
self.nextWP1 = nextWP
eta = self.etaFP1
else:
self.nextWP2 = nextWP
eta = self.etaFP2
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,nextWP,eta)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.P2P:
self.guidanceMode = GuidanceMode.POINT2POINT
point = cmd.commandU.p2pCommand.point
speed = cmd.commandU.p2pCommand.speed
fp = [(*self.position,0),(*point,speed)]
waypoints = ConstructWaypointsFromList(fp)
self.Guidance.InputFlightplanData("P2P",waypoints)
self.activePlan = "P2P"
self.nextWP2 = 1
self.Guidance.SetGuidanceMode(self.guidanceMode,self.activePlan,1,False)
if self.verbose > 0:
print(self.callsign, ": active plan = ",self.activePlan)
elif cmd.commandType == CommandTypes.VELOCITY:
self.guidanceMode = GuidanceMode.VECTOR
vn = cmd.commandU.velocityCommand.vn
ve = cmd.commandU.velocityCommand.ve
vu = -cmd.commandU.velocityCommand.vu
trkGsVs = ConvertVnedToTrkGsVs(vn,ve,vu)
self.Guidance.InputVelocityCmd(trkGsVs)
self.Guidance.SetGuidanceMode(self.guidanceMode,"",0)
elif cmd.commandType == CommandTypes.TAKEOFF:
self.guidanceMode = GuidanceMode.TAKEOFF
elif cmd.commandType == CommandTypes.SPEED_CHANGE:
self.etaFP1 = False
speedChange = cmd.commandU.speedChange.speed
planID= cmd.commandU.speedChange.name.decode('utf-8')
nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2
self.Guidance.ChangeWaypointSpeed(planID,nextWP,speedChange,False)
elif cmd.commandType == CommandTypes.ALT_CHANGE:
altChange = cmd.commandU.altChange.altitude
planID= cmd.commandU.altChange.name.decode('utf-8')
nextWP = self.nextWP1 if planID == "Plan0" else self.nextWP2
self.Guidance.ChangeWaypointAlt(planID,nextWP,altChange,cmd.commandU.altChange.hold)
elif cmd.commandType == CommandTypes.STATUS_MESSAGE:
if self.verbose > 0:
message = cmd.commandU.statusMessage.buffer
print(self.callsign,":",message.decode('utf-8'))
elif cmd.commandType == CommandTypes.FP_REQUEST:
self.flightplan2 = []
self.numSecPlan += 1
startPos = cmd.commandU.fpRequest.fromPosition
stopPos = cmd.commandU.fpRequest.toPosition
startVel = cmd.commandU.fpRequest.startVelocity
endVel = cmd.commandU.fpRequest.endVelocity
planID = cmd.commandU.fpRequest.name.decode('utf-8')
# Find the new path
numWP = self.Trajectory.FindPath(
planID,
startPos,
stopPos,
startVel,
endVel)
if numWP > 0:
if self.verbose > 0:
print("%s : At %f s, Computed flightplan %s with %d waypoints" % (self.callsign,self.currTime,planID,numWP))
# offset the new path with current time (because new paths start with t=0)
self.Trajectory.SetPlanOffset(planID,0,self.currTime)
# Combine new plan with rest of old plan
self.Trajectory.CombinePlan(planID,"Plan0",-1)
self.flightplan2 = self.Trajectory.GetFlightPlan(planID)
self.Cog.InputFlightplanData(planID,self.flightplan2)
self.Guidance.InputFlightplanData(planID,self.flightplan2)
# Set guidance mode with new flightplan and wp 0 to offset plan times
self.Guidance.SetGuidanceMode(GuidanceMode.FLIGHTPLAN,planID,0,False)
self.plans.append(self.flightplan2)
self.localPlans.append(self.GetLocalFlightPlan(self.flightplan2))
else:
if self.verbose > 0:
print(self.callsign,"Error finding path")