[python] split message and transport, add serial interface

sw/lib/python/pprz_msg/message.py

@@ -8,69 +8,6 @@ import sys
 import json
 import struct
 import messages_xml_map
-from enum import Enum
-
-STX = 0x99
-STX_TS = 0x98
-
-class PprzParserState(Enum):
-    WaitSTX = 1
-    GotSTX = 2
-    GotLength = 3
-    GotPayload = 4
-    GotCRC1 = 5
-
-class PprzParser(object):
-    """parser for binary Paparazzi messages"""
-    def __init__(self):
-        self.reset_parser()
-
-    def parse_byte(self, c):
-        """parse new byte, return True when a new full message is available"""
-        b = ord(c)
-        if self.state == PprzParserState.WaitSTX:
-            if b == STX:
-                self.state = PprzParserState.GotSTX
-        elif self.state == PprzParserState.GotSTX:
-            self.length = b - 4
-            self.buf = []
-            self.ck_a = b % 256
-            self.ck_b = b % 256
-            self.idx = 0
-            self.state = PprzParserState.GotLength
-        elif self.state == PprzParserState.GotLength:
-            self.buf.append(c);
-            self.ck_a = (self.ck_a + b) % 256
-            self.ck_b = (self.ck_b + self.ck_a) % 256
-            self.idx += 1
-            if self.idx == self.length:
-                self.state = PprzParserState.GotPayload
-        elif self.state == PprzParserState.GotPayload:
-            if self.ck_a == b:
-                self.state = PprzParserState.GotCRC1
-            else:
-                self.state = PprzParserState.WaitSTX
-        elif self.state == PprzParserState.GotCRC1:
-            self.state = PprzParserState.WaitSTX
-            if self.ck_b == b:
-                """New message available"""
-                return True
-        else:
-            self.state = PprzParserState.WaitSTX
-        return False
-
-    def get_buffer(self):
-        return self.buf
-
-    def reset_parser(self):
-        self.state = PprzParserState.WaitSTX
-        self.length = 0
-        self.buf = []
-        self.ck_a = 0
-        self.ck_b = 0
-        self.idx = 0
-
-            
 
 class PprzMessageError(Exception):
     def __init__(self, message, inner_exception=None):
@@ -104,6 +41,11 @@ class PprzMessage(object):
         """Get the message name."""
         return self._name
 
+    @property
+    def msg_id(self):
+        """Get the message id."""
+        return self._id
+
     @property
     def msg_class(self):
         """Get the message class."""
@@ -239,23 +181,6 @@ class PprzMessage(object):
                 values.append(value)
         self.set_values(values)
 
-    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:]:
-            ck_a = (ck_a + ord(c)) % 256
-            ck_b = (ck_b + ck_a) % 256
-        return (ck_a, ck_b)
-
-    def payload_to_pprz_msg(self, sender):
-        stx = STX
-        data = self.payload_to_binary()
-        length = 6 + len(data)
-        msg = struct.pack("=BBBB", STX, length, sender, self._id) + data
-        (ck_a, ck_b) = self.calculate_checksum(msg)
-        msg = msg + struct.pack('=BB', ck_a, ck_b)
-        return msg
 
 def test():
     import argparse

sw/lib/python/pprz_msg/pprz_transport.py

@@ -0,0 +1,103 @@
+"""
+Paparazzi transport encoding utilities
+
+"""
+
+from __future__ import print_function
+import struct
+from pprz_msg.message import PprzMessage
+import messages_xml_map
+from enum import Enum
+
+STX = 0x99
+STX_TS = 0x98
+
+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 = ord(c)
+        if self.state == PprzParserState.WaitSTX:
+            if b == STX:
+                self.state = PprzParserState.GotSTX
+        elif self.state == PprzParserState.GotSTX:
+            self.length = b - 4
+            self.buf = []
+            self.ck_a = b % 256
+            self.ck_b = b % 256
+            self.idx = 0
+            self.state = PprzParserState.GotLength
+        elif self.state == PprzParserState.GotLength:
+            self.buf.append(c);
+            self.ck_a = (self.ck_a + b) % 256
+            self.ck_b = (self.ck_b + self.ck_a) % 256
+            self.idx += 1
+            if self.idx == self.length:
+                self.state = PprzParserState.GotPayload
+        elif self.state == PprzParserState.GotPayload:
+            if self.ck_a == b:
+                self.state = PprzParserState.GotCRC1
+            else:
+                self.state = PprzParserState.WaitSTX
+        elif self.state == PprzParserState.GotCRC1:
+            self.state = PprzParserState.WaitSTX
+            if self.ck_b == b:
+                """New message available"""
+                return True
+        else:
+            self.state = PprzParserState.WaitSTX
+        return False
+
+    def get_buffer(self):
+        return self.buf
+
+    def unpack_pprz_msg(self, msg_class, data):
+        """Unpack a raw PPRZ message"""
+        sender_id = ord(data[0])
+        msg_id = ord(data[1])
+        msg_name = messages_xml_map.get_msg_name(msg_class, msg_id)
+        msg = PprzMessage(msg_class, msg_name)
+        msg.binary_to_payload(data[2:])
+        return (sender_id, msg)
+
+    def unpack(self):
+        """Unpack the last received message"""
+        return self.unpack_pprz_msg(self.msg_class, 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:]:
+            ck_a = (ck_a + ord(c)) % 256
+            ck_b = (ck_b + ck_a) % 256
+        return (ck_a, ck_b)
+
+    def pack_pprz_msg(self, sender, msg):
+        data = msg.payload_to_binary()
+        # STX + length + sender_id + msg_id + data + ck_a + ck_b
+        length = 6 + len(data)
+        msg = struct.pack("=BBBB", STX, length, sender, msg.msg_id) + data
+        (ck_a, ck_b) = self.calculate_checksum(msg)
+        msg = msg + struct.pack('=BB', ck_a, ck_b)
+        return msg
+

sw/lib/python/serial_msg_interface.py

@@ -0,0 +1,103 @@
+from __future__ import absolute_import, print_function
+
+import threading
+import serial
+import os
+import sys
+
+# if PAPARAZZI_SRC not set, then assume the tree containing this
+# file is a reasonable substitute
+PPRZ_SRC = os.getenv("PAPARAZZI_SRC", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),
+                                                                    '../../../../')))
+sys.path.append(PPRZ_SRC + "/sw/lib/python")
+
+from pprz_msg.message import PprzMessage
+from pprz_msg.pprz_transport import PprzTransport
+import pprz_msg.messages_xml_map
+
+
+class SerialMessagesInterface(threading.Thread):
+    def __init__(self, callback, init=True, verbose=False, device='/dev/ttyUSB0', baudrate=115200, msg_class='telemetry', messages_file=''):
+        threading.Thread.__init__(self)
+        self.callback = callback
+        self.verbose = verbose
+        self.msg_class = msg_class
+        self.running = True
+        try:
+            self.ser = serial.Serial(device, baudrate, timeout=1.0)
+        except:
+            print("Error: unable to open serial port '%s'" % device)
+            exit(0)
+        self.trans = PprzTransport(msg_class)
+        pprz_msg.messages_xml_map.parse_messages(messages_file)
+
+    def stop(self):
+        print("End thread and close serial link")
+        self.running = False
+        self.ser.close()
+
+    def shutdown(self):
+        self.stop()
+
+    def __init__del__(self):
+        try:
+            self.ser.close()
+        except:
+            pass
+
+    def send(self, msg, sender_id):
+        """ Send a message over a serial link"""
+        if isinstance(msg, PprzMessage):
+            data = self.trans.pack_pprz_msg(sender_id, msg)
+            self.ser.write(data)
+            self.ser.flush()
+
+    def run(self):
+        """Thread running function"""
+        try:
+            while self.running:
+                # Parse incoming data
+                c = self.ser.read(1)
+                if len(c) == 1:
+                    if self.trans.parse_byte(c):
+                        (sender_id, msg) = self.trans.unpack()
+                        if self.verbose:
+                            print("New incoming message '%i' from %s" % (sender_id, msg.name))
+                        # Callback function on new message 
+                        self.callback(sender_id, msg)
+
+        except StopIteration:
+            pass
+
+# FIXME not working because of the thread ?
+import signal
+def signal_term_handler(signal, frame):
+        print("got SIGINT")
+        #sys.exit(0)
+
+def test():
+    signal.signal(signal.SIGINT, signal_term_handler)
+    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')
+    parser.add_argument("-d", "--device", help="device name", dest='dev', default='/dev/ttyUSB0')
+    parser.add_argument("-b", "--baudrate", help="baudrate", dest='baud', default=115200, type=int)
+    args = parser.parse_args()
+    serial_interface = SerialMessagesInterface(lambda s, m : print("new message from %i: %s" % (s, m)), device = args.dev, baudrate = args.baud, msg_class=args.msg_class, messages_file=args.file, verbose=True)
+    att_msg = PprzMessage('telemetry', 'ATTITUDE')
+    att_msg.set_value_by_name('phi', 0.1)
+    att_msg.set_value_by_name('theta', 0.2)
+    att_msg.set_value_by_name('psi', 0.3)
+    serial_interface.send(att_msg, 42)
+    to_msg = PprzMessage('telemetry', 'TAKEOFF')
+    to_msg.set_value_by_name('cpu_time', 10)
+    serial_interface.send(to_msg, 42)
+    print("Starting serial interface on %s at %i baud" % (args.dev, args.baud))
+    serial_interface.start()
+    signal.pause()
+    serial_interface.stop()
+    serial_interface.join()
+
+if __name__ == '__main__':
+    test()