diff --git a/.gitignore b/.gitignore index 83ace62..7416d7a 100644 --- a/.gitignore +++ b/.gitignore @@ -19,3 +19,4 @@ /build /var +/.idea/ diff --git a/Makefile b/Makefile index 8bfc51c..3d79c2f 100644 --- a/Makefile +++ b/Makefile @@ -24,8 +24,11 @@ Q=@ MAKE = make -MESSAGES_XML ?= message_definitions/v1.0/messages.xml -MESSAGES_DTD ?= message_definitions/v1.0/messages.dtd +PPRZLINK_MSG_VERSION ?= 1.0 +PPRZLINK_LIB_VERSION ?= 1.0 + +MESSAGES_XML ?= message_definitions/v$(PPRZLINK_MSG_VERSION)/messages.xml +MESSAGES_DTD ?= message_definitions/v$(PPRZLINK_MSG_VERSION)/messages.dtd UNITS_XML ?= message_definitions/common/units.xml MESSAGES_INSTALL ?= var MESSAGES_INCLUDE ?= $(MESSAGES_INSTALL)/include/pprzlink @@ -43,49 +46,35 @@ else VALIDATE_FLAG = endif -all: libs - -libs: ocaml_lib_v1 - -generators: ocaml_lib_v1 - $(Q)$(MAKE) -C tools/generator install +all: libpprzlink -ocaml_lib_v1: - $(Q)$(MAKE) -C lib/v1.0/ocaml +libpprzlink: + $(Q)Q=$(Q) $(MAKE) -C lib/v$(PPRZLINK_LIB_VERSION)/ocaml libpprzlink-install: - $(Q)$(MAKE) -C lib/v1.0/ocaml install + $(Q)Q=$(Q) DESTDIR=$(DESTDIR)/ocaml $(MAKE) -C lib/v$(PPRZLINK_LIB_VERSION)/ocaml install + $(Q)Q=$(Q) DESTDIR=$(DESTDIR)/python $(MAKE) -C lib/v$(PPRZLINK_LIB_VERSION)/python install pre_messages_dir: $(Q)test -d $(MESSAGES_INCLUDE) || mkdir -p $(MESSAGES_INCLUDE) $(Q)test -d $(MESSAGES_LIB) || mkdir -p $(MESSAGES_LIB) -gen_messages: generators pre_messages_dir - @echo 'Generate C messages (OCaml) at location $(MESSAGES_INCLUDE)' - $(Q)./bin/gen_messages_c.byte $(MESSAGES_XML) telemetry $(TELEMETRY_H) - $(Q)./bin/gen_messages_c.byte $(MESSAGES_XML) datalink $(DATALINK_H) - $(Q)./bin/gen_messages_c.byte $(MESSAGES_XML) intermcu $(INTERMCU_H) - $(Q)cp tools/generator/C/include_v1.0/*.h $(MESSAGES_INCLUDE) - post_messages_install: @echo 'Copy extra lib files' $(Q)cp $(MESSAGES_XML) $(MESSAGES_DTD) $(UNITS_XML) $(MESSAGES_INSTALL) - $(Q)cp lib/v1.0/C/*.h $(MESSAGES_INCLUDE) - $(Q)cp lib/v1.0/C/*.c $(MESSAGES_LIB) + $(Q)Q=$(Q) MESSAGES_INCLUDE=$(MESSAGES_INCLUDE) MESSAGES_LIB=$(MESSAGES_LIB) $(MAKE) -C lib/v$(PPRZLINK_LIB_VERSION)/C install pygen_messages: pre_messages_dir @echo 'Generate C messages (Python) at location $(MESSAGES_INCLUDE)' - $(Q)./tools/generator/gen_messages.py $(VALIDATE_FLAG) --lang C -o $(TELEMETRY_H) $(MESSAGES_XML) telemetry - $(Q)./tools/generator/gen_messages.py $(VALIDATE_FLAG) --lang C -o $(DATALINK_H) $(MESSAGES_XML) datalink - $(Q)./tools/generator/gen_messages.py $(VALIDATE_FLAG) --lang C -o $(INTERMCU_H) $(MESSAGES_XML) intermcu - -messages: gen_messages post_messages_install + $(Q)./tools/generator/gen_messages.py $(VALIDATE_FLAG) --protocol $(PPRZLINK_LIB_VERSION) --messages $(PPRZLINK_MSG_VERSION) --lang C -o $(TELEMETRY_H) $(MESSAGES_XML) telemetry + $(Q)./tools/generator/gen_messages.py $(VALIDATE_FLAG) --protocol $(PPRZLINK_LIB_VERSION) --messages $(PPRZLINK_MSG_VERSION) --lang C -o $(DATALINK_H) $(MESSAGES_XML) datalink + $(Q)./tools/generator/gen_messages.py $(VALIDATE_FLAG) --protocol $(PPRZLINK_LIB_VERSION) --messages $(PPRZLINK_MSG_VERSION) --lang C -o $(INTERMCU_H) $(MESSAGES_XML) intermcu pymessages: pygen_messages post_messages_install clean : $(Q)$(MAKE) -C tools/generator clean - $(Q)$(MAKE) -C lib/v1.0/ocaml clean + $(Q)$(MAKE) -C lib/v$(PPRZLINK_LIB_VERSION)/ocaml clean uninstall: clean $(Q)rm -rf var bin build $(MESSAGES_INCLUDE) $(MESSAGES_LIB) @@ -95,4 +84,4 @@ validate_messages: $(Q)./tools/generator/gen_messages.py --only-validate $(MESSAGES_XML) datalink $(Q)./tools/generator/gen_messages.py --only-validate $(MESSAGES_XML) intermcu -.PHONY: all libs generators ocaml_lib_v1 pre_messages_dir post_messages_install gen_messages pygen_messages messages pymessages clean uninstall validate_messages +.PHONY: libpprzlink libpprzlink-install pre_messages_dir post_messages_install pygen_messages pymessages clean uninstall validate_messages diff --git a/lib/v1.0/C/print_utils.h b/lib/common/C/print_utils.h similarity index 100% rename from lib/v1.0/C/print_utils.h rename to lib/common/C/print_utils.h diff --git a/lib/common/ocaml/Makefile b/lib/common/ocaml/Makefile new file mode 100644 index 0000000..0fea12a --- /dev/null +++ b/lib/common/ocaml/Makefile @@ -0,0 +1,123 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2003 Pascal Brisset, Antoine Drouin +# Copyright (C) 2015 Gautier Hattenberger +# +# 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 +# . +# + +# Quiet compilation +Q=@ + +# redirect output if quiet compilation +ifeq ($(Q),@) +QUIET= -quiet +Q_OUT= >/dev/null 2>&1 +else +Q_OUT= +endif + + +OCAMLBUILD=ocamlbuild -use-ocamlfind +CAMLP4_DEFS ?= + +# +# NOTE: due to a bug in older ocamlbuild versions the build-dir can NOT be absolute +# see http://caml.inria.fr/mantis/view.php?id=5503 +# +OCAML_VERSION := $(shell ocamlc -version) +OCAML_MAJOR := $(shell echo $(OCAML_VERSION) | cut -f1 -d.) +OCAMLC_MINOR := $(shell echo $(OCAML_VERSION) | cut -f2 -d.) +ifeq ($(shell test $(OCAML_MAJOR) -lt 4; echo $$?),0) +# need to use an old buggy ocaml(build) version: build-dir MUST be relative +BUILDDIR ?= ../../../build/ocaml/pprzlink +else +# using a recent ocaml(build) version without THAT particular bug, +# but build-dir MUST NOT contain .. +# so use some magic here to get a normalized absolute path if PPRZLINK_DIR is not set +PPRZLINK_DIR ?= $(realpath $(dir $(lastword $(MAKEFILE_LIST)))/../../..) +BUILDDIR ?= $(PPRZLINK_DIR)/build/ocaml/pprzlink + +ifeq ($(shell test $(OCAMLC_MINOR) -ge 2; echo $$?),0) +# the Bytes module is available since OCaml 4.02.0 +CAMLP4_DEFS += -DHAS_BYTES_MODULE +endif +endif + +INSTALL_FLAGS ?= +# use this option leave ocamlfind ldconf file unchanged +INSTALL_FLAGS += -ldconf ignore +DESTDIR ?= +ifneq ($(DESTDIR),) +INSTALL_FLAGS += -destdir $(DESTDIR) +endif + +PP_OPTS = -pp "camlp4o pa_macro.cmo $(CAMLP4_DEFS)" +INSTALL_FILES = $(shell ls $(BUILDDIR)/*.so $(BUILDDIR)/*.a $(BUILDDIR)/*.mli $(BUILDDIR)/*.cm* $(BUILDDIR)/common/*.mli $(BUILDDIR)/common/*.cm*) + + +all: byte native + +byte: _tags META + @echo Build bytecode lib + $(Q)test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) + $(Q)$(OCAMLBUILD) $(QUIET) $(PP_OPTS) -build-dir $(BUILDDIR) -I common lib-pprzlink.cma + $(Q)cp META $(BUILDDIR) + +# byte with statically linked libs +static: _tags META + @echo Build bytecode lib with static linking + $(Q)test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) + $(Q)$(OCAMLBUILD) $(QUIET) -build-dir $(BUILDDIR) -tag static -I common lib-pprzlink.cma + $(Q)cp META $(BUILDDIR) + +native: _tags META + @echo Build native lib + $(Q)test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) + $(Q)$(OCAMLBUILD) $(QUIET) $(PP_OPTS) -build-dir $(BUILDDIR) -I common lib-pprzlink.cmxa + $(Q)cp META $(BUILDDIR) + +install: clean_lib byte native META + @echo INSTALL at location $(DESTDIR) + $(Q)test -d $(DESTDIR) || mkdir -p $(DESTDIR) + $(Q)ocamlfind remove $(INSTALL_FLAGS) pprzlink $(Q_OUT) + $(Q)ocamlfind install $(INSTALL_FLAGS) pprzlink META $(INSTALL_FILES) $(Q_OUT) + +UNAME = $(shell uname -s) +ifeq ("$(UNAME)","Darwin") + MKTEMP = gmktemp +else + MKTEMP = mktemp +endif + +clean : + $(Q)rm -f *.cm* *.out *.opt .depend *.a *.o *.so + $(Q)ocamlbuild -clean -classic-display -build-dir $(BUILDDIR) + +# test if an other version of the lib is installed and remove if needed +clean_lib: + @echo Check for already installed pprzlink ocaml lib +ifneq ("$(wildcard $(DESTDIR)/pprzlink/META)","") + @echo ' -> found lib version $(shell grep version $(DESTDIR)/pprzlink/META | cut -d \" -f2) while installing $(PPRZLINK_LIB_VERSION)' +ifneq ($(shell grep version $(DESTDIR)/pprzlink/META | cut -d \" -f2), $(PPRZLINK_LIB_VERSION)) + @echo ' -> removing files' + $(Q)ocamlbuild -clean -classic-display -build-dir $(BUILDDIR) + $(Q)ocamlfind remove $(INSTALL_FLAGS) pprzlink $(Q_OUT) +endif +endif + +.PHONY: all byte static native install install_local clean clean_lib diff --git a/lib/v1.0/ocaml/compatPL.ml b/lib/common/ocaml/compatPL.ml similarity index 100% rename from lib/v1.0/ocaml/compatPL.ml rename to lib/common/ocaml/compatPL.ml diff --git a/lib/v1.0/ocaml/compatPL.mli b/lib/common/ocaml/compatPL.mli similarity index 100% rename from lib/v1.0/ocaml/compatPL.mli rename to lib/common/ocaml/compatPL.mli diff --git a/lib/v1.0/ocaml/convert.c b/lib/common/ocaml/convert.c similarity index 100% rename from lib/v1.0/ocaml/convert.c rename to lib/common/ocaml/convert.c diff --git a/lib/v1.0/ocaml/debugPL.ml b/lib/common/ocaml/debugPL.ml similarity index 100% rename from lib/v1.0/ocaml/debugPL.ml rename to lib/common/ocaml/debugPL.ml diff --git a/lib/v1.0/ocaml/debugPL.mli b/lib/common/ocaml/debugPL.mli similarity index 100% rename from lib/v1.0/ocaml/debugPL.mli rename to lib/common/ocaml/debugPL.mli diff --git a/lib/v1.0/ocaml/pprz_transport.ml b/lib/common/ocaml/pprz_transport.ml similarity index 100% rename from lib/v1.0/ocaml/pprz_transport.ml rename to lib/common/ocaml/pprz_transport.ml diff --git a/lib/v1.0/ocaml/pprz_transport.mli b/lib/common/ocaml/pprz_transport.mli similarity index 100% rename from lib/v1.0/ocaml/pprz_transport.mli rename to lib/common/ocaml/pprz_transport.mli diff --git a/lib/v1.0/ocaml/pprzlog_transport.ml b/lib/common/ocaml/pprzlog_transport.ml similarity index 100% rename from lib/v1.0/ocaml/pprzlog_transport.ml rename to lib/common/ocaml/pprzlog_transport.ml diff --git a/lib/v1.0/ocaml/pprzlog_transport.mli b/lib/common/ocaml/pprzlog_transport.mli similarity index 100% rename from lib/v1.0/ocaml/pprzlog_transport.mli rename to lib/common/ocaml/pprzlog_transport.mli diff --git a/lib/v1.0/ocaml/protocol.ml b/lib/common/ocaml/protocol.ml similarity index 100% rename from lib/v1.0/ocaml/protocol.ml rename to lib/common/ocaml/protocol.ml diff --git a/lib/v1.0/ocaml/protocol.mli b/lib/common/ocaml/protocol.mli similarity index 100% rename from lib/v1.0/ocaml/protocol.mli rename to lib/common/ocaml/protocol.mli diff --git a/lib/v1.0/ocaml/xbee_transport.ml b/lib/common/ocaml/xbee_transport.ml similarity index 100% rename from lib/v1.0/ocaml/xbee_transport.ml rename to lib/common/ocaml/xbee_transport.ml diff --git a/lib/v1.0/ocaml/xbee_transport.mli b/lib/common/ocaml/xbee_transport.mli similarity index 100% rename from lib/v1.0/ocaml/xbee_transport.mli rename to lib/common/ocaml/xbee_transport.mli diff --git a/lib/v1.0/C/Makefile b/lib/v1.0/C/Makefile new file mode 100644 index 0000000..ad84314 --- /dev/null +++ b/lib/v1.0/C/Makefile @@ -0,0 +1,33 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2017 Fabien Garcia +# +# 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 +# . +# + +MAKE = make + +all: + +install: +# $(Q)cp ../../common/C/*.c $(MESSAGES_LIB) + $(Q)cp ../../common/C/*.h $(MESSAGES_INCLUDE) + $(Q)cp ./*.h $(MESSAGES_INCLUDE) + $(Q)cp ./*.c $(MESSAGES_LIB) + + +.PHONY: install diff --git a/lib/v1.0/ocaml/Makefile b/lib/v1.0/ocaml/Makefile index d2e5e75..7d28d02 100644 --- a/lib/v1.0/ocaml/Makefile +++ b/lib/v1.0/ocaml/Makefile @@ -1,7 +1,6 @@ # Hey Emacs, this is a -*- makefile -*- # -# Copyright (C) 2003 Pascal Brisset, Antoine Drouin -# Copyright (C) 2015 Gautier Hattenberger +# Copyright (C) 2017 Gautier Hattenberger # # This file is part of paparazzi. # @@ -20,92 +19,5 @@ # . # -# Quiet compilation -Q=@ +include ../../common/ocaml/Makefile -# redirect output if quiet compilation -ifeq ($(Q),@) -QUIET= -quiet -Q_OUT= >/dev/null 2>&1 -else -Q_OUT= -endif - - -OCAMLBUILD=ocamlbuild -use-ocamlfind -CAMLP4_DEFS ?= - -# -# NOTE: due to a bug in older ocamlbuild versions the build-dir can NOT be absolute -# see http://caml.inria.fr/mantis/view.php?id=5503 -# -OCAML_VERSION := $(shell ocamlc -version) -OCAML_MAJOR := $(shell echo $(OCAML_VERSION) | cut -f1 -d.) -OCAMLC_MINOR := $(shell echo $(OCAML_VERSION) | cut -f2 -d.) -ifeq ($(shell test $(OCAML_MAJOR) -lt 4; echo $$?),0) -# need to use an old buggy ocaml(build) version: build-dir MUST be relative -BUILDDIR ?= ../../../build/ocaml/pprzlink -else -# using a recent ocaml(build) version without THAT particular bug, -# but build-dir MUST NOT contain .. -# so use some magic here to get a normalized absolute path if PPRZLINK_DIR is not set -PPRZLINK_DIR ?= $(realpath $(dir $(lastword $(MAKEFILE_LIST)))/../../..) -BUILDDIR ?= $(PPRZLINK_DIR)/build/ocaml/pprzlink - -ifeq ($(shell test $(OCAMLC_MINOR) -ge 2; echo $$?),0) -# the Bytes module is available since OCaml 4.02.0 -CAMLP4_DEFS += -DHAS_BYTES_MODULE -endif -endif - -INSTALL_FLAGS ?= -# use this option leave ocamlfind ldconf file unchanged -INSTALL_FLAGS += -ldconf ignore -DESTDIR ?= -ifneq ($(DESTDIR),) -INSTALL_FLAGS += -destdir $(DESTDIR) -endif - -PP_OPTS = -pp "camlp4o pa_macro.cmo $(CAMLP4_DEFS)" -INSTALL_FILES = $(shell ls $(BUILDDIR)/*.so $(BUILDDIR)/*.a $(BUILDDIR)/*.mli $(BUILDDIR)/*.cm*) - - -all: byte native - -byte: _tags META - @echo Build bytecode lib - $(Q)test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) - $(Q)$(OCAMLBUILD) $(QUIET) $(PP_OPTS) -build-dir $(BUILDDIR) lib-pprzlink.cma - $(Q)cp META $(BUILDDIR) - -# byte with statically linked libs -static: _tags META - @echo Build bytecode lib with static linking - $(Q)test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) - $(Q)$(OCAMLBUILD) $(QUIET) -build-dir $(BUILDDIR) -tag static lib-pprzlink.cma - $(Q)cp META $(BUILDDIR) - -native: _tags META - @echo Build native lib - $(Q)test -d $(BUILDDIR) || mkdir -p $(BUILDDIR) - $(Q)$(OCAMLBUILD) $(QUIET) $(PP_OPTS) -build-dir $(BUILDDIR) lib-pprzlink.cmxa - $(Q)cp META $(BUILDDIR) - -install: byte native META - @echo INSTALL at location $(DESTDIR) - $(Q)test -d $(DESTDIR) || mkdir -p $(DESTDIR) - $(Q)ocamlfind remove $(INSTALL_FLAGS) pprzlink $(Q_OUT) - $(Q)ocamlfind install $(INSTALL_FLAGS) pprzlink META $(INSTALL_FILES) $(Q_OUT) - -UNAME = $(shell uname -s) -ifeq ("$(UNAME)","Darwin") - MKTEMP = gmktemp -else - MKTEMP = mktemp -endif - -clean : - $(Q)rm -f *.cm* *.out *.opt .depend *.a *.o *.so - $(Q)ocamlbuild -clean -classic-display -build-dir $(BUILDDIR) - -.PHONY: all byte static native install install_local clean diff --git a/lib/v1.0/ocaml/common b/lib/v1.0/ocaml/common new file mode 120000 index 0000000..7b9179b --- /dev/null +++ b/lib/v1.0/ocaml/common @@ -0,0 +1 @@ +../../common/ocaml \ No newline at end of file diff --git a/lib/v1.0/python/Makefile b/lib/v1.0/python/Makefile new file mode 100644 index 0000000..245b819 --- /dev/null +++ b/lib/v1.0/python/Makefile @@ -0,0 +1,34 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2017 Fabien Garcia +# +# 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 +# . +# + +MAKE = make + +all: + +install: + # Install correct version python library to DESTDIR + @echo INSTALL at location $(DESTDIR) + $(Q)test -d $(DESTDIR) || mkdir -p $(DESTDIR) + $(Q)cp -r pprzlink $(DESTDIR) + + +.PHONY: all install + diff --git a/lib/v1.0/python/pprzlink/ivy.py b/lib/v1.0/python/pprzlink/ivy.py index 77c5429..58501bf 100644 --- a/lib/v1.0/python/pprzlink/ivy.py +++ b/lib/v1.0/python/pprzlink/ivy.py @@ -84,7 +84,7 @@ def unbind(self, bind_id): IvyUnBindMsg(bind_id) del self.bindings[bind_id] - def subscribe(self, callback, regex='(.*)'): + def subscribe(self, callback, regex_or_msg='(.*)'): """ Subscribe to Ivy message matching regex and call callback with ac_id and PprzMessage TODO: possibility to directly specify PprzMessage instead of regex @@ -92,6 +92,11 @@ def subscribe(self, callback, regex='(.*)'): :param callback: function called on new message with ac_id and PprzMessage as params :param regex: regular expression for matching message """ + if not isinstance(regex_or_msg,PprzMessage): + regex = regex_or_msg + else: + regex = '^([^ ]* +%s(.*|$))'%(regex_or_msg.name) + bind_id = IvyBindMsg(lambda agent, *larg: self.parse_pprz_msg(callback, larg[0]), regex) self.bindings[bind_id] = (callback, regex) return bind_id @@ -139,7 +144,11 @@ def parse_pprz_msg(callback, ivy_msg): if msg_class is None: print("Ignoring unknown message " + ivy_msg) return - # pass non-telemetry messages with ac_id 0 + payload = data[3:] if advanced else data[2:] + values = list(filter(None, payload)) + msg = PprzMessage(msg_class, msg_name) + msg.set_values(values) + # pass non-telemetry messages with ac_id 0 or ac_id attrib value if msg_class == "telemetry": try: sdata = data[0] @@ -151,11 +160,11 @@ def parse_pprz_msg(callback, ivy_msg): print("ignoring message " + ivy_msg) sys.stdout.flush() else: - ac_id = 0 - payload = data[3:] if advanced else data[2:] - values = list(filter(None, payload)) - msg = PprzMessage(msg_class, msg_name) - msg.set_values(values) + if 'ac_id' in msg.fieldnames: + ac_id_idx = msg.fieldnames.index('ac_id') + ac_id = msg.fieldvalues[ac_id_idx] + else: + ac_id = 0 # finally call the callback, passing the aircraft id and parsed message callback(ac_id, msg) diff --git a/lib/v1.0/python/pprzlink/message.py b/lib/v1.0/python/pprzlink/message.py index 0fb4ff2..7001769 100644 --- a/lib/v1.0/python/pprzlink/message.py +++ b/lib/v1.0/python/pprzlink/message.py @@ -43,6 +43,10 @@ def __init__(self, class_name, msg): self._fieldvalues.append([0]) else: self._fieldvalues.append(0) + if messages_xml_map.message_dictionary_broadcast[self._name]=='forwarded': + self.broadcasted = False + else: + self.broadcasted = True @property def name(self): @@ -117,8 +121,9 @@ def __setitem__(self, key, value): self.set_value_by_name(key, value) def set_values(self, values): - #print("msg %s: %s" % (self.name, ", ".join(self.fieldnames))) + # print("msg %s: %s" % (self.name, ", ".join(self.fieldnames))) if len(values) == len(self.fieldnames): + # for idx in range(len(values)): self._fieldvalues = values else: raise PprzMessageError("Error: Msg %s has %d fields, tried to set %i values" % @@ -161,7 +166,10 @@ def payload_to_ivy_string(self, sep=' '): ivy_str = '' for idx, t in enumerate(self.fieldtypes): if "char[" in t: - ivy_str += '"' + self.fieldvalues[idx] + '"' + str_value ='' + for c in self.fieldvalues[idx]: + str_value += c + ivy_str += '"' + str_value + '"' elif '[' in t: ivy_str += ','.join([str(x) for x in self.fieldvalues[idx]]) else: @@ -186,7 +194,13 @@ def payload_to_binary(self): for x in self.fieldvalues[idx]: data.append(x) else: - data.append(self.fieldvalues[idx]) + # Assign the right type according to field description + if bin_type[0]=='f' or bin_type[0]== 'd': + data.append(float(self.fieldvalues[idx])) + elif bin_type[0]== 'B' or bin_type[0]== 'H' or bin_type[0]== 'L' or bin_type[0]== 'b' or bin_type[0]== 'h' or bin_type[0]== 'l': + data.append(int(self.fieldvalues[idx])) + else: + data.append(self.fieldvalues[idx]) length += bin_type[1] * array_length msg = struct.pack(struct_string, *data) return msg diff --git a/lib/v1.0/python/pprzlink/messages_xml_map.py b/lib/v1.0/python/pprzlink/messages_xml_map.py index 0f8bca8..23af7f2 100755 --- a/lib/v1.0/python/pprzlink/messages_xml_map.py +++ b/lib/v1.0/python/pprzlink/messages_xml_map.py @@ -5,19 +5,25 @@ import os # if PAPARAZZI_HOME is set use $PAPARAZZI_HOME/var/messages.xml -# else use messages.xml from pprzlink message_definitions directly +# else assume this file is installed in var/lib/python/pprzlink +# and use messages.xml from var +# Message definition file should be installed at the same +# time as this file so it should be ok PPRZ_HOME = os.getenv("PAPARAZZI_HOME") if PPRZ_HOME is not None: default_messages_file = '%s/var/messages.xml' % PPRZ_HOME else: default_messages_file = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), "../../../../message_definitions/v1.0/messages.xml")) +# Define the pprzlink protocol version +PROTOCOL_VERSION="1.0" message_dictionary = {} message_dictionary_types = {} message_dictionary_coefs = {} message_dictionary_id_name = {} message_dictionary_name_id = {} +message_dictionary_broadcast = {} class MessagesNotFound(Exception): @@ -55,6 +61,11 @@ def parse_messages(messages_file=''): else: message_id = int(message_id) + if 'link' in the_message.attrib: + message_dictionary_broadcast[message_name] = the_message.attrib['link'] + else: + message_dictionary_broadcast[message_name] = 'forwarded' # Default behavior is to send message to destination only + message_dictionary_id_name[class_name][message_id] = message_name message_dictionary_name_id[class_name][message_name] = message_id diff --git a/lib/v1.0/python/pprzlink/udp.py b/lib/v1.0/python/pprzlink/udp.py index fc90ec6..06c7a83 100644 --- a/lib/v1.0/python/pprzlink/udp.py +++ b/lib/v1.0/python/pprzlink/udp.py @@ -67,13 +67,14 @@ def run(self): # Parse incoming data try: (msg, address) = self.server.recvfrom(2048) + length = len(msg) for c in msg: if self.trans.parse_byte(c): (sender_id, msg) = self.trans.unpack() if self.verbose: print("New incoming message '%s' from %i (%s)" % (msg.name, sender_id, address)) # Callback function on new message - self.callback(sender_id, address, msg) + self.callback(sender_id, address, msg, length) except socket.timeout: pass diff --git a/lib/v2.0/C/Makefile b/lib/v2.0/C/Makefile new file mode 100644 index 0000000..ad84314 --- /dev/null +++ b/lib/v2.0/C/Makefile @@ -0,0 +1,33 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2017 Fabien Garcia +# +# 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 +# . +# + +MAKE = make + +all: + +install: +# $(Q)cp ../../common/C/*.c $(MESSAGES_LIB) + $(Q)cp ../../common/C/*.h $(MESSAGES_INCLUDE) + $(Q)cp ./*.h $(MESSAGES_INCLUDE) + $(Q)cp ./*.c $(MESSAGES_LIB) + + +.PHONY: install diff --git a/lib/v2.0/C/ivy_transport.c b/lib/v2.0/C/ivy_transport.c new file mode 100644 index 0000000..3c59b64 --- /dev/null +++ b/lib/v2.0/C/ivy_transport.c @@ -0,0 +1,207 @@ +/* + * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/ivy_transport.c + * + * Building Paparazzi frames over IVY. + * + */ + +#include "pprzlink/ivy_transport.h" + +#include +#include + +static struct ivy_transport * get_ivy_trans(struct pprzlink_msg *msg) +{ + return (struct ivy_transport *)(msg->trans->impl); +} + +static void put_bytes(struct pprzlink_msg *msg, long fd __attribute__((unused)), + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + const void *bytes, uint16_t len) +{ + const uint8_t *b = (const uint8_t *) bytes; + struct ivy_transport *trans = get_ivy_trans(msg); + + // Start delimiter "quote" for char arrays (strings) + if (format == DL_FORMAT_ARRAY && type == DL_TYPE_CHAR) { + trans->ivy_p += sprintf(trans->ivy_p, "\""); + } + + int i = 0; + while (i < len) { + // print data with correct type + switch (type) { + case DL_TYPE_CHAR: + trans->ivy_p += sprintf(trans->ivy_p, "%c", (char)(*((char *)(b + i)))); + i++; + break; + case DL_TYPE_UINT8: + trans->ivy_p += sprintf(trans->ivy_p, "%u", b[i]); + i++; + break; + case DL_TYPE_UINT16: + trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint16_t)(*((uint16_t *)(b + i)))); + i += 2; + break; + case DL_TYPE_UINT32: + case DL_TYPE_TIMESTAMP: + trans->ivy_p += sprintf(trans->ivy_p, "%u", (uint32_t)(*((uint32_t *)(b + i)))); + i += 4; + break; + case DL_TYPE_UINT64: +#if __WORDSIZE == 64 + trans->ivy_p += sprintf(trans->ivy_p, "%lu", (uint64_t)(*((uint64_t *)(b + i)))); +#else + trans->ivy_p += sprintf(trans->ivy_p, "%llu", (uint64_t)(*((uint64_t *)(b + i)))); +#endif + i += 8; + break; + case DL_TYPE_INT8: + trans->ivy_p += sprintf(trans->ivy_p, "%d", (int8_t)(*((int8_t *)(b + i)))); + i++; + break; + case DL_TYPE_INT16: + trans->ivy_p += sprintf(trans->ivy_p, "%d", (int16_t)(*((int16_t *)(b + i)))); + i += 2; + break; + case DL_TYPE_INT32: + trans->ivy_p += sprintf(trans->ivy_p, "%d", (int32_t)(*((int32_t *)(b + i)))); + i += 4; + break; + case DL_TYPE_INT64: +#if __WORDSIZE == 64 + trans->ivy_p += sprintf(trans->ivy_p, "%ld", (uint64_t)(*((uint64_t *)(b + i)))); +#else + trans->ivy_p += sprintf(trans->ivy_p, "%lld", (uint64_t)(*((uint64_t *)(b + i)))); +#endif + i += 8; + break; + case DL_TYPE_FLOAT: + trans->ivy_p += sprintf(trans->ivy_p, "%f", (float)(*((float *)(b + i)))); + i += 4; + break; + case DL_TYPE_DOUBLE: + trans->ivy_p += sprintf(trans->ivy_p, "%f", (double)(*((double *)(b + i)))); + i += 8; + break; + case DL_TYPE_ARRAY_LENGTH: + default: + // Don't print array length but increment index + i++; + break; + } + // Coma delimiter for array, no delimiter for char array (string), space otherwise + if (format == DL_FORMAT_ARRAY) { + if (type != DL_TYPE_CHAR) { + trans->ivy_p += sprintf(trans->ivy_p, ","); + } + } else { + trans->ivy_p += sprintf(trans->ivy_p, " "); + } + } + + // space end delimiter for arrays, additionally un-quote char arrays (strings) + if (format == DL_FORMAT_ARRAY) { + if (type == DL_TYPE_CHAR) { + trans->ivy_p += sprintf(trans->ivy_p, "\" "); + } else { + trans->ivy_p += sprintf(trans->ivy_p, " "); + } + } +} + +static void put_named_byte(struct pprzlink_msg *msg, long fd __attribute__((unused)), + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte __attribute__((unused)), const char *name __attribute__((unused))) +{ + if (name != NULL) { + get_ivy_trans(msg)->ivy_p += sprintf(get_ivy_trans(msg)->ivy_p, "%s ", name); + } +} + +static uint8_t size_of(struct pprzlink_msg *msg __attribute__((unused)), uint8_t len) +{ + return len; +} + +static void start_message(struct pprzlink_msg *msg, long fd __attribute__((unused)), + uint8_t payload_len __attribute__((unused))) +{ + get_ivy_trans(msg)->ivy_p = get_ivy_trans(msg)->ivy_buf; +} + +static void end_message(struct pprzlink_msg *msg, long fd __attribute__((unused))) +{ + struct ivy_transport *trans = get_ivy_trans(msg); + *(--trans->ivy_p) = '\0'; + if (trans->ivy_dl_enabled) { + IvySendMsg("%s", trans->ivy_buf); + msg->dev->nb_msgs++; + } +} + +static void overrun(struct pprzlink_msg *msg) +{ + msg->dev->nb_ovrn++; +} + +static void count_bytes(struct pprzlink_msg *msg, uint8_t bytes) +{ + msg->dev->nb_bytes += bytes; +} + +static int check_available_space(struct pprzlink_msg *msg __attribute__((unused)), long *fd __attribute__((unused)), uint16_t bytes __attribute__((unused))) +{ + return 1; +} + +static int check_free_space(struct ivy_transport *t __attribute__((unused)), long *fd __attribute__((unused)), uint8_t len __attribute__((unused))) { return 1; } +static void put_byte(struct ivy_transport *t __attribute__((unused)), long fd __attribute__((unused)), uint8_t byte __attribute__((unused))) {} +static void put_buffer(struct ivy_transport *t __attribute__((unused)), long fd __attribute__((unused)), const uint8_t *buffer __attribute__((unused)), uint16_t len __attribute__((unused))) {} +static void send_message(struct ivy_transport *t __attribute__((unused)), long fd __attribute__((unused))) {} +static int null_function(struct ivy_transport *t __attribute__((unused))) { return 0; } + +void ivy_transport_init(struct ivy_transport *t) +{ + t->ivy_p = t->ivy_buf; + t->ivy_dl_enabled = true; + + t->trans_tx.size_of = (size_of_t) size_of; + t->trans_tx.check_available_space = (check_available_space_t) check_available_space; + t->trans_tx.put_bytes = (put_bytes_t) put_bytes; + t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + t->trans_tx.start_message = (start_message_t) start_message; + t->trans_tx.end_message = (end_message_t) end_message; + t->trans_tx.overrun = (overrun_t) overrun; + t->trans_tx.count_bytes = (count_bytes_t) count_bytes; + t->trans_tx.impl = (void *)(t); + t->device.check_free_space = (check_free_space_t) check_free_space; + t->device.put_byte = (put_byte_t) put_byte; + t->device.put_buffer = (put_buffer_t) put_buffer; + t->device.send_message = (send_message_t) send_message; + t->device.char_available = (char_available_t) null_function; + t->device.get_byte = (get_byte_t) null_function; + t->device.periph = (void *)(t); +} diff --git a/lib/v2.0/C/ivy_transport.h b/lib/v2.0/C/ivy_transport.h new file mode 100644 index 0000000..838bb5d --- /dev/null +++ b/lib/v2.0/C/ivy_transport.h @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014-2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/ivy_transport.h + * + * Building Paparazzi frames over IVY. + * + */ + +#ifndef IVY_TRANSPORT_H +#define IVY_TRANSPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "pprzlink/pprzlink_transport.h" +#include "pprzlink/pprzlink_device.h" + +// IVY transport +struct ivy_transport { + char ivy_buf[256]; + char *ivy_p; + int ivy_dl_enabled; + // generic transmission interface + struct transport_tx trans_tx; + // generic (dummy) device + struct link_device device; +}; + +// Init function +extern void ivy_transport_init(struct ivy_transport *t); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // IVY_TRANSPORT_H + diff --git a/lib/v2.0/C/pprz_transport.c b/lib/v2.0/C/pprz_transport.c new file mode 100644 index 0000000..625e284 --- /dev/null +++ b/lib/v2.0/C/pprz_transport.c @@ -0,0 +1,203 @@ +/* + * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014-2017 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/pprz_transport.c + * + * Building and parsing Paparazzi frames. + * + * Pprz frame: + * + * |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B| + * + * where checksum is computed over length and payload: + * @code + * ck_A = ck_B = length + * for each byte b in payload + * ck_A += b; + * ck_b += ck_A; + * @endcode + */ + +#include +#include "pprzlink/pprz_transport.h" + +// PPRZ parsing state machine +#define UNINIT 0 +#define GOT_STX 1 +#define GOT_LENGTH 2 +#define GOT_PAYLOAD 3 +#define GOT_CRC1 4 + +static struct pprz_transport * get_pprz_trans(struct pprzlink_msg *msg) +{ + return (struct pprz_transport *)(msg->trans->impl); +} + +static void accumulate_checksum(struct pprz_transport *trans, const uint8_t byte) +{ + trans->ck_a_tx += byte; + trans->ck_b_tx += trans->ck_a_tx; +} + +static void put_bytes(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + const void *bytes, uint16_t len) +{ + const uint8_t *b = (const uint8_t *) bytes; + int i; + for (i = 0; i < len; i++) { + accumulate_checksum(get_pprz_trans(msg), b[i]); + } + msg->dev->put_buffer(msg->dev->periph, fd, b, len); +} + +static void put_named_byte(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) +{ + accumulate_checksum(get_pprz_trans(msg), byte); + msg->dev->put_byte(msg->dev->periph, fd, byte); +} + +static uint8_t size_of(struct pprzlink_msg *msg __attribute__((unused)), uint8_t len) +{ + // message length: payload + protocol overhead (STX + len + ck_a + ck_b = 4) + return len + 4; +} + +static void start_message(struct pprzlink_msg *msg, long fd, uint8_t payload_len) +{ + msg->dev->put_byte(msg->dev->periph, fd, PPRZ_STX); + const uint8_t msg_len = size_of(msg, payload_len); + msg->dev->put_byte(msg->dev->periph, fd, msg_len); + get_pprz_trans(msg)->ck_a_tx = msg_len; + get_pprz_trans(msg)->ck_b_tx = msg_len; +} + +static void end_message(struct pprzlink_msg *msg, long fd) +{ + msg->dev->put_byte(msg->dev->periph, fd, get_pprz_trans(msg)->ck_a_tx); + msg->dev->put_byte(msg->dev->periph, fd, get_pprz_trans(msg)->ck_b_tx); + msg->dev->send_message(msg->dev->periph, fd); +} + +static void overrun(struct pprzlink_msg *msg) +{ + msg->dev->nb_ovrn++; +} + +static void count_bytes(struct pprzlink_msg *msg, uint8_t bytes) +{ + msg->dev->nb_bytes += bytes; +} + +static int check_available_space(struct pprzlink_msg *msg, long *fd, uint16_t bytes) +{ + return msg->dev->check_free_space(msg->dev->periph, fd, bytes); +} + +// Init pprz transport structure +void pprz_transport_init(struct pprz_transport *t) +{ + t->status = UNINIT; + t->trans_rx.msg_received = false; + t->trans_tx.size_of = (size_of_t) size_of; + t->trans_tx.check_available_space = (check_available_space_t) check_available_space; + t->trans_tx.put_bytes = (put_bytes_t) put_bytes; + t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + t->trans_tx.start_message = (start_message_t) start_message; + t->trans_tx.end_message = (end_message_t) end_message; + t->trans_tx.overrun = (overrun_t) overrun; + t->trans_tx.count_bytes = (count_bytes_t) count_bytes; + t->trans_tx.impl = (void *)(t); +} + + +// Parsing function +void parse_pprz(struct pprz_transport *t, uint8_t c) +{ + switch (t->status) { + case UNINIT: + if (c == PPRZ_STX) { + t->status++; + } + break; + case GOT_STX: + if (t->trans_rx.msg_received) { + t->trans_rx.ovrn++; + goto error; + } + t->trans_rx.payload_len = c - 4; /* Counting STX, LENGTH and CRC1 and CRC2 */ + t->ck_a_rx = t->ck_b_rx = c; + t->status++; + t->payload_idx = 0; + break; + case GOT_LENGTH: + t->trans_rx.payload[t->payload_idx] = c; + t->ck_a_rx += c; t->ck_b_rx += t->ck_a_rx; + t->payload_idx++; + if (t->payload_idx == t->trans_rx.payload_len) { + t->status++; + } + break; + case GOT_PAYLOAD: + if (c != t->ck_a_rx) { + goto error; + } + t->status++; + break; + case GOT_CRC1: + if (c != t->ck_b_rx) { + goto error; + } + t->trans_rx.msg_received = true; + goto restart; + default: + goto error; + } + return; +error: + t->trans_rx.error++; +restart: + t->status = UNINIT; + return; +} + + +/** Parsing a frame data and copy the payload to the datalink buffer */ +void pprz_check_and_parse(struct link_device *dev, struct pprz_transport *trans, uint8_t *buf, bool *msg_available) +{ + uint8_t i; + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !trans->trans_rx.msg_received) { + parse_pprz(trans, dev->get_byte(dev->periph)); + } + if (trans->trans_rx.msg_received) { + for (i = 0; i < trans->trans_rx.payload_len; i++) { + buf[i] = trans->trans_rx.payload[i]; + } + *msg_available = true; + trans->trans_rx.msg_received = false; + } + } +} diff --git a/lib/v2.0/C/pprz_transport.h b/lib/v2.0/C/pprz_transport.h new file mode 100644 index 0000000..b5c4dc8 --- /dev/null +++ b/lib/v2.0/C/pprz_transport.h @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/pprz_transport.h + * + * Building and parsing Paparazzi frames. + * + * Pprz frame: + * + * |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B| + * + * where checksum is computed over length and payload: + * @code + * ck_A = ck_B = length + * for each byte b in payload + * ck_A += b; + * ck_b += ck_A; + * @endcode + */ + +#ifndef PPRZ_TRANSPORT_H +#define PPRZ_TRANSPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "pprzlink/pprzlink_transport.h" +#include "pprzlink/pprzlink_device.h" + +// Start byte +#define PPRZ_STX 0x99 + +/* PPRZ Transport + */ + +struct pprz_transport { + // generic reception interface + struct transport_rx trans_rx; + // specific pprz transport_rx variables + uint8_t status; + uint8_t payload_idx; + uint8_t ck_a_rx, ck_b_rx; + // generic transmission interface + struct transport_tx trans_tx; + // specific pprz transport_tx variables + uint8_t ck_a_tx, ck_b_tx; +}; + +// Init function +extern void pprz_transport_init(struct pprz_transport *t); + +// Checking new data and parsing +extern void pprz_check_and_parse(struct link_device *dev, struct pprz_transport *trans, uint8_t *buf, bool *msg_available); + +// Parsing function, only needed for modules doing their own parsing +// without using the pprz_check_and_parse function +extern void parse_pprz(struct pprz_transport *t, uint8_t c); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* PPRZ_TRANSPORT_H */ + diff --git a/lib/v2.0/C/pprzlog_transport.c b/lib/v2.0/C/pprzlog_transport.c new file mode 100644 index 0000000..aa06e5e --- /dev/null +++ b/lib/v2.0/C/pprzlog_transport.c @@ -0,0 +1,128 @@ +/* + * Copyright (C) 2014-2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/pprzlog_transport.c + * + * Building and Paparazzi frames with timestamp for data logger. + * + * LOG-message: ABCDEFGHxxxxxxxI + * A PPRZ_STX (0x99) + * B LENGTH (H->H) + * C SOURCE (0=uart0, 1=uart1, 2=i2c0, ...) + * D TIMESTAMP_LSB (100 microsec raster) + * E TIMESTAMP + * F TIMESTAMP + * G TIMESTAMP_MSB + * H PPRZ_DATA + * 0 SENDER_ID + * 1 MSG_ID + * 2 MSG_PAYLOAD + * . DATA (messages.xml) + * I CHECKSUM (sum[B->H]) + * + */ + +#include +#include "pprzlink/pprzlog_transport.h" + +#define STX_LOG 0x99 + +static struct pprzlog_transport * get_pprzlog_trans(struct pprzlink_msg *msg) +{ + return (struct pprzlog_transport *)(msg->trans->impl); +} + +static void accumulate_checksum(struct pprzlog_transport *trans, const uint8_t byte) +{ + trans->ck += byte; +} + +static void put_bytes(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + const void *bytes, uint16_t len) +{ + const uint8_t *b = (const uint8_t *) bytes; + int i; + for (i = 0; i < len; i++) { + accumulate_checksum(get_pprzlog_trans(msg), b[i]); + } + msg->dev->put_buffer(msg->dev->periph, fd, b, len); +} + +static void put_named_byte(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) +{ + accumulate_checksum(get_pprzlog_trans(trans), byte); + msg->dev->put_byte(msg->dev->periph, fd, byte); +} + +static uint8_t size_of(struct pprzlink_msg *msg __attribute__((unused)), uint8_t len) +{ + // add offset: STX(1), LENGTH(1), SOURCE(1), TIMESTAMP(4), CHECKSUM(1) + return len + 8; +} + +static void start_message(struct pprzlink_msg *msg, long fd, uint8_t payload_len) +{ + msg->dev->put_byte(msg->dev->periph, fd, STX_LOG); + const uint8_t msg_len = payload_len; // only the payload length here + get_pprzlog_trans(msg)->ck = 0; + uint8_t buf[] = { msg_len, 0 }; // TODO use correct source ID + put_bytes(msg, fd, DL_TYPE_UINT8, DL_FORMAT_SCALAR, buf, 2); + uint32_t ts = get_pprzlog_trans(msg)->get_time_usec() / 100; + put_bytes(msg, fd, DL_TYPE_TIMESTAMP, DL_FORMAT_SCALAR, (uint8_t *)(&ts), 4); +} + +static void end_message(struct pprzlink_msg *msg, long fd) +{ + msg->dev->put_byte(msg->dev->periph, fd, get_pprzlog_trans(msg)->ck); + msg->dev->send_message(msg->dev->periph, fd); +} + +static void overrun(struct pprzlink_msg *msg __attribute__((unused))) +{ +} + +static void count_bytes(struct pprzlink_msg *msg __attribute__((unused)), uint8_t bytes __attribute__((unused))) +{ +} + +static int check_available_space(struct pprzlink_msg *msg, long *fd, uint16_t bytes) +{ + return msg->dev->check_free_space(msg->dev->periph, fd, bytes); +} + +void pprzlog_transport_init(struct pprzlog_transport *t, get_time_usec_t get_time_usec) +{ + t->trans_tx.size_of = (size_of_t) size_of; + t->trans_tx.check_available_space = (check_available_space_t) check_available_space; + t->trans_tx.put_bytes = (put_bytes_t) put_bytes; + t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + t->trans_tx.start_message = (start_message_t) start_message; + t->trans_tx.end_message = (end_message_t) end_message; + t->trans_tx.overrun = (overrun_t) overrun; + t->trans_tx.count_bytes = (count_bytes_t) count_bytes; + t->trans_tx.impl = (void *)(t); + t->get_time_usec = get_time_usec; +} + diff --git a/lib/v2.0/C/pprzlog_transport.h b/lib/v2.0/C/pprzlog_transport.h new file mode 100644 index 0000000..e81bdab --- /dev/null +++ b/lib/v2.0/C/pprzlog_transport.h @@ -0,0 +1,57 @@ +/* + * Copyright (C) 2014-2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/pprzlog_transport.h + * + * Protocol for on-board data logger with timestamp + * + */ + +#ifndef PPRZLOG_TRANSPORT_H +#define PPRZLOG_TRANSPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "pprzlink/pprzlink_transport.h" + +typedef uint32_t (*get_time_usec_t)(void); + +struct pprzlog_transport { + // generic transmission interface + struct transport_tx trans_tx; + // specific pprz transport_tx variables + uint8_t ck; + // get current time function pointer + get_time_usec_t get_time_usec; +}; + +// Init function +extern void pprzlog_transport_init(struct pprzlog_transport *t, uint32_t (*get_time_usec_t)(void)); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif + diff --git a/lib/v2.0/C/short_transport.c b/lib/v2.0/C/short_transport.c new file mode 100644 index 0000000..ae43775 --- /dev/null +++ b/lib/v2.0/C/short_transport.c @@ -0,0 +1,87 @@ +/* + * Copyright (C) 2016 Freek van Tienen + * + * 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 + * . + * + */ + +/** + * @file pprzlink/short_transport.c + * + * Paparazzi frame with only PPRZ_DATA + * + */ + +#include +#include "pprzlink/short_transport.h" + +static void put_bytes(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + const void *bytes, uint16_t len) +{ + uint8_t *b = (uint8_t *)bytes; + msg->dev->put_buffer(msg->dev->periph, fd, b, len); +} + +static void put_named_byte(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) +{ + msg->dev->put_byte(msg->dev->periph, fd, byte); +} + +static uint8_t size_of(struct pprzlink_msg *msg __attribute__((unused)), uint8_t len) +{ + return len; +} + +static void start_message(struct pprzlink_msg *msg __attribute__((unused)), + long fd __attribute__((unused)), uint8_t payload_len __attribute__((unused))) +{ +} + +static void end_message(struct pprzlink_msg *msg, long fd) +{ + msg->dev->send_message(msg->dev->periph, fd); +} + +static void overrun(struct pprzlink_msg *msg __attribute__((unused))) +{ +} + +static void count_bytes(struct pprzlink_msg *msg __attribute__((unused)), uint8_t bytes __attribute__((unused))) +{ +} + +static int check_available_space(struct pprzlink_msg *msg, long *fd, uint16_t bytes) +{ + return msg->dev->check_free_space(msg->dev->periph, fd, bytes); +} + +void short_transport_init(struct short_transport *t) +{ + t->trans_tx.size_of = (size_of_t) size_of; + t->trans_tx.check_available_space = (check_available_space_t) check_available_space; + t->trans_tx.put_bytes = (put_bytes_t) put_bytes; + t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + t->trans_tx.start_message = (start_message_t) start_message; + t->trans_tx.end_message = (end_message_t) end_message; + t->trans_tx.overrun = (overrun_t) overrun; + t->trans_tx.count_bytes = (count_bytes_t) count_bytes; + t->trans_tx.impl = (void *)(t); +} + diff --git a/lib/v2.0/C/short_transport.h b/lib/v2.0/C/short_transport.h new file mode 100644 index 0000000..4e40366 --- /dev/null +++ b/lib/v2.0/C/short_transport.h @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2016 Freek van Tienen + * + * 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 + * . + * + */ + +/** + * @file pprzlink/short_transport.h + * + * Protocol without any overhead and only the paparazzi data + * This includes AC_ID, MSG_ID and MSG_DATA + */ + +#ifndef SHORT_TRANSPORT_H +#define SHORT_TRANSPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "pprzlink/pprzlink_transport.h" + +struct short_transport { + // generic transmission interface + struct transport_tx trans_tx; +}; + +// Init function +extern void short_transport_init(struct short_transport *t); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/lib/v2.0/C/xbee_transport.c b/lib/v2.0/C/xbee_transport.c new file mode 100644 index 0000000..e06037b --- /dev/null +++ b/lib/v2.0/C/xbee_transport.c @@ -0,0 +1,411 @@ +/* + * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014-2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/xbee_transport.c + * Maxstream XBee serial input and output + */ + +#include +#include "pprzlink/xbee_transport.h" +#include "pprzlink/print_utils.h" + +/** Ground station address */ +#define GROUND_STATION_ADDR 0x100 + +/** Constants for the API protocol */ +#define TX_OPTIONS 0x00 +#define NO_FRAME_ID 0 +#define XBEE_API_OVERHEAD 5 /* start + len_msb + len_lsb + API_id + checksum */ + +#define AT_COMMAND_SEQUENCE "+++" +#define AT_SET_MY "ATMY" +#define AT_AP_MODE "ATAP1\r" +#define AT_EXIT "ATCN\r" + +/** XBEE 2.4 specific parameters */ +#define XBEE_24_TX_ID 0x01 /* 16 bits address */ +#define XBEE_24_RX_ID 0x81 /* 16 bits address */ +#define XBEE_24_RFDATA_OFFSET 5 +#define XBEE_24_ADDR_OFFSET 2 +#define XBEE_24_TX_OVERHEAD 4 +#define XBEE_24_TX_HEADER { \ + XBEE_24_TX_ID, \ + NO_FRAME_ID, \ + (GROUND_STATION_ADDR >> 8), \ + (GROUND_STATION_ADDR & 0xff), \ + TX_OPTIONS \ + } + +/** XBEE 868 specific parameters */ +#define XBEE_868_TX_ID 0x10 +#define XBEE_868_RX_ID 0x90 +#define XBEE_868_RFDATA_OFFSET 12 +#define XBEE_868_ADDR_OFFSET 8 +#define XBEE_868_TX_OVERHEAD 13 +#define XBEE_868_TX_HEADER { \ + XBEE_868_TX_ID, \ + NO_FRAME_ID, \ + 0x00, \ + 0x00, \ + 0x00, \ + 0x00, \ + 0x00, \ + 0x00, \ + (GROUND_STATION_ADDR >> 8), \ + (GROUND_STATION_ADDR & 0xff), \ + 0xff, \ + 0xfe, \ + 0x00, \ + TX_OPTIONS \ + } + + +/** Start byte */ +#define XBEE_START 0x7e + +/** Status of the API packet receiver automata */ +#define XBEE_UNINIT 0 +#define XBEE_GOT_START 1 +#define XBEE_GOT_LENGTH_MSB 2 +#define XBEE_GOT_LENGTH_LSB 3 +#define XBEE_GOT_PAYLOAD 4 + +/** Xbee protocol implementation */ + +static struct xbee_transport * get_xbee_trans(struct pprzlink_msg *msg) +{ + return (struct xbee_transport *)(msg->trans->impl); +} + +/** set destination address in XBEE header + * if 0xFF is passed, set XBEE broadcast (0xFFFF) + * if 0x0 is passed, set default ground station address (0x100) + */ +static void set_xbee_receiver_addr(uint8_t *buf, uint8_t offset, uint8_t addr) +{ + uint16_t receiver = 0; + if (addr == 0x0) { + receiver = GROUND_STATION_ADDR; + } else if (addr == 0xFF) { + receiver = 0xFFFF; + } else { + receiver = (uint16_t) addr; + } + buf[offset] = (receiver >> 8); + buf[offset+1] = (receiver & 0xFF); +} + +static void accumulate_checksum(struct xbee_transport *trans, const uint8_t byte) +{ + trans->cs_tx += byte; +} + +static void put_bytes(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + const void *bytes, uint16_t len) +{ + const uint8_t *b = (const uint8_t *) bytes; + int i; + for (i = 0; i < len; i++) { + accumulate_checksum(get_xbee_trans(msg), b[i]); + } + msg->dev->put_buffer(msg->dev->periph, fd, b, len); +} + +static void put_named_byte(struct pprzlink_msg *msg, long fd, + enum TransportDataType type __attribute__((unused)), enum TransportDataFormat format __attribute__((unused)), + uint8_t byte, const char *name __attribute__((unused))) +{ + accumulate_checksum(get_xbee_trans(msg), byte); + msg->dev->put_byte(msg->dev->periph, fd, byte); +} + +static uint8_t size_of(struct pprzlink_msg *msg, uint8_t len) +{ + // message length: payload + API overhead + XBEE TX overhead (868 or 2.4) + if (get_xbee_trans(msg)->type == XBEE_24) { + return len + XBEE_API_OVERHEAD + XBEE_24_TX_OVERHEAD; + } else { + return len + XBEE_API_OVERHEAD + XBEE_868_TX_OVERHEAD; + } +} + +static void start_message(struct pprzlink_msg *msg, long fd, uint8_t payload_len) +{ + msg->dev->nb_msgs++; + msg->dev->put_byte(msg->dev->periph, fd, XBEE_START); + const uint16_t len = payload_len + XBEE_API_OVERHEAD; + msg->dev->put_byte(msg->dev->periph, fd, (len >> 8)); + msg->dev->put_byte(msg->dev->periph, fd, (len & 0xff)); + get_xbee_trans(msg)->cs_tx = 0; + if (get_xbee_trans(msg)->type == XBEE_24) { + uint8_t header[] = XBEE_24_TX_HEADER; + set_xbee_receiver_addr(header, XBEE_24_ADDR_OFFSET, msg->receiver_id); + put_bytes(msg, fd, DL_TYPE_UINT8, DL_FORMAT_SCALAR, header, XBEE_24_TX_OVERHEAD + 1); + } else { + uint8_t header[] = XBEE_868_TX_HEADER; + set_xbee_receiver_addr(header, XBEE_868_ADDR_OFFSET, msg->receiver_id); + put_bytes(msg, fd, DL_TYPE_UINT8, DL_FORMAT_SCALAR, header, XBEE_868_TX_OVERHEAD + 1); + } +} + +static void end_message(struct pprzlink_msg *msg, long fd) +{ + get_xbee_trans(msg)->cs_tx = 0xff - get_xbee_trans(msg)->cs_tx; + msg->dev->put_byte(msg->dev->periph, fd, get_xbee_trans(msg)->cs_tx); + msg->dev->send_message(msg->dev->periph, fd); +} + +static void overrun(struct pprzlink_msg *msg) +{ + msg->dev->nb_ovrn++; +} + +static void count_bytes(struct pprzlink_msg *msg, uint8_t bytes) +{ + msg->dev->nb_bytes += bytes; +} + +static int check_available_space(struct pprzlink_msg *msg, long *fd, + uint16_t bytes) +{ + return msg->dev->check_free_space(msg->dev->periph, fd, bytes); +} + +static bool xbee_text_reply_is_ok(struct link_device *dev) +{ + char c[2]; + int count = 0; + + while (dev->char_available(dev->periph)) { + char cc = dev->get_byte(dev->periph); + if (count < 2) { + c[count] = cc; + } + count++; + } + + if ((count > 2) && (c[0] == 'O') && (c[1] == 'K')) { + return true; + } + + return false; +} + +static bool xbee_try_to_enter_api(struct link_device *dev, void (*wait)(uint32_t)) +{ + + /** Switching to AT mode (FIXME: busy waiting) */ + print_string(dev, 0, AT_COMMAND_SEQUENCE); + + /** - busy wait 1.25s */ + if (wait != NULL) { + wait(1250000); + } + // TODO else do something ? should not append + + return xbee_text_reply_is_ok(dev); +} + +// Init function +void xbee_transport_init(struct xbee_transport *t, struct link_device *dev, uint16_t addr, enum XBeeType type, uint32_t baudrate, void (*wait)(uint32_t), char *xbee_init) +{ + t->status = XBEE_UNINIT; + t->type = type; + t->rssi = 0; + t->trans_rx.msg_received = false; + t->trans_tx.size_of = (size_of_t) size_of; + t->trans_tx.check_available_space = (check_available_space_t) check_available_space; + t->trans_tx.put_bytes = (put_bytes_t) put_bytes; + t->trans_tx.put_named_byte = (put_named_byte_t) put_named_byte; + t->trans_tx.start_message = (start_message_t) start_message; + t->trans_tx.end_message = (end_message_t) end_message; + t->trans_tx.overrun = (overrun_t) overrun; + t->trans_tx.count_bytes = (count_bytes_t) count_bytes; + t->trans_tx.impl = (void *)(t); + + // Empty buffer before init process + while (dev->char_available(dev->periph)) { + dev->get_byte(dev->periph); + } + + /** - busy wait 1.25s + * Mandatory to configure dynamically the xbee module + * if no wait function are provided, skipping this + * and assuming static configuration + */ + if (wait != NULL) { + wait(1250000); + + // try to figure out the alternate baudrate + // skip if baudrate is not 9600 or 57600 + uint32_t alternate; + if (baudrate == 9600) { + alternate = 57600; + } else if (baudrate == 57600) { + alternate = 9600; + } else { + alternate = 0; + } + + if (! xbee_try_to_enter_api(dev, wait)) { + // skip autobaud if baudrate is 0 + if (alternate > 0) { + // Badly configured... try the alternate baudrate: + dev->set_baudrate(dev->periph, alternate); + if (xbee_try_to_enter_api(dev, wait)) { + // The alternate baudrate worked, + if (alternate == 9600) { + print_string(dev, 0, "ATBD6\rATWR\r"); + } else if (alternate == 57600) { + print_string(dev, 0, "ATBD3\rATWR\r"); + } + } else { + // Complete failure, none of the 2 baudrates result in any reply + // TODO: set LED? + + // Set the default baudrate, just in case everything is right + dev->set_baudrate(dev->periph, baudrate); + print_string(dev, 0, "\r"); + } + // Continue changing settings until the EXIT is issued. + } + } + + /** Setting my address */ + print_string(dev, 0, AT_SET_MY); + print_hex16(dev, 0, addr); + print_string(dev, 0, "\r"); + + print_string(dev, 0, AT_AP_MODE); + + // Extra configuration AT commands + if (xbee_init != NULL) { + print_string(dev, 0, xbee_init); + } + + // Switching back to normal mode (and apply all parameters' changes) + print_string(dev, 0, AT_EXIT); + + // Wait for all AT operations to finish before ending init + wait(250000); + + // Set the desired baudrate for normal operation + if (baudrate > 0) { + dev->set_baudrate(dev->periph, baudrate); + } + + } +} + +/** Parsing a XBee API frame */ +static inline void parse_xbee(struct xbee_transport *t, uint8_t c) +{ + switch (t->status) { + case XBEE_UNINIT: + if (c == XBEE_START) { + t->status++; + } + break; + case XBEE_GOT_START: + if (t->trans_rx.msg_received) { + t->trans_rx.ovrn++; + goto error; + } + t->trans_rx.payload_len = c << 8; + t->status++; + break; + case XBEE_GOT_LENGTH_MSB: + t->trans_rx.payload_len |= c; + t->status++; + t->payload_idx = 0; + t->cs_rx = 0; + break; + case XBEE_GOT_LENGTH_LSB: + t->trans_rx.payload[t->payload_idx] = c; + t->cs_rx += c; + t->payload_idx++; + if (t->payload_idx == t->trans_rx.payload_len) { + t->status++; + } + break; + case XBEE_GOT_PAYLOAD: + if (c + t->cs_rx != 0xff) { + goto error; + } + t->trans_rx.msg_received = true; + goto restart; + break; + default: + goto error; + } + return; +error: + t->trans_rx.error++; +restart: + t->status = XBEE_UNINIT; + return; +} + +/** Parsing a frame data and copy the payload to the datalink buffer */ +void xbee_check_and_parse(struct link_device *dev, struct xbee_transport *trans, uint8_t *buf, bool *msg_available) +{ + uint8_t i; + if (dev->char_available(dev->periph)) { + while (dev->char_available(dev->periph) && !trans->trans_rx.msg_received) { + parse_xbee(trans, dev->get_byte(dev->periph)); + } + if (trans->trans_rx.msg_received) { + if (trans->type == XBEE_24) { + switch (trans->trans_rx.payload[0]) { + case XBEE_24_RX_ID: + case XBEE_24_TX_ID: /* Useful if A/C is connected to the PC with a cable */ + trans->rssi = trans->trans_rx.payload[3]; + for (i = XBEE_24_RFDATA_OFFSET; i < trans->trans_rx.payload_len; i++) { + buf[i - XBEE_24_RFDATA_OFFSET] = trans->trans_rx.payload[i]; + } + *msg_available = true; + break; + default: + break; + } + trans->trans_rx.msg_received = false; + } + else if (trans->type == XBEE_868) { + switch (trans->trans_rx.payload[0]) { + case XBEE_868_RX_ID: + case XBEE_868_TX_ID: /* Useful if A/C is connected to the PC with a cable */ + for (i = XBEE_868_RFDATA_OFFSET; i < trans->trans_rx.payload_len; i++) { + buf[i - XBEE_868_RFDATA_OFFSET] = trans->trans_rx.payload[i]; + } + *msg_available = true; + break; + default: + break; + } + trans->trans_rx.msg_received = false; + } + } + } +} + diff --git a/lib/v2.0/C/xbee_transport.h b/lib/v2.0/C/xbee_transport.h new file mode 100644 index 0000000..de6a70c --- /dev/null +++ b/lib/v2.0/C/xbee_transport.h @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2006 Pascal Brisset, Antoine Drouin + * Copyright (C) 2014-2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** + * @file pprzlink/xbee_transport.h + * Maxstream XBee Protocol handling + */ + +#ifndef XBEE_TRANSPORT_H +#define XBEE_TRANSPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "pprzlink/pprzlink_transport.h" +#include "pprzlink/pprzlink_device.h" + +/** Type of XBee module: 2.4 GHz or 868 MHz + */ +enum XBeeType { + XBEE_24, + XBEE_868 +}; + +struct xbee_transport { + enum XBeeType type; ///< type of xbee module (2.4GHz or 868MHz) + // generic reception interface + struct transport_rx trans_rx; + // specific xbee transport variables + uint8_t status; + uint8_t payload_idx; + uint8_t cs_rx; + uint8_t rssi; + // generic transmission interface + struct transport_tx trans_tx; + // specific pprz transport_tx variables + uint8_t cs_tx; +}; + +/** Initialisation in API mode and setting of the local address + * FIXME: busy wait */ +extern void xbee_transport_init(struct xbee_transport *t, struct link_device *dev, uint16_t addr, enum XBeeType type, uint32_t baudrate, void (*wait)(uint32_t), char *xbee_init); + + +extern void xbee_check_and_parse(struct link_device *dev, struct xbee_transport *trans, uint8_t *buf, bool *msg_available); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* XBEE_TRANSPORT_H */ diff --git a/lib/v2.0/ocaml/META b/lib/v2.0/ocaml/META new file mode 100644 index 0000000..e687b57 --- /dev/null +++ b/lib/v2.0/ocaml/META @@ -0,0 +1,8 @@ +description = "PPRZLINK communication package for Paparazzi UAV System" +requires = "unix,str,xml-light,glibivy" +version = "2.0" +directory = "" + +archive(byte) = "lib-pprzlink.cma" +archive(native) = "lib-pprzlink.cmxa" + diff --git a/lib/v2.0/ocaml/Makefile b/lib/v2.0/ocaml/Makefile new file mode 100644 index 0000000..7d28d02 --- /dev/null +++ b/lib/v2.0/ocaml/Makefile @@ -0,0 +1,23 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2017 Gautier Hattenberger +# +# 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 +# . +# + +include ../../common/ocaml/Makefile + diff --git a/lib/v2.0/ocaml/_tags b/lib/v2.0/ocaml/_tags new file mode 100644 index 0000000..fc1d46c --- /dev/null +++ b/lib/v2.0/ocaml/_tags @@ -0,0 +1,12 @@ +# uncomment to add debugging for all +#true: debug + +# +# packages for lib-pprzlink files +# +<*[xX]ml*.ml*>: package(xml-light) +: package(xml-light), package(ivy) + +# link C libs +: linklib(pprzlink_stub.a) + diff --git a/lib/v2.0/ocaml/common b/lib/v2.0/ocaml/common new file mode 120000 index 0000000..7b9179b --- /dev/null +++ b/lib/v2.0/ocaml/common @@ -0,0 +1 @@ +../../common/ocaml \ No newline at end of file diff --git a/lib/v2.0/ocaml/lib-pprzlink.mllib b/lib/v2.0/ocaml/lib-pprzlink.mllib new file mode 100644 index 0000000..c75b172 --- /dev/null +++ b/lib/v2.0/ocaml/lib-pprzlink.mllib @@ -0,0 +1,7 @@ +debugPL +pprzLink +protocol +pprz_transport +pprzlog_transport +xbee_transport +compatPL diff --git a/lib/v2.0/ocaml/libpprzlink_stub.clib b/lib/v2.0/ocaml/libpprzlink_stub.clib new file mode 100644 index 0000000..e7d90fd --- /dev/null +++ b/lib/v2.0/ocaml/libpprzlink_stub.clib @@ -0,0 +1 @@ +convert.o diff --git a/lib/v2.0/ocaml/myocamlbuild.ml b/lib/v2.0/ocaml/myocamlbuild.ml new file mode 100644 index 0000000..5c16039 --- /dev/null +++ b/lib/v2.0/ocaml/myocamlbuild.ml @@ -0,0 +1,36 @@ +(* Ocamlbuild plugin *) +open Ocamlbuild_plugin + +let _ = + dispatch & function + | After_rules -> + (* uncomment to compile any C files with -fPIC, should be default *) + (*flag ["c"; "compile";] (S[A"-ccopt"; A"-fPIC"]);*) + + (* link a simple dependency, e.g. single object file: linkdep(foo.o) *) + pdep ["link"] "linkdep" (fun param -> [param]); + + (* generate mix C/Caml library *) + pflag ["ocamlmklib"; "c"] "linkclib" (fun param -> (S[A("-l"^param)])); + + (* Generate and link a library: + * e.g. linklib(foo.a) to build libfoo.a from libfoo.clib + *) + (* add it as a dependency so the lib gets built *) + pdep ["link"] "linklib" (fun param -> ["lib"^param]); + (* link the lib in bytecode mode *) + pflag ["link";"ocaml";"byte"] "linklib" (fun param -> + let libname = String.sub param 0 (String.length param - 2) in + (S[A"-dllib"; A("-l"^libname); A"-cclib"; A("-l"^libname)])); + (* link the lib in native mode *) + pflag ["link";"ocaml";"native"] "linklib" (fun param -> + let libname = String.sub param 0 (String.length param - 2) in + (S[A"-cclib"; A("-l"^libname)])); + + (* If `static' tag is given, then every ocaml link in bytecode will add -custom *) + flag ["link"; "ocaml"; "byte"; "static"] (A"-custom"); + + (* possibility to add defines for camlp4 in _tags file, e.g. define(FOO) *) + pflag ["ocaml";"compile";] "define" (fun s -> S [A"-ppopt"; A ("-D"^s)]); + pflag ["ocaml";"ocamldep";] "define" (fun s -> S [A"-ppopt"; A ("-D"^s)]) + | _ -> () diff --git a/lib/v2.0/ocaml/pprzLink.ml b/lib/v2.0/ocaml/pprzLink.ml new file mode 100644 index 0000000..09139db --- /dev/null +++ b/lib/v2.0/ocaml/pprzLink.ml @@ -0,0 +1,806 @@ +(* + * PPRZLINK message protocol handling + * + * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2015 Gautier Hattenberger + * + * 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 + * . + * + *) + +open Printf +(*open Latlong*) + +type sender_id = int +type receiver_id = int +type component_id = int +type class_id = int +type message_id = int +type format = string + +type _type = + Scalar of string + | ArrayType of string + | FixedArrayType of string * int +type value = + Int of int | Float of float | String of string | Int32 of int32 | Char of char | Int64 of int64 + | Array of value array +type field = { + _type : _type; + fformat : format; + alt_unit_coef : string; + enum : string list +} + +type link_mode = Forwarded | Broadcasted +type message = { + name : string; (** Lowercase *) + fields : (string * field) list; + link : link_mode option +} + +type type_descr = { + format : string ; + glib_type : string; + inttype : string; + size : int; + value : value +} + +type values = (string * value) list + +type payload = string + + +let separator = "," +let regexp_separator = Str.regexp "," +let split_array = fun s -> Str.split regexp_separator s + +let (//) = Filename.concat + +let pprzlink_dir = + try Sys.getenv "PPRZLINK_DIR" + with _ -> + (* fallback to paparazzi home *) + begin try (Sys.getenv "PAPARAZZI_HOME") // "var" with _ -> + (* fallback to system install *) + begin match Sys.os_type with + | "Unix" -> "/usr/share/pprzlink" + | "Win32" | "Cygwin" | _ -> failwith "MS Windows not supported yet" + end + end + +let messages_file = pprzlink_dir // "messages.xml" +let lazy_messages_xml = lazy (Xml.parse_file messages_file) +let messages_xml = fun () -> Lazy.force lazy_messages_xml +let units_file = pprzlink_dir // "units.xml" + +external float_of_bytes : string -> int -> float = "c_float_of_indexed_bytes" +external double_of_bytes : string -> int -> float = "c_double_of_indexed_bytes" +external int8_of_bytes : string -> int -> int = "c_int8_of_indexed_bytes" +external int16_of_bytes : string -> int -> int = "c_int16_of_indexed_bytes" +external int32_of_bytes : string -> int -> int32 = "c_int32_of_indexed_bytes" +external uint32_of_bytes : string -> int -> int64 = "c_uint32_of_indexed_bytes" +external int64_of_bytes : string -> int -> int64 = "c_int64_of_indexed_bytes" +external sprint_float : string -> int -> float -> unit = "c_sprint_float" +external sprint_double : string -> int -> float -> unit = "c_sprint_double" +external sprint_int64 : string -> int -> int64 -> unit = "c_sprint_int64" +external sprint_int32 : string -> int -> int32 -> unit = "c_sprint_int32" +external sprint_int16 : string -> int -> int -> unit = "c_sprint_int16" +external sprint_int8 : string -> int -> int -> unit = "c_sprint_int8" + +let types = [ + ("uint8", { format = "%u"; glib_type = "guint8"; inttype = "uint8_t"; size = 1; value=Int 42 }); + ("uint16", { format = "%u"; glib_type = "guint16"; inttype = "uint16_t"; size = 2; value=Int 42 }); + ("uint32", { format = "%Lu" ; glib_type = "guint32"; inttype = "uint32_t"; size = 4; value=Int 42 }); (* uint32 should be lu, but doesn't fit into Int32 so Int64 (Lu) is used *) + ("uint64", { format = "%Lu" ; glib_type = "guint64"; inttype = "uint64_t"; size = 8; value=Int 42 }); + ("int8", { format = "%d"; glib_type = "gint8"; inttype = "int8_t"; size = 1; value= Int 42 }); + ("int16", { format = "%d"; glib_type = "gint16"; inttype = "int16_t"; size = 2; value= Int 42 }); + ("int32", { format = "%ld" ; glib_type = "gint32"; inttype = "int32_t"; size = 4; value=Int 42 }); + ("int64", { format = "%Ld" ; glib_type = "gint64"; inttype = "int64_t"; size = 8; value=Int 42 }); + ("float", { format = "%f" ; glib_type = "gfloat"; inttype = "float"; size = 4; value=Float 4.2 }); + ("double", { format = "%f" ; glib_type = "gdouble"; inttype = "double"; size = 8; value=Float 4.2 }); + ("char", { format = "%c" ; glib_type = "gchar"; inttype = "char"; size = 1; value=Char '*' }); + ("string", { format = "%s" ; glib_type = "gchar*"; inttype = "char*"; size = max_int; value=String "42" }) +] + +let is_array_type = fun s -> + let n = CompatPL.bytes_length s in + n >= 2 && CompatPL.bytes_sub s (n-2) 2 = "[]" + +let type_of_array_type = fun s -> + let n = CompatPL.bytes_length s in + CompatPL.bytes_sub s 0 (n-2) + +let is_fixed_array_type = fun s -> + let type_parts = Str.full_split (Str.regexp "[][]") s in + match type_parts with + | [Str.Text _; Str.Delim "["; Str.Text _ ; Str.Delim "]"] -> true + | _ -> false + +let type_of_fixed_array_type = fun s -> + try + let type_parts = Str.full_split (Str.regexp "[][]") s in + match type_parts with + | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); ty end + | _ -> failwith("PprzLink.type_of_fixed_array_type is not a fixed array type") + with + | Failure str -> failwith(sprintf "PprzLink.type_of_fixed_array_type: length is not an integer") + +let length_of_fixed_array_type = fun s -> + try + let type_parts = Str.full_split (Str.regexp "[][]") s in + match type_parts with + | [Str.Text ty; Str.Delim "["; Str.Text len ; Str.Delim "]"] -> begin ignore( int_of_string (len)); len end + | _ -> failwith("PprzLink.type_of_fixed_array_type is not a fixed array type") + with + | Failure str -> failwith(sprintf "PprzLink.type_of_fixed_array_type: length is not an integer") + +let int_of_string = fun x -> + try int_of_string x with + _ -> try int_of_string ("0x"^x) with (* try hex format in case *) + _ -> failwith (sprintf "PprzLink.int_of_string: %s" x) + +let rec value = fun t v -> + match t with + Scalar ("uint8" | "uint16" | "int8" | "int16") -> Int (int_of_string v) + | Scalar "int32" -> Int32 (Int32.of_string v) + | Scalar "uint32" -> Int64 (Int64.of_string v) + | Scalar ("uint64" | "int64") -> Int64 (Int64.of_string v) + | Scalar ("float" | "double") -> Float (float_of_string v) + | ArrayType "char" | FixedArrayType ("char", _) | Scalar "string" -> String v + | Scalar "char" -> Char v.[0] + | ArrayType t' -> + Array (Array.map (value (Scalar t')) (Array.of_list (split_array v))) + | FixedArrayType (t',l') -> + Array (Array.map (value (Scalar t')) (Array.of_list (split_array v))) + | Scalar t -> failwith (sprintf "PprzLink.value: Unexpected type: %s" t) + + +let rec string_of_value = function + Int x -> string_of_int x + | Float x -> string_of_float x + | Int32 x -> Int32.to_string x + | Int64 x -> Int64.to_string x + | Char c -> CompatPL.bytes_make 1 c + | String s -> s + | Array a -> + let l = (Array.to_list (Array.map string_of_value a)) in + match a.(0) with + | Char _ -> "\""^(CompatPL.bytes_concat "" l)^"\"" + | _ -> CompatPL.bytes_concat separator l + + +let rec formatted_string_of_value = fun format v -> + let f = fun x -> Scanf.format_from_string format x in + match v with + | Int x -> sprintf (f "%d") x + | Float x -> sprintf (f "%f") x + | Int32 x -> sprintf (f "%ld") x + | Int64 x -> sprintf (f "%Ld") x + | Char x -> sprintf (f "%c") x + | String x ->sprintf "%s" x + | Array a -> + let l = (Array.to_list (Array.map (formatted_string_of_value format) a)) in + match a.(0) with + | Char _ -> "\""^(CompatPL.bytes_concat "" l)^"\"" + | _ -> CompatPL.bytes_concat separator l + + +let sizeof = fun f -> + match f with + Scalar t -> (List.assoc t types).size + | ArrayType t -> failwith "sizeof: Array" + | FixedArrayType (t,l) -> failwith "sizeof: Array" +let size_of_field = fun f -> sizeof f._type +let default_format = function Scalar x | ArrayType x | FixedArrayType (x,_) -> + try (List.assoc x types).format with + Not_found -> failwith (sprintf "Unknown format '%s'" x) +let default_value = fun x -> + match x with + Scalar t -> (List.assoc t types).value + | ArrayType t -> failwith "default_value: Array" + | FixedArrayType (t,l) -> failwith "default_value: Array" + +let payload_size_of_message = fun message -> + List.fold_right + (fun (_, f) s -> size_of_field f + s) + message.fields + 2 (** + message id + aircraft id *) + +exception Unit_conversion_error of string +exception Unknown_conversion of string * string +exception No_automatic_conversion of string * string + +let scale_of_units = fun ?auto from_unit to_unit -> + if (from_unit = to_unit) then + 1.0 + else + try + let units_xml = Xml.parse_file units_file in + (* find the first occurence of matching units or raise Not_found *) + let _unit = List.find (fun u -> + (* will raise Xml.No_attribute if not a valid attribute *) + let f = Xml.attrib u "from" + and t = Xml.attrib u "to" + and a = try Some (Xml.attrib u "auto") with _ -> None in + let a = match auto, a with + | Some _, None | None, None -> "" (* No auto conversion *) + | Some t, Some _ | None, Some t -> CompatPL.bytes_lowercase t (* param auto is used before attribute *) + in + if (f = from_unit || a = "display") && (t = to_unit || a = "code") then true else false + ) (Xml.children units_xml) in + (* return coef, raise Failure if coef is not a numerical value *) + float_of_string (Xml.attrib _unit "coef") + with Xml.File_not_found _ -> raise (Unit_conversion_error ("Parse error of conf/units.xml")) + | Xml.No_attribute _ | Xml.Not_element _ -> raise (Unit_conversion_error ("File conf/units.xml has errors")) + | Failure "float_of_string" -> raise (Unit_conversion_error ("Unit coef is not numerical value")) + | Not_found -> + if from_unit = "" || to_unit = "" then raise (No_automatic_conversion (from_unit, to_unit)) + else raise (Unknown_conversion (from_unit, to_unit)) + | _ -> raise (Unknown_conversion (from_unit, to_unit)) + + +let alt_unit_coef_of_xml = fun ?auto xml -> + try Xml.attrib xml "alt_unit_coef" + with _ -> + let u = try Xml.attrib xml "unit" with _ -> "" in + let au = try Xml.attrib xml "alt_unit" with _ -> "" in + let coef = try string_of_float (match auto with + | None -> scale_of_units u au + | Some a -> scale_of_units u au ~auto:a) + with + | Unit_conversion_error s -> prerr_endline (sprintf "Unit conversion error: %s" s); flush stderr; "1." (* Use coef 1. *) + | Unknown_conversion _ -> "1." (* Use coef 1. *) + | _ -> "1." + in + coef + +(** Some XML utility functions *) +exception Error of string + +let sprint_fields = fun () l -> + "<"^ + List.fold_right (fun (a, b) -> (^) (Printf.sprintf "%s=\"%s\" " a b)) l ">" + +let xml_attrib = fun x a -> + try + Xml.attrib x a + with + Xml.No_attribute _ -> + raise (Error (Printf.sprintf "PprzLink error: Attribute '%s' expected in <%a>" a sprint_fields (Xml.attribs x))) + +let xml_int_attrib = fun xml a -> + let v = xml_attrib xml a in + try + Pervasives.int_of_string v + with + _ -> failwith (Printf.sprintf "Error: integer expected in '%s'" v) + +let xml_float_attrib = fun xml a -> + let v = xml_attrib xml a in + try + Pervasives.float_of_string v + with + _ -> failwith (Printf.sprintf "Error: float expected in '%s'" v) + +let xml_child xml ?select c = + let rec find = function + Xml.Element (tag, _attributes, _children) as elt :: elts -> + if tag = c then + match select with + None -> elt + | Some p -> + if p elt then elt else find elts + else + find elts + | _ :: elts -> find elts + | [] -> raise Not_found + in + let children = Xml.children xml in + (* Let's try with a numeric index *) + try (Array.of_list children).(Pervasives.int_of_string c) with + Failure "int_of_string" -> (* Bad luck. Go through the children *) + find children + +let pipe_regexp = Str.regexp "|" +let field_of_xml = fun xml -> + let t = xml_attrib xml "type" in + let t = if is_array_type t then ArrayType (type_of_array_type t) + else if is_fixed_array_type t then FixedArrayType (type_of_fixed_array_type t, int_of_string(length_of_fixed_array_type t)) + else Scalar t in + let f = try Xml.attrib xml "format" with _ -> default_format t in + let auc = alt_unit_coef_of_xml xml in + let values = try Str.split pipe_regexp (Xml.attrib xml "values") with _ -> [] in + + ( CompatPL.bytes_lowercase (xml_attrib xml "name"), + { _type = t; fformat = f; alt_unit_coef = auc; enum=values }) + +let string_of_values = fun vs -> + CompatPL.bytes_concat " " (List.map (fun (a,v) -> sprintf "%s=%s" a (string_of_value v)) vs) + +let assoc = fun a vs -> + try List.assoc (CompatPL.bytes_lowercase a) vs with Not_found -> + failwith (sprintf "Attribute '%s' not found in '%s'" a (string_of_values vs)) + +let float_assoc = fun (a:string) vs -> + match assoc a vs with + Float x -> x + | _ -> invalid_arg "PprzLink.float_assoc" + +let int_of_value = fun value -> + match value with + Int x -> x + | Int32 x -> + let i = Int32.to_int x in + if Int32.compare x (Int32.of_int i) <> 0 then + failwith "PprzLink.int_assoc: Int32 too large to be converted into an int"; + i + | Int64 x -> + let i = Int64.to_int x in + if Int64.compare x (Int64.of_int i) <> 0 then + failwith "PprzLink.int_assoc: Int64 too large to be converted into an int"; + i + | _ -> invalid_arg "PprzLink.int_assoc" + +let int_assoc = fun (a:string) vs -> + int_of_value (assoc a vs) + +let int32_assoc = fun (a:string) vs -> + match assoc a vs with + Int32 x -> x + | _ -> invalid_arg "PprzLink.int32_assoc" + +let uint32_assoc = fun (a:string) vs -> + match assoc a vs with + Int64 x -> x + | _ -> invalid_arg "PprzLink.uint32_assoc" + +let int64_assoc = fun (a:string) vs -> + match assoc a vs with + Int64 x -> x + | _ -> invalid_arg "PprzLink.int64_assoc" + +let string_assoc = fun (a:string) (vs:values) -> string_of_value (assoc a vs) + +let char_assoc = fun (a:string) (vs:values) -> (string_of_value (assoc a vs)).[0] + +let link_mode_of_string = function +"forwarded" -> Forwarded + | "broadcasted" -> Broadcasted + | x -> invalid_arg (sprintf "link_mode_of_string: %s" x) + +let parse_class = fun xml_class -> + let by_id = Hashtbl.create 13 + and by_name = Hashtbl.create 13 + and class_id = try Some (int_of_string (Xml.attrib xml_class "id")) with _ -> None in + List.iter + (fun xml_msg -> + let name = xml_attrib xml_msg "name" + and link = + try + Some (link_mode_of_string (Xml.attrib xml_msg "link")) + with + Xml.No_attribute("link") -> None + in + (* only keep a "field" nodes *) + let xml_children = List.filter (fun f -> Xml.tag f = "field") (Xml.children xml_msg) in + let msg = { + name = name; + fields = List.map field_of_xml xml_children; + link = link + } in + let id = int_of_string (xml_attrib xml_msg "id") in + if Hashtbl.mem by_id id then + failwith (sprintf "Duplicated id in messages.xml: %d" id); + Hashtbl.add by_id id msg; + Hashtbl.add by_name name (id, msg)) + (Xml.children xml_class); + (by_id, by_name, class_id) + + +(** Returns a value and its length *) +let rec value_of_bin = fun buffer index _type -> + match _type with + Scalar "uint8" -> Int (Char.code buffer.[index]), sizeof _type + | Scalar "char" -> Char (buffer.[index]), sizeof _type + | Scalar "int8" -> Int (int8_of_bytes buffer index), sizeof _type + | Scalar "uint16" -> Int (Char.code buffer.[index+1] lsl 8 + Char.code buffer.[index]), sizeof _type + | Scalar "int16" -> Int (int16_of_bytes buffer index), sizeof _type + | Scalar "float" -> Float (float_of_bytes buffer index), sizeof _type + | Scalar "double" -> Float (double_of_bytes buffer index), sizeof _type + | Scalar "int32" -> Int32 (int32_of_bytes buffer index), sizeof _type + | Scalar "uint32" -> Int64 (uint32_of_bytes buffer index), sizeof _type + | Scalar ("int64" | "uint64") -> Int64 (int64_of_bytes buffer index), sizeof _type + | ArrayType t -> + (** First get the number of values *) + let n = int8_of_bytes buffer index in + let type_of_elt = Scalar t in + let s = sizeof type_of_elt in + let size = 1 + n * s in + (Array (Array.init n + (fun i -> fst (value_of_bin buffer (index+1+i*s) type_of_elt))), size) + | FixedArrayType (t,l) -> + (** First get the number of values *) + let n = l in + let type_of_elt = Scalar t in + let s = sizeof type_of_elt in + let size = 0 + n * s in + (Array (Array.init n + (fun i -> fst (value_of_bin buffer (index+0+i*s) type_of_elt))), size) + | Scalar "string" -> + let n = Char.code buffer.[index] in + (String (CompatPL.bytes_sub buffer (index+1) n), (1+n)) + | _ -> failwith "value_of_bin" + +let value_field = fun buf index field -> + value_of_bin buf index field._type + +let byte = fun x -> Char.chr (x land 0xff) + +(** Returns the size of outputed data *) +let rec sprint_value = fun buf i _type v -> + match _type, v with + Scalar "uint8", Int x -> + if x < 0 || x > 0xff then + failwith (sprintf "Value too large to fit in a uint8: %d" x); + CompatPL.bytes_set buf i (Char.chr x); sizeof _type + | Scalar "int8", Int x -> + if x < -0x7f || x > 0x7f then + failwith (sprintf "Value too large to fit in a int8: %d" x); + sprint_int8 buf i x; sizeof _type + | Scalar "float", Float f -> sprint_float buf i f; sizeof _type + | Scalar "double", Float f -> sprint_double buf i f; sizeof _type + | Scalar "int32", Int32 x -> sprint_int32 buf i x; sizeof _type + | Scalar ("int64"|"uint64"|"uint32"), Int64 x -> sprint_int64 buf i x; sizeof _type + | Scalar "int16", Int x -> sprint_int16 buf i x; sizeof _type + | Scalar ("int32" | "uint32"), Int value -> + assert (_type <> Scalar "uint32" || value >= 0); + CompatPL.bytes_set buf (i+3) (byte (value asr 24)); + CompatPL.bytes_set buf (i+2) (byte (value lsr 16)); + CompatPL.bytes_set buf (i+1) (byte (value lsr 8)); + CompatPL.bytes_set buf (i+0) (byte value); + sizeof _type + | Scalar ("int64" | "uint64"), Int value -> + assert (_type <> Scalar "uint64" || value >= 0); + CompatPL.bytes_set buf (i+7) (byte (value asr 56)); + CompatPL.bytes_set buf (i+6) (byte (value lsr 48)); + CompatPL.bytes_set buf (i+5) (byte (value lsr 40)); + CompatPL.bytes_set buf (i+4) (byte (value lsr 32)); + CompatPL.bytes_set buf (i+3) (byte (value lsr 24)); + CompatPL.bytes_set buf (i+2) (byte (value lsr 16)); + CompatPL.bytes_set buf (i+1) (byte (value lsr 8)); + CompatPL.bytes_set buf (i+0) (byte value); + sizeof _type + | Scalar "uint16", Int value -> + assert (value >= 0); + CompatPL.bytes_set buf (i+1) (byte (value lsr 8)); + CompatPL.bytes_set buf (i+0) (byte value); + sizeof _type + | ArrayType t, Array values -> + (** Put the size first, then the values *) + let n = Array.length values in + ignore (sprint_value buf i (Scalar "uint8") (Int n)); + let type_of_elt = Scalar t in + let s = sizeof type_of_elt in + for j = 0 to n - 1 do + ignore (sprint_value buf (i+1+j*s) type_of_elt values.(j)) + done; + 1 + n * s + | FixedArrayType (t,l), Array values -> + (** Put the size first, then the values *) + let n = Array.length values in + let type_of_elt = Scalar t in + let s = sizeof type_of_elt in + for j = 0 to n - 1 do + ignore (sprint_value buf (i+0+j*s) type_of_elt values.(j)) + done; + 0 + n * s + | Scalar "string", String s -> + let n = CompatPL.bytes_length s in + assert (n < 256); + (** Put the length first, then the bytes *) + CompatPL.bytes_set buf i (Char.chr n); + if (i + n >= CompatPL.bytes_length buf) then + failwith "Error in sprint_value: message too long"; + CompatPL.bytes_blit s 0 buf (i+1) n; + 1 + n + | Scalar "char", Char c -> + CompatPL.bytes_set buf i c; sizeof _type + | (Scalar x|ArrayType x), _ -> failwith (sprintf "PprzLink.sprint_value (%s)" x) + | FixedArrayType (x,l), _ -> failwith (sprintf "PprzLink.sprint_value (%s)" x) + + + +let hex_of_int_array = function +Array array -> + let n = Array.length array in + (* One integer -> 2 chars *) + let s = CompatPL.bytes_create (2*n) in + Array.iteri + (fun i dec -> + let x = int_of_value array.(i) in + assert (0 <= x && x <= 0xff); + let hex = sprintf "%02x" x in + CompatPL.bytes_blit hex 0 s (2*i) 2) + array; + s + | value -> + failwith (sprintf "Error: expecting array in PprzLink.hex_of_int_array, found %s" (string_of_value value)) + + + +exception Unknown_msg_name of string * string + +let offset_sender_id = 0 +let offset_receiver_id = 1 +let offset_component_id = 2 +let offset_class_id = 2 +let offset_msg_id = 3 +let offset_fields = 4 +(* class and component IDs are encoded on the same byte *) +let mask_component_id = 0xF0 +let shift_component_id = 4 +let mask_class_id = 0x0F +let shift_class_id = 0 + + +module type CLASS_Xml = sig + val xml : Xml.xml + val name : string +end + +module type CLASS = sig + val name : string +end + +module type MESSAGES = sig + val messages : (message_id, message) Hashtbl.t + val message_of_id : message_id -> message + val message_of_name : string -> message_id * message + + val values_of_payload : Protocol.payload -> message_id * sender_id * values + (** [values_of_bin payload] Parses a raw payload, returns the + message id, the A/C id and the list of (field_name, value) *) + + val payload_of_values : message_id -> sender_id -> values -> Protocol.payload + (** [payload_of_values id sender_id vs] Returns a payload *) + + val values_of_string : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : ?sep:string -> message -> values -> string + (** [string_of_message ?sep msg values] Default [sep] is space *) + + val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit + (** [message_send sender link_id msg_name values] *) + + val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding + (** [message_bind ?sender msg_name callback] *) + + val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding + (** [message_answerer sender msg_name callback] *) + + val message_req : string -> string -> values -> (string -> values -> unit) -> Ivy.binding * bool ref +(** [message_answerer sender msg_name values receiver] Sends a request on the Ivy bus for the specified message. On reception, [receiver] will be applied on [sender_name] and expected values. Returns Ivy binding for manual unbind of the request listener. *) +end + + + +module MessagesOfXml(Class:CLASS_Xml) = struct + let max_length = 256 + let messages_by_id, messages_by_name, class_id = + try + let select = fun x -> Xml.attrib x "name" = Class.name in + let xml_class = try xml_child Class.xml ~select "msg_class" with Not_found -> xml_child Class.xml ~select "class" in + parse_class xml_class + with + Not_found -> failwith (sprintf "Unknown message class: %s" Class.name) + let messages = messages_by_id + let message_of_id = fun id -> try Hashtbl.find messages_by_id id with Not_found -> fprintf stderr "message_of_id :%d\n%!" id; raise Not_found + let message_of_name = fun name -> + try + Hashtbl.find messages_by_name name + with + Not_found -> raise (Unknown_msg_name (name, Class.name)) + + let value_of_byte = fun byte mask shift -> (byte land mask) lsr shift + + let valid_class_id = fun c -> + match class_id with + | None -> true + | Some x -> x = c + + + let values_of_payload = fun buffer -> + let buffer = Protocol.string_of_payload buffer in + try + let id = Char.code buffer.[offset_msg_id] in + let sender_id = Char.code buffer.[offset_sender_id] in + let c_id = value_of_byte (Char.code buffer.[offset_class_id]) mask_class_id shift_class_id in + if not (valid_class_id c_id) then failwith (sprintf "PprzLink.invalid class ID %d" c_id); + let message = message_of_id id in + DebugPL.call 'T' (fun f -> fprintf f "PprzLink.values id=%d\n" id); + let rec loop = fun index fields -> + match fields with + [] -> + if index = CompatPL.bytes_length buffer then + [] + else + failwith (sprintf "PprzLink.values_of_payload, too many bytes in message %s: %s" message.name (DebugPL.xprint buffer)) + | (field_name, field_descr)::fs -> + let (value, n) = value_field buffer index field_descr in + (field_name, value) :: loop (index+n) fs in + (id, sender_id, loop offset_fields message.fields) + with + Invalid_argument("index out of bounds") -> + failwith (sprintf "PprzLink.values_of_payload, wrong argument: %s" (DebugPL.xprint buffer)) + + + let payload_of_values = fun id sender_id values -> + let message = message_of_id id in + + (** The actual length is computed from the values *) + let p = CompatPL.bytes_make max_length '#' in + + CompatPL.bytes_set p offset_sender_id (Char.chr sender_id); + CompatPL.bytes_set p offset_receiver_id (Char.chr 0); (* for now, broadcast id *) + let id_of_class = match class_id with None -> 0 | Some id -> id in (* if class id is not defined (e.g. ground class) it means that this function should not be called either *) + CompatPL.bytes_set p offset_class_id (Char.chr id_of_class); (* for now, component id is set to 0 *) + CompatPL.bytes_set p offset_msg_id (Char.chr id); + let i = ref offset_fields in + List.iter + (fun (field_name, field) -> + let v = + try List.assoc field_name values with + Not_found -> default_value field._type in + let size = sprint_value p !i field._type v in + i := !i + size + ) + message.fields; + + (** Cut to the actual length *) + let p = CompatPL.bytes_sub p 0 !i in + Protocol.payload_of_string p + + + let space = Str.regexp "[ \t]+" + let array_sep = Str.regexp "[\"|]" (* also search for old separator '|' for backward compatibility *) + let values_of_string = fun s -> + (* split arguments and arrays *) + let array_split = Str.full_split array_sep s in + let rec loop = fun fields -> + match fields with + | [] -> [] + | (Str.Delim "\"")::((Str.Text l)::[Str.Delim "\""]) | (Str.Delim "|")::((Str.Text l)::[Str.Delim "|"]) -> [l] + | (Str.Delim "\"")::((Str.Text l)::((Str.Delim "\"")::xs)) | (Str.Delim "|")::((Str.Text l)::((Str.Delim "|")::xs)) -> [l] @ (loop xs) + | [Str.Text x] -> Str.split space x + | (Str.Text x)::xs -> (Str.split space x) @ (loop xs) + | (Str.Delim _)::_ -> failwith "PprzLink.values_of_string: incorrect array delimiter" + in + let msg_split = loop array_split in + match msg_split with + msg_name::args -> + begin + try + let msg_id, msg = message_of_name msg_name in + let values = List.map2 (fun (field_name, field) v -> (field_name, value field._type v)) msg.fields args in + (msg_id, values) + with + Invalid_argument "List.map2" -> failwith (sprintf "PprzLink.values_of_string: incorrect number of fields in '%s'" s) + end + | [] -> invalid_arg (sprintf "PprzLink.values_of_string: %s" s) + + let string_of_message = fun ?(sep=" ") msg values -> + (** Check that the values are compatible with this message *) + List.iter + (fun (k, _) -> + if not (List.mem_assoc k msg.fields) + then invalid_arg (sprintf "PprzLink.string_of_message: unknown field '%s' in message '%s'" k msg.name)) + values; + + CompatPL.bytes_concat sep + (msg.name:: + List.map + (fun (field_name, field) -> + let v = + try List.assoc field_name values with + Not_found -> + default_value field._type in + formatted_string_of_value field.fformat v) + msg.fields) + + let message_send = fun ?timestamp ?link_id sender msg_name values -> + let m = snd (message_of_name msg_name) in + let s = string_of_message m values in + let timestamp_string = + match timestamp with + None -> "" + | Some x -> sprintf "%f " x in + let msg = sprintf "%s%s %s" timestamp_string sender s in + let n = CompatPL.bytes_length msg in + if n > 10000 then (** prevent really long Ivy message, should not happen with normal usage *) + fprintf stderr "Discarding long ivy message %s (%d bytes)\n%!" msg_name n + else + match link_id with + None -> Ivy.send msg + | Some the_link_id -> begin + let index = ref 0 in + let modified_msg = CompatPL.bytes_copy msg in + let func = fun c -> + match c with + ' ' -> begin + CompatPL.bytes_set modified_msg !index ';'; + index := !index + 1 + end + | x -> index := !index + 1; in + CompatPL.bytes_iter func modified_msg; + Ivy.send (Printf.sprintf "redlink TELEMETRY_MESSAGE %s %i %s" sender the_link_id modified_msg); + end + + let message_bind = fun ?sender ?(timestamp=false) msg_name cb -> + let tsregexp, tsoffset = if timestamp then "([0-9]+\\.[0-9]+ )?", 1 else "", 0 in + match sender with + None -> + Ivy.bind + (fun _ args -> + let values = try snd (values_of_string args.(1+tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in + cb args.(tsoffset) values) + (sprintf "^%s([^ ]*) +(%s( .*|$))" tsregexp msg_name) + | Some s -> + Ivy.bind + (fun _ args -> + let values = try snd (values_of_string args.(tsoffset)) with exc -> prerr_endline (Printexc.to_string exc); [] in + cb s values) + (sprintf "^%s%s +(%s( .*|$))" tsregexp s msg_name) + + let message_answerer = fun sender msg_name cb -> + let ivy_cb = fun _ args -> + let asker = args.(0) + and asker_id = args.(1) in + try (** Against [cb] exceptions *) + let values = cb asker (snd (values_of_string args.(2))) in + let m = string_of_message (snd (message_of_name msg_name)) values in + Ivy.send (sprintf "%s %s %s" asker_id sender m) + with + exc -> fprintf stderr "PprzLink.answerer %s:%s: %s\n%!" sender msg_name (Printexc.to_string exc) + in + Ivy.bind ivy_cb (sprintf "^([^ ]*) +([^ ]*) +(%s_REQ.*)" msg_name) + + let gen_id = let r = ref 0 in fun () -> incr r; !r + let message_req = fun sender msg_name values (f:string -> (string * value) list -> unit) -> + let b = ref (Obj.magic ()) in + let flag = ref true in + let cb = fun _ args -> + flag := false; (* tells that the message is not binded any more *) + Ivy.unbind !b; + f args.(0) (snd (values_of_string args.(1))) in + let id = sprintf "%d_%d" (Unix.getpid ()) (gen_id ()) in + let r = sprintf "^%s ([^ ]*) +(%s.*)" id msg_name in + b := Ivy.bind cb r; + let msg_name_req = msg_name ^ "_REQ" in + let m = sprintf "%s %s %s" sender id (string_of_message (snd (message_of_name msg_name_req)) values) in + Ivy.send m; + (!b, flag) (* return binding if application side wants to unbind manually *) +end + +module Messages(Class:CLASS) = struct + include MessagesOfXml(struct + let xml = messages_xml () + let name = Class.name + end) +end diff --git a/lib/v2.0/ocaml/pprzLink.mli b/lib/v2.0/ocaml/pprzLink.mli new file mode 100644 index 0000000..2391a76 --- /dev/null +++ b/lib/v2.0/ocaml/pprzLink.mli @@ -0,0 +1,188 @@ +(* + * PPRZLINK message protocol handling + * + * Copyright (C) 2003 Pascal Brisset, Antoine Drouin + * Copyright (C) 2015-2017 Gautier Hattenberger + * + * 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 + * . + * + *) + +val messages_xml : unit -> Xml.xml + +type sender_id = int +type receiver_id = int +type component_id = int +type class_id = int +type message_id = int +type format = string + +type _type = + Scalar of string + | ArrayType of string + | FixedArrayType of string * int +type value = + Int of int | Float of float | String of string | Int32 of int32 | Char of char | Int64 of int64 + | Array of value array +type field = { + _type : _type; + fformat : format; + alt_unit_coef : string; (* May be empty *) + enum : string list (* 'values' attribute *) + } +type link_mode = Forwarded | Broadcasted +type message = { + name : string; + fields : (string * field) list; + link : link_mode option + } +(** Message specification *) + + +external int32_of_bytes : string -> int -> int32 = "c_int32_of_indexed_bytes" +external uint32_of_bytes : string -> int -> int64 = "c_uint32_of_indexed_bytes" +external int64_of_bytes : string -> int -> int64 = "c_int64_of_indexed_bytes" +(** [int32_of_bytes buffer offset] *) + +val separator : string +(** Separator in array values *) + +val is_array_type : string -> bool +val is_fixed_array_type : string -> bool + +val size_of_field : field -> int +val string_of_value : value -> string +val formatted_string_of_value : format -> value -> string +val int_of_value : value -> int (* May raise Invalid_argument *) +type type_descr = { + format : string ; + glib_type : string; + inttype : string; + size : int; + value : value + } +val types : (string * type_descr) list +type values = (string * value) list + +val value : _type -> string -> value +(** return a value from a string and a type *) + +val assoc : string -> values -> value +(** Safe assoc taking into accound characters case. May raise Failure ... *) + +val string_assoc : string -> values -> string +(** May raise Not_found *) + +val float_assoc : string -> values -> float +val int_assoc : string -> values -> int +val int32_assoc : string -> values -> Int32.t +val uint32_assoc : string -> values -> Int64.t +val int64_assoc : string -> values -> Int64.t +(** May raise Not_found or Invalid_argument *) + +val hex_of_int_array : value -> string +(** Returns the hexadecimal string of an array of integers *) + +exception Unit_conversion_error of string +(** Unit_conversion_error raised when parsing error occurs *) +exception Unknown_conversion of string * string +(** Unknown_conversion raised when conversion fails *) +exception No_automatic_conversion of string * string +(** No_automatic_conversion raised when no conversion found + * and from_unit or to_unit are empty string + *) + +val scale_of_units : ?auto:string -> string -> string -> float +(** scale_of_units from to + * Returns conversion factor between two units + * The possible conversions are described in conf/units.xml + * May raise Invalid_argument if one of the unit is not valid + * or if units.xml is not valid + *) + +val alt_unit_coef_of_xml : ?auto:string -> Xml.xml -> string +(** Return coef for alternate unit + *) +exception Error of string +(** Generic error *) + +val xml_attrib : Xml.xml -> string -> string +(** Extended version of Xml.attrib, may rise Error *) + +val xml_int_attrib : Xml.xml -> string -> int +val xml_float_attrib : Xml.xml -> string -> float + +val xml_child : Xml.xml -> ?select:(Xml.xml -> bool) -> string -> Xml.xml +(** [child xml ?p i] If [i] is an integer, returns the [i]'th (first is 0) child of [xml]. +Else returns the child of [xml] with tag [i] (the first one satisfying [p] +if specified). Else raises [Not_found]. *) + +exception Unknown_msg_name of string * string +(** [Unknown_msg_name (name, class_name)] Raised if message [name] is not +found in class [class_name]. *) + +val offset_fields : int + +module type CLASS = sig + val name : string +end + +module type CLASS_Xml = sig + val xml : Xml.xml + val name : string +end + +module type MESSAGES = sig + val messages : (message_id, message) Hashtbl.t + val message_of_id : message_id -> message + val message_of_name : string -> message_id * message + + val values_of_payload : Protocol.payload -> message_id * sender_id * values + (** [values_of_bin payload] Parses a raw payload, returns the + message id, the A/C id and the list of (field_name, value) *) + + val payload_of_values : message_id -> sender_id -> values -> Protocol.payload + (** [payload_of_values id sender_id vs] Returns a payload *) + + val values_of_string : string -> message_id * values + (** May raise [(Unknown_msg_name msg_name)] *) + + val string_of_message : ?sep:string -> message -> values -> string + (** [string_of_message ?sep msg values] Default [sep] is space *) + + val message_send : ?timestamp:float -> ?link_id:int -> string -> string -> values -> unit + (** [message_send sender msg_name values] *) + + val message_bind : ?sender:string -> ?timestamp:bool -> string -> (string -> values -> unit) -> Ivy.binding + (** [message_bind ?sender msg_name callback] *) + + val message_answerer : string -> string -> (string -> values -> values) -> Ivy.binding + (** [message_answerer sender msg_name callback] Set a handler for a + [message_req] (which will send a [msg_name]_REQ message). + [callback asker args] must return the list of attributes of the answer. *) + + val message_req : string -> string -> values -> (string -> values -> unit) -> Ivy.binding * bool ref + (** [message_req sender msg_name values receiver] Sends a request on the Ivy + bus for the specified message. A [msg_name]_REQ message is send and a + [msg_name] message is expected for the reply. On reception, [receiver] + will be applied on [sender_name] and attribute values of the values. + Message are unbinded automatically only when getting the answer, + timeout should be implemenet on application side. *) +end + +module Messages : functor (Class : CLASS) -> MESSAGES +module MessagesOfXml : functor (Class : CLASS_Xml) -> MESSAGES diff --git a/lib/v2.0/python/Makefile b/lib/v2.0/python/Makefile new file mode 100644 index 0000000..5c2c97c --- /dev/null +++ b/lib/v2.0/python/Makefile @@ -0,0 +1,33 @@ +# Hey Emacs, this is a -*- makefile -*- +# +# Copyright (C) 2017 Fabien Garcia +# +# 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 +# . +# + +MAKE = make + +all: + +install: + # Install correct version python library to DESTDIR + @echo INSTALL at location $(DESTDIR) + $(Q)test -d $(DESTDIR) || mkdir -p $(DESTDIR) + $(Q)cp -r pprzlink $(DESTDIR) + +.PHONY: all install + diff --git a/lib/v2.0/python/README.md b/lib/v2.0/python/README.md new file mode 100644 index 0000000..57053d1 --- /dev/null +++ b/lib/v2.0/python/README.md @@ -0,0 +1,26 @@ +PPRZLINK Python library +======================= + +Available interfaces +-------------------- + +- serial +- ivy +- udp + +Supported protocols +------------------- + +- pprz binary format +- ascii (ivy) + +The XBee binary protocol is not supported at the moment. + +Running the test programms +-------------------------- + +The PYTHONPATH environnement variable needs to be set to add the current folder: +On bask-like shells: `export PYTHONPATH=:$PYTHONPATH` + +Then you can run test programs with `python -m pprzlink.serial` in the case of the serial interface. + diff --git a/lib/v2.0/python/pprzlink/__init__.py b/lib/v2.0/python/pprzlink/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/lib/v2.0/python/pprzlink/ivy.py b/lib/v2.0/python/pprzlink/ivy.py new file mode 100644 index 0000000..2778daf --- /dev/null +++ b/lib/v2.0/python/pprzlink/ivy.py @@ -0,0 +1,210 @@ +from __future__ import absolute_import, division, print_function + +from ivy.std_api import * +from ivy.ivy import IvyIllegalStateError +import logging +import os +import sys +import re +import platform + +from .message import PprzMessage +from . import messages_xml_map + + +if os.getenv('IVY_BUS') is not None: + IVY_BUS = os.getenv('IVY_BUS') +elif platform.system() == 'Darwin': + IVY_BUS = "224.255.255.255:2010" +else: + IVY_BUS = "" + + +class IvyMessagesInterface(object): + def __init__(self, agent_name=None, start_ivy=True, verbose=False, ivy_bus=IVY_BUS): + if agent_name is None: + agent_name = "IvyMessagesInterface %i" % os.getpid() + self.verbose = verbose + self._ivy_bus = ivy_bus + self._running = False + + # make sure all messages are parsed before we start creating them in callbacks + # the message parsing should really be redone... + messages_xml_map.parse_messages() + + # bindings with associated callback functions + self.bindings = {} + + IvyInit(agent_name, "READY") + logging.getLogger('Ivy').setLevel(logging.WARN) + if start_ivy: + self.start() + + def __del__(self): + try: + self.shutdown() + except Exception as e: + print(e) + + def start(self): + if not self._running: + IvyStart(self._ivy_bus) + self._running = True + + def stop(self): + if self._running: + self._running = False + IvyStop() + + def unsubscribe_all(self): + for b in self.bindings.keys(): + IvyUnBindMsg(b) + del self.bindings[b] + + def shutdown(self): + try: + self.unsubscribe_all() + self.stop() + except IvyIllegalStateError as e: + print(e) + + def bind_raw(self, callback, regex='(.*)'): + """ + Bind callback to Ivy messages matching regex (without any extra parsing) + + :param callback: function called on new message with agent, message, from as params + :param regex: regular expression for matching message + """ + bind_id = IvyBindMsg(callback, regex) + self.bindings[bind_id] = (callback, regex) + return bind_id + + def unbind(self, bind_id): + if bind_id in self.bindings: + IvyUnBindMsg(bind_id) + del self.bindings[bind_id] + + def subscribe(self, callback, regex_or_msg='(.*)'): + """ + Subscribe to Ivy message matching regex and call callback with ac_id and PprzMessage + TODO: possibility to directly specify PprzMessage instead of regex + + :param callback: function called on new message with ac_id and PprzMessage as params + :param regex: regular expression for matching message + """ + if not isinstance(regex_or_msg,PprzMessage): + regex = regex_or_msg + else: + regex = '^([^ ]* +%s(.*|$))'%(regex_or_msg.name) + + bind_id = IvyBindMsg(lambda agent, *larg: self.parse_pprz_msg(callback, larg[0]), regex) + self.bindings[bind_id] = (callback, regex) + return bind_id + + def unsubscribe(self, bind_id): + self.unbind(bind_id) + + @staticmethod + def parse_pprz_msg(callback, ivy_msg): + """ + Parse an Ivy message into a PprzMessage. + Basically parts/args in string are separated by space, but char array can also contain a space: + ``|f,o,o, ,b,a,r|`` in old format or ``"foo bar"`` in new format + + :param callback: function to call with ac_id and parsed PprzMessage as params + :param ivy_msg: Ivy message string to parse into PprzMessage + """ + # first split on array delimiters + l = re.split('([|\"][^|\"]*[|\"])', ivy_msg) + # strip spaces and filter out emtpy strings + l = [str.strip(s) for s in l if str.strip(s) is not ''] + data = [] + for s in l: + # split non-array strings further up + if '|' not in s and '"' not in s: + data += s.split(' ') + else: + data.append(s) + # ignore ivy message with less than 3 elements + if len(data) < 3: + return + + # normal format is "sender_name msg_name msg_payload..." + # advanced format has requests and answers (with request_id as 'pid_index') + # request: "sender_name request_id msg_name_REQ msg_payload..." + # answer: "request_id sender_name msg_name msg_payload..." + + # check for request_id in first or second string (-> advanced format with msg_name in third string) + advanced = False + if re.search("[0-9]+_[0-9]+", data[0]) or re.search("[0-9]+_[0-9]+", data[1]): + advanced = True + msg_name = data[2] if advanced else data[1] + # check which message class it is + msg_class, msg_name = messages_xml_map.find_msg_by_name(msg_name) + if msg_class is None: + print("Ignoring unknown message " + ivy_msg) + return + payload = data[3:] if advanced else data[2:] + values = list(filter(None, payload)) + msg = PprzMessage(msg_class, msg_name) + msg.set_values(values) + # pass non-telemetry messages with ac_id 0 or ac_id attrib value + if msg_class == "telemetry": + try: + sdata = data[0] + if(sdata[0:6] == 'replay'): + ac_id = int(sdata[6:]) + else: + ac_id = int(sdata) + except ValueError: + print("ignoring message " + ivy_msg) + sys.stdout.flush() + else: + if 'ac_id' in msg.fieldnames: + ac_id_idx = msg.fieldnames.index('ac_id') + ac_id = msg.fieldvalues[ac_id_idx] + else: + ac_id = 0 + # finally call the callback, passing the aircraft id and parsed message + callback(ac_id, msg) + + def send_raw_datalink(self, msg): + """ + Send a PprzMessage of datalink msg_class embedded in RAW_DATALINK message + + :param msg: PprzMessage + :returns: Number of clients the message sent to, None if msg was invalid + """ + if not isinstance(msg, PprzMessage): + print("Can only send PprzMessage") + return None + if "datalink" not in msg.msg_class: + print("Message to embed in RAW_DATALINK needs to be of 'datalink' class") + return None + raw = PprzMessage("ground", "RAW_DATALINK") + raw['ac_id'] = msg['ac_id'] + raw['message'] = msg.to_csv() + return self.send(raw) + + def send(self, msg, sender_id=None, receiver_id=None, component_id=None): + """ + Send a message + + :param msg: PprzMessage or simple string + :param ac_id: Needed if sending a PprzMessage of telemetry msg_class + :returns: Number of clients the message sent to, None if msg was invalid + """ + if not self._running: + print("Ivy server not running!") + return + if isinstance(msg, PprzMessage): + if "telemetry" in msg.msg_class: + if sender_id is None: + print("ac_id needed to send telemetry message.") + return None + else: + return IvySendMsg("%d %s %s" % (sender_id, msg.name, msg.payload_to_ivy_string())) + else: + return IvySendMsg("%s %s %s" % (msg.msg_class, msg.name, msg.payload_to_ivy_string())) + else: + return IvySendMsg(msg) diff --git a/lib/v2.0/python/pprzlink/message.py b/lib/v2.0/python/pprzlink/message.py new file mode 100644 index 0000000..0e28166 --- /dev/null +++ b/lib/v2.0/python/pprzlink/message.py @@ -0,0 +1,255 @@ +""" +Paparazzi message representation + +""" + +from __future__ import division, print_function +import sys +import json +import struct +import messages_xml_map + + +class PprzMessageError(Exception): + def __init__(self, message, inner_exception=None): + self.message = message + self.inner_exception = inner_exception + self.exception_info = sys.exc_info() + + def __str__(self): + return self.message + + +class PprzMessage(object): + """base Paparazzi message class""" + + def __init__(self, class_name, msg, component_id=0): + if isinstance(class_name, int): + # class_name is an integer, find the name + # TODO handle None case + self._class_id = class_name + self._class_name = messages_xml_map.get_class_name(self._class_id) + else: + self._class_name = class_name + self._class_id = messages_xml_map.get_class_id(class_name) + self._component_id = component_id + if isinstance(msg, int): + self._id = msg + self._name = messages_xml_map.get_msg_name(self._class_name, msg) + else: + self._name = msg + self._id = messages_xml_map.get_msg_id(self._class_name, msg) + self._fieldnames = messages_xml_map.get_msg_fields(self._class_name, self._name) + self._fieldtypes = messages_xml_map.get_msg_fieldtypes(self._class_name, self._id) + self._fieldcoefs = messages_xml_map.get_msg_fieldcoefs(self._class_name, self._id) + self._fieldvalues = [] + # set empty values according to type + for t in self._fieldtypes: + if t == "char[]": + self._fieldvalues.append('') + elif '[' in t: + self._fieldvalues.append([0]) + else: + self._fieldvalues.append(0) + if messages_xml_map.message_dictionary_broadcast[self._name]=='forwarded': + self.broadcasted = False + else: + self.broadcasted = True + + @property + def name(self): + """Get the message name.""" + return self._name + + @property + def msg_id(self): + """Get the message id.""" + return self._id + + @property + def class_id(self): + """Get the class id.""" + return self._class_id + + @property + def msg_class(self): + """Get the message class.""" + return self._class_name + + @property + def fieldnames(self): + """Get list of field names.""" + return self._fieldnames + + @property + def fieldvalues(self): + """Get list of field values.""" + return self._fieldvalues + + @property + def fieldtypes(self): + """Get list of field types.""" + return self._fieldtypes + + @property + def fieldcoefs(self): + """Get list of field coefs.""" + return self._fieldcoefs + + def fieldbintypes(self, t): + """Get type and length for binary format""" + data_types = { + 'float': ['f', 4], + 'double': ['d', 8], + 'uint8': ['B', 1], + 'uint16': ['H', 2], + 'uint32': ['L', 4], + 'int8': ['b', 1], + 'int16': ['h', 2], + 'int32': ['l', 4], + 'char': ['c', 1] + } + base_type = t.split('[')[0] + return data_types[base_type] + + def get_field(self, idx): + """Get field value by index.""" + return self._fieldvalues[idx] + + def __getattr__(self, attr): + # Try to dynamically return the field value for the given name + for idx, f in enumerate(self.fieldnames): + if f == attr: + return self.fieldvalues[idx] + raise AttributeError("No such attribute %s" % attr) + + def __getitem__(self, key): + # Try to dynamically return the field value for the given name + for idx, f in enumerate(self.fieldnames): + if f == key: + return self.fieldvalues[idx] + raise AttributeError("Msg %s has no field of name %s" % (self.name, key)) + + def __setitem__(self, key, value): + self.set_value_by_name(key, value) + + def set_values(self, values): + #print("msg %s: %s" % (self.name, ", ".join(self.fieldnames))) + if len(values) == len(self.fieldnames): + self._fieldvalues = values + else: + raise PprzMessageError("Error: Msg %s has %d fields, tried to set %i values" % + (self.name, len(self.fieldnames), len(values))) + + def set_value_by_name(self, name, value): + # Try to set a value from its name + for idx, f in enumerate(self.fieldnames): + if f == name: + self._fieldvalues[idx] = value + return + raise AttributeError("Msg %s has no field of name %s" % (self.name, name)) + + def __str__(self): + ret = '%s.%s {' % (self.msg_class, self.name) + for idx, f in enumerate(self.fieldnames): + ret += '%s : %s, ' % (f, self.fieldvalues[idx]) + ret = ret[0:-2] + '}' + return ret + + def to_dict(self, payload_only=False): + d = {} + if not payload_only: + d['msgname'] = self.name + d['msgclass'] = self.msg_class + for idx, f in enumerate(self.fieldnames): + d[f] = self.fieldvalues[idx] + return d + + def to_json(self, payload_only=False): + return json.dumps(self.to_dict(payload_only)) + + def to_csv(self, payload_only=False): + """ return message as CSV string for use with RAW_DATALINK + msg_name;field1;field2; + """ + return str(self.name) + ';' + self.payload_to_ivy_string(sep=';') + + def payload_to_ivy_string(self, sep=' '): + ivy_str = '' + for idx, t in enumerate(self.fieldtypes): + if "char[" in t: + str_value ='' + for c in self.fieldvalues[idx]: + str_value += c + ivy_str += '"' + str_value + '"' + elif '[' in t: + ivy_str += ','.join([str(x) for x in self.fieldvalues[idx]]) + else: + ivy_str += str(self.fieldvalues[idx]) + ivy_str += sep + return ivy_str + + def payload_to_binary(self): + struct_string = "<" + data = [] + length = 0 + for idx, t in enumerate(self.fieldtypes): + bin_type = self.fieldbintypes(t) + struct_string += bin_type[0] + array_length = 1 + if "char[" in t: + array_length = len(self.fieldvalues[idx]) + for c in self.fieldvalues[idx]: + data.append(int(c)) + elif '[' in t: + array_length = len(self.fieldvalues[idx]) + for x in self.fieldvalues[idx]: + data.append(x) + else: + # Assign the right type according to field description + if bin_type[0]=='f' or bin_type[0]== 'd': + data.append(float(self.fieldvalues[idx])) + elif bin_type[0]== 'B' or bin_type[0]== 'H' or bin_type[0]== 'L' or bin_type[0]== 'b' or bin_type[0]== 'h' or bin_type[0]== 'l': + data.append(int(self.fieldvalues[idx])) + else: + data.append(self.fieldvalues[idx]) + length += bin_type[1] * array_length + msg = struct.pack(struct_string, *data) + return msg + + def binary_to_payload(self, data): + msg_offset = 0 + values = [] + for idx, t in enumerate(self.fieldtypes): + bin_type = self.fieldbintypes(t) + if '[' in t: + array_length = data[msg_offset] + msg_offset += 1 + array_value = [] + for count in range(0, array_length): + array_value.append(struct.unpack('<' + bin_type[0], data[msg_offset:msg_offset + bin_type[1]])[0]) + msg_offset = msg_offset + bin_type[1] + values.append(array_value) + else: + value = struct.unpack('<' + bin_type[0], data[msg_offset:msg_offset + bin_type[1]])[0] + msg_offset = msg_offset + bin_type[1] + values.append(value) + self.set_values(values) + + +def test(): + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("-f", "--file", help="path to messages.xml file") + parser.add_argument("-c", "--class", help="message class", dest="msg_class", default="telemetry") + args = parser.parse_args() + messages_xml_map.parse_messages(args.file) + messages = [PprzMessage(args.msg_class, n) for n in messages_xml_map.get_msgs(args.msg_class)] + print("Listing %i messages in '%s' msg_class" % (len(messages), args.msg_class)) + for msg in messages: + print(msg) + + +if __name__ == '__main__': + test() diff --git a/lib/v2.0/python/pprzlink/messages_xml_map.py b/lib/v2.0/python/pprzlink/messages_xml_map.py new file mode 100755 index 0000000..7dc656c --- /dev/null +++ b/lib/v2.0/python/pprzlink/messages_xml_map.py @@ -0,0 +1,213 @@ +#!/usr/bin/env python + +from __future__ import absolute_import, print_function + +import os + +# if PAPARAZZI_HOME is set use $PAPARAZZI_HOME/var/messages.xml +# else assume this file is installed in var/lib/python/pprzlink +# and use messages.xml from var +# Message definition file should be installed at the same +# time as this file so it should be ok +PPRZ_HOME = os.getenv("PAPARAZZI_HOME") +if PPRZ_HOME is not None: + default_messages_file = '%s/var/messages.xml' % PPRZ_HOME +else: + default_messages_file = os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)), + "../../../../message_definitions/v1.0/messages.xml")) +# Define the pprzlink protocol version +PROTOCOL_VERSION="2.0" + +message_dictionary = {} +message_dictionary_types = {} +message_dictionary_coefs = {} +message_dictionary_id_name = {} +message_dictionary_name_id = {} +message_dictionary_class_id_name = {} +message_dictionary_class_name_id = {} +message_dictionary_broadcast = {} + +class MessagesNotFound(Exception): + def __init__(self, filename): + self.filename = filename + + def __str__(self): + return "messages file " + repr(self.filename) + " not found" + + +def parse_messages(messages_file=''): + if not messages_file: + messages_file = default_messages_file + if not os.path.isfile(messages_file): + raise MessagesNotFound(messages_file) + #print("Parsing %s" % messages_file) + from lxml import etree + tree = etree.parse(messages_file) + for the_class in tree.xpath("//msg_class[@name]"): + class_name = the_class.attrib['name'] + if 'id' in the_class.attrib: + class_id = int(the_class.attrib['id']) + message_dictionary_class_id_name[class_id] = class_name + message_dictionary_class_name_id[class_name] = class_id + elif 'ID' in the_class.attrib: + class_id = int(the_class.attrib['ID']) + message_dictionary_class_id_name[class_id] = class_name + message_dictionary_class_name_id[class_name] = class_id + + if class_name not in message_dictionary: + message_dictionary_id_name[class_name] = {} + message_dictionary_name_id[class_name] = {} + message_dictionary[class_name] = {} + message_dictionary_types[class_name] = {} + message_dictionary_coefs[class_name] = {} + for the_message in the_class.xpath("message[@name]"): + message_name = the_message.attrib['name'] + if 'id' in the_message.attrib: + message_id = the_message.attrib['id'] + else: + message_id = the_message.attrib['ID'] + if message_id[0:2] == "0x": + message_id = int(message_id, 16) + else: + message_id = int(message_id) + + if 'link' in the_message.attrib: + message_dictionary_broadcast[message_name] = the_message.attrib['link'] + else: + message_dictionary_broadcast[message_name] = 'forwarded' # Default behavior is to send message to destination only + + message_dictionary_id_name[class_name][message_id] = message_name + message_dictionary_name_id[class_name][message_name] = message_id + + # insert this message into our dictionary as a list with room for the fields + message_dictionary[class_name][message_name] = [] + message_dictionary_types[class_name][message_id] = [] + message_dictionary_coefs[class_name][message_id] = [] + + for the_field in the_message.xpath('field[@name]'): + # for now, just save the field names -- in the future maybe expand this to save a struct? + message_dictionary[class_name][message_name].append(the_field.attrib['name']) + message_dictionary_types[class_name][message_id].append(the_field.attrib['type']) + try: + message_dictionary_coefs[class_name][message_id].append(float(the_field.attrib['alt_unit_coef'])) + except KeyError: + # print("no such key") + message_dictionary_coefs[class_name][message_id].append(1.) + + +def find_msg_by_name(name): + if not message_dictionary: + parse_messages() + for msg_class in message_dictionary: + if name in message_dictionary[msg_class]: + #print("found msg name %s in class %s" % (name, msg_class)) + return msg_class, name + print("Error: msg_name %s not found." % name) + return None, None + + +def get_msgs(msg_class): + if not message_dictionary: + parse_messages() + if msg_class in message_dictionary: + return message_dictionary[msg_class] + else: + print("Error: msg_class %s not found." % msg_class) + return [] + + +def get_class_name(class_id): + if not message_dictionary: + parse_messages() + if class_id in message_dictionary_class_id_name: + return message_dictionary_class_id_name[class_id] + else: + print("Error: class_id %d not found." % class_id) + return None + +def get_class_id(class_name): + if not message_dictionary: + parse_messages() + if class_name in message_dictionary_class_name_id: + return message_dictionary_class_name_id[class_name] + else: + print("Error: class_name %s not found." % class_name) + return None + + +def get_msg_name(msg_class, msg_id): + if not message_dictionary: + parse_messages() + if msg_class in message_dictionary: + if msg_id in message_dictionary_id_name[msg_class]: + return message_dictionary_id_name[msg_class][msg_id] + else: + print("Error: msg_id %d not found in msg_class %s." % (msg_id, msg_class)) + else: + print("Error: msg_class %s not found." % msg_class) + return "" + + +def get_msg_fields(msg_class, msg_name): + if not message_dictionary: + parse_messages() + if msg_class in message_dictionary: + if msg_name in message_dictionary[msg_class]: + return message_dictionary[msg_class][msg_name] + else: + print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class)) + else: + print("Error: msg_class %s not found." % msg_class) + return [] + + +def get_msg_id(msg_class, msg_name): + if not message_dictionary: + parse_messages() + try: + return message_dictionary_name_id[msg_class][msg_name] + except KeyError: + print("Error: msg_name %s not found in msg_class %s." % (msg_name, msg_class)) + return 0 + + +def get_msg_fieldtypes(msg_class, msg_id): + if not message_dictionary: + parse_messages() + if msg_class in message_dictionary_types: + if msg_id in message_dictionary_types[msg_class]: + return message_dictionary_types[msg_class][msg_id] + else: + print("Error: message with ID %d not found in msg_class %s." % (msg_id, msg_class)) + else: + print("Error: msg_class %s not found." % msg_class) + return [] + +def get_msg_fieldcoefs(msg_class, msg_id): + if not message_dictionary: + parse_messages() + if msg_class in message_dictionary_coefs: + if msg_id in message_dictionary_coefs[msg_class]: + return message_dictionary_coefs[msg_class][msg_id] + else: + print("Error: message with ID %d not found in msg_class %s." % (msg_id, msg_class)) + else: + print("Error: msg_class %s not found." % msg_class) + return [] + + +def test(): + import argparse + parser = argparse.ArgumentParser() + parser.add_argument("-f", "--file", help="path to messages.xml file") + parser.add_argument("-l", "--list", help="list parsed messages", action="store_true", dest="list_messages") + parser.add_argument("-c", "--class", help="message class", dest="msg_class", default="telemetry") + args = parser.parse_args() + parse_messages(args.file) + if args.list_messages: + print("Listing %i messages in '%s' msg_class" % (len(message_dictionary[args.msg_class]), args.msg_class)) + for msg_name, msg_fields in message_dictionary[args.msg_class].iteritems(): + print(msg_name + ": " + ", ".join(msg_fields)) + +if __name__ == '__main__': + test() diff --git a/lib/v2.0/python/pprzlink/pprz_transport.py b/lib/v2.0/python/pprzlink/pprz_transport.py new file mode 100644 index 0000000..1236865 --- /dev/null +++ b/lib/v2.0/python/pprzlink/pprz_transport.py @@ -0,0 +1,113 @@ +""" +Paparazzi transport encoding utilities + +""" + +from __future__ import absolute_import, division, print_function +import struct +from .message import PprzMessage + +# use Enum from python 3.4 if available (https://www.python.org/dev/peps/pep-0435/) +# (backports as enum34 on pypi) +try: + from enum import Enum +except ImportError: + Enum = object + +STX = 0x99 + +class PprzParserState(Enum): + WaitSTX = 1 + GotSTX = 2 + GotLength = 3 + GotPayload = 4 + GotCRC1 = 5 + +class PprzTransport(object): + """parser for binary Paparazzi messages""" + def __init__(self, msg_class='telemetry'): + self.msg_class = msg_class + self.reset_parser() + + def reset_parser(self): + self.state = PprzParserState.WaitSTX + self.length = 0 + self.buf = [] + self.ck_a = 0 + self.ck_b = 0 + self.idx = 0 + + def parse_byte(self, c): + """parse new byte, return True when a new full message is available""" + b = struct.unpack("> 4 + msg_id = data[3] + msg = PprzMessage(class_id, msg_id) + msg.binary_to_payload(data[4:]) + return sender_id, receiver_id, component_id, msg + + def unpack(self): + """Unpack the last received message""" + return self.unpack_pprz_msg(self.buf) + + def calculate_checksum(self, msg): + ck_a = 0 + ck_b = 0 + # start char not included in checksum for pprz protocol + for c in msg[1:]: + # try to handle differences between python 2.x and 3.x + if isinstance(c, str): + c = struct.unpack(" diff --git a/message_definitions/v1.0/messages.xml b/message_definitions/v1.0/messages.xml index 3ea4920..62d704a 100644 --- a/message_definitions/v1.0/messages.xml +++ b/message_definitions/v1.0/messages.xml @@ -1,8 +1,8 @@ - - + + version encoded as: MAJOR * 10000 + MINOR * 100 + PATCH @@ -1944,7 +1944,7 @@ - + @@ -2764,7 +2764,7 @@ - + diff --git a/message_definitions/v1.0/pprz_schema.xsd b/message_definitions/v1.0/pprz_schema.xsd new file mode 100644 index 0000000..e811ee2 --- /dev/null +++ b/message_definitions/v1.0/pprz_schema.xsd @@ -0,0 +1,91 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/tools/generator/C/include_v1.0/pprzlink_utils.h b/tools/generator/C/include_v1.0/pprzlink_utils.h index f31745e..d44bb5a 100644 --- a/tools/generator/C/include_v1.0/pprzlink_utils.h +++ b/tools/generator/C/include_v1.0/pprzlink_utils.h @@ -37,6 +37,8 @@ extern "C" { #endif +#define PPRZLINK_PROTOCOL_VERSION "1.0" + #include #ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */ @@ -175,6 +177,10 @@ typedef union __attribute__((packed)) { #endif // PPRZLINK_UNALIGNED_ACCESS +/* Message id helpers */ +#define SenderIdOfPprzMsg(x) (x[0]) +#define IdOfPprzMsg(x) (x[1]) + #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/tools/generator/C/include_v2.0/pprzlink_device.h b/tools/generator/C/include_v2.0/pprzlink_device.h new file mode 100644 index 0000000..bda1924 --- /dev/null +++ b/tools/generator/C/include_v2.0/pprzlink_device.h @@ -0,0 +1,74 @@ +/* + * Copyright (C) 2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** \file pprzlink_device.h + * + * Generic device header for the PPRZLINK message system + */ + +#ifndef PPRZLINK_DEVICE_H +#define PPRZLINK_DEVICE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + + +/** Function pointers definition + * + * they are used to cast the real functions with the correct type + * to store in the device structure + */ +typedef int (*check_free_space_t)(void *, long *, uint16_t); +typedef void (*put_byte_t)(void *, long, uint8_t); +typedef void (*put_buffer_t)(void *, long, const uint8_t *, uint16_t); +typedef void (*send_message_t)(void *, long); +typedef int (*char_available_t)(void *); +typedef uint8_t (*get_byte_t)(void *); +typedef void (*set_baudrate_t)(void *, uint32_t baudrate); + +/** Device structure + */ +struct link_device { + check_free_space_t check_free_space; ///< check if transmit buffer is not full + put_byte_t put_byte; ///< put one byte + put_buffer_t put_buffer; ///< put several bytes from a buffer + send_message_t send_message; ///< send completed buffer + char_available_t char_available; ///< check if a new character is available + get_byte_t get_byte; ///< get a new char + set_baudrate_t set_baudrate; ///< set device baudrate + void *periph; ///< pointer to parent implementation + + uint16_t nb_msgs; ///< The number of messages send + uint8_t nb_ovrn; ///< The number of overruns + uint32_t nb_bytes; ///< The number of bytes send + +}; + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // PPRZLINK_DEVICE_H + diff --git a/tools/generator/C/include_v2.0/pprzlink_message.h b/tools/generator/C/include_v2.0/pprzlink_message.h new file mode 100644 index 0000000..6612c3e --- /dev/null +++ b/tools/generator/C/include_v2.0/pprzlink_message.h @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2017 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** \file pprzlink_message.h + * + * Generic message header for PPRZLINK message system + */ + +#ifndef PPRZLINK_MESSAGE_H +#define PPRZLINK_MESSAGE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "pprzlink_transport.h" +#include "pprzlink_device.h" + +// broadcast id (all receivers) +#define PPRZLINK_MSG_BROADCAST 0xFF + +// components broadcast id +#define PPRZLINK_COMPONENT_BROADCAST 0 + +/** Message configuration + */ +struct pprzlink_msg { + uint8_t sender_id; ///< sender id + uint8_t receiver_id; ///< destination id + uint8_t component_id; ///< component id + struct transport_tx *trans; ///< transport protocol + struct link_device *dev; ///< device +}; + +/* Message id helpers */ + +/** Getter for the sender id of a message + * @param msg pointer to the message + * @return the sender id of the message + */ +static inline uint8_t pprzlink_get_msg_sender_id(void *msg) +{ + return ((uint8_t*)msg)[0]; +} + +/** Getter for the receiver id of a message + * @param msg pointer to the message + * @return the receiver id of the message + */ +static inline uint8_t pprzlink_get_msg_receiver_id(void *msg) +{ + return ((uint8_t*)msg)[1]; +} + +/** Getter for the component id of a message + * @param msg pointer to the message + * @return the component id of the message + */ +static inline uint8_t pprzlink_get_msg_component_id(void *msg) +{ + return (((uint8_t*)msg)[2] & 0x0F)>>4; +} + +/** Getter for the class id of a message + * @param msg pointer to the message + * @return the class ID of the message + */ +static inline uint8_t pprzlink_get_msg_class_id(void *msg) +{ + return (((uint8_t*)msg)[2] & 0x0F); +} + +/** Getter for the id of a message in its class + * @param msg pointer to the message + * @return the id of a message in its class + */ +static inline uint8_t pprzlink_get_msg_id(void *msg) +{ + return ((uint8_t*)msg)[3]; +} + +/* Compatibility macros */ +#define SenderIdOfPprzMsg pprzlink_get_msg_sender_id +#define IdOfPprzMsg pprzlink_get_msg_id + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* PPRZLINK_MESSAGE_H */ + diff --git a/tools/generator/C/include_v2.0/pprzlink_transport.h b/tools/generator/C/include_v2.0/pprzlink_transport.h new file mode 100644 index 0000000..fbf05f9 --- /dev/null +++ b/tools/generator/C/include_v2.0/pprzlink_transport.h @@ -0,0 +1,112 @@ +/* + * Copyright (C) 2015 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** \file pprzlink_transport.h + * + * Generic transport header for PPRZLINK message system + */ + +#ifndef PPRZLINK_TRANSPORT_H +#define PPRZLINK_TRANSPORT_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include "pprzlink_device.h" +#include "pprzlink_message.h" + +#ifndef TRANSPORT_PAYLOAD_LEN +#define TRANSPORT_PAYLOAD_LEN 256 +#endif + +/** Generic reception transport header + */ +struct transport_rx { + uint8_t payload[TRANSPORT_PAYLOAD_LEN]; ///< payload buffer + volatile uint8_t payload_len; ///< payload buffer length + volatile bool msg_received; ///< message received flag + uint8_t ovrn, error; ///< overrun and error flags +}; + +/** Data type + */ +enum TransportDataType { + DL_TYPE_ARRAY_LENGTH, + DL_TYPE_CHAR, + DL_TYPE_UINT8, + DL_TYPE_INT8, + DL_TYPE_UINT16, + DL_TYPE_INT16, + DL_TYPE_UINT32, + DL_TYPE_INT32, + DL_TYPE_UINT64, + DL_TYPE_INT64, + DL_TYPE_FLOAT, + DL_TYPE_DOUBLE, + DL_TYPE_TIMESTAMP +}; + +/** Data format (scalar or array) + */ +enum TransportDataFormat { + DL_FORMAT_SCALAR, + DL_FORMAT_ARRAY +}; + +/** Function pointers definition + * + * they are used to cast the real functions with the correct type + * to store in the transport structure + */ +typedef uint8_t (*size_of_t)(struct pprzlink_msg *, uint8_t); +typedef int (*check_available_space_t)(struct pprzlink_msg *, long *, uint16_t); +typedef void (*put_bytes_t)(struct pprzlink_msg *, long, enum TransportDataType, enum TransportDataFormat, + const void *, uint16_t); +typedef void (*put_named_byte_t)(struct pprzlink_msg *, long, enum TransportDataType, enum TransportDataFormat, + uint8_t, const char *); +typedef void (*start_message_t)(struct pprzlink_msg *, long, uint8_t); +typedef void (*end_message_t)(struct pprzlink_msg *, long); +typedef void (*overrun_t)(struct pprzlink_msg *); +typedef void (*count_bytes_t)(struct pprzlink_msg *, uint8_t); + +/** Generic transmission transport header + */ +struct transport_tx { + size_of_t size_of; ///< get size of payload with transport header and trailer + check_available_space_t check_available_space; ///< check if transmit buffer is not full + put_bytes_t put_bytes; ///< send bytes + put_named_byte_t put_named_byte; ///< send a single byte or its name + start_message_t start_message; ///< transport header + end_message_t end_message; ///< transport trailer + overrun_t overrun; ///< overrun + count_bytes_t count_bytes; ///< count bytes to send + void *impl; ///< pointer to parent implementation +}; + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif /* PPRZLINK_TRANSPORT_H */ + diff --git a/tools/generator/C/include_v2.0/pprzlink_utils.h b/tools/generator/C/include_v2.0/pprzlink_utils.h new file mode 100644 index 0000000..f39a9db --- /dev/null +++ b/tools/generator/C/include_v2.0/pprzlink_utils.h @@ -0,0 +1,184 @@ +/* + * Copyright (C) 2016 Gautier Hattenberger + * + * 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 + * . + * + */ + +/** \file pprzlink_utils.h + * + * Utility macros and functions for PPRZLINK + * + * Reading macros to access incoming messages values (which might not be aligned). + * + * Define PPRZLINK_UNALIGNED_ACCESS to TRUE if the target CPU/MMU allows unaligned access. + * This is true for x86/64 and most recent ARM platforms (ARMv7, Cortex-A, Cortex-M3/4). + * Examples for targets WITHOUT unaligned access support: LPC21xx, Cortex-M0 + */ + +#ifndef PPRZLINK_UTILS_H +#define PPRZLINK_UTILS_H + + #ifdef __cplusplus +extern "C" { +#endif + +#define PPRZLINK_PROTOCOL_VERSION "2.0" + +#include + +#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */ +#define Swap32IfBigEndian(_u) { _u = (_u << 32) | (_u >> 32); } +#else +#define Swap32IfBigEndian(_) {} +#endif + +// Single byte values are always aligned +#define _PPRZ_VAL_char(_payload, _offset) ((char)(*((uint8_t*)_payload+_offset))) +#define _PPRZ_VAL_int8_t(_payload, _offset) ((int8_t)(*((uint8_t*)_payload+_offset))) +#define _PPRZ_VAL_uint8_t(_payload, _offset) ((uint8_t)(*((uint8_t*)_payload+_offset))) +#define _PPRZ_VAL_char_array(_payload, _offset) ((char*)(_payload+_offset)) +#define _PPRZ_VAL_int8_t_array(_payload, _offset) ((int8_t*)(_payload+_offset)) +#define _PPRZ_VAL_uint8_t_array(_payload, _offset) ((uint8_t*)(_payload+_offset)) + +// Macros returning array pointers might not be aligned +// but there is not much we can do about it. +// To prevent errors, the array size should be forced to zero +// on platforms where unaligned data is not supported. +#define _PPRZ_VAL_int16_t_array(_payload, _offset) ((int16_t*)(_payload+_offset)) +#define _PPRZ_VAL_uint16_t_array(_payload, _offset) ((uint16_t*)(_payload+_offset)) +#define _PPRZ_VAL_int32_t_array(_payload, _offset) ((int32_t*)(_payload+_offset)) +#define _PPRZ_VAL_uint32_t_array(_payload, _offset) ((uint32_t*)(_payload+_offset)) +#define _PPRZ_VAL_int64_t_array(_payload, _offset) ((int64_t*)(_payload+_offset)) +#define _PPRZ_VAL_uint64_t_array(_payload, _offset) ((uint64_t*)(_payload+_offset)) +#define _PPRZ_VAL_float_array(_payload, _offset) ((float*)(_payload+_offset)) +#define _PPRZ_VAL_double_array(_payload, _offset) ((double*)(_payload+_offset)) + +// Use macros according to alignment capabilities +// be conservative by default +#ifndef PPRZLINK_UNALIGNED_ACCESS +#define PPRZLINK_UNALIGNED_ACCESS 0 +#endif + +#if PPRZLINK_UNALIGNED_ACCESS + +// This way of reading is more efficient when data is actually aligned +// but is still working if the CPU/MMU supports unaligned access. +// The use of 'packed' forces the compiler to assume no better +// than 1-byte alignment, and issue sequential byte reads/writes. +typedef union __attribute__((packed)) { + int16_t int16; + uint16_t uint16; + int32_t int32; + uint32_t uint32; + int64_t int64; + uint64_t uint64; + float f32; + double f64; +} unaligned_t; + +#define _PPRZ_VAL_int16_t(_payload, _offset) (((unaligned_t*)(_payload+_offset))->int16) +#define _PPRZ_VAL_uint16_t(_payload, _offset) (((unaligned_t*)(_payload+_offset))->uint16) +#define _PPRZ_VAL_int32_t(_payload, _offset) (((unaligned_t*)(_payload+_offset))->int32) +#define _PPRZ_VAL_uint32_t(_payload, _offset) (((unaligned_t*)(_payload+_offset))->uint32) +#define _PPRZ_VAL_int64_t(_payload, _offset) (((unaligned_t*)(_payload+_offset))->int64) +#define _PPRZ_VAL_uint64_t(_payload, _offset) (((unaligned_t*)(_payload+_offset))->uint64) +#define _PPRZ_VAL_float(_payload, _offset) (((unaligned_t*)(_payload+_offset))->f32) +#ifndef __IEEE_BIG_ENDIAN +#define _PPRZ_VAL_double(_payload, _offset) (((unaligned_t*)(_payload+_offset))->f64) +#else +#define _PPRZ_VAL_double(_payload, _offset) ({ \ + union { uint64_t u; double f; } _f; \ + _f.u = (uint64_t)(_PPRZ_VAL_uint64_t(_payload, _offset)); \ + Swap32IfBigEndian(_f.u); \ + _f.f; }) +#endif + +// In this case, data is not aligned but we are still able to read them +#define _PPRZ_VAL_len_aligned(_payload, _offset) _PPRZ_VAL_uint8_t(_payload, _offset) +#define _PPRZ_VAL_fixed_len_aligned(_len) (_len) + +#else // PPRZLINK_UNALIGNED_ACCESS set to false + +#define _PPRZ_VAL_int16_t(_payload, _offset) ({ \ + union { int16_t i; uint8_t t[2]; } _r; \ + _r.t[0] = _PPRZ_VAL_uint8_t(_payload, _offset); \ + _r.t[1] = _PPRZ_VAL_uint8_t(_payload, _offset+1); \ + _r.i; }) +#define _PPRZ_VAL_uint16_t(_payload, _offset) ({ \ + union { uint16_t i; uint8_t t[2]; } _r; \ + _r.t[0] = _PPRZ_VAL_uint8_t(_payload, _offset); \ + _r.t[1] = _PPRZ_VAL_uint8_t(_payload, _offset+1); \ + _r.i; }) +#define _PPRZ_VAL_int32_t(_payload, _offset) ({ \ + union { int32_t i; uint8_t t[4]; } _r; \ + _r.t[0] = _PPRZ_VAL_uint8_t(_payload, _offset); \ + _r.t[1] = _PPRZ_VAL_uint8_t(_payload, _offset+1); \ + _r.t[2] = _PPRZ_VAL_uint8_t(_payload, _offset+2); \ + _r.t[3] = _PPRZ_VAL_uint8_t(_payload, _offset+3); \ + _r.i; }) +#define _PPRZ_VAL_uint32_t(_payload, _offset) ({ \ + union { uint32_t i; uint8_t t[4]; } _r; \ + _r.t[0] = _PPRZ_VAL_uint8_t(_payload, _offset); \ + _r.t[1] = _PPRZ_VAL_uint8_t(_payload, _offset+1); \ + _r.t[2] = _PPRZ_VAL_uint8_t(_payload, _offset+2); \ + _r.t[3] = _PPRZ_VAL_uint8_t(_payload, _offset+3); \ + _r.i; }) +#define _PPRZ_VAL_float(_payload, _offset) ({ \ + union { uint32_t u; float f; } _f; \ + _f.u = _PPRZ_VAL_uint32_t(_payload, _offset); \ + _f.f; }) +#define _PPRZ_VAL_int64_t(_payload, _offset) ({ \ + union { int64_t i; uint8_t t[8]; } _r; \ + _r.t[0] = _PPRZ_VAL_uint8_t(_payload, _offset); \ + _r.t[1] = _PPRZ_VAL_uint8_t(_payload, _offset+1); \ + _r.t[2] = _PPRZ_VAL_uint8_t(_payload, _offset+2); \ + _r.t[3] = _PPRZ_VAL_uint8_t(_payload, _offset+3); \ + _r.t[4] = _PPRZ_VAL_uint8_t(_payload, _offset+4); \ + _r.t[5] = _PPRZ_VAL_uint8_t(_payload, _offset+5); \ + _r.t[6] = _PPRZ_VAL_uint8_t(_payload, _offset+6); \ + _r.t[7] = _PPRZ_VAL_uint8_t(_payload, _offset+7); \ + _r.i; }) +#define _PPRZ_VAL_uint64_t(_payload, _offset) ({ \ + union { uint64_t i; uint8_t t[8]; } _r; \ + _r.t[0] = _PPRZ_VAL_uint8_t(_payload, _offset); \ + _r.t[1] = _PPRZ_VAL_uint8_t(_payload, _offset+1); \ + _r.t[2] = _PPRZ_VAL_uint8_t(_payload, _offset+2); \ + _r.t[3] = _PPRZ_VAL_uint8_t(_payload, _offset+3); \ + _r.t[4] = _PPRZ_VAL_uint8_t(_payload, _offset+4); \ + _r.t[5] = _PPRZ_VAL_uint8_t(_payload, _offset+5); \ + _r.t[6] = _PPRZ_VAL_uint8_t(_payload, _offset+6); \ + _r.t[7] = _PPRZ_VAL_uint8_t(_payload, _offset+7); \ + _r.i; }) +#define _PPRZ_VAL_double(_payload, _offset) ({ \ + union { uint64_t u; double f; } _f; \ + _f.u = (uint64_t)(_PPRZ_VAL_uint64_t(_payload, _offset)); \ + Swap32IfBigEndian(_f.u); \ + _f.f; }) + +// In this case, data are not aligned so we force array len to 0 +// to notify users that the array should not be read +#define _PPRZ_VAL_len_aligned(_payload, _offset) (0) +#define _PPRZ_VAL_fixed_len_aligned(_len) (0) + +#endif // PPRZLINK_UNALIGNED_ACCESS + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // PPRZLINK_UTILS_H diff --git a/tools/generator/gen_messages.py b/tools/generator/gen_messages.py index b24b426..98127a9 100755 --- a/tools/generator/gen_messages.py +++ b/tools/generator/gen_messages.py @@ -15,11 +15,12 @@ import sys, textwrap, os import pprz_parse -# XSD schema file -schemaFile = os.path.join(os.path.dirname(os.path.realpath(__file__)), "pprz_schema.xsd") +# XSD schema file => must be pprz_schema.xsd and be stored in the same directory as messages.xml +schemaFileName = "pprz_schema.xsd" # Set defaults for generating MAVLink code DEFAULT_PROTOCOL = pprz_parse.PROTOCOL_1_0 +DEFAULT_MESSAGES = pprz_parse.MESSAGES_1_0 DEFAULT_LANGUAGE = 'C' DEFAULT_ERROR_LIMIT = 200 DEFAULT_VALIDATE = True @@ -61,7 +62,14 @@ def pprz_validate(fname, schema, errorLimitNumber) : # Process XML file, validating as necessary. validation_result = 0 if opts.validate: - print("Validating msg_class %s in %s" % (opts.class_name, fname)) + import os.path + directory = os.path.dirname(fname) + schemaFile = os.path.join(directory, schemaFileName) + if not os.path.isfile(schemaFile): + print ("Schema file %s does not exist." % (schemaFile)) + sys.exit(1); + print("Validating msg_class %s in %s with %s" % (opts.class_name, fname,schemaFile)) + validation_result = pprz_validate(fname, schemaFile, opts.error_limit) else: print("Validation skipped for msg_class %s in %s." % (opts.class_name, fname)) @@ -81,8 +89,8 @@ def pprz_validate(fname, schema, errorLimitNumber) : # Convert language option to lowercase and validate opts.language = opts.language.lower() if opts.language == 'c': - import gen_messages_c - gen_messages_c.generate(opts.output, xml) + gen_message_c = __import__(xml.generator_module + "_c") + gen_message_c.generate(opts.output, xml) else: print("Unsupported language %s" % opts.language) @@ -94,7 +102,8 @@ def pprz_validate(fname, schema, errorLimitNumber) : parser = ArgumentParser(description="This tool generate implementations from PPRZLink message definitions") parser.add_argument("-o", "--output", default="stdout", help="output file or stream [default: %(default)s]") parser.add_argument("--lang", dest="language", choices=supportedLanguages, default=DEFAULT_LANGUAGE, help="language of generated code [default: %(default)s]") - parser.add_argument("--protocol", choices=[pprz_parse.PROTOCOL_1_0], default=DEFAULT_PROTOCOL, help="PPRZLink protocol version. [default: %(default)s]") + parser.add_argument("--protocol", choices=[pprz_parse.PROTOCOL_1_0,pprz_parse.PROTOCOL_2_0], default=DEFAULT_PROTOCOL, help="PPRZLink protocol version. [default: %(default)s]") + parser.add_argument("--messages", choices=[pprz_parse.MESSAGES_1_0], default=DEFAULT_MESSAGES, help="PPRZLink message definitino version. [default: %(default)s]") parser.add_argument("--no-validate", action="store_false", dest="validate", default=DEFAULT_VALIDATE, help="Do not perform XML validation. Can speed up code generation if XML files are known to be correct.") parser.add_argument("--only-validate", action="store_true", dest="only_validate", help="Only validate messages without generation.") parser.add_argument("--error-limit", default=DEFAULT_ERROR_LIMIT, help="maximum number of validation errors to display") diff --git a/tools/generator/gen_messages_c.py b/tools/generator/gen_messages_v1_0_c.py similarity index 100% rename from tools/generator/gen_messages_c.py rename to tools/generator/gen_messages_v1_0_c.py diff --git a/tools/generator/gen_messages_v2_0_c.py b/tools/generator/gen_messages_v2_0_c.py new file mode 100644 index 0000000..4734fc8 --- /dev/null +++ b/tools/generator/gen_messages_v2_0_c.py @@ -0,0 +1,267 @@ +#!/usr/bin/env python +''' +parse a PPRZLink protocol XML file and generate a C implementation +for version 2.0 of the protocol + +Copyright (C) 2017 Fabien Garcia +For the Paparazzi UAV and PPRZLINK projects + +based on: + Copyright Andrew Tridgell 2011 + Released under GNU GPL version 3 or later +''' + +from __future__ import print_function +import sys, os +import gen_messages, pprz_template, pprz_parse + +t = pprz_template.PPRZTemplate() + +def generate_main_h(directory, name, xml): + '''generate main header per XML file''' + f = open(os.path.join(directory, name), mode='w') + t.write(f, ''' +/** @file + * @brief PPRZLink message header built from ${filename} + * @see http://paparazziuav.org + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef PPRZLINK_DEFAULT_VER +#define PPRZLINK_DEFAULT_VER 2 +#endif + +#ifndef PPRZLINK_ENABLE_FD +#define PPRZLINK_ENABLE_FD FALSE +#endif + +// dummy fd to save ROM if this is not used +#if !PPRZLINK_ENABLE_FD +#define _FD 0 +#define _FD_ADDR 0 +#else +#define _FD_ADDR &_FD +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#if DOWNLINK +#define DL_MSG_${class_name}_NB ${nb_messages} +#endif // DOWNLINK + +#define DL_${class_name}_CLASS_ID ${class_id} + +${{message:#include "${class_name}/${msg_name}.h" +}} + +// Macros for keeping compatibility between versions +// These should not be used directly +#define _inner_send_msg(NAME,PROTO_VERSION) pprzlink_msg_v##PROTO_VERSION##_send_##NAME +#define _send_msg(NAME,PROTO_VERSION) _inner_send_msg(NAME,PROTO_VERSION) + +''', xml) + + f.close() + + +def copy_fixed_headers(directory, protocol_version): + '''copy the fixed protocol headers to the target directory''' + import shutil + hlist = [ 'pprzlink_device.h', 'pprzlink_transport.h', 'pprzlink_utils.h', 'pprzlink_message.h' ] + basepath = os.path.dirname(os.path.realpath(__file__)) + srcpath = os.path.join(basepath, 'C/include_v%s' % protocol_version) + if directory == '': + print("Skip copying fixed headers") + return + print("Copying fixed headers") + for h in hlist: + src = os.path.realpath(os.path.join(srcpath, h)) + dest = os.path.realpath(os.path.join(directory, h)) + if src == dest: + continue + shutil.copy(src, dest) + +def generate_one(directory, xml, m): + f = open(os.path.join(os.path.join(directory, xml.class_name), m.msg_name + ".h"), mode='w') + t.write(f, ''' +/** @file + * @brief PPRZLink message header for ${msg_name} in class ${class_name} + * + * ${description} + * @see http://paparazziuav.org + */ + +#ifndef _VAR_MESSAGES_${class_name}_${msg_name}_H_ +#define _VAR_MESSAGES_${class_name}_${msg_name}_H_ + + +#include "pprzlink/pprzlink_device.h" +#include "pprzlink/pprzlink_transport.h" +#include "pprzlink/pprzlink_utils.h" +#include "pprzlink/pprzlink_message.h" + + +#ifdef __cplusplus +extern "C" { +#endif + +#if DOWNLINK + +#define DL_${msg_name} ${id} +#define PPRZ_MSG_ID_${msg_name} ${id} + +/** + * Macro that redirect calls to the default version of pprzlink API + * Used for compatibility between versions. + */ +#define pprzlink_msg_send_${msg_name} _send_msg(${msg_name},PPRZLINK_DEFAULT_VER) + +/** + * Sends a ${msg_name} message (API V2.0 version) + * + * @param msg the pprzlink_msg structure for this message${{fields:\n * @param ${attrib_param} ${description}}} + */ +static inline void pprzlink_msg_v2_send_${msg_name}(struct pprzlink_msg * msg${{fields:, ${attrib_fun}}}) { +#if PPRZLINK_ENABLE_FD + long _FD = 0; /* can be an address, an index, a file descriptor, ... */ +#endif + const uint8_t size = msg->trans->size_of(msg, /* msg header overhead */4${{fields:${array_extra_length}+${length}}}); + if (msg->trans->check_available_space(msg, _FD_ADDR, size)) { + msg->trans->count_bytes(msg, size); + msg->trans->start_message(msg, _FD, /* msg header overhead */4${{fields:${array_extra_length}+${length}}}); + msg->trans->put_bytes(msg, _FD, DL_TYPE_UINT8, DL_FORMAT_SCALAR, &(msg->sender_id), 1); + msg->trans->put_named_byte(msg, _FD, DL_TYPE_UINT8, DL_FORMAT_SCALAR, msg->receiver_id, NULL); + uint8_t comp_class = (msg->component_id & 0x0F) << 4 | (${class_id} & 0x0F); + msg->trans->put_named_byte(msg, _FD, DL_TYPE_UINT8, DL_FORMAT_SCALAR, comp_class, NULL); + msg->trans->put_named_byte(msg, _FD, DL_TYPE_UINT8, DL_FORMAT_SCALAR, DL_${msg_name}, "${msg_name}"); + ${{fields:${array_byte}msg->trans->put_bytes(msg, _FD, DL_TYPE_${type_upper}, ${dl_format}, (void *) _${field_name}, ${length}); + }}msg->trans->end_message(msg, _FD); + } else + msg->trans->overrun(msg); +} + +// Compatibility with the protocol v1.0 API +#define pprzlink_msg_v1_send_${msg_name} pprz_msg_send_${msg_name} +#define DOWNLINK_SEND_${msg_name}(_trans, _dev${{fields:, ${attrib_macro}}}) pprz_msg_send_${msg_name}(&((_trans).trans_tx), &((_dev).device), AC_ID${{fields:, ${attrib_macro}}}) +/** + * Sends a ${msg_name} message (API V1.0 version) + * + * @param trans A pointer to the transport_tx structure used for sending the message + * @param dev A pointer to the link_device structure through which the message will be sent + * @param ac_id The id of the sender of the message${{fields:\n * @param ${attrib_param} ${description}}} + */ +static inline void pprz_msg_send_${msg_name}(struct transport_tx *trans, struct link_device *dev, uint8_t ac_id${{fields:, ${attrib_fun}}}) { + struct pprzlink_msg msg; + msg.trans = trans; + msg.dev = dev; + msg.sender_id = ac_id; + msg.receiver_id = 0; + msg.component_id = 0; + pprzlink_msg_v2_send_${msg_name}(&msg${{fields:,${attrib_param}}}); +} + + +#else // DOWNLINK + +#define DOWNLINK_SEND_${msg_name}(_trans, _dev${{fields:, ${attrib_macro}}}) {} +static inline void pprz_send_msg_${msg_name}(struct transport_tx *trans __attribute__((unused)), struct link_device *dev __attribute__((unused)), uint8_t ac_id __attribute__((unused))${{fields:, ${attrib_fun_unused}}}) {} + +#endif // DOWNLINK + +${{fields:${fun_read_array_byte} +/** Getter for field ${field_name} in message ${msg_name} + * + * @param _payload : a pointer to the ${msg_name} message + * @return ${description} + */ +static inline ${return_type} pprzlink_get_DL_${msg_name}_${field_name}(uint8_t * _payload) +{ + return _PPRZ_VAL_${read_type}(_payload, ${offset}); +} + +}} + +/* Compatibility macros */ +${{fields:${read_array_byte}#define DL_${msg_name}_${field_name}(_payload) pprzlink_get_DL_${msg_name}_${field_name}(_payload)\n}}\n\n + +#ifdef __cplusplus +} +#endif // __cplusplus + +#endif // _VAR_MESSAGES_${class_name}_${msg_name}_H_ + + ''', {'msg_name' : m.msg_name, 'description' : m.description ,'class_id' : xml.class_id, 'class_name' : xml.class_name, 'id' : m.id, 'fields' : m.fields, 'message' : xml.message}) + + +def generate(output, xml): + '''generate complete MAVLink C implemenation''' + + directory, name = os.path.split(output) + print("Generating C implementation in %s" % output) + if directory != '': + pprz_parse.mkdir_p(directory) + pprz_parse.mkdir_p(os.path.join(directory, xml.class_name)) + + # add some extra field attributes for convenience with arrays + for m in xml.message: + offset = 4 # 4 bytes initial offset (sender id, receiver id, component and class id, message id) + for f in m.fields: + if f.array_type == 'VariableArray': + f.attrib_macro = 'nb_%s, %s' % (f.field_name, f.field_name) + f.attrib_param = 'nb_%s,_%s' % (f.field_name, f.field_name) + f.attrib_fun = 'uint8_t nb_%s, %s *_%s' % (f.field_name, f.type, f.field_name) + f.attrib_fun_unused = 'uint8_t nb_%s __attribute__((unused)), %s *_%s __attribute__((unused))' % (f.field_name, f.type, f.field_name) + f.array_byte = 'msg->trans->put_bytes(msg, _FD, DL_TYPE_ARRAY_LENGTH, DL_FORMAT_SCALAR, (void *) &nb_%s, 1);\n ' % f.field_name + f.read_type = f.type+'_array' + f.return_type = f.type + ' *' + if (offset + 1) % min(4, int(f.type_length)) == 0: # data are aligned + f.fun_read_array_byte = '/** Getter for length of array %s in message %s\n *\n * @return %s : %s\n */\n static inline uint8_t pprzlink_get_%s_%s_length(void* _payload) {\n return _PPRZ_VAL_uint8_t(_payload, %d);\n}\n' \ + % (f.field_name,m.msg_name, f.field_name, f.description, m.msg_name, f.field_name ,offset) + else: # rely on arch capability to read or not + f.fun_read_array_byte = '/** Getter for length of array %s in message %s\n *\n * @return %s : %s\n */\n static inline uint8_t pprzlink_get_%s_%s_length(__attribute__ ((unused)) void* _payload) {\n return _PPRZ_VAL_len_aligned(_payload, %d);\n}\n' \ + % (f.field_name,m.msg_name, f.field_name, f.description, m.msg_name, f.field_name ,offset) + f.read_array_byte = '#define DL_%s_%s_length(_payload) pprzlink_get_%s_%s_length(_payload)\n' % (m.msg_name, f.field_name, m.msg_name, f.field_name) + offset += 1 + # variable arrays are last (for now) + f.offset = offset + f.dl_format = 'DL_FORMAT_ARRAY' + elif f.array_type == 'FixedArray': + f.attrib_macro = '%s' % f.field_name + f.attrib_fun = '%s *_%s' % (f.type, f.field_name) + f.attrib_param = '_%s' % (f.field_name) + f.attrib_fun_unused = '%s *_%s __attribute__((unused))' % (f.type, f.field_name) + f.array_byte = '' + f.read_type = f.type+'_array' + f.return_type = f.type + ' *' + if offset % min(4, int(f.type_length)) == 0: # data are aligned + f.fun_read_array_byte = '/** Getter for length of array %s in message %s\n *\n * @return %s : %s\n */\n static inline uint8_t pprzlink_get_%s_%s_length(__attribute__ ((unused)) void* _payload) {\n return %d;\n}\n' \ + % (f.field_name,m.msg_name, f.field_name, f.description, m.msg_name, f.field_name ,int(f.array_length)) + else: # rely on arch capability to read or not + f.fun_read_array_byte = '/** Getter for length of array %s in message %s\n *\n * @return %s : %s\n */\n static inline uint8_t pprzlink_get_%s_%s_length(__attribute__ ((unused)) void* _payload) {\n return _PPRZ_VAL_fixed_len_aligned(%d);\n}\n' \ + % (f.field_name,m.msg_name, f.field_name, f.description, m.msg_name, f.field_name ,int(f.array_length)) + f.read_array_byte = '#define DL_%s_%s_length(_payload) pprzlink_get_%s_%s_length(_payload)\n' % (m.msg_name, f.field_name, m.msg_name, f.field_name) + f.offset = offset + offset += int(f.length) + f.dl_format = 'DL_FORMAT_ARRAY' + else: + f.offset = offset + offset += int(f.length) + f.attrib_macro = '%s' % f.field_name + f.attrib_fun = '%s *_%s' % (f.type, f.field_name) + f.attrib_param = '_%s' % (f.field_name) + f.attrib_fun_unused = '%s *_%s __attribute__((unused))' % (f.type, f.field_name) + f.array_byte = '' + f.read_type = f.type + f.return_type = f.type + f.read_array_byte = '' + f.fun_read_array_byte = '' + f.dl_format = 'DL_FORMAT_SCALAR' + generate_one(directory, xml, m) + + generate_main_h(directory, name, xml) + copy_fixed_headers(directory, xml.protocol_version) diff --git a/tools/generator/pprz_parse.py b/tools/generator/pprz_parse.py index 0b679d9..acba11e 100644 --- a/tools/generator/pprz_parse.py +++ b/tools/generator/pprz_parse.py @@ -13,6 +13,8 @@ import xml.parsers.expat, os, errno, time, sys, operator, struct PROTOCOL_1_0 = "1.0" +PROTOCOL_2_0 = "2.0" +MESSAGES_1_0 = "1.0" class PPRZParseError(Exception): def __init__(self, message, inner_exception=None): @@ -87,12 +89,18 @@ class PPRZXML(object): def __init__(self, filename, class_name, protocol_version=PROTOCOL_1_0): self.filename = filename self.class_name = class_name + self.class_id= None self.message = [] self.protocol_version = protocol_version if protocol_version == PROTOCOL_1_0: self.protocol_version_major = 1 self.protocol_version_minor = 0 + self.generator_module = "gen_messages_v1_0" + elif protocol_version == PROTOCOL_2_0: + self.protocol_version_major = 2 + self.protocol_version_minor = 0 + self.generator_module = "gen_messages_v2_0" else: print("Unknown wire protocol version") print("Available versions are: %s" % (PROTOCOL_1_0)) @@ -100,6 +108,7 @@ def __init__(self, filename, class_name, protocol_version=PROTOCOL_1_0): in_element_list = [] self.current_class = '' + self.current_class_id = None def check_attrs(attrs, check, where): for c in check: @@ -114,6 +123,11 @@ def start_element(name, attrs): if in_element == "protocol.msg_class": check_attrs(attrs, ['name'], 'msg_class') self.current_class = attrs['name'] + try: + check_attrs(attrs, ['id'], 'msg_class') + self.current_class_id = attrs['id'] + except PPRZParseError: + self.current_class_id = None elif in_element == "protocol.msg_class.message": check_attrs(attrs, ['name', 'id'], 'message') if self.current_class == self.class_name: @@ -128,6 +142,10 @@ def end_element(name): if in_element == "protocol.msg_class": if self.current_class == self.class_name: self.nb_messages = len(self.message) + if self.current_class_id is not None: + self.class_id = int(self.current_class_id) + else: + raise PPRZParseError('Cannot generate code for class (%s) without id' % self.current_class) self.current_class = '' in_element_list.pop() diff --git a/tools/generator/pprz_schema.xsd b/tools/generator/pprz_schema.xsd index 22c2d5b..e811ee2 100644 --- a/tools/generator/pprz_schema.xsd +++ b/tools/generator/pprz_schema.xsd @@ -76,6 +76,7 @@ +