https://github.com/tudelft/paparazzi
Tip revision: 1eca622606d21d88c561cfb3da9803c95ad79a82 authored by Christophe De Wagter on 17 December 2023, 19:02:05 UTC
--- FP REST ----
--- FP REST ----
Tip revision: 1eca622
flight_plan.py
#!/usr/bin/env python3
#
# Copyright (C) 2020 Fabien Bonneval <fabien.bonneval@enac.fr>
#
# This file is part of paparazzi.
#
# paparazzi is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2, or (at your option)
# any later version.
#
# paparazzi is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License
# along with paparazzi; see the file COPYING. If not, see
# <http://www.gnu.org/licenses/>.
#
from lxml import etree
from typing import List, Union
import sys
from xml_utils import get_attrib, get_attrib_default
class FlightPlan:
def __init__(self):
self.header = "" # type: str
self.waypoints = [] # type: List[Waypoint]
self.exceptions = [] # type: List[Exc]
self.blocks = [] # type: List[Block]
self.name = None
self.lat0 = None
self.lon0 = None
self.max_dist_from_home = None
self.ground_alt = None
self.security_height = None
self.alt = None
@staticmethod
def parse(fp_xml):
fp = FlightPlan()
fp_tree = etree.parse(fp_xml)
fp_element = fp_tree.find("flight_plan")
fp.name = get_attrib(fp_element, "name")
fp.lat0 = get_attrib(fp_element, "lat0")
fp.lon0 = get_attrib(fp_element, "lon0")
fp.max_dist_from_home = get_attrib(fp_element, "max_dist_from_home")
fp.ground_alt = get_attrib(fp_element, "ground_alt")
fp.security_height = get_attrib(fp_element, "security_height")
fp.alt = get_attrib(fp_element, "alt")
if fp_element.find("header") is not None:
fp.header = fp_element.find("header").text
ways_elt = fp_element.find("waypoints")
fp.waypoints = FlightPlan.parse_waypoints(ways_elt)
blocks_elt = fp_element.find("blocks")
fp.blocks = FlightPlan.parse_blocks(blocks_elt)
excs_elt = fp_element.find("exceptions")
fp.exceptions = FlightPlan.parse_exceptions(excs_elt)
return fp
@staticmethod
def parse_waypoints(ways_elt: etree.Element):
waypoints = []
w_no = 1 # first waypoint number is 1.
for way_e in ways_elt.findall("waypoint"):
name = get_attrib(way_e, "name")
x = get_attrib_default(way_e, "x", None, float)
y = get_attrib_default(way_e, "y", None, float)
lat = get_attrib_default(way_e, "lat", None)
lon = get_attrib_default(way_e, "lon", None)
alt = get_attrib_default(way_e, "alt", None, float)
height = get_attrib_default(way_e, "height", None, float)
waypoint = Waypoint(name, x, y, lat, lon, alt, height, w_no)
waypoints.append(waypoint)
w_no += 1
return waypoints
@staticmethod
def parse_blocks(blocks_elt):
blocks = []
for b_e in blocks_elt.findall("block"):
name = get_attrib(b_e, "name")
no = get_attrib(b_e, "no")
block = Block(name, int(no), b_e)
blocks.append(block)
return blocks
@staticmethod
def parse_exceptions(exs_elt):
if exs_elt is None:
return []
excs = []
for ex_e in exs_elt.findall("exception"):
cond = get_attrib(ex_e, "cond")
deroute = get_attrib(ex_e, "deroute")
exc = Exc(cond, deroute)
excs.append(exc)
return excs
def get_waypoint(self, key: Union[str, int]):
"""
:param key: Waypoint name or number
:type key: str or int
"""
if type(key) == str:
for wp in self.waypoints:
if wp.name == key:
return wp
elif type(key) == int:
for wp in self.waypoints:
if wp.no == key:
return wp
def get_block(self, key: Union[str, int]):
"""
:param key: Block name or number
:type key: str or int
"""
if type(key) == str:
for block in self.blocks:
if block.name == key:
return block
elif type(key) == int:
for block in self.blocks:
if block.no == key:
return block
def get_block_groups(self):
return list(set(filter(lambda x: x is not None,
[get_attrib_default(block.xml, "group", None) for block in self.blocks])))
def get_blocks_from_group(self, groupname):
return list(filter(lambda block: get_attrib_default(block.xml, "group", None) == groupname, self.blocks))
class Waypoint:
def __init__(self, name, x, y, lat, lon, alt, height, no):
self.name = name
self.x = x
self.y = y
self.lat = lat
self.lon = lon
self.alt = alt
self.height = height
self.no = no
class Block:
def __init__(self, name:str, no:int, xml):
self.name = name
self.no = no
self.xml = xml
class Exc:
def __init__(self, cond, deroute):
self.cond = cond
self.deroute = deroute
if __name__ == "__main__":
flight_plan = FlightPlan.parse(sys.argv[1])
for b in flight_plan.blocks:
print(b.name)