[payload] Payload Forward over UDP.

conf/airframes/TUDELFT/tudelft_control_panel.xml

@@ -60,6 +60,7 @@
     </program>
     <program name="SVInfo" command="sw/ground_segment/python/svinfo/svinfo.py"/>
     <program name="IridiumDialer" command="sw/tools/iridium/iridium_link.py"/>
+    <program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
   </section>
 
   <section name="sessions">

conf/control_panel_example.xml

@@ -66,6 +66,7 @@
     </program>
     <program name="SVInfo" command="sw/ground_segment/python/svinfo/svinfo.py"/>
     <program name="IridiumDialer" command="sw/tools/iridium/iridium_link.py"/>
+    <program name="PayloadForward" command="sw/ground_segment/python/payload_forward/payload.py"/>
  </section>
 
   <section name="sessions">

sw/ground_segment/python/payload_forward/README.md

@@ -0,0 +1,13 @@
+PAYLOAD
+=======
+
+PAYLOAD (downlink) and PAYLOAD_COMMAND (uplink) messages in the pprzlink telemetry act as easy pass-through of information from an autopilot subsystem like a computer vision board to ground station payload control system.
+
+The paparazzi autopilot does not need to understand the data, but just needs to relay it. Payload is typically highly compressed information and only piggy-backs on telemetry when it needs to be transferred over the same long-range low bitrate datalink as the autopilot telemetry.
+
+The PAYLOAD sender (typically a paparazzi module) must make sure the telemetry datalink does not get flooded with payload.
+
+
+Over the years some standards have developed for payload. One is sending jpeg-thumbnails-chunks. A decoder for this 'jpeg100' is added to the forwarding program.
+
+use ```payload.py --help``` to find currently supported options.

sw/ground_segment/python/payload_forward/jpeg100_decoder.py

@@ -0,0 +1,70 @@
+#
+# Copyright (C) 2016 TUDelft
+#
+# 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 3 of the License, 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.  If not, see <http://www.gnu.org/licenses/>.
+#
+
+import wx
+import array
+import Image
+from cStringIO import StringIO
+
+
+jpegheader = b'\xff\xd8\xff\xe0\x00\x10\x4a\x46\x49\x46\x00\x01\x01\x00\x00\x01\x00\x01\x00\x00\xff\xdb\x00\x43\x00\x20\x16\x18\x1c\x18\x14\x20\x1c\x1a\x1c\x24\x22\x20\x26\x30\x50\x34\x30\x2c\x2c\x30\x62\x46\x4a\x3a\x50\x74\x66\x7a\x78\x72\x66\x70\x6e\x80\x90\xb8\x9c\x80\x88\xae\x8a\x6e\x70\xa0\xda\xa2\xae\xbe\xc4\xce\xd0\xce\x7c\x9a\xe2\xf2\xe0\xc8\xf0\xb8\xca\xce\xc6\xff\xdb\x00\x43\x01\x22\x24\x24\x30\x2a\x30\x5e\x34\x34\x5e\xc6\x84\x70\x84\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xc6\xff\xc0\x00\x11\x08\x00\x64\x00\x64\x03\x01\x22\x00\x02\x11\x01\x03\x11\x01\xff\xc4\x00\x1f\x00\x00\x01\x05\x01\x01\x01\x01\x01\x01\x00\x00\x00\x00\x00\x00\x00\x00\x01\x02\x03\x04\x05\x06\x07\x08\x09\x0a\x0b\xff\xc4\x00\xb5\x10\x00\x02\x01\x03\x03\x02\x04\x03\x05\x05\x04\x04\x00\x00\x01\x7d\x01\x02\x03\x00\x04\x11\x05\x12\x21\x31\x41\x06\x13\x51\x61\x07\x22\x71\x14\x32\x81\x91\xa1\x08\x23\x42\xb1\xc1\x15\x52\xd1\xf0\x24\x33\x62\x72\x82\x09\x0a\x16\x17\x18\x19\x1a\x25\x26\x27\x28\x29\x2a\x34\x35\x36\x37\x38\x39\x3a\x43\x44\x45\x46\x47\x48\x49\x4a\x53\x54\x55\x56\x57\x58\x59\x5a\x63\x64\x65\x66\x67\x68\x69\x6a\x73\x74\x75\x76\x77\x78\x79\x7a\x83\x84\x85\x86\x87\x88\x89\x8a\x92\x93\x94\x95\x96\x97\x98\x99\x9a\xa2\xa3\xa4\xa5\xa6\xa7\xa8\xa9\xaa\xb2\xb3\xb4\xb5\xb6\xb7\xb8\xb9\xba\xc2\xc3\xc4\xc5\xc6\xc7\xc8\xc9\xca\xd2\xd3\xd4\xd5\xd6\xd7\xd8\xd9\xda\xe1\xe2\xe3\xe4\xe5\xe6\xe7\xe8\xe9\xea\xf1\xf2\xf3\xf4\xf5\xf6\xf7\xf8\xf9\xfa\xff\xc4\x00\x1f\x01\x00\x03\x01\x01\x01\x01\x01\x01\x01\x01\x01\x00\x00\x00\x00\x00\x00\x01\x02\x03\x04\x05\x06\x07\x08\x09\x0a\x0b\xff\xc4\x00\xb5\x11\x00\x02\x01\x02\x04\x04\x03\x04\x07\x05\x04\x04\x00\x01\x02\x77\x00\x01\x02\x03\x11\x04\x05\x21\x31\x06\x12\x41\x51\x07\x61\x71\x13\x22\x32\x81\x08\x14\x42\x91\xa1\xb1\xc1\x09\x23\x33\x52\xf0\x15\x62\x72\xd1\x0a\x16\x24\x34\xe1\x25\xf1\x17\x18\x19\x1a\x26\x27\x28\x29\x2a\x35\x36\x37\x38\x39\x3a\x43\x44\x45\x46\x47\x48\x49\x4a\x53\x54\x55\x56\x57\x58\x59\x5a\x63\x64\x65\x66\x67\x68\x69\x6a\x73\x74\x75\x76\x77\x78\x79\x7a\x82\x83\x84\x85\x86\x87\x88\x89\x8a\x92\x93\x94\x95\x96\x97\x98\x99\x9a\xa2\xa3\xa4\xa5\xa6\xa7\xa8\xa9\xaa\xb2\xb3\xb4\xb5\xb6\xb7\xb8\xb9\xba\xc2\xc3\xc4\xc5\xc6\xc7\xc8\xc9\xca\xd2\xd3\xd4\xd5\xd6\xd7\xd8\xd9\xda\xe2\xe3\xe4\xe5\xe6\xe7\xe8\xe9\xea\xf2\xf3\xf4\xf5\xf6\xf7\xf8\xf9\xfa\xff\xda\x00\x0c\x03\x01\x00\x02\x11\x03\x11\x00\x3f\x00'
+
+class ThumbNailFromPayload:
+
+    def add_payload(self, lst):
+
+        if lst[1] == 255:
+            self.thumb = self.pay
+        elif lst[1] == 0:
+            self.pay = lst[3:]
+        else:
+            self.pay.extend(lst[3:])
+
+
+    def __init__(self):
+
+        self.pay = []
+        self.thumb = []
+        self.last_good_bitmap = None
+
+    def get_image(self):
+
+        self.jpgdata = jpegheader + bytearray(self.thumb)
+        file_jpgdata = StringIO(self.jpgdata)
+        img = Image.open(file_jpgdata)
+        return img
+
+    def get_bitmap(self):
+
+        myPilImage = self.get_image()
+        myWxImage = wx.EmptyImage( myPilImage.size[0], myPilImage.size[1] )
+        myWxImage.SetData( myPilImage.convert( 'RGB' ).tostring() )
+        bitmap = wx.BitmapFromImage(myWxImage)
+        return bitmap
+
+    def draw(self, dc, x, y):
+        # Jpeg: might be corrupt/incomplete
+        try:
+            bitmap = self.get_bitmap()
+            dc.DrawBitmap(bitmap, x,y, True)
+            self.last_good_bitmap = bitmap
+        except IOError:
+            if self.last_good_bitmap is not None:
+                dc.DrawBitmap(self.last_good_bitmap, x,y, True)
+

sw/ground_segment/python/payload_forward/payload.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+#
+# Copyright (C) 2016 TUDelft
+#
+# 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 3 of the License, 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.  If not, see <http://www.gnu.org/licenses/>.
+#
+
+import wx
+import sys
+import argparse
+import payload_forward
+
+parser = argparse.ArgumentParser(description='Capture PAYLOAD messages over the IVY bus and forward to a remote application.', epilog='payload.py is part of the paparazzi-uav project.')
+parser.add_argument('--ip', '-i', default="192.168.0.1", help='Destination IP ADDRESS (default=192.168.0.1)')
+parser.add_argument('--port','-p', type=int, default=32000, help='Destination PORT (default=32000)')
+parser.add_argument('--decoder','-d', default='jpeg100', help='Payload decoder. (default=jpeg100)', choices=['none', 'jpeg100'])
+settings = parser.parse_args()
+
+print(settings)
+
+class PayloadFrame(wx.App):
+    def OnInit(self):
+        self.main = payload_forward.PayloadForwarderFrame(settings)
+        self.main.Show()
+        self.SetTopWindow(self.main)
+        return True
+
+def main():
+    application = PayloadFrame(0)
+    application.MainLoop()
+
+if __name__ == '__main__':
+    main()

sw/ground_segment/python/payload_forward/payload_forward.py

@@ -0,0 +1,135 @@
+#
+# Copyright (C) 2016 TUDelft
+#
+# 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 3 of the License, 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.  If not, see <http://www.gnu.org/licenses/>.
+#
+
+import wx
+import sys
+import os
+import threading
+import socket
+import array
+import jpeg100_decoder
+from cStringIO import StringIO
+
+
+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/ext/pprzlink/lib/v1.0/python")
+
+from pprzlink.ivy import IvyMessagesInterface
+
+WIDTH = 300
+
+
+# Minimal Decoder
+class MinimalDecoder:
+    def __init__(self):
+        self.data = []
+    def add_payload(self,bytes):
+        self.data = bytes
+
+    def draw(self,dc,x,y):
+        dc.DrawText( "Payload: " + str(self.data),x,y)
+
+
+class PayloadForwarderFrame(wx.Frame):
+
+    def message_recv(self, ac_id, msg):
+        if msg.name == "PAYLOAD":
+            # convert text to binary
+            pld = msg.get_field(0).split(",")
+            b = []
+            for p in pld:
+                b.append(int(p))
+
+            # forward over UDP
+            self.data['packets'] = self.data['packets'] + 1
+            self.data['bytes'] = self.data['bytes'] + len(b)
+            self.sock.sendto(bytearray(b), (self.settings.ip, self.settings.port))
+
+            # send to decoder
+            self.decoder.add_payload(b)
+
+            # graphical update
+            wx.CallAfter(self.update)
+
+    def update(self):
+        self.Refresh()
+
+    def OnSize(self, event):
+        self.w = event.GetSize()[0]
+        self.h = event.GetSize()[1]
+        self.Refresh()
+
+    def OnPaint(self, e):
+        # Paint Area
+        dc = wx.PaintDC(self)
+        brush = wx.Brush("white")
+        dc.SetBackground(brush)
+        dc.Clear()
+
+        # Background
+        dc.SetBrush(wx.Brush(wx.Colour(0,0,0), wx.TRANSPARENT))
+        font = wx.Font(11, wx.ROMAN, wx.BOLD, wx.NORMAL)
+        dc.SetFont(font)
+        dc.DrawText("UDP: " + self.settings.ip + ":" + str(self.settings.port),2,2)
+        dc.DrawText("Data: " + str(self.data['packets']) + " packets, " + str(round(float(self.data['bytes'])/1024.0,2)) + "kb)",2,22)
+        dc.DrawText("Decoder: " + self.settings.decoder ,2,42)
+
+        # Payload visual representation
+        self.decoder.draw(dc, 2, 62)
+
+
+    def __init__(self, _settings):
+
+        # Command line arguments
+        self.settings = _settings
+
+        # Statistics
+        self.data = { 'packets': 0, 'bytes': 0}
+
+        # Decoder
+        if (self.settings.decoder is 'jpeg100'):
+            self.decoder = jpeg100_decoder.ThumbNailFromPayload()
+        else:
+            self.decoder = MinimalDecoder()
+
+        self.w = WIDTH
+        self.h = WIDTH
+
+        # Socket
+        self.sock = socket.socket(socket.AF_INET, # Internet
+socket.SOCK_DGRAM) # UDP
+
+        # Frame
+        wx.Frame.__init__(self, id=-1, parent=None, name=u'Payload Forwarding',
+                          size=wx.Size(self.w, self.h), title=u'Payload Forwarding')
+        ico = wx.Icon(os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)))) + "/camera.ico", wx.BITMAP_TYPE_ICO)
+        self.SetIcon(ico)
+
+        self.Bind(wx.EVT_PAINT, self.OnPaint)
+        self.Bind(wx.EVT_SIZE, self.OnSize)
+        self.Bind(wx.EVT_CLOSE, self.OnClose)
+
+        # IVY
+        self.interface = IvyMessagesInterface("PayloadForwarder")
+        self.interface.subscribe(self.message_recv)
+
+    def OnClose(self, event):
+        self.interface.shutdown()
+        self.Destroy()