diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml
index 2f8d66aaa2..c3b284e2e1 100644
--- a/conf/control_panel_example.xml
+++ b/conf/control_panel_example.xml
@@ -47,6 +47,7 @@
     <program name="Attitude Visualizer" command="sw/tools/attitude_viz.py"/>
     <program name="App Server" command="sw/ground_segment/tmtc/app_server"/>
     <program name="NatNet" command="sw/ground_segment/misc/natnet2ivy"/>
+    <program name="NatNet3" command="sw/ground_segment/python/natnet3.x/natnet2ivy.py"/>
     <program name="Ivy2Nmea" command="sw/ground_segment/tmtc/ivy2nmea">
       <arg flag="--port" constant="/dev/ttyUSB1"/>
       <arg flag="--id" constant="1"/>
diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c
index 02f5c86b9b..e59ad6f3a0 100644
--- a/sw/ground_segment/misc/natnet2ivy.c
+++ b/sw/ground_segment/misc/natnet2ivy.c
@@ -723,8 +723,11 @@ static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data)
   bytes_data += udp_socket_recv(&natnet_data, buffer_data, MAX_PACKETSIZE);
 
   // Parse NatNet data
-  if (bytes_data >= 2 && bytes_data >= buffer_data[1]) {
-    natnet_parse(buffer_data);
+  if (bytes_data >= 2) {
+    uint16_t packet_size = ((uint16_t)buffer_data[3])<<8 | (uint16_t)buffer_data[2];
+    if( bytes_data - 4 >= packet_size) {  // 4 bytes for message id and packet size
+      natnet_parse(buffer_data);
+    }
     bytes_data = 0;
   }
 
diff --git a/sw/ground_segment/python/natnet3.x/NatNetClient.py b/sw/ground_segment/python/natnet3.x/NatNetClient.py
new file mode 100644
index 0000000000..c45fcee8d6
--- /dev/null
+++ b/sw/ground_segment/python/natnet3.x/NatNetClient.py
@@ -0,0 +1,510 @@
+#
+# Modified version of the NatNet 3.0 Python Client example from NatNetSDK
+#
+
+import socket
+import struct
+from threading import Thread
+
+# Create structs for reading various object types to speed up parsing.
+Vector3 = struct.Struct( '<fff' )
+Quaternion = struct.Struct( '<ffff' )
+FloatValue = struct.Struct( '<f' )
+DoubleValue = struct.Struct( '<d' )
+
+class NatNetClient:
+    def __init__( self, server="127.0.0.1", multicast="239.255.42.99", commandPort=1510, dataPort=1511, rigidBodyListener=None, newFrameListener=None, verbose=False ):
+        # IP address of the NatNet server.
+        self.serverIPAddress = server
+
+        # This should match the multicast address listed in Motive's streaming settings.
+        self.multicastAddress = multicast
+
+        # NatNet Command channel
+        self.commandPort = commandPort
+        
+        # NatNet Data channel
+        self.dataPort = dataPort
+
+        # Set this to a callback method of your choice to receive per-rigid-body data at each frame.
+        self.rigidBodyListener = rigidBodyListener
+
+        # Set this to a callback method of your choice to receive data at each frame.
+        self.newFrameListener = newFrameListener
+        
+        # NatNet stream version. This will be updated to the actual version the server is using during initialization.
+        self.__natNetStreamVersion = (3,0,0,0)
+
+        # Trace verbose level
+        self.verbose = verbose
+
+        # flag to stop threads
+        self.running = False
+
+    # Client/server message ids
+    NAT_PING                  = 0 
+    NAT_PINGRESPONSE          = 1
+    NAT_REQUEST               = 2
+    NAT_RESPONSE              = 3
+    NAT_REQUEST_MODELDEF      = 4
+    NAT_MODELDEF              = 5
+    NAT_REQUEST_FRAMEOFDATA   = 6
+    NAT_FRAMEOFDATA           = 7
+    NAT_MESSAGESTRING         = 8
+    NAT_DISCONNECT            = 9 
+    NAT_UNRECOGNIZED_REQUEST  = 100
+
+    def __trace( self, *args ):
+        if self.verbose:
+            print( "".join(map(str,args)) )
+
+    # Create a data socket to attach to the NatNet stream
+    def __createDataSocket( self, port ):
+        result = socket.socket( socket.AF_INET,     # Internet
+                              socket.SOCK_DGRAM,
+                              socket.IPPROTO_UDP)    # UDP
+        result.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        result.bind( ('', port) )
+
+        mreq = struct.pack("4sl", socket.inet_aton(self.multicastAddress), socket.INADDR_ANY)
+        result.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
+        return result
+
+    # Create a command socket to attach to the NatNet stream
+    def __createCommandSocket( self ):
+        result = socket.socket( socket.AF_INET, socket.SOCK_DGRAM )
+        result.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+        result.bind( ('', 0) )
+        result.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
+
+        return result
+
+    # Unpack a rigid body object from a data packet
+    def __unpackRigidBody( self, data ):
+        offset = 0
+
+        # ID (4 bytes)
+        id = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "ID:", id )
+
+        # Position and orientation
+        pos = Vector3.unpack( data[offset:offset+12] )
+        offset += 12
+        self.__trace( "\tPosition:", pos[0],",", pos[1],",", pos[2] )
+        rot = Quaternion.unpack( data[offset:offset+16] )
+        offset += 16
+        self.__trace( "\tOrientation:", rot[0],",", rot[1],",", rot[2],",", rot[3] )
+
+        # Send information to any listener.
+        if self.rigidBodyListener is not None:
+            self.rigidBodyListener( id, pos, rot )
+
+        # RB Marker Data ( Before version 3.0.  After Version 3.0 Marker data is in description )
+        if( self.__natNetStreamVersion[0] < 3 ) :
+            # Marker count (4 bytes)
+            markerCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            markerCountRange = range( 0, markerCount )
+            self.__trace( "\tMarker Count:", markerCount )
+
+            # Marker positions
+            for i in markerCountRange:
+                pos = Vector3.unpack( data[offset:offset+12] )
+                offset += 12
+                self.__trace( "\tMarker", i, ":", pos[0],",", pos[1],",", pos[2] )
+
+            if( self.__natNetStreamVersion[0] >= 2 ):
+                # Marker ID's
+                for i in markerCountRange:
+                    id = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                    offset += 4
+                    self.__trace( "\tMarker ID", i, ":", id )
+
+                # Marker sizes
+                for i in markerCountRange:
+                    size = FloatValue.unpack( data[offset:offset+4] )
+                    offset += 4
+                    self.__trace( "\tMarker Size", i, ":", size[0] )
+                    
+        # Skip padding inserted by the server
+        if( self.__natNetStreamVersion[0] > 2 ):
+            offset += 4
+
+        if( self.__natNetStreamVersion[0] >= 2 ):
+            markerError, = FloatValue.unpack( data[offset:offset+4] )
+            offset += 4
+            self.__trace( "\tMarker Error:", markerError )
+
+        # Version 2.6 and later
+        if( ( ( self.__natNetStreamVersion[0] == 2 ) and ( self.__natNetStreamVersion[1] >= 6 ) ) or self.__natNetStreamVersion[0] > 2 or self.__natNetStreamVersion[0] == 0 ):
+            param, = struct.unpack( 'h', data[offset:offset+2] )
+            trackingValid = ( param & 0x01 ) != 0
+            offset += 2
+            self.__trace( "\tTracking Valid:", 'True' if trackingValid else 'False' )
+
+        return offset
+
+    # Unpack a skeleton object from a data packet
+    def __unpackSkeleton( self, data ):
+        offset = 0
+        
+        id = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "ID:", id )
+        
+        rigidBodyCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "Rigid Body Count:", rigidBodyCount )
+        for j in range( 0, rigidBodyCount ):
+            offset += self.__unpackRigidBody( data[offset:] )
+
+        return offset
+
+    # Unpack data from a motion capture frame message
+    def __unpackMocapData( self, data ):
+        self.__trace( "Begin MoCap Frame\n-----------------\n" )
+
+        data = memoryview( data )
+        offset = 0
+        
+        # Frame number (4 bytes)
+        frameNumber = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "Frame #:", frameNumber )
+
+        # Marker set count (4 bytes)
+        markerSetCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "Marker Set Count:", markerSetCount )
+
+        for i in range( 0, markerSetCount ):
+            # Model name
+            modelName, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+            offset += len( modelName ) + 1
+            self.__trace( "Model Name:", modelName.decode( 'utf-8' ) )
+
+            # Marker count (4 bytes)
+            markerCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            self.__trace( "Marker Count:", markerCount )
+
+            for j in range( 0, markerCount ):
+                pos = Vector3.unpack( data[offset:offset+12] )
+                offset += 12
+                #self.__trace( "\tMarker", j, ":", pos[0],",", pos[1],",", pos[2] )
+                 
+        # Unlabeled markers count (4 bytes)
+        unlabeledMarkersCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "Unlabeled Markers Count:", unlabeledMarkersCount )
+
+        for i in range( 0, unlabeledMarkersCount ):
+            pos = Vector3.unpack( data[offset:offset+12] )
+            offset += 12
+            self.__trace( "\tMarker", i, ":", pos[0],",", pos[1],",", pos[2] )
+
+        # Rigid body count (4 bytes)
+        rigidBodyCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        self.__trace( "Rigid Body Count:", rigidBodyCount )
+
+        for i in range( 0, rigidBodyCount ):
+            offset += self.__unpackRigidBody( data[offset:] )
+
+        # Version 2.1 and later
+        skeletonCount = 0
+        if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] > 0 ) or self.__natNetStreamVersion[0] > 2 ):
+            skeletonCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            self.__trace( "Skeleton Count:", skeletonCount )
+            for i in range( 0, skeletonCount ):
+                offset += self.__unpackSkeleton( data[offset:] )
+
+        # Labeled markers (Version 2.3 and later)
+        labeledMarkerCount = 0
+        if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] > 3 ) or self.__natNetStreamVersion[0] > 2 ):
+            labeledMarkerCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            self.__trace( "Labeled Marker Count:", labeledMarkerCount )
+            for i in range( 0, labeledMarkerCount ):
+                id = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                offset += 4
+                pos = Vector3.unpack( data[offset:offset+12] )
+                offset += 12
+                size = FloatValue.unpack( data[offset:offset+4] )
+                offset += 4
+
+                # Version 2.6 and later
+                if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 6 ) or self.__natNetStreamVersion[0] > 2 or self.__natNetStreamVersion[0] == 0 ):
+                    param, = struct.unpack( 'h', data[offset:offset+2] )
+                    offset += 2
+                    occluded = ( param & 0x01 ) != 0
+                    pointCloudSolved = ( param & 0x02 ) != 0
+                    modelSolved = ( param & 0x04 ) != 0
+
+                # Version 3.0 and later
+                if( self.__natNetStreamVersion[0] >= 3 or self.__natNetStreamVersion[0] == 0 ):
+                    residual, = FloatValue.unpack( data[offset:offset+4] )
+                    offset += 4
+                    self.__trace( "Residual:", residual )
+
+        # Force Plate data (version 2.9 and later)
+        if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 9 ) or self.__natNetStreamVersion[0] > 2 ):
+            forcePlateCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            self.__trace( "Force Plate Count:", forcePlateCount )
+            for i in range( 0, forcePlateCount ):
+                # ID
+                forcePlateID = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                offset += 4
+                self.__trace( "Force Plate", i, ":", forcePlateID )
+
+                # Channel Count
+                forcePlateChannelCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                offset += 4
+
+                # Channel Data
+                for j in range( 0, forcePlateChannelCount ):
+                    self.__trace( "\tChannel", j, ":", forcePlateID )
+                    forcePlateChannelFrameCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                    offset += 4
+                    for k in range( 0, forcePlateChannelFrameCount ):
+                        forcePlateChannelVal = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                        offset += 4
+                        self.__trace( "\t\t", forcePlateChannelVal )
+
+        # Device data (version 2.11 and later)
+        if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 11 ) or self.__natNetStreamVersion[0] > 2 ):
+            deviceCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            self.__trace( "Device Count:", deviceCount )
+            for i in range( 0, deviceCount ):
+                # ID
+                deviceID = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                offset += 4
+                self.__trace( "Device", i, ":", deviceID )
+
+                # Channel Count
+                deviceChannelCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                offset += 4
+
+                # Channel Data
+                for j in range( 0, deviceChannelCount ):
+                    self.__trace( "\tChannel", j, ":", deviceID )
+                    deviceChannelFrameCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                    offset += 4
+                    for k in range( 0, deviceChannelFrameCount ):
+                        deviceChannelVal = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                        offset += 4
+                        self.__trace( "\t\t", deviceChannelVal )
+						       
+        # Timecode            
+        timecode = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+        timecodeSub = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        # Timestamp (increased to double precision in 2.7 and later)
+        if( ( self.__natNetStreamVersion[0] == 2 and self.__natNetStreamVersion[1] >= 7 ) or self.__natNetStreamVersion[0] > 2 ):
+            timestamp, = DoubleValue.unpack( data[offset:offset+8] )
+            offset += 8
+        else:
+            timestamp, = FloatValue.unpack( data[offset:offset+4] )
+            offset += 4
+
+        # Hires Timestamp (Version 3.0 and later)
+        if( self.__natNetStreamVersion[0] >= 3 or self.__natNetStreamVersion[0] == 0 ):
+            stampCameraExposure = int.from_bytes( data[offset:offset+8], byteorder='little' )
+            offset += 8
+            stampDataReceived = int.from_bytes( data[offset:offset+8], byteorder='little' )
+            offset += 8
+            stampTransmit = int.from_bytes( data[offset:offset+8], byteorder='little' )
+            offset += 8
+
+        # Frame parameters
+        param, = struct.unpack( 'h', data[offset:offset+2] )
+        isRecording = ( param & 0x01 ) != 0
+        trackedModelsChanged = ( param & 0x02 ) != 0
+        offset += 2
+
+        # Send information to any listener.
+        if self.newFrameListener is not None:
+            self.newFrameListener( frameNumber, markerSetCount, unlabeledMarkersCount, rigidBodyCount, skeletonCount,
+                                  labeledMarkerCount, timecode, timecodeSub, timestamp, isRecording, trackedModelsChanged )
+
+    # Unpack a marker set description packet
+    def __unpackMarkerSetDescription( self, data ):
+        offset = 0
+
+        name, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+        offset += len( name ) + 1
+        self.__trace( "Markerset Name:", name.decode( 'utf-8' ) )
+        
+        markerCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        for i in range( 0, markerCount ):
+            name, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+            offset += len( name ) + 1
+            self.__trace( "\tMarker Name:", name.decode( 'utf-8' ) )
+        
+        return offset
+
+    # Unpack a rigid body description packet
+    def __unpackRigidBodyDescription( self, data ):
+        offset = 0
+
+        # Version 2.0 or higher
+        if( self.__natNetStreamVersion[0] >= 2 ):
+            name, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+            offset += len( name ) + 1
+            self.__trace( "\tMarker Name:", name.decode( 'utf-8' ) )
+
+        id = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        parentID = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        timestamp = Vector3.unpack( data[offset:offset+12] )
+        offset += 12
+        
+        return offset
+
+    # Unpack a skeleton description packet
+    def __unpackSkeletonDescription( self, data ):
+        offset = 0
+
+        name, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+        offset += len( name ) + 1
+        self.__trace( "\tMarker Name:", name.decode( 'utf-8' ) )
+        
+        id = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        rigidBodyCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        for i in range( 0, rigidBodyCount ):
+            offset += self.__unpackRigidBodyDescription( data[offset:] )
+
+        return offset
+
+    # Unpack a data description packet
+    def __unpackDataDescriptions( self, data ):
+        offset = 0
+        datasetCount = int.from_bytes( data[offset:offset+4], byteorder='little' )
+        offset += 4
+
+        for i in range( 0, datasetCount ):
+            type = int.from_bytes( data[offset:offset+4], byteorder='little' )
+            offset += 4
+            if( type == 0 ):
+                offset += self.__unpackMarkerSetDescription( data[offset:] )
+            elif( type == 1 ):
+                offset += self.__unpackRigidBodyDescription( data[offset:] )
+            elif( type == 2 ):
+                offset += self.__unpackSkeletonDescription( data[offset:] )
+            
+    def __dataThreadFunction( self, sock ):
+        sock.settimeout(0.5)
+        while self.running:
+            # Block for input
+            try:
+                data, addr = sock.recvfrom( 32768 ) # 32k byte buffer size
+                if( len( data ) >= 4):
+                    self.__processMessage( data )
+            except socket.timeout:
+                pass
+
+    def __processMessage( self, data ):
+        self.__trace( "Begin Packet\n------------\n" )
+
+        messageID = int.from_bytes( data[0:2], byteorder='little' )
+        self.__trace( "Message ID:", messageID )
+        
+        packetSize = int.from_bytes( data[2:4], byteorder='little' )
+        self.__trace( "Packet Size:", packetSize )
+        
+        if not len( data ) - 4 >= packetSize:
+          # Not enough data
+          return
+
+        offset = 4
+        if( messageID == self.NAT_FRAMEOFDATA ):
+            self.__unpackMocapData( data[offset:] )
+        elif( messageID == self.NAT_MODELDEF ):
+            self.__unpackDataDescriptions( data[offset:] )
+        elif( messageID == self.NAT_PINGRESPONSE ):
+            offset += 256   # Skip the sending app's Name field
+            offset += 4     # Skip the sending app's Version info
+            self.__natNetStreamVersion = struct.unpack( 'BBBB', data[offset:offset+4] )
+            offset += 4
+        elif( messageID == self.NAT_RESPONSE ):
+            if( packetSize == 4 ):
+                commandResponse = int.from_bytes( data[offset:offset+4], byteorder='little' )
+                offset += 4
+            else:
+                message, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+                offset += len( message ) + 1
+                self.__trace( "Command response:", message.decode( 'utf-8' ) )
+        elif( messageID == self.NAT_UNRECOGNIZED_REQUEST ):
+            self.__trace( "Received 'Unrecognized request' from server" )
+        elif( messageID == self.NAT_MESSAGESTRING ):
+            message, separator, remainder = bytes(data[offset:]).partition( b'\0' )
+            offset += len( message ) + 1
+            self.__trace( "Received message from server:", message.decode( 'utf-8' ) )
+        else:
+            self.__trace( "ERROR: Unrecognized packet type" )
+            
+        self.__trace( "End Packet\n----------\n" )
+
+    def sendCommand( self, command, commandStr, socket, address ):
+        # Compose the message in our known message format
+        if( command == self.NAT_REQUEST_MODELDEF or command == self.NAT_REQUEST_FRAMEOFDATA ):
+            packetSize = 0
+            commandStr = ""
+        elif( command == self.NAT_REQUEST ):
+            packetSize = len( commandStr ) + 1
+        elif( command == self.NAT_PING ):
+            commandStr = "Ping"
+            packetSize = len( commandStr ) + 1
+
+        data = command.to_bytes( 2, byteorder='little' )
+        data += packetSize.to_bytes( 2, byteorder='little' )
+        
+        data += commandStr.encode( 'utf-8' )
+        data += b'\0'
+
+        socket.sendto( data, address )
+        
+    def run( self ):
+        # Set running flag to True
+        self.running = True
+
+        # Create the data socket
+        self.dataSocket = self.__createDataSocket( self.dataPort )
+        if( self.dataSocket is None ):
+            print( "Could not open data channel" )
+            exit
+
+        # Create the command socket
+        self.commandSocket = self.__createCommandSocket()
+        if( self.commandSocket is None ):
+            print( "Could not open command channel" )
+            exit
+
+        # Create a separate thread for receiving data packets
+        dataThread = Thread( target = self.__dataThreadFunction, args = (self.dataSocket, ))
+        dataThread.start()
+
+        # Create a separate thread for receiving command packets
+        commandThread = Thread( target = self.__dataThreadFunction, args = (self.commandSocket, ))
+        commandThread.start()
+
+        self.sendCommand( self.NAT_REQUEST_MODELDEF, "", self.commandSocket, (self.serverIPAddress, self.commandPort) )
+
+    def stop( self ):
+        self.running = False
+
diff --git a/sw/ground_segment/python/natnet3.x/natnet2ivy.py b/sw/ground_segment/python/natnet3.x/natnet2ivy.py
new file mode 100755
index 0000000000..ee239d2964
--- /dev/null
+++ b/sw/ground_segment/python/natnet3.x/natnet2ivy.py
@@ -0,0 +1,177 @@
+#!/usr/bin/env python3
+#
+# Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+#
+# 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
+# <http://www.gnu.org/licenses/>.
+#
+
+'''
+Forward rigid body position from NatNet (Optitrack positioning system)
+to the IVY bus as a REMOTE_GPS_LOCAL message
+
+As the NatNetClient is only compatible with Python 3.x, the Ivy python
+should be installed for this version, eventually by hand as paparazzi
+packages are only providing an install for Python 2.x (although the
+source code itself is compatile for both version)
+
+Manual installation of Ivy:
+    1. git clone https://gitlab.com/ivybus/ivy-python.git
+    2. cd ivy-python
+    3. sudo python3 setup.py install
+Otherwise, you can use PYTHONPATH if you don't want to install the code
+in your system
+
+'''
+
+
+from __future__ import print_function
+
+import sys
+from os import path, getenv
+from time import time, sleep
+import numpy as np
+from collections import deque
+import argparse
+
+# import NatNet client
+from NatNetClient import NatNetClient
+
+# if PAPARAZZI_HOME not set, then assume the tree containing this
+# file is a reasonable substitute
+PPRZ_HOME = getenv("PAPARAZZI_HOME", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
+sys.path.append(PPRZ_HOME + "/var/lib/python")
+sys.path.append(PPRZ_HOME + "/var/lib/python/pprzlink") # seems needed for messages_xml_map file
+from pprzlink.ivy import IvyMessagesInterface
+from pprzlink.message import PprzMessage
+
+# parse args
+parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+parser.add_argument('-ac', action='append', nargs=2,
+                    metavar=('rigid_id','ac_id'), help='pair of rigid body and A/C id (multiple possible)')
+parser.add_argument('-b', '--ivy_bus', dest='ivy_bus', help="Ivy bus address and port")
+parser.add_argument('-s', '--server', dest='server', default="127.0.0.1", help="NatNet server IP address")
+parser.add_argument('-m', '--multicast_addr', dest='multicast', default="239.255.42.99", help="NatNet server multicast address")
+parser.add_argument('-dp', '--data_port', dest='data_port', type=int, default=1511, help="NatNet server data socket UDP port")
+parser.add_argument('-cp', '--command_port', dest='command_port', type=int, default=1510, help="NatNet server command socket UDP port")
+parser.add_argument('-v', '--verbose', dest='verbose', action='store_true', help="display debug messages")
+parser.add_argument('-f', '--freq', dest='freq', default=10, type=int, help="transmit frequency")
+parser.add_argument('-vs', '--vel_samples', dest='vel_samples', default=4, type=int, help="amount of samples to compute velocity (should be greater than 2)")
+args = parser.parse_args()
+
+if args.ac is None:
+    print("At least one pair of rigid boby / AC id must be declared")
+    exit()
+
+# dictionary of ID associations
+id_dict = dict(args.ac)
+
+# initial time per AC
+timestamp = dict([(ac_id, time()) for ac_id in id_dict.keys()])
+period = 1. / args.freq
+
+# initial track per AC
+track = dict([(ac_id, deque()) for ac_id in id_dict.keys()])
+
+# start ivy interface
+if args.ivy_bus is not None:
+    ivy = IvyMessagesInterface("natnet2ivy", ivy_bus=args.ivy_bus)
+else:
+    ivy = IvyMessagesInterface("natnet2ivy")
+
+# store track function
+def store_track(ac_id, pos, t):
+    if ac_id in id_dict.keys():
+        track[ac_id].append((pos, t))
+        if len(track[ac_id]) > args.vel_samples:
+            track[ac_id].popleft()
+
+# compute velocity from track
+# returns zero if not enough samples
+def compute_velocity(ac_id):
+    vel = [ 0., 0., 0. ]
+    if len(track[ac_id]) >= args.vel_samples:
+        nb = -1
+        for (p2, t2) in track[ac_id]:
+            nb = nb + 1
+            if nb == 0:
+                p1 = p2
+                t1 = t2
+            else:
+                dt = t2 - t1
+                vel[0] = (p2[0] - p1[0]) / dt
+                vel[1] = (p2[1] - p1[1]) / dt
+                vel[2] = (p2[2] - p1[2]) / dt
+                p1 = p2
+                t1 = t2
+        if nb > 0:
+            vel[0] / nb
+            vel[1] / nb
+            vel[2] / nb
+    return vel
+
+
+# This is a callback function that gets connected to the NatNet client. It is called once per rigid body per frame
+def receiveRigidBodyFrame( ac_id, pos, quat ):
+    t = time()
+    i = str(ac_id)
+    if i not in id_dict.keys():
+      return
+
+    store_track(i, pos, t)
+    if abs(t - timestamp[i]) < period:
+        return # too early for next message
+    timestamp[i] = t
+
+    msg = PprzMessage("datalink", "REMOTE_GPS_LOCAL")
+    msg['ac_id'] = id_dict[i]
+    msg['pad'] = 0
+    msg['enu_x'] = pos[0]
+    msg['enu_y'] = pos[1]
+    msg['enu_z'] = pos[2]
+    vel = compute_velocity(i)
+    msg['enu_xd'] = vel[0]
+    msg['enu_yd'] = vel[1]
+    msg['enu_zd'] = vel[2]
+    msg['tow'] = int(timestamp[i]) # TODO convert to GPS itow ?
+    # convert quaternion to psi euler angle
+    dcm_0_0 = 1.0 - 2.0 * (quat[1] * quat[1] + quat[2] * quat[2])
+    dcm_1_0 = 2.0 * (quat[0] * quat[1] - quat[3] * quat[2])
+    msg['course'] = 180. * np.arctan2(dcm_1_0, dcm_0_0) / 3.14
+    ivy.send(msg)
+
+
+# start natnet interface
+natnet = NatNetClient(
+        server=args.server,
+        rigidBodyListener=receiveRigidBodyFrame,
+        dataPort=args.data_port,
+        commandPort=args.command_port,
+        verbose=args.verbose)
+
+
+print("Starting Natnet3.x to Ivy interface at %s" % (args.server))
+try:
+    # Start up the streaming client.
+    # This will run perpetually, and operate on a separate thread.
+    natnet.run()
+    while True:
+        sleep(1)
+except (KeyboardInterrupt, SystemExit):
+    print("Shutting down ivy and natnet interfaces...")
+    natnet.stop()
+    ivy.shutdown()
+