diff --git a/conf/messages_ng.xml b/conf/messages_ng.xml
new file mode 100644
index 0000000000..16f8914974
--- /dev/null
+++ b/conf/messages_ng.xml
@@ -0,0 +1,2800 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/sw/tools/tcp_aircraft_server/phoenix/__init__.py b/sw/tools/tcp_aircraft_server/phoenix/__init__.py
new file mode 100644
index 0000000000..fbd8ef673a
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/__init__.py
@@ -0,0 +1,163 @@
+#Copyright 2014, Antoine Drouin
+"""
+Phoenix is a Python library for interacting with Paparazzi
+"""
+
+import math
+
+"""
+Unit convertions
+"""
+def rad_of_deg(d): return d/180.*math.pi
+
+def deg_of_rad(r): return r*180./math.pi
+
+def rps_of_rpm(r): return r*2.*math.pi/60.
+
+def rpm_of_rps(r): return r/2./math.pi*60.
+
+def m_of_inch(i): return i*0.0254
+
+
+"""
+Plotting
+"""
+import matplotlib
+import matplotlib.pyplot as plt
+import matplotlib.gridspec as gridspec
+
+my_title_spec = {'color' : 'k', 'fontsize' : 20 }
+
+def save_if(filename):
+ if filename: matplotlib.pyplot.savefig(filename, dpi=80)
+
+def prepare_fig(fig=None, window_title=None, figsize=(20.48, 10.24), margins=None):
+ if fig == None:
+ fig = plt.figure(figsize=figsize)
+# else:
+# plt.figure(fig.number)
+ if margins:
+ left, bottom, right, top, wspace, hspace = margins
+ fig.subplots_adjust(left=left, right=right, bottom=bottom, top=top,
+ hspace=hspace, wspace=wspace)
+ if window_title:
+ fig.canvas.set_window_title(window_title)
+ return fig
+
+def decorate(ax, title=None, xlab=None, ylab=None, legend=None, xlim=None, ylim=None):
+ ax.xaxis.grid(color='k', linestyle='-', linewidth=0.2)
+ ax.yaxis.grid(color='k', linestyle='-', linewidth=0.2)
+ if xlab:
+ ax.xaxis.set_label_text(xlab)
+ if ylab:
+ ax.yaxis.set_label_text(ylab)
+ if title:
+ ax.set_title(title, my_title_spec)
+ if legend <> None:
+ ax.legend(legend, loc='best')
+ if xlim <> None:
+ ax.set_xlim(xlim[0], xlim[1])
+ if ylim <> None:
+ ax.set_ylim(ylim[0], ylim[1])
+
+
+"""
+Messages
+"""
+
+#: dictionary mapping the C type to its length in bytes (e.g char -> 1)
+TYPE_TO_LENGTH_MAP = {
+ "char" : 1,
+ "uint8" : 1,
+ "int8" : 1,
+ "uint16" : 2,
+ "int16" : 2,
+ "uint32" : 4,
+ "int32" : 4,
+ "float" : 4,
+ "double" : 8,
+}
+
+#: dictionary mapping the C type to correct format string
+TYPE_TO_PRINT_MAP = {
+ float : "%f",
+ str : "%s",
+ chr : "%c",
+ int : "%d"
+}
+
+ACID_ALL = 0xFF
+ACID_TEST = 0xFE
+ACID_GROUNDSTATION = 0xFD
+
+#: dictionary mapping debug types to format characters
+DEBUG_MESSAGES = {
+ "DEBUG_UINT8" : "%d",
+ "DEBUG_INT32" : "%d",
+ "DEBUG_FLOAT" : "%#f"
+}
+
+
+
+
+"""
+Binary logs
+
+See format description in sw/airborne/subsystems/datalink/fms_link.c
+
+"""
+
+import struct
+
+def hex_of_bin(b): return ' '.join( [ "%02X" % ord( x ) for x in b ] )
+import pdb
+
+def read_binary_log(filename, tick_freq = 2*512.):
+ f = open(filename, "rb")
+ d = f.read()
+ packet_header_len = 6
+ msg_header_len = 2
+
+ def read_packet(d, packet_start):
+ payload_start = packet_start+packet_header_len
+ timestamp, payload_len = struct.unpack("IH", d[packet_start:payload_start])
+ msgs = read_packet_payload(d, payload_start, payload_len)
+ next_packet = payload_start+payload_len+2
+ return timestamp, msgs, next_packet
+
+ def read_packet_payload(d, s, l):
+ msgs = []
+ packet_end = s+l; msg_start = s
+ while msg_start= t_min and t<= t_max:
+ for id, payload in msgs:
+ m = protocol.get_message_by_id('telemetry', id)
+ try: i = msg_names.index(m.name)
+ except: pass
+ finally: ret[i]['time'].append(t); ret[i]['data'].append(m.unpack_scaled_values(payload))
+ return ret
diff --git a/sw/tools/tcp_aircraft_server/phoenix/messages.py b/sw/tools/tcp_aircraft_server/phoenix/messages.py
new file mode 100644
index 0000000000..67537a4657
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/messages.py
@@ -0,0 +1,621 @@
+#Copyright 2014, Antoine Drouin
+# vim: ai ts=4 sts=4 et sw=4
+
+import re
+import os.path
+import struct
+import logging
+import pdb
+
+import phoenix
+import xmlobject
+
+LOG = logging.getLogger('PhoenixMessages')
+LOG.setLevel(logging.DEBUG)
+#ch = logging.StreamHandler()
+#ch.setLevel(logging.DEBUG)
+#LOG.addHandler(ch)
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class Field:
+ """
+ **Attributes:**
+ - name : field name
+
+ - ctype : string representing the C type of the field,
+ i.e. uint8[ARRAY_LEN] or uint8[]
+ - type : string representing the C type of a single
+ instance of the field, i.e. uint8
+
+ - is_array :
+
+ - num_elements :
+
+ - length : the number of bytes to store the field,
+ i.e. sizeof(uint8) * ARRAY_LEN
+ - pytype : the python type used to store the field value
+ """
+
+ ARRAY_RE = re.compile("^([a-z0-9]+)\[(\d{,})\]$")
+
+ def __init__(self, f, **kwargs):
+ self.name = f.name
+ self.ctype = f.type
+ m = self.ARRAY_RE.match(f.type)
+ #check if it is an array
+ if m:
+ self.is_array = True
+ elmt_type, elmt_nb = m.groups()
+ self.type = elmt_type
+ self.element_length = phoenix.TYPE_TO_LENGTH_MAP[elmt_type]
+ if elmt_nb != '':
+ self.num_elements = int(elmt_nb)
+ self.length = phoenix.TYPE_TO_LENGTH_MAP[elmt_type] * self.num_elements
+ else:
+ self.num_elements = -1
+ self.length = -1
+ else:
+ self.is_array = False
+ self.type = f.type
+ self.num_elements = 1
+ self.length = phoenix.TYPE_TO_LENGTH_MAP[f.type]
+ self.element_length = None
+
+ #cache the python type for speed
+ if self.type == "float" or self.type == "double":
+ self.pytype = float
+ elif self.type == "char":
+ if self.is_array:
+ self.pytype = str
+ else:
+ self.pytype = chr
+ else:
+ self.pytype = int
+
+
+ def __str__(self):
+ return "" % (self.name, self.ctype)
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class Message:
+ """
+ **Attributes:**
+ - id: message integer id (from messages.xml)
+ - name: message name (upper case) (from messages.xml)
+ - fields: a list of :class:`Field` object (type according to field_class)
+ - size: the length of the message (in bytes)
+ - num_values: the total number of values in the message (as a
+ field can have multiple elements, such as if it is an array)
+ - num_fields: the number of fields in the message
+ - doc: documentation string
+ - is_command: is the message a command (i.e. needs to be ACK'd)
+ """
+ def __init__(self, m, field_klass, **field_kwargs):
+ self._field_kwargs = field_kwargs
+ self.name = m.name.upper()
+ if int(m.id) <= 255 and int(m.id) > 0:
+ self.id = int(m.id)
+ else:
+ raise Exception("Message IDs must be 0 > ID <= 255")
+
+ try:
+ self.fields = [field_klass(f, **self._field_kwargs) for f in xmlobject.ensure_list(m.field)]
+ except AttributeError:
+ self.fields = []
+
+ try:
+ self.is_command = m.command == "1"
+ except AttributeError:
+ self.is_command = False
+
+ try:
+ self.doc = m.doc
+ except AttributeError:
+ self.doc = ""
+
+ self.size = 0
+ self.num_values = 0
+ for f in self.fields:
+ self.size += f.length
+ self.num_values += f.num_elements
+
+ self.num_fields = len(self.fields)
+
+ @property
+ def pretty_name(self):
+ return self.name.replace("_"," ").title()
+
+ def __str__(self):
+ return "" % (self.name, self.id)
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class PyField(Field):
+ """
+ A pythonic object representing a field in a message
+
+ **Attributes:**
+ - is_enum: True if the Field is an enum
+ - struct_format: the :mod:`struct` format string for this type
+ - coef: the unit coefficient for this type (from messages.xml)
+ """
+
+ #: maps user defined names to python struct compatible ids
+ TYPE_TO_STRUCT_MAP = {
+ "char" : "B", #treat char as uint8 internally, only modify how they are displayed
+ "uint8" : "B",
+ "int8" : "b",
+ "uint16": "H",
+ "int16" : "h",
+ "uint32": "I",
+ "int32" : "i",
+ "float" : "f",
+ "double": "f"
+ }
+ #: maps type to range of acceptible values
+ TYPE_TO_RANGE_MAP = {
+ "char" : (0,2**8-1),
+ "uint8" : (0,2**8-1),
+ "int8" : (-2**(8-1),2**(8-1)-1),
+ "uint16": (0,2**16-1),
+ "int16" : (-2**(16-1),2**(16-1)-1),
+ "uint32": (0,2**32-1),
+ "int32" : (-2**(32-1),2**(32-1)-1),
+ "float" : (-3.4e38,3.4e38), #not exactly correct
+ "double": (-3.4e38,3.4e38) #not exactly correct
+ }
+
+ def __init__(self, node, **kwargs):
+ Field.__init__(self, node, **kwargs)
+
+ shared_values = kwargs["shared_values"]
+ self.display_real_unit = False #kwargs["display_real_unit"]
+
+ self.is_enum = False
+ if self.type == "uint8":
+ try:
+ values = node.values
+ if values[0] == "@":
+ self._enum_values = shared_values[values[1:]]
+ else:
+ self._enum_values = values.split("|")
+ self.is_enum = True
+ except AttributeError:
+ self._enum_values = []
+ except KeyError, e:
+ raise Exception("Referenced value does not exist: %s", e)
+
+ if self.is_array:
+ if self.num_elements > 0:
+ self.struct_format = "%d%s" % (self.num_elements, self.TYPE_TO_STRUCT_MAP[self.type])
+ self._size = struct.calcsize(self.struct_format)
+ else:
+ # variable size array, FIXME :(
+ self.struct_format = ""
+ self._size = -1
+ else:
+ self.struct_format = self.TYPE_TO_STRUCT_MAP[self.type]
+ self._size = struct.calcsize(self.struct_format)
+
+ try:
+ self._fstr = node.format
+ except AttributeError:
+ self._fstr = "%s"
+ #if self.is_array:
+ # self._fstr = "%s"
+ #else:
+ # self._fstr = phoenix.TYPE_TO_PRINT_MAP[self.pytype]
+
+ try:
+ self._fstr += " %s" % node.unit
+ except AttributeError:
+ pass
+
+ # if an alternate unit is provided,
+ # update display accordingly
+ try:
+ alt_unit = node.alt_unit
+ except AttributeError:
+ alt_unit = ""
+
+ try:
+ alt_unit_format = node.alt_unit_format
+ except AttributeError:
+ alt_unit_format = "%s"
+
+ try:
+ self.alt_unit_coef = float(node.alt_unit_coef)
+ if self.display_real_unit:
+ self._fstr = alt_unit_format + " " + alt_unit + " ("+ self._fstr +")"
+ else:
+ self._fstr = alt_unit_format + " " + alt_unit
+ except:
+ self.alt_unit_coef = None
+
+ def get_default_value(self):
+ """ Returns a sensible default value for the type """
+ if self.is_array and self.type != "char":
+ return list( [self.pytype() for i in range(self.num_elements)] )
+ else:
+ return self.pytype()
+
+ def interpret_value_from_user_string(self, string, default=None, sep=","):
+ """
+ Tries to interpret the supplied string as best according to the type
+ of the field. For example, the following are legal
+ - "foo bar": if field is char[]
+ - 5: if field is an integer type
+ - 3.4: if field is a float
+ - 1,2,3: if field is an array of integers
+ - ENUM_VALUE: if field is an enum
+
+ In case of failure, the default value (or the value passed in the
+ default argument) is returned
+ """
+ try:
+ if self.is_array and self.type != "char":
+ vals = string.split(sep)
+ if len(vals) != self.num_elements:
+ raise ValueError
+ return list( [self.pytype(v) for v in vals] )
+ elif self.is_enum:
+ try:
+ #first look if the user suppled a string is an enum value
+ return self._enum_values.index(string)
+ except ValueError:
+ #if not, assume it is a number, the constructor of the field
+ #will take care of it
+ return self.pytype(string)
+ else:
+ return self.pytype(string)
+ except ValueError:
+ #invalid user input for type
+ if default:
+ return default
+ else:
+ return self.get_default_value()
+
+ def get_printable_value(self, value):
+ """ Returns a printable string in a human readable format """
+ if self.is_array:
+ if self.type == "char":
+ return "".join([chr(c) for c in value])
+ else:
+ #Returns a printable array, e.g '[1, 2, 3]'
+ return str(value)
+ else:
+ #Return a single formatted number string
+ if self.is_enum:
+ #If this is an uint8 enum type then return the
+ #enum value
+ try:
+ return "%s" % self._enum_values[value]
+ except IndexError:
+ return "?%s?" % value
+ else:
+ if self.alt_unit_coef:
+ if self.display_real_unit:
+ return self._fstr % (self.alt_unit_coef * value, value)
+ else:
+ return self._fstr % (self.alt_unit_coef * value)
+ else:
+ return self._fstr % (value)
+
+ def get_scaled_value(self, value):
+ """
+ Returns the scaled (according to alt_unit_coef). Note, that unlike
+ the other get_ functions, this does not respect the original type of
+ the field. A float is always returned
+ """
+ if self.is_array:
+ if self.type == "char": return value
+ else: return [float(val * self.alt_unit_coef) for val in value]
+ else:
+ if self.is_enum: return value
+ else: return float(value * self.alt_unit_coef) if self.alt_unit_coef else value
+
+ def get_value_range(self):
+ """
+ Returns the legal values this type is allowed to hold. If the type is
+ and enum this returns a n-tuple of all allowed enum values. Otherwise
+ this returns a 2-tuple of the minimum and maximum values this
+ field can hold
+ """
+ if self.is_enum:
+ return self._enum_values
+ else:
+ return self.TYPE_TO_RANGE_MAP[self.ctype]
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class PyMessage(Message):
+ """
+ Represents a message to/from the UAV
+ """
+
+ #Messages are packed in the payload in little endian format
+ MESSAGE_ENDIANESS = "<"
+
+ def __init__(self, name, id, node, **kwargs):
+ Message.__init__(self, node, PyField, **kwargs)
+ self._fields_by_name = {}
+
+ format = self.MESSAGE_ENDIANESS
+ for f in self.fields:
+ format += f.struct_format
+ self._fields_by_name[f.name] = f
+
+ #cache the struct for performace reasons
+ self._struct = struct.Struct(format)
+
+ def __getitem__(self, key):
+ f = self.get_field_by_name(key)
+ if not f:
+ raise KeyError("Could not find field %s" % key)
+ return f
+
+ def get_fields(self):
+ """ Returns a list of :class:`PyField` objects """
+ return self.fields
+
+ def get_field_by_name(self, name):
+ try:
+ return self._fields_by_name[name]
+ except KeyError:
+ return None
+
+ def get_field_index(self, name):
+ i = -1
+ for f in self.fields:
+ i = i + 1
+ if f.name == name:
+ break
+ return i
+
+ def get_field_values(self, vals):
+ """
+ Returns a list of values for each field in the message. The return
+ type is dependent on the type of the field. If the field is an array
+ type then the returned list will contain a tuple of the field array
+ elements. For example ::
+
+
+
+
+
+
+
+
+
+
+
+
+ will return the following ::
+
+ [1, -1, 1000, -1000, 100000, -100000, 1.5, (1, 2, 3)]
+
+ """
+ i = 0
+ v = []
+ for f in self.fields:
+ ne = f.num_elements
+ if f.is_array:
+ v.append( vals[i:i+ne] )
+ else:
+ v.append( vals[i] )
+ i += ne
+ return v
+
+ def pack_values(self, *values):
+ """
+ Assemble the list of field values into a message payload string
+
+ :param values: a list of values of the type expected by the appropriate
+ field. For example, if the 3rd field in the message is a *uint8* then
+ the third value should be an Int
+ """
+ assert len(values) == self.num_values, "%s != %s" % (len(values), self.num_values)
+
+ if self.fields:
+ return self._struct.pack(*values)
+ return ""
+
+ def unpack_values(self, string):
+ """
+ Unlike :func:`get_field_values` this function flattens array
+ fields into the returned list. For example ::
+
+
+
+
+
+
+
+
+
+
+
+
+ will return the following ::
+
+ (1,-1, 1000, -1000, 100000, -100000, 1.5, 1, 2, 3)
+
+ :param string: the message paylod string to unpack
+ """
+ if self.fields:
+ assert type(string) == str
+ assert len(string) == self._struct.size, "%s != %s" % (len(string), self._struct.size)
+ return self._struct.unpack(string)
+ return ()
+
+ def unpack_printable_values(self, string, joiner=" "):
+ """
+ Returns a string, suitable for printing or displaying to the user,
+ of the given message paylod. The string contains values for each
+ field in the message
+
+ :param string: the message payload to unpack
+ :param joiner: the string used between different message fields
+ """
+ if self.fields:
+ vals = self.unpack_values(string)
+ assert len(vals) == self.num_values
+ fvals = self.get_field_values(vals)
+ assert len(fvals) == self.num_fields
+
+ pvals = [self.fields[i].get_printable_value(fvals[i]) for i in range(self.num_fields)]
+
+ if joiner:
+ return joiner.join(pvals)
+ else:
+ return pvals
+ else:
+ return ""
+
+ def unpack_scaled_values(self, string):
+ """
+ As :func:`unpack_values` but returns the values scaled as per the
+ unit coefficient
+ """
+ if self.fields:
+ vals = self.unpack_values(string)
+ assert len(vals) == self.num_values
+ fvals = self.get_field_values(vals)
+ assert len(fvals) == self.num_fields
+
+ return [self.fields[i].get_scaled_value(fvals[i]) for i in range(self.num_fields)]
+ return ()
+
+ def get_default_values(self):
+ """ Returns a list of sensible default values for each field """
+ return [ f.get_default_value() for f in self.fields ]
+
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class MessageClass:
+ """
+ Represents a class of messages in *messages.xml*
+ """
+ def __init__(self, x, shared_values):
+ self.name = x.name
+ self._messages = xmlobject.ensure_list( x.message )
+ self._msgs_by_id = {}
+ self._msgs_by_name = {}
+ for m in self._messages:
+ msg = PyMessage(m.name, m.id, m, shared_values=shared_values)
+ #print m.name
+ self._msgs_by_id[int(m.id)] = msg
+ self._msgs_by_name[m.name] = msg
+
+ def get_message_by_name(self, name):
+ try:
+ return self._msgs_by_name[name]
+ except KeyError:
+ LOG.error("No message with name {:s} in class {:s}".format(name, self.name))
+ return None
+
+ def get_message_by_id(self, id):
+ try:
+ return self._msgs_by_id[id]
+ except KeyError:
+ LOG.error("No message with id {:s} in class {:s}".format(id, self.name))
+ return None
+
+
+
+
+#################################################################################
+#
+#
+#
+#
+#################################################################################
+class Protocol:
+ """
+ A pythonic wrapper for parsing *messages.xml*
+ """
+ def __init__(self, **kwargs):
+ """
+ **Keywords:**
+ - debug - should extra information be printed while parsing
+ *messages.xml*
+ - path - a pathname from which the file can be read
+ - file - an open file object from which the raw xml
+ can be read
+ - raw - the raw xml itself
+ - root - name of root tag, if not reading content
+ """
+ self._debug = kwargs.get("debug", False)
+
+ path = kwargs.get("path")
+ if path and not os.path.exists(path):
+ raise Exception("Could not find message file")
+
+ try:
+ root = xmlobject.XMLFile(**kwargs).root
+ except AttributeError:
+ raise Exception("Invalid XML")
+
+ try:
+ classes = root.msg_class
+ except AttributeError:
+ raise Exception("Missing message class")
+
+ # FIXME: remove that
+ self._values = {}
+ self._msg_classes = {}
+ for msg_class in root.msg_class:
+ if msg_class.name == "telemetry" or msg_class.name == "datalink":
+ self._msg_classes[msg_class.name] = MessageClass(msg_class, self._values)
+
+
+ def __getitem__(self, key):
+ m = self.get_message_by_name(key)
+ if not m:
+ raise KeyError("Could not find message: %s" % key)
+ return m
+
+
+
+ def get_messages(self):
+ """ Returns a list of :class:`PyMessage` objects """
+ return self._msgs_by_id.values()
+
+ def get_message_by_name(self, class_name, msg_name):
+ try:
+ return self._msg_classes[class_name].get_message_by_name(msg_name)
+ except KeyError:
+ LOG.error("No message class with name {:s}".format(class_name))
+ return None
+
+ def get_message_by_id(self, class_name, msg_id):
+ try:
+ return self._msg_classes[class_name].get_message_by_id(msg_id)
+ except KeyError:
+ LOG.error("No message class with name {:s}".format(class_name))
+ return None
+
+
diff --git a/sw/tools/tcp_aircraft_server/phoenix/pprz_transport.py b/sw/tools/tcp_aircraft_server/phoenix/pprz_transport.py
new file mode 100644
index 0000000000..8654120302
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/pprz_transport.py
@@ -0,0 +1,209 @@
+#Copyright 2014, Antoine Drouin
+import array
+
+STX = 0x99
+#6 non payload bytes; STX, LEN, MSGID, ACID, CK_A, CK_B
+NUM_NON_PAYLOAD_BYTES = 6
+
+class TransportHeaderFooter:
+ """
+ The header/footer of a message
+
+ **Attributes:**
+ - stx: start byte
+ - length: length in bytes (total length, i.e. including header/footer
+ - acid: aircraft ID of sender/destination
+ - msgid: message ID
+ - ck_a: checksum high byte
+ - ck_b: checksum low byte
+ """
+ def __init__(self, stx=STX, length=NUM_NON_PAYLOAD_BYTES, acid=0, msgid=0, ck_a=0, ck_b=0):
+ self.stx = stx
+ self.length = length
+ self.acid = acid
+ self.msgid = msgid
+ self.ck_a = ck_a
+ self.ck_b = ck_b
+
+class Transport:
+ """
+ Class that extracts a wasp payload from a string or sequence of
+ characters (data is sent in little endian byte order)
+
+ Data is expected in the following form ::
+
+ |STX|length|AC_ID|MESSAGE_ID|... payload=(length-6) bytes ...|Checksum A|Checksum B|
+
+ Payload ::
+
+ |... MESSAGE DATA ...|
+
+ There are 6 non payload bytes in a packet (described in :mod:`TransportHeaderFooter`
+ - STX
+ - length
+ - AC_ID
+ - MESSAGE_ID
+ - Checksum A
+ - Checksum B
+ """
+
+ STATE_UNINIT, \
+ STATE_GOT_STX, \
+ STATE_GOT_LENGTH, \
+ STATE_GOT_ACID, \
+ STATE_GOT_MSGID, \
+ STATE_GOT_PAYLOAD, \
+ STATE_GOT_CRC1 = range(0,7)
+
+ def __init__(self, check_crc=True, debug=False):
+ self._check_crc = check_crc
+ self._debug = debug
+ self._buf = array.array('c','\0'*256)
+ self._state = self.STATE_UNINIT
+ self._total_len = 0
+ self._payload_len = 0
+ self._payload_idx = 0
+ self._ck_a = 0
+ self._ck_b = 0
+ self._error = 0
+ self._acid = 0
+ self._msgid = 0
+
+ def _debug_msg(self, msg):
+ if self._debug:
+ print msg
+
+ def pack_message_with_values(self, header, message, *values):
+ return self.pack_one(
+ header,
+ message,
+ message.pack_values(*values))
+
+ def pack_one(self, header, message, payload):
+ payload_len = len(payload)
+ total_len = payload_len + NUM_NON_PAYLOAD_BYTES
+
+ #create an array big enough to hold data before the payload,
+ #i.e. exclude the checksum
+ buf = array.array('c','\0'*(NUM_NON_PAYLOAD_BYTES - 2))
+
+ buf[0] = chr(header.stx)
+ buf[1] = chr(total_len)
+ buf[2] = chr(header.acid)
+ buf[3] = chr(message.id)
+
+ buf.fromstring(payload)
+
+ ck_a = total_len
+ ck_b = total_len
+ for i in range(2,len(buf)):
+ ck_a = (ck_a + ord(buf[i])) % 256
+ ck_b = (ck_b + ck_a) % 256
+
+ buf.append(chr(ck_a))
+ buf.append(chr(ck_b))
+
+ return buf
+
+ def parse_many(self, string):
+ """
+ Similar to parse_one, but operates on a string, returning
+ multiple payloads if successful
+
+ :returns: A list of payloads strings
+ """
+ payloads = []
+ for c in string:
+ ok,h,p = self.parse_one(c)
+ if ok:
+ payloads.append((h,p))
+ return payloads
+
+ def parse_one(self, c):
+ """
+ Attempts to parse one character. Returns just the payload, and
+ not the data in the transport layer, i.e. it does not return
+ STX, the length, or the checksums
+
+ :returns: The payload string, or an empty string if insuficcient data is available
+ """
+
+ def update_checksum(d):
+ #wrap to 8bit (simulate 8 bit addition)
+ self._ck_a = (self._ck_a + d) % 256
+ self._ck_b = (self._ck_b + self._ck_a) % 256
+
+ def add_to_buf(char, uint8):
+ self._buf[self._payload_idx] = char
+ self._payload_idx += 1
+ update_checksum(uint8)
+
+ payload = ""
+ error = False
+ received = False
+ #convert to 8bit int
+ d = ord(c)
+
+ if self._state == self.STATE_UNINIT:
+ if d == STX:
+ self._state += 1
+ self._ck_a = STX
+ self._ck_b = STX
+ self._debug_msg("-- STX")
+ elif self._state == self.STATE_GOT_STX:
+ self._total_len = d
+ self._payload_len = d - NUM_NON_PAYLOAD_BYTES
+ self._payload_idx = 0
+ update_checksum(d)
+ self._state += 1
+ self._debug_msg("-- SIZE: PL (%s) TOT (%s)" % (self._payload_len, self._total_len))
+ elif self._state == self.STATE_GOT_LENGTH:
+ self._debug_msg("-- ACID: %x" % d)
+ self._acid = d
+ update_checksum(d)
+ self._state += 1
+ elif self._state == self.STATE_GOT_ACID:
+ self._debug_msg("-- MSGID: %x" % d)
+ self._msgid = d
+ update_checksum(d)
+ if self._payload_len == 0:
+ self._state = self.STATE_GOT_PAYLOAD
+ else:
+ self._state += 1
+ elif self._state == self.STATE_GOT_MSGID:
+ add_to_buf(c, d)
+ if self._payload_idx == self._payload_len:
+ self._state += 1
+ self._debug_msg("-- PL")
+ elif self._state == self.STATE_GOT_PAYLOAD:
+ if d != self._ck_a and self._check_crc:
+ error = True
+ self._debug_msg("-- CRC_A ERROR %x v %x" % (d, self._ck_a))
+ else:
+ self._state += 1
+ self._debug_msg("-- CRC_A OK")
+ elif self._state == self.STATE_GOT_CRC1:
+ if d != self._ck_b and self._check_crc:
+ error = True
+ self._debug_msg("-- CRC_B ERROR")
+ else:
+ payload = self._buf[:self._payload_len].tostring()
+ received = True
+ self._state = self.STATE_UNINIT
+ self._debug_msg("-- CRC_B OK")
+
+ if error:
+ self._error += 1
+ self._state = self.STATE_UNINIT
+ elif received:
+ header = TransportHeaderFooter(
+ length=self._total_len,
+ acid=self._acid,
+ msgid=self._msgid,
+ ck_a=self._ck_a,
+ ck_b=self._ck_b)
+ return True, header, payload
+
+ return False, None, None
+
+
diff --git a/sw/tools/tcp_aircraft_server/phoenix/serial_link.py b/sw/tools/tcp_aircraft_server/phoenix/serial_link.py
new file mode 100644
index 0000000000..3f8e0596ca
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/serial_link.py
@@ -0,0 +1,107 @@
+#Copyright 2014, Antoine Drouin
+import os, serial, logging
+from gi.repository import GLib, Gio, GObject
+
+import phoenix.pprz_transport as transp
+
+LOG = logging.getLogger('serial_link')
+LOG.setLevel(logging.ERROR)
+#LOG.setLevel(logging.DEBUG)
+import pdb
+
+class PhoenixCommunication(GObject.GObject):
+
+ DEFAULT_PORT = "/dev/ttyO4"
+ DEFAULT_SPEED = 57600
+
+ __gsignals__ = {
+ "message-received" : (GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, [
+ GObject.TYPE_PYOBJECT, #header
+ GObject.TYPE_PYOBJECT]), #payload
+ "status-changed" : (GObject.SIGNAL_RUN_LAST, GObject.TYPE_NONE, [])
+ }
+
+ def __init__(self, pprz_protocol):
+ GObject.GObject.__init__(self)
+ self._port = PhoenixCommunication.DEFAULT_PORT
+ self._speed = PhoenixCommunication.DEFAULT_SPEED
+ self._watch = None
+ self._is_opened = False;
+ self._transp = transp.Transport(False, False)
+ self._pprz_protocol = pprz_protocol
+ self._serial = None
+ self._available_ports = []
+ self.update_available_ports()
+ self._nb_rx_msgs = 0
+ self._nb_rx_bytes = 0
+ self._nb_tx_msgs = 0
+ self._nb_tx_bytes = 0
+
+
+ def open(self):
+ """
+ Open serial port
+ """
+ LOG.debug("Opening Port: %s @ %d" % (self._port, self._speed))
+ try:
+ self._serial = serial.Serial(self._port, self._speed, timeout=0)
+ self._is_opened = True;
+ except serial.SerialException:
+ self._is_opened = False;
+ self.on_connection_changed()
+
+ def send_msg(self, msg_class, msg_name, *fields_values):
+ """
+ send a message
+ """
+ m = self._pprz_protocol.get_message_by_name(msg_class, msg_name)
+ h = transp.TransportHeaderFooter(acid=0)
+ bin_msg = self._transp.pack_message_with_values(h, m, *fields_values)
+ self._serial.write(bin_msg)
+ self._nb_tx_msgs += 1
+ self._nb_tx_bytes += len(bin_msg)
+
+ def on_connection_changed(self):
+ if self._watch:
+ GObject.source_remove(self._watch)
+ if self._is_opened:
+ self.watch = GObject.io_add_watch(
+ self._serial.fileno(),
+ GObject.IO_IN | GObject.IO_PRI,
+ self.on_serial_data_available,
+ priority=GObject.PRIORITY_HIGH)
+ self.update_available_ports()
+ self.emit("status-changed")
+
+ def get_status(self):
+ return self._is_opened, self._port
+
+ def get_stats(self):
+ return self._nb_rx_msgs, self._nb_rx_bytes, self._nb_tx_msgs, self._nb_tx_bytes
+
+
+ def on_serial_data_available(self, fd, condition):
+ try:
+ data = self._serial.read(4096)
+ LOG.debug("read serial data : %d" % (len(data)))
+ self._nb_rx_bytes += len(data)
+ # print data.encode("hex")
+ msgs = self._transp.parse_many(data)
+ LOG.debug("parsed msg : %d" % (len(msgs)))
+ # print msgs
+ for header, payload in msgs:
+ self.emit("message-received", header, payload)
+ self._nb_rx_msgs += 1
+ return True
+ except serial.SerialException:
+ self._is_opened = False;
+ self.on_connection_changed()
+ return False
+
+ def update_available_ports(self):
+# self.available_ports = filter(lambda x: x.startswith("ttyUSB") or x.startswith("ttyS") , os.listdir("/dev"))
+ self.available_ports = filter(lambda x:x.startswith("ttyUSB") , os.listdir("/dev"))
+ self.available_ports.sort()
+
+ def get_available_ports(self):
+ return self.available_ports
diff --git a/sw/tools/tcp_aircraft_server/phoenix/xmlobject.py b/sw/tools/tcp_aircraft_server/phoenix/xmlobject.py
new file mode 100644
index 0000000000..a640e98626
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/phoenix/xmlobject.py
@@ -0,0 +1,566 @@
+#Copyright 2014, Antoine Drouin
+"""
+Allows XML files to be operated on like Python objects.
+
+Features:
+ - load XML source from file pathnames, readable file objects or raw strings
+ - add, get and set tag attributes like with python attributes
+ - iterate over nodes
+ - save the modified XMLFile or XMLObject to file
+
+Example XML file::
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+Example usage::
+
+ >>> from xmlobject import XMLFile
+
+ >>> x = XMLFile(path="sample.xml")
+
+ >>> print x
+
+
+ >>> print x.root
+
+
+ >>> print x.root._children
+ [, , ,
+ , ]
+
+ >>> print x.root.person
+ [, ]
+
+ >>> print x.root.person[0].name
+ John Smith
+
+ >>> john = x.root.person[0]
+
+ >>> john.height = 184
+
+ >>> c = john._addNode("crime")
+
+ >>> c.name = "Grand Theft Auto"
+
+ >>> c.date = "4 May, 2005"
+
+ >>> print x.toxml()
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ >>>
+
+"""
+
+import sys, os
+import xml.dom
+import xml.dom.minidom
+from xml.dom.minidom import parse, parseString, getDOMImplementation
+
+impl = getDOMImplementation()
+
+def ensure_list(obj):
+ """
+ ensures the object passed is a list, so it is iterable.
+
+ useful workaround until i decide if XMLNode.foo should always
+ return a list of foo, even if there is only one foo child
+ """
+ if len(obj):
+ return obj
+ else:
+ return [obj]
+
+class MissingRootTag(Exception):
+ """root tag name was not given"""
+
+class InvalidXML(Exception):
+ """failed to parse XML input"""
+
+class CannotSave(Exception):
+ """unable to save"""
+
+class InvalidNode(Exception):
+ """not a valid minidom node"""
+
+class XMLFile:
+ """
+ Allows an xml file to be viewed and operated on
+ as a python object.
+
+ (If you're viewing the epydoc-generated HTML documentation, click the 'show private'
+ link at the top right of this page to see all the methods)
+
+ Holds the root node in the .root attribute, also in an attribute
+ with the same name as this root node.
+ """
+ def __init__(self, **kw):
+ """
+ Create an XMLFile
+
+ Keywords:
+ - path - a pathname from which the file can be read
+ - file - an open file object from which the raw xml
+ can be read
+ - raw - the raw xml itself
+ - root - name of root tag, if not reading content
+
+ Usage scenarios:
+ 1. Working with existing content - you must supply input in
+ one of the following ways:
+ - 'path' must be an existing file, or
+ - 'file' must be a readable file object, or
+ - 'raw' must contain raw xml as a string
+ 2. Creating whole new content - you must give the name
+ of the root tag in the 'root' keyword
+
+ Notes:
+ - Keyword precedence governing existing content is:
+ 1. path (if existing file)
+ 2. file
+ 3. raw
+ - If working with existing content:
+ - if the 'root' is given, then the content's toplevel tag
+ MUST match the value given for 'root'
+ - trying to _save will raise an exception unless 'path'
+ has been given
+ - if not working with existing content:
+ - 'root' must be given
+ - _save() will raise an exception unless 'path' has been given
+ """
+ path = kw.get("path", None)
+ fobj = kw.get("file", None)
+ raw = kw.get("raw", None)
+ root = kw.get("root", None)
+
+ if path:
+ self.path = path
+ try:
+ fobj = file(path)
+ except IOError:
+ pass
+ else:
+ self.path = None
+
+ if fobj:
+ raw = fobj.read()
+
+ if raw:
+ self.dom = xml.dom.minidom.parseString(raw)
+ else:
+ # could not source content, so create a blank slate
+ if not root:
+ # in which case, must give a root node name
+ raise MissingRootTag(
+ "No existing content, so must specify root")
+
+ # ok, create a blank dom
+ self.dom = impl.createDocument(None, root, None)
+
+ # get the root node, save it as attributes 'root' and name of node
+ rootnode = self.dom.documentElement
+
+ # now validate root tag
+ if root:
+ if rootnode.nodeName != root:
+ raise IncorrectRootTag("Gave root='%s', input has root='%s'" % (
+ root, rootnode.nodeName))
+
+ # need this for recursion in XMLNode
+ self._childrenByName = {}
+ self._children = []
+
+ # add all the child nodes
+ for child in self.dom.childNodes:
+ childnode = XMLNode(self, child)
+ #print "compare %s to %s" % (rootnode, child)
+ if child == rootnode:
+ #print "found root"
+ self.root = childnode
+ setattr(self, rootnode.nodeName, self.root)
+
+ def save(self, where=None, obj=None):
+ """
+ Saves the document.
+
+ If argument 'where' is given, saves to it, otherwise
+ tries to save to the original given 'path' (or barfs)
+
+ Value can be a string (taken to be a file path), or an open
+ file object.
+ """
+ obj = obj or self.dom
+
+ if not where:
+ if self._root.path:
+ where = self._root.path
+
+ if isinstance(where, str):
+ where = file(where, "w")
+
+ if not where:
+ raise CannotSave("No save destination, and no original path")
+
+ where.write(obj.toxml())
+ where.flush()
+
+ def saveAs(self, path):
+ """
+ save this time, and all subsequent times, to filename 'path'
+ """
+ self.path = path
+ self.save()
+
+ def toxml(self):
+ return self.dom.toxml()
+
+ def __len__(self):
+ """
+ returns number of child nodes
+ """
+ return len(self._children)
+
+ def __getitem__(self, idx):
+ if isinstance(idx, int):
+ return self._children[idx]
+ else:
+ return self._childrenByName[idx]
+
+
+class XMLNode:
+ """
+ This is the workhorse for the xml object interface
+
+ (If you're viewing the epydoc-generated HTML documentation, click the 'show private'
+ link at the top right of this page to see all the methods)
+
+ """
+
+ # http://docs.python.org/reference/lexical_analysis.html#id6
+ __RESERVED_WORDS = (
+ "and","del","class","from","not","while"
+ "as","elif","global","or","with","assert","else","if",
+ "pass","yield","break","except","import","print",
+ "class","exec","in","raise","continue","finally",
+ "is","return","def","for","lambda","try"
+ )
+
+ def __init__(self, parent, node):
+ """
+ You shouldn't need to instantiate this directly
+ """
+ self._parent = parent
+ if isinstance(parent, XMLFile):
+ self._root = parent
+ else:
+ self._root = parent._root
+ self._node = node
+ self._childrenByName = {}
+ self._children = []
+
+ # add ourself to parent's children registry
+ parent._children.append(self)
+
+ # the deal with named subtags is that we store the first instance
+ # as itself, and with second and subsequent instances, we make a list
+ parentDict = self._parent._childrenByName
+
+ # If the name of the node is a python reserved word then captilize it
+ nodeName = node.nodeName
+ if nodeName in self.__RESERVED_WORDS:
+ nodeName = nodeName.upper()
+
+ if not parentDict.has_key(nodeName):
+ parentDict[nodeName] = parent.__dict__[nodeName] = self
+ else:
+ if isinstance(parentDict[nodeName], XMLNode):
+ # this is the second child node of a given tag name, so convert
+ # the instance to a list
+ parentDict[nodeName] = parent.__dict__[nodeName] = [parentDict[nodeName]]
+ parentDict[nodeName].append(self)
+
+ # figure out our type
+ self._value = None
+ if isinstance(node, xml.dom.minidom.Text):
+ self._type = "text"
+ self._value = node.nodeValue
+ elif isinstance(node, xml.dom.minidom.Element):
+ self._type = "node"
+ elif isinstance(node, xml.dom.minidom.Comment):
+ self._type = "comment"
+ self._value = node.nodeValue
+ elif isinstance(node, xml.dom.minidom.DocumentType):
+ #
+ #Ignore doctype, could possibly check it....
+ pass
+ else:
+ raise InvalidNode("node class %s" % node.__class__)
+
+ # and wrap all the child nodes
+ for child in node.childNodes:
+ XMLNode(self, child)
+
+ def _render(self):
+ """
+ Produces well-formed XML of this node's contents,
+ indented as required
+ """
+ return self._node.toxml()
+
+ def __repr__(self):
+ if self._type == "node":
+ return "" % self._node.nodeName
+ else:
+ return "" % self._type
+
+ def __getattr__(self, attr):
+ """
+ Fetches an attribute or child node of this tag
+
+ If it's an attribute, then returns the attribute value as a string.
+
+ If a child node, then:
+ - if there is only one child node of that name, return it
+ - if there is more than one child node of that name, return a list
+ of child nodes of that tag name
+
+ Supports some magic attributes:
+ - _text - the value of the first child node of type text
+ """
+ #print "%s: __getattr__: attr=%s" % (self, attr)
+
+ if attr == '_text':
+ # magic attribute to return text
+ tnode = self['#text']
+ if isinstance(tnode, list):
+ tnode = tnode[0]
+ return tnode._value
+
+ if self._type in ['text', 'comment']:
+ if attr == '_value':
+ return self._node.nodeValue
+ else:
+ raise AttributeError(attr)
+
+ if self._node.hasAttribute(attr):
+ return self._node.getAttribute(attr)
+ elif self._childrenByName.has_key(attr):
+ return self._childrenByName[attr]
+
+ #elif attr == 'value':
+ # magic attribute
+
+ else:
+ raise AttributeError(attr)
+
+
+ def __setattr__(self, attr, val):
+ """
+ Change the value of an attribute of this tag
+
+ The magic attribute '_text' can be used to set the first child
+ text node's value
+
+ For example::
+
+ Consider:
+
+
+ foo
+
+
+ >>> somenode
+
+ >>> somenode.child
+
+ >>> somenode.child._text
+ 'foo'
+ >>> somenode._toxml()
+ u'foo'
+ >>> somenode.child._text = 'bar'
+ >>> somenode.child._text
+ 'bar'
+ >>> somenode.child._toxml()
+ u'bar/child>'
+
+ """
+ if attr.startswith("_"):
+
+ # magic attribute for setting _text
+ if attr == '_text':
+ tnode = self['#text']
+ if isinstance(tnode, list):
+ tnode = tnode[0]
+ tnode._node.nodeValue = val
+ tnode._value = val
+ return
+
+ self.__dict__[attr] = val
+ elif self._type in ['text', 'comment']:
+ self._node.nodeValue = val
+ else:
+ # discern between attribute and child node
+ if self._childrenByName.has_key(attr):
+ raise Exception("Attribute Exists")
+ self._node.setAttribute(attr, str(val))
+
+ def _keys(self):
+ """
+ Return a list of attribute names
+ """
+ return self._node.attributes.keys()
+
+ def _values(self):
+ """
+ Returns a list of (attrname, attrval) tuples for this tag
+ """
+ return [self._node.getAttribute(k) for k in self._node.attributes.keys()]
+
+ def _items(self):
+ """
+ returns a list of attribute values for this tag
+ """
+ return [(k, self._node.getAttribute(k)) for k in self._node.attributes.keys()]
+
+ def _has_key(self, k):
+ """
+ returns True if this tag has an attribute of the given name
+ """
+ return self._node.hasAttribute(k) or self._childrenByName.has_key(k)
+
+ def _get(self, k, default=None):
+ """
+ returns the value of attribute k, or default if no such attribute
+ """
+ if self._has_key(k):
+ return getattr(self, k)
+ else:
+ return default
+ def __len__(self):
+ """
+ returns number of child nodes
+ """
+ return len(self._children)
+
+ def __getitem__(self, idx):
+ """
+ if given key is numeric, return the nth child, otherwise
+ try to return the child tag (or list of child tags) having
+ the key as the tag name
+ """
+ #print "__getitem__: idx=%s" % str(idx)
+
+ if isinstance(idx, slice) or isinstance(idx, int):
+ return self._children[idx]
+ elif isinstance(idx, str):
+ return self._childrenByName[idx]
+ else:
+ raise IndexError(idx)
+
+ def _addNode(self, child):
+ """
+ Tries to append a child node to the tree, and returns it
+
+ Value of 'child' must be one of:
+ - a string (in which case it is taken to be the name
+ of the new node's tag)
+ - a dom object, in which case it will be wrapped and added
+ - an XMLNode object, in which case it will be added without
+ wrapping
+ """
+
+ if isinstance(child, XMLNode):
+
+ # add it to our children registry
+ self._children.append(child)
+
+ parentDict = self._childrenByName
+ nodeName = child._node.nodeName
+
+ if not parentDict.has_key(nodeName):
+ parentDict[nodeName] = parent.__dict__[nodeName] = child
+ else:
+ if isinstance(parentDict[nodeName], XMLNode):
+ # this is the second child node of a given tag name, so convert
+ # the instance to a list
+ parentDict[nodeName] = self.__dict__[nodeName] = [parentDict[nodeName]]
+ parentDict[nodeName].append(child)
+
+ # and stick it in the dom
+ self._node.appendChild(child._node)
+
+ return child
+
+ elif isinstance(child, str):
+ childNode = self._root.dom.createElement(child)
+ self._node.appendChild(childNode)
+
+ elif isinstance(child, xml.dom.minidom.Element):
+ childNode = child
+ child = childNode.nodeName
+ self._node.appendChild(childNode)
+
+
+ return XMLNode(self, childNode)
+
+ def _addText(self, value):
+ """
+ Tries to append a child text node, with the given text, to the tree,
+ and returns the created node object
+ """
+ childNode = self._root.dom.createTextNode(value)
+ self._node.appendChild(childNode)
+ return XMLNode(self, childNode)
+
+ def _addComment(self, comment):
+ """
+ Tries to append a child comment node (with the given text value)
+ to the tree, and returns the create node object
+ """
+ childNode = self._root.dom.createCommentNode(comment)
+ self._node.appendChild(childNode)
+ return XMLNode(self, childNode)
+
+ def _save(self, where=None):
+ """
+ Generates well-formed XML from just this node, and saves it
+ to a file.
+
+ Argument 'where' is either an open file object, or a pathname
+
+ If 'where' is not given, then saves the entire document tree.
+ """
+ if not where:
+ self._root.save()
+ else:
+ self._root.save(where, self._node)
+
+ def _toxml(self):
+ """
+ renders just this node out to raw xml code
+ """
+ return self._node.toxml()
+
diff --git a/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py b/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py
new file mode 100755
index 0000000000..2553bf8b3f
--- /dev/null
+++ b/sw/tools/tcp_aircraft_server/tcp_aircraft_server.py
@@ -0,0 +1,71 @@
+#!/usr/bin/env python
+#-*- coding: utf-8 -*-
+
+#Copyright 2014, Antoine Drouin
+
+import logging, os, base64, socket
+from gi.repository import GLib, GObject
+import ivy.ivy as ivy
+ivylogger = logging.getLogger('Ivy')
+ivylogger.setLevel(logging.CRITICAL)
+
+import phoenix.messages
+import phoenix.pprz_transport
+
+default_ivybus = '127.255.255.255:2010'
+
+class Server(ivy.IvyServer):
+ def __init__(self, bus, tcp_port=4242):
+ ivy.IvyServer.__init__(self, 'TCP_aircraft_server', usesDaemons=True)
+
+ self.nb_msgs = 0
+ self.nb_bytes = 0
+
+ cs = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ cs.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ cs.bind(('', tcp_port))
+ cs.listen(1)
+ GObject.io_add_watch(cs, GObject.IO_IN, self.handle_conn)
+ print "server listening on {:d}".format(tcp_port)
+
+ self.transp = phoenix.pprz_transport.Transport(check_crc=False, debug=False)
+ self.protocol = phoenix.messages.Protocol(path=os.path.abspath("../../../conf/messages_ng.xml"), debug=True)
+ self.start(bus)
+
+ GObject.timeout_add(500, self.periodic, priority=GObject.PRIORITY_HIGH)
+
+ def handle_conn(self, sock, cond):
+ conn, addr = sock.accept()
+ print "Connection from {}".format(addr)
+ GObject.io_add_watch(conn, GObject.IO_IN, self.handle_data)
+ return True
+
+ def handle_data(self, sock, cond):
+ buf = sock.recv(4096)
+ if not len(buf):
+ print "Connection closed."
+ return False
+ else:
+ #print phoenix.hex_of_bin(buf)
+ msgs = self.transp.parse_many(buf)
+ for hdr, payload in msgs:
+ msg = self.protocol.get_message_by_id("telemetry", hdr.msgid)
+ try:
+ ivy_str = '{} {} {}'.format(hdr.acid, msg.name, ' '.join([str(v) for v in msg.unpack_values(payload)]))
+ #print ivy_str
+ self.send_msg(ivy_str)
+ self.nb_msgs += 1
+ self.nb_bytes += len(payload)
+ except:
+ print 'FAILED', msg.name
+ return True
+
+ def periodic(self):
+ print 'msgs {} ({} bytes)'.format(self.nb_msgs, self.nb_bytes)
+ return True
+
+if __name__ == '__main__':
+ logging.basicConfig(format='%(levelname)s:%(message)s', level=logging.INFO)
+ server = Server(default_ivybus)
+ GLib.MainLoop().run()
+