diff --git a/sw/lib/python/flight_plan.py b/sw/lib/python/flight_plan.py new file mode 100755 index 0000000000..668463633b --- /dev/null +++ b/sw/lib/python/flight_plan.py @@ -0,0 +1,174 @@ +#!/usr/bin/env python3 +# +# Copyright (C) 2020 Fabien Bonneval +# +# 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 +# . +# + + +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, 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, no, 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) diff --git a/sw/lib/python/settings.py b/sw/lib/python/settings.py new file mode 100755 index 0000000000..a9c66ef53d --- /dev/null +++ b/sw/lib/python/settings.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python3 +# +# Copyright (C) 2020 Fabien Bonneval +# +# 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 +# . +# + +from lxml import etree +from typing import List +import sys +from os import path, getenv +import time +from xml_utils import get_attrib, get_attrib_default + +PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../'))) +sys.path.append(PPRZ_HOME + "/var/lib/python") +from pprzlink.ivy import IvyMessagesInterface +from pprzlink.message import PprzMessage + + +class PprzSettingsManager: + def __init__(self, settings_xml_path, ac_id, ivy: IvyMessagesInterface): + self.ac_id = ac_id + self.ivy = ivy + self.settings_grp = PprzSettingsParser.parse(settings_xml_path) + self.ivy.subscribe(self.update_values, PprzMessage('ground', 'DL_VALUES')) + + def update_values(self, sender, msg): + if self.ac_id == msg.ac_id: + for i, value in enumerate(msg.values): + if value != '?': + value = float(value) + s = self.settings_grp[i] # type: PprzSetting + s.set(value) + + def __getitem__(self, item): + return self.settings_grp.__getitem__(item) + + def __setitem__(self, key, value): + setting = self.settings_grp[key] + msg = PprzMessage("ground", "DL_SETTING") + msg['ac_id'] = self.ac_id + msg['index'] = setting.index + msg['value'] = value + self.ivy.send(msg) + + def __len__(self): + return self.settings_grp.__len__() + + +class PprzSettingsParser: + def __init__(self): + self.setting_nb = 0 + + @staticmethod + def parse(settings_xml_path): + parser = PprzSettingsParser() + settings_tree = etree.parse(settings_xml_path) + sets_element = settings_tree.find("dl_settings") + return parser.parse_dl_settings(sets_element) + + def parse_dl_setting(self, element): + var = get_attrib(element, "var") + min_v = get_attrib(element, "min", typ=float) + max_v = get_attrib(element, "max", typ=float) + shortname = get_attrib_default(element, "shortname", var) + step = get_attrib_default(element, "step", 1, typ=float) + values = get_attrib_default(element, "values", None) + + if values is None: + values = [] + else: + values = values.split("|") + count = int((max_v - min_v + step) / step) + if len(values) != count: + print("Warning: possibly wrong number of values ({}) for {} (expected {})" + .format(len(values), shortname, count)) + setting = PprzSetting(var, self.setting_nb, shortname, min_v, max_v, step, values, element) + + for e_button in element.findall("strip_button"): + name = get_attrib(e_button, "name") + value = get_attrib(e_button, "value") + icon = get_attrib_default(e_button, "icon", None) + group = get_attrib_default(e_button, "group", None) + + button = StripButton(value, name, icon, group) + setting.buttons.append(button) + + for e_key in element.findall("key_press"): + key = get_attrib(e_key, "key") + value = get_attrib(e_key, "value") + key_press = KeyPress(key, value) + setting.key_press.append(key_press) + + self.setting_nb += 1 + return setting + + def parse_dl_settings(self, element): + name = get_attrib_default(element, "name", None) + group = PprzSettingsGrp(name) + for child in element.getchildren(): + if child.tag == "dl_settings": + setting_group = self.parse_dl_settings(child) + group.groups_list.append(setting_group) + elif child.tag == "dl_setting": + setting = self.parse_dl_setting(child) + group.settings_list.append(setting) + else: + raise Exception("tag {} not supported!".format(child.tag)) + return group + + +class PprzSetting: + """Paparazzi Setting Class""" + + def __init__(self, var, index, shortname, min_value, max_value, step, values, xml): + self.var = var # type: str + self.index = index # type: int + self.shortname = shortname # type: str + self.min_value = min_value # type: float + self.max_value = max_value # type: float + self.step = step # type: float + self.values = values # type: List[str] + self.value = None # type: float + self.buttons = [] # type: List[StripButton] + self.key_press = [] # type: List[KeyPress] + self.xml = xml + + def __str__(self): + return "{{var: {}, shortname: {}, index: {}}}".format(self.var, self.shortname, self.index) + + def value_from_name(self, name): + """Return the index in 'values' table matching a given name. Raise ValueError if the name is unknown.""" + if self.values is None: + raise ValueError("No named values in this setting") + return self.values.index(name) + self.min_value + + def set(self, val): + if isinstance(val, str): + self.value = self.value_from_name(val) + elif isinstance(val, float) or isinstance(val, int): + self.value = val + else: + raise Exception("Bad type!") + + +class StripButton: + def __init__(self, value, name, icon, group): + self.value = value + self.name = name + self.icon = icon + self.group = group + + +class KeyPress: + def __init__(self, key, value): + self.value = value + self.key = key + + +class PprzSettingsGrp: + """"Paparazzi Setting Group Class""" + + def __init__(self, name): + self.name = name + self.settings_list = [] # type: List[PprzSetting] + self.groups_list = [] # type: List[PprzSettingsGrp] + + def __str__(self): + grp_names = [grp.name for grp in self.groups_list] + settings = [str(setting) for setting in self.settings_list] + return ", ".join(grp_names + settings) + + def get_all_settings(self): + all_settings = [] + all_settings += self.settings_list + for group in self.groups_list: + all_settings += group.get_all_settings() + return sorted(all_settings, key=lambda s: s.index) + + def __getitem__(self, item): + if type(item) == int: + for setting in self.get_all_settings(): + if item == setting.index: + return setting + raise IndexError("No setting #{}".format(item)) + elif type(item) == str: + for setting in self.get_all_settings(): + if item == setting.shortname: + return setting + raise AttributeError('No such setting named "{}"'.format(item)) + + def __len__(self): + return len(self.get_all_settings()) + + +def example1(): + # Usage: ./setting.py /settings.xml + settings = PprzSettingsParser.parse(sys.argv[1]) + set_nav_radius = settings["nav_radius"] + print(set_nav_radius) + set_no_2 = settings[2] + print(set_no_2) + + +def example2(): + # Usage: ./setting.py /settings.xml + ivy = IvyMessagesInterface("DemoSettings") + try: + setting_manager = PprzSettingsManager(sys.argv[1], sys.argv[2], ivy) + while True: + time.sleep(1) + setting_manager["altitude"] = setting_manager["altitude"].value + 2 + except KeyboardInterrupt: + ivy.shutdown() + print("Stopping on request") + + +if __name__ == "__main__": + # example1 usage: ./setting.py /settings.xml + example1() + + # example2 usage: ./setting.py /settings.xml + # example2() diff --git a/sw/lib/python/xml_utils.py b/sw/lib/python/xml_utils.py new file mode 100644 index 0000000000..e388a5e1e3 --- /dev/null +++ b/sw/lib/python/xml_utils.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 +# +# Copyright (C) 2020 Fabien Bonneval +# +# 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 +# . +# + +from lxml import etree + + +def get_attrib(node: etree.Element, tag: str, typ=None): + """XML Element attribute getter which is insensitive to case, handle default value and type conversion""" + try: + value = node.attrib[tag.lower()] + except KeyError: + value = node.attrib[tag.upper()] + + if typ is None: + return value + else: + return typ(value) + + +def get_attrib_default(node: etree.Element, tag: str, default, typ=None): + try: + value = get_attrib(node, tag, typ=typ) + except KeyError: + value = default + + if typ is not None and default is not None: + return typ(value) + else: + return value