[natnet3] only send valid rigid bodies

paparazzi · May 11, 2018 · 5a00206 · 5a00206
1 parent c108f70
commit 5a00206
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/sw/ground_segment/python/natnet3.x/NatNetClient.py b/sw/ground_segment/python/natnet3.x/NatNetClient.py
@@ -100,9 +100,6 @@ def __unpackRigidBody( self, data ):
         offset += 16
         self.__trace( "\tOrientation:", rot[0],",", rot[1],",", rot[2],",", rot[3] )
 
-        # Store data
-        self.rigidBodyList.append((id, pos, rot))
-
         # Send information to any listener.
         if self.rigidBodyListener is not None:
             self.rigidBodyListener( id, pos, rot )
@@ -140,12 +137,16 @@ def __unpackRigidBody( self, data ):
             self.__trace( "\tMarker Error:", markerError )
 
         # Version 2.6 and later
+        trackingValid = True # set valid by default
         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' )
 
+        # Store data
+        self.rigidBodyList.append((id, pos, rot, trackingValid))
+
         return offset
 
     # Unpack a skeleton object from a data packet

diff --git a/sw/ground_segment/python/natnet3.x/natnet2ivy.py b/sw/ground_segment/python/natnet3.x/natnet2ivy.py
@@ -126,7 +126,11 @@ def compute_velocity(ac_id):
     return vel
 
 def receiveRigidBodyList( rigidBodyList, stamp ):
-    for (ac_id, pos, quat) in rigidBodyList:
+    for (ac_id, pos, quat, valid) in rigidBodyList:
+        if not valid:
+            # skip if rigid body is not valid
+            continue
+
         i = str(ac_id)
         if i not in id_dict.keys():
             continue