diff --git a/.gitignore b/.gitignore index 69730bd541..de78b4a36c 100644 --- a/.gitignore +++ b/.gitignore @@ -147,6 +147,7 @@ # /sw/ground_segment/misc /sw/ground_segment/misc/davis2ivy /sw/ground_segment/misc/kestrel2ivy +/sw/ground_segment/misc/natnet2ivy # /sw/airborne/arch/lpc21/test/bootloader diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml index cb761f8b7b..f8956a1ca9 100644 --- a/conf/control_panel_example.xml +++ b/conf/control_panel_example.xml @@ -41,11 +41,11 @@ - + - + - + @@ -78,7 +78,9 @@ - + + + diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile index b703963916..4cb79de65e 100644 --- a/sw/ground_segment/misc/Makefile +++ b/sw/ground_segment/misc/Makefile @@ -25,6 +25,7 @@ Q=@ CC = gcc +PAPARAZZI_SRC=../../.. UNAME = $(shell uname -s) ifeq ("$(UNAME)","Darwin") @@ -34,11 +35,14 @@ else LIBRARYS = -s endif +# Optitrack specific librarys and includes +NATNET_LIBRARYS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm +INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ -all: davis2ivy kestrel2ivy +all: davis2ivy kestrel2ivy natnet2ivy clean: - $(Q)rm -f *.o davis2ivy kestrel2ivy + $(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy davis2ivy: davis2ivy.o $(Q)$(CC) -o davis2ivy davis2ivy.o $(LIBRARYS) -livy @@ -46,6 +50,9 @@ davis2ivy: davis2ivy.o kestrel2ivy: kestrel2ivy.o $(Q)$(CC) -o kestrel2ivy kestrel2ivy.o $(LIBRARYS) -livy +natnet2ivy: natnet2ivy.o $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.o $(PAPARAZZI_SRC)/sw/airborne/fms/fms_network.o + $(Q)$(CC) -o natnet2ivy natnet2ivy.o pprz_geodetic_double.o fms_network.o $(LIBRARYS) $(NATNET_LIBRARYS) -livy + %.o : %.c $(Q)$(CC) -c -O2 -Wall $(INCLUDES) $< diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c new file mode 100644 index 0000000000..a5709070e4 --- /dev/null +++ b/sw/ground_segment/misc/natnet2ivy.c @@ -0,0 +1,726 @@ +/* + * Copyright (C) 2014 Freek van Tienen + * + * 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, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + + /** \file natnet2ivy.c + * \brief NatNet (GPS) to ivy forwarder + * + * This receives aircraft position information through the Optitrack system + * NatNet UDP stream and forwards it to the ivy bus. An aircraft with the gps + * subsystem "datalink" is then able to parse the GPS position and use it to + * navigate inside the Optitrack system. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "std.h" +#include "fms/fms_network.h" +#include "math/pprz_geodetic_double.h" +#include "math/pprz_algebra_double.h" + +/** Debugging options */ +uint8_t verbose = 0; +#define printf_natnet if(verbose > 1) printf +#define printf_debug if(verbose > 0) printf + +/** NatNet defaults */ +char *natnet_addr = "255.255.255.255"; +char *natnet_multicast_addr = "239.255.42.99"; +uint16_t natnet_cmd_port = 1510; +uint16_t natnet_data_port = 1511; +uint8_t natnet_major = 2; +uint8_t natnet_minor = 5; + +/** Ivy Bus default */ +#ifdef __APPLE__ +char *ivy_bus = "224.255.255.255"; +#else +char *ivy_bus = "127.255.255.255:2010"; +#endif + +/** Sample frequency and derevitive defaults */ +uint32_t freq_transmit = 30; ///< Transmitting frequency in Hz +uint16_t min_velocity_samples = 4; ///< The amount of position samples needed for a valid velocity + +/** NatNet parsing defines */ +#define MAX_PACKETSIZE 100000 +#define MAX_NAMELENGTH 256 +#define MAX_RIGIDBODIES 128 + +#define NAT_PING 0 +#define NAT_PINGRESPONSE 1 +#define NAT_REQUEST 2 +#define NAT_RESPONSE 3 +#define NAT_REQUEST_MODELDEF 4 +#define NAT_MODELDEF 5 +#define NAT_REQUEST_FRAMEOFDATA 6 +#define NAT_FRAMEOFDATA 7 +#define NAT_MESSAGESTRING 8 +#define NAT_UNRECOGNIZED_REQUEST 100 +#define UNDEFINED 999999.9999 + +/** Tracked rigid bodies */ +struct RigidBody { + int id; ///< Rigid body ID from the tracking system + float x, y, z; ///< Rigid body x, y and z coordinates in meters (note y and z are swapped) + float qx, qy, qz, qw; ///< Rigid body qx, qy, qz and qw rotations (note qy and qz are swapped) + int nMarkers; ///< Number of markers inside the rigid body (both visible and not visible) + float error; ///< Error of the position in cm + int nSamples; ///< Number of samples since last transmit + bool posSampled; ///< If the position is sampled last sampling + + double vel_x, vel_y, vel_z; ///< Sum of the (last_vel_* - current_vel_*) during nVelocitySamples + struct EcefCoor_d ecef_vel; ///< Last valid ECEF velocity in meters + int nVelocitySamples; ///< Number of velocity samples gathered + int totalVelocitySamples; ///< Total amount of velocity samples possible + int nVelocityTransmit; ///< Amount of transmits since last valid velocity transmit +}; +struct RigidBody rigidBodies[MAX_RIGIDBODIES]; ///< All rigid bodies which are tracked +uint8_t ac_ids[MAX_RIGIDBODIES]; ///< Mapping from rigid body ID to aircraft ID + +/** Natnet socket connections */ +struct FmsNetwork *natnet_data, *natnet_cmd; + +/** Tracking location LTP and angle offset from north */ +struct LtpDef_d tracking_ltp; ///< The tracking system LTP definition +double tracking_offset_angle; ///< The offset from the tracking system to the North in degrees + +/** Save the latency from natnet */ +float natnet_latency; + +/** Parse the packet from NatNet */ +void natnet_parse(unsigned char *in) { + int i,j,k; + + // Create a pointer to go trough the packet + char *ptr = (char *)in; + printf_natnet("Begin Packet\n-------\n"); + + // Message ID + int MessageID = 0; + memcpy(&MessageID, ptr, 2); ptr += 2; + printf_natnet("Message ID : %d\n", MessageID); + + // Packet size + int nBytes = 0; + memcpy(&nBytes, ptr, 2); ptr += 2; + printf_natnet("Byte count : %d\n", nBytes); + + if(MessageID == NAT_FRAMEOFDATA) // FRAME OF MOCAP DATA packet + { + // Frame number + int frameNumber = 0; memcpy(&frameNumber, ptr, 4); ptr += 4; + printf_natnet("Frame # : %d\n", frameNumber); + + // ========== MARKERSETS ========== + // Number of data sets (markersets, rigidbodies, etc) + int nMarkerSets = 0; memcpy(&nMarkerSets, ptr, 4); ptr += 4; + printf_natnet("Marker Set Count : %d\n", nMarkerSets); + + for (i=0; i < nMarkerSets; i++) + { + // Markerset name + char szName[256]; + strcpy(szName, ptr); + int nDataBytes = (int) strlen(szName) + 1; + ptr += nDataBytes; + printf_natnet("Model Name: %s\n", szName); + + // marker data + int nMarkers = 0; memcpy(&nMarkers, ptr, 4); ptr += 4; + printf_natnet("Marker Count : %d\n", nMarkers); + + for(j=0; j < nMarkers; j++) + { + float x = 0; memcpy(&x, ptr, 4); ptr += 4; + float y = 0; memcpy(&y, ptr, 4); ptr += 4; + float z = 0; memcpy(&z, ptr, 4); ptr += 4; + printf_natnet("\tMarker %d : [x=%3.2f,y=%3.2f,z=%3.2f]\n",j,x,y,z); + } + } + + // Unidentified markers + int nOtherMarkers = 0; memcpy(&nOtherMarkers, ptr, 4); ptr += 4; + printf_natnet("Unidentified Marker Count : %d\n", nOtherMarkers); + for(j=0; j < nOtherMarkers; j++) + { + float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; + float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; + float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; + printf_natnet("\tMarker %d : pos = [%3.2f,%3.2f,%3.2f]\n",j,x,y,z); + } + + // ========== RIGID BODIES ========== + // Rigid bodies + int nRigidBodies = 0; + memcpy(&nRigidBodies, ptr, 4); ptr += 4; + printf_natnet("Rigid Body Count : %d\n", nRigidBodies); + + // Check if there ie enough space for the rigid bodies + if(nRigidBodies > MAX_RIGIDBODIES) { + fprintf(stderr, "Could not sample all the rigid bodies because the amount of rigid bodies is bigger then %d (MAX_RIGIDBODIES).\r\n", MAX_RIGIDBODIES); + exit(EXIT_FAILURE); + } + + for (j=0; j < nRigidBodies; j++) + { + // rigid body pos/ori + struct RigidBody old_rigid; + memcpy(&old_rigid, &rigidBodies[j], sizeof(struct RigidBody)); + + memcpy(&rigidBodies[j].id, ptr, 4); ptr += 4; + memcpy(&rigidBodies[j].x, ptr, 4); ptr += 4; //x --> X + memcpy(&rigidBodies[j].z, ptr, 4); ptr += 4; //y --> Z + memcpy(&rigidBodies[j].y, ptr, 4); ptr += 4; //z --> Y + memcpy(&rigidBodies[j].qx, ptr, 4); ptr += 4; //qx --> QX + memcpy(&rigidBodies[j].qz, ptr, 4); ptr += 4; //qy --> QZ + memcpy(&rigidBodies[j].qy, ptr, 4); ptr += 4; //qz --> QY + memcpy(&rigidBodies[j].qw, ptr, 4); ptr += 4; //qw --> QW + printf_natnet("ID (%d) : %d\n", j, rigidBodies[j].id); + printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].x,rigidBodies[j].y,rigidBodies[j].z); + printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", rigidBodies[j].qx,rigidBodies[j].qy,rigidBodies[j].qz,rigidBodies[j].qw); + + // Differentiate the position to get the speed (TODO: crossreference with labeled markers for occlussion) + rigidBodies[j].totalVelocitySamples++; + if(old_rigid.x != rigidBodies[j].x || old_rigid.y != rigidBodies[j].y || old_rigid.z != rigidBodies[j].z + || old_rigid.qx != rigidBodies[j].qx || old_rigid.qy != rigidBodies[j].qy || old_rigid.qz != rigidBodies[j].qz || old_rigid.qw != rigidBodies[j].qw) { + + if(old_rigid.posSampled) { + rigidBodies[j].vel_x += (rigidBodies[j].x - old_rigid.x); + rigidBodies[j].vel_y += (rigidBodies[j].y - old_rigid.y); + rigidBodies[j].vel_z += (rigidBodies[j].z - old_rigid.z); + rigidBodies[j].nVelocitySamples++; + } + + rigidBodies[j].nSamples++; + rigidBodies[j].posSampled = TRUE; + } + else + rigidBodies[j].posSampled = FALSE; + + // When marker id changed, reset the velocity + if(old_rigid.id != rigidBodies[j].id) { + rigidBodies[j].vel_x = 0; + rigidBodies[j].vel_y = 0; + rigidBodies[j].vel_z = 0; + rigidBodies[j].nSamples = 0; + rigidBodies[j].nVelocitySamples = 0; + rigidBodies[j].totalVelocitySamples = 0; + rigidBodies[j].posSampled = FALSE; + } + + // Associated marker positions + memcpy(&rigidBodies[j].nMarkers, ptr, 4); ptr += 4; + printf_natnet("Marker Count: %d\n", rigidBodies[j].nMarkers); + int nBytes = rigidBodies[j].nMarkers*3*sizeof(float); + float* markerData = (float*)malloc(nBytes); + memcpy(markerData, ptr, nBytes); + ptr += nBytes; + + if(natnet_major >= 2) + { + // Associated marker IDs + nBytes = rigidBodies[j].nMarkers*sizeof(int); + int* markerIDs = (int*)malloc(nBytes); + memcpy(markerIDs, ptr, nBytes); + ptr += nBytes; + + // Associated marker sizes + nBytes = rigidBodies[j].nMarkers*sizeof(float); + float* markerSizes = (float*)malloc(nBytes); + memcpy(markerSizes, ptr, nBytes); + ptr += nBytes; + + for(k=0; k < rigidBodies[j].nMarkers; k++) + { + printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } + + if(markerIDs) + free(markerIDs); + if(markerSizes) + free(markerSizes); + + } + else + { + for(k=0; k < rigidBodies[j].nMarkers; k++) + { + printf_natnet("\tMarker %d: pos = [%3.2f,%3.2f,%3.2f]\n", k, markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } + } + if(markerData) + free(markerData); + + if(natnet_major >= 2) + { + // Mean marker error + memcpy(&rigidBodies[j].error, ptr, 4); ptr += 4; + printf_natnet("Mean marker error: %3.8f\n", rigidBodies[j].error); + } + } // next rigid body + + // ========== SKELETONS ========== + // Skeletons (version 2.1 and later) + if(((natnet_major == 2) && (natnet_minor>0)) || (natnet_major>2)) + { + int nSkeletons = 0; + memcpy(&nSkeletons, ptr, 4); ptr += 4; + printf_natnet("Skeleton Count : %d\n", nSkeletons); + for (j=0; j < nSkeletons; j++) + { + // Skeleton id + int skeletonID = 0; + memcpy(&skeletonID, ptr, 4); ptr += 4; + // # of rigid bodies (bones) in skeleton + int nRigidBodies = 0; + memcpy(&nRigidBodies, ptr, 4); ptr += 4; + printf_natnet("Rigid Body Count : %d\n", nRigidBodies); + for (j=0; j < nRigidBodies; j++) + { + // Rigid body pos/ori + int ID = 0; memcpy(&ID, ptr, 4); ptr += 4; + float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; + float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; + float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; + float qx = 0; memcpy(&qx, ptr, 4); ptr += 4; + float qy = 0; memcpy(&qy, ptr, 4); ptr += 4; + float qz = 0; memcpy(&qz, ptr, 4); ptr += 4; + float qw = 0; memcpy(&qw, ptr, 4); ptr += 4; + printf_natnet("ID : %d\n", ID); + printf_natnet("pos: [%3.2f,%3.2f,%3.2f]\n", x,y,z); + printf_natnet("ori: [%3.2f,%3.2f,%3.2f,%3.2f]\n", qx,qy,qz,qw); + + // Sssociated marker positions + int nRigidMarkers = 0; memcpy(&nRigidMarkers, ptr, 4); ptr += 4; + printf_natnet("Marker Count: %d\n", nRigidMarkers); + int nBytes = nRigidMarkers*3*sizeof(float); + float* markerData = (float*)malloc(nBytes); + memcpy(markerData, ptr, nBytes); + ptr += nBytes; + + // Associated marker IDs + nBytes = nRigidMarkers*sizeof(int); + int* markerIDs = (int*)malloc(nBytes); + memcpy(markerIDs, ptr, nBytes); + ptr += nBytes; + + // Associated marker sizes + nBytes = nRigidMarkers*sizeof(float); + float* markerSizes = (float*)malloc(nBytes); + memcpy(markerSizes, ptr, nBytes); + ptr += nBytes; + + for(k=0; k < nRigidMarkers; k++) + { + printf_natnet("\tMarker %d: id=%d\tsize=%3.1f\tpos=[%3.2f,%3.2f,%3.2f]\n", k, markerIDs[k], markerSizes[k], markerData[k*3], markerData[k*3+1],markerData[k*3+2]); + } + + // Mean marker error + float fError = 0.0f; memcpy(&fError, ptr, 4); ptr += 4; + printf_natnet("Mean marker error: %3.2f\n", fError); + + // Release resources + if(markerIDs) + free(markerIDs); + if(markerSizes) + free(markerSizes); + if(markerData) + free(markerData); + } // next rigid body + } // next skeleton + } + + // ========== LABELED MARKERS ========== + // Labeled markers (version 2.3 and later) + if( ((natnet_major == 2)&&(natnet_minor>=3)) || (natnet_major>2)) + { + int nLabeledMarkers = 0; + memcpy(&nLabeledMarkers, ptr, 4); ptr += 4; + printf_natnet("Labeled Marker Count : %d\n", nLabeledMarkers); + for (j=0; j < nLabeledMarkers; j++) + { + int ID = 0; memcpy(&ID, ptr, 4); ptr += 4; + float x = 0.0f; memcpy(&x, ptr, 4); ptr += 4; + float y = 0.0f; memcpy(&y, ptr, 4); ptr += 4; + float z = 0.0f; memcpy(&z, ptr, 4); ptr += 4; + float size = 0.0f; memcpy(&size, ptr, 4); ptr += 4; + + printf_natnet("ID : %d\n", ID); + printf_natnet("pos : [%3.2f,%3.2f,%3.2f]\n", x,y,z); + printf_natnet("size: [%3.2f]\n", size); + } + } + + // Latency + natnet_latency = 0.0f; memcpy(&natnet_latency, ptr, 4); ptr += 4; + printf_natnet("latency : %3.3f\n", natnet_latency); + + // Timecode + unsigned int timecode = 0; memcpy(&timecode, ptr, 4); ptr += 4; + unsigned int timecodeSub = 0; memcpy(&timecodeSub, ptr, 4); ptr += 4; + printf_natnet("timecode : %d %d", timecode, timecodeSub); + + // End of data tag + int eod = 0; memcpy(&eod, ptr, 4); ptr += 4; + printf_natnet("End Packet\n-------------\n"); + } + else + { + printf("Error: Unrecognized packet type from Optitrack NatNet.\n"); + } +} + +/** The transmitter periodic function */ +gboolean timeout_transmit_callback(gpointer data) { + int i; + + // Loop trough all the available rigidbodies (TODO: optimize) + for(i = 0; i < MAX_RIGIDBODIES; i++) { + // Check if ID's are correct + if(rigidBodies[i].id >= MAX_RIGIDBODIES) { + fprintf(stderr, "Could not parse rigid body %d from NatNet, because ID is higher then or equal to %d (MAX_RIGIDBODIES-1).\r\n", rigidBodies[i].id, MAX_RIGIDBODIES-1); + exit(EXIT_FAILURE); + } + // Check if we want to transmit this rigid and it was sampled + if(ac_ids[rigidBodies[i].id] == 0 || rigidBodies[i].nSamples < 1) + continue; + + // Defines to make easy use of paparazzi math + struct EnuCoor_d pos, speed; + struct EcefCoor_d ecef_pos; + struct LlaCoor_d lla_pos; + struct DoubleQuat orient; + struct DoubleEulers orient_eulers; + + // Add the Optitrack angle to the x and y positions + pos.x = cos(tracking_offset_angle) * rigidBodies[i].x + sin(tracking_offset_angle) * rigidBodies[i].y; + pos.y = sin(tracking_offset_angle) * rigidBodies[i].x - cos(tracking_offset_angle) * rigidBodies[i].y; + pos.z = rigidBodies[i].z; + + // Convert the position to ecef and lla based on the Optitrack LTP + ecef_of_enu_point_d(&ecef_pos ,&tracking_ltp ,&pos); + lla_of_ecef_d(&lla_pos, &ecef_pos); + + // Check if we have enough samples to estimate the velocity + rigidBodies[i].nVelocityTransmit++; + if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) { + // Calculate the derevative of the sum to get the correct velocity (1 / freq_transmit) * (samples / total_samples) + double sample_time = //((double)rigidBodies[i].nVelocitySamples / (double)rigidBodies[i].totalVelocitySamples) / + ((double)rigidBodies[i].nVelocityTransmit / (double)freq_transmit); + rigidBodies[i].vel_x = rigidBodies[i].vel_x / sample_time; + rigidBodies[i].vel_y = rigidBodies[i].vel_y / sample_time; + rigidBodies[i].vel_z = rigidBodies[i].vel_z / sample_time; + + // Add the Optitrack angle to the x and y velocities + speed.x = cos(tracking_offset_angle) * rigidBodies[i].vel_x + sin(tracking_offset_angle) * rigidBodies[i].vel_y; + speed.y = sin(tracking_offset_angle) * rigidBodies[i].vel_x - cos(tracking_offset_angle) * rigidBodies[i].vel_y; + speed.z = rigidBodies[i].vel_z; + + // Conver the speed to ecef based on the Optitrack LTP + ecef_of_enu_vect_d(&rigidBodies[i].ecef_vel ,&tracking_ltp ,&speed); + } + + // Copy the quaternions and convert to euler angles for the heading + orient.qi = rigidBodies[i].qw; + orient.qx = rigidBodies[i].qx; + orient.qy = rigidBodies[i].qy; + orient.qz = rigidBodies[i].qz; + DOUBLE_EULERS_OF_QUAT(orient_eulers, orient); + + // Calculate the heading by adding the Natnet offset angle and normalizing it + double heading = -orient_eulers.psi-tracking_offset_angle; + NormRadAngle(heading); + + printf_debug("[%d -> %d]Samples: %d\t%d\t\tTiming: %3.3f latency\n", rigidBodies[i].id, ac_ids[rigidBodies[i].id] + , rigidBodies[i].nSamples, rigidBodies[i].nVelocitySamples, natnet_latency); + printf_debug(" Heading: %f\t\tPosition: %f\t%f\t%f\t\tVelocity: %f\t%f\t%f\n", DegOfRad(heading), + rigidBodies[i].x, rigidBodies[i].y, rigidBodies[i].z, + rigidBodies[i].ecef_vel.x, rigidBodies[i].ecef_vel.y, rigidBodies[i].ecef_vel.z); + + // Transmit the REMOTE_GPS packet on the ivy bus + IvySendMsg("0 REMOTE_GPS %d %d %d %d %d %d %d %d %d %d %d %d %d %d", ac_ids[rigidBodies[i].id], + rigidBodies[i].nMarkers, //uint8 Number of markers (sv_num) + (int)(ecef_pos.x*100.0), //int32 ECEF X in CM + (int)(ecef_pos.y*100.0), //int32 ECEF Y in CM + (int)(ecef_pos.z*100.0), //int32 ECEF Z in CM + (int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7 + (int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7 + (int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid + (int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm + (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s + (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s + (int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s + 0, + (int)(heading*10000000.0)); //int32 Course in rad*1e7 + + // Reset the velocity differentiator if we calculated the velocity + if(rigidBodies[i].nVelocitySamples >= min_velocity_samples) { + rigidBodies[i].vel_x = 0; + rigidBodies[i].vel_y = 0; + rigidBodies[i].vel_z = 0; + rigidBodies[i].nVelocitySamples = 0; + rigidBodies[i].totalVelocitySamples = 0; + rigidBodies[i].nVelocityTransmit = 0; + } + + rigidBodies[i].nSamples = 0; + } + + return TRUE; +} + +/** The NatNet sampler periodic function */ +static gboolean sample_data(GIOChannel *chan, GIOCondition cond, gpointer data) { + static unsigned char buffer_data[MAX_PACKETSIZE]; + static int bytes_data = 0; + + // Keep on reading until we have the whole packet + bytes_data += network_read(natnet_data, buffer_data, MAX_PACKETSIZE); + + // Parse NatNet data + if(bytes_data >= 2 && bytes_data >= buffer_data[1]) { + natnet_parse(buffer_data); + bytes_data = 0; + } + + return TRUE; +} + + +/** Print the program help */ +void print_help(char* filename) { + static const char *usage = + "Usage: %s [options]\n" + " Options :\n" + " -h, --help Display this help\n" + " -v, --verbose Verbosity level 0-2 (0)\n\n" + + " -ac Use rigid ID for GPS of ac_id (multiple possible)\n\n" + + " -multicast_addr NatNet server multicast address (239.255.42.99)\n" + " -server NatNet server IP address (255.255.255.255)\n" + " -version NatNet server version (2.5)\n" + " -data_port NatNet server data socket UDP port (1510)\n" + " -cmd_port NatNet server command socket UDP port (1511)\n\n" + + " -ecef ECEF coordinates of the tracking system\n" + " -lla Latitude, longitude and altitude of the tracking system\n" + " -offset_angle Tracking system angle offset compared to the North in degrees\n\n" + + " -tf Transmit frequency to the ivy bus in hertz (60)\n" + " -vel_samples Minimum amount of samples for the velocity differentiator (4)\n\n" + + " -ivy_bus Ivy bus address and port (127.255.255.255:2010)\n"; + fprintf(stderr, usage, filename); +} + +/** Check the amount of arguments */ +void check_argcount(int argc, char** argv, int i, int expected) { + if(i+expected >= argc) { + fprintf(stderr, "Option %s expected %d arguments\r\n\r\n", argv[i], expected); + print_help(argv[0]); + exit(0); + } +} + +/** Parse the options from the commandline */ +static void parse_options(int argc, char** argv) { + int i, count_ac = 0; + for(i = 1; i < argc; ++i) { + + // Print help + if(strcmp(argv[i], "--help") == 0 || strcmp(argv[i], "-h") == 0) { + print_help(argv[0]); + exit(0); + } + // Set the verbosity level + if(strcmp(argv[i], "--verbosity") == 0 || strcmp(argv[i], "-v") == 0) { + check_argcount(argc, argv, i, 1); + + verbose = atoi(argv[++i]); + } + + // Set an rigid body to ivy ac_id + else if(strcmp(argv[i], "-ac") == 0) { + check_argcount(argc, argv, i, 2); + + int rigid_id = atoi(argv[++i]); + uint8_t ac_id = atoi(argv[++i]); + + if(rigid_id >= MAX_RIGIDBODIES) { + fprintf(stderr, "Rigid body ID must be less then %d (MAX_RIGIDBODIES)\n\n", MAX_RIGIDBODIES); + print_help(argv[0]); + exit(EXIT_FAILURE); + } + ac_ids[rigid_id] = ac_id; + count_ac++; + } + + // Set the NatNet multicast address + else if(strcmp(argv[i], "-multicast_addr") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_multicast_addr = argv[++i]; + } + // Set the NatNet server ip address + else if(strcmp(argv[i], "-server") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_addr = argv[++i]; + } + // Set the NatNet server version + else if(strcmp(argv[i], "-version") == 0) { + check_argcount(argc, argv, i, 1); + + float version = atof(argv[++i]); + natnet_major = (uint8_t)version; + natnet_minor = (uint8_t)(version * 10.0) % 10; + } + // Set the NatNet server data port + else if(strcmp(argv[i], "-data_port") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_data_port = atoi(argv[++i]); + } + // Set the NatNet server command port + else if(strcmp(argv[i], "-cmd_port") == 0) { + check_argcount(argc, argv, i, 1); + + natnet_cmd_port = atoi(argv[++i]); + } + + // Set the Tracking system position in ECEF + else if (strcmp(argv[i], "-ecef") == 0) { + check_argcount(argc, argv, i, 3); + + struct EcefCoor_d tracking_ecef; + tracking_ecef.x = atof(argv[++i]); + tracking_ecef.y = atof(argv[++i]); + tracking_ecef.z = atof(argv[++i]); + ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); + } + // Set the tracking system position in LLA + else if (strcmp(argv[i], "-lla") == 0) { + check_argcount(argc, argv, i, 3); + + struct EcefCoor_d tracking_ecef; + struct LlaCoor_d tracking_lla; + tracking_lla.lat = atof(argv[++i]); + tracking_lla.lon = atof(argv[++i]); + tracking_lla.alt = atof(argv[++i]); + ecef_of_lla_d(&tracking_ecef, &tracking_lla); + ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); + } + // Set the tracking system offset angle in degrees + else if(strcmp(argv[i], "-offset_angle") == 0) { + check_argcount(argc, argv, i, 1); + + tracking_offset_angle = atof(argv[++i]); + } + + // Set the transmit frequency + else if(strcmp(argv[i], "-tf") == 0) { + check_argcount(argc, argv, i, 1); + + freq_transmit = atoi(argv[++i]); + } + // Set the minimum amount of velocity samples for the differentiator + else if(strcmp(argv[i], "-vel_samples") == 0) { + check_argcount(argc, argv, i, 1); + + min_velocity_samples = atoi(argv[++i]); + } + + // Set the ivy bus + else if(strcmp(argv[i], "-ivy_bus") == 0) { + check_argcount(argc, argv, i, 1); + + ivy_bus = argv[++i]; + } + + // Unknown option + else { + fprintf(stderr, "Unknown option %s\r\n\r\n", argv[i]); + print_help(argv[0]); + exit(0); + } + } + + // Check if at least one aircraft is set + if(count_ac < 1) { + fprintf(stderr, "You must specify at least one aircraft (-ac )\n\n"); + print_help(argv[0]); + exit(EXIT_FAILURE); + } +} + +int main(int argc, char** argv) +{ + // Set the default tracking system position and angle + struct EcefCoor_d tracking_ecef; + tracking_ecef.x = 3924304; + tracking_ecef.y = 300360; + tracking_ecef.z = 5002162; + tracking_offset_angle = 123.0 / 57.6; + ltp_def_from_ecef_d(&tracking_ltp, &tracking_ecef); + + // Parse the options from cmdline + parse_options(argc, argv); + printf_debug("Tracking system Latitude: %f Longitude: %f Offset to North: %f degrees\n", DegOfRad(tracking_ltp.lla.lat), DegOfRad(tracking_ltp.lla.lon), DegOfRad(tracking_offset_angle)); + + // Create the network connections + printf_debug("Starting NatNet listening (multicast address: %s, data port: %d, version: %d.%d)\n", natnet_multicast_addr, natnet_data_port, natnet_major, natnet_minor); + natnet_data = network_new("", -1, natnet_data_port, 0); // Only receiving + network_subscribe_multicast(natnet_data, natnet_multicast_addr); + network_set_recvbuf(natnet_data, 0x100000); // 1MB + + printf_debug("Starting NatNet command socket (server address: %s, command port: %d)\n", natnet_addr, natnet_cmd_port); + natnet_cmd = network_new(natnet_addr, natnet_cmd_port, 0, 1); + network_set_recvbuf(natnet_cmd, 0x100000); // 1MB + + // Create the Ivy Client + GMainLoop *ml = g_main_loop_new(NULL, FALSE); + IvyInit("natnet2ivy", "natnet2ivy READY", 0, 0, 0, 0); + IvyStart(ivy_bus); + + // Create the main timers + printf_debug("Starting transmitting and sampling timeouts (transmitting frequency: %dHz, minimum velocity samples: %d)\n", + freq_transmit, min_velocity_samples); + g_timeout_add(1000/freq_transmit, timeout_transmit_callback, NULL); + + GIOChannel *sk = g_io_channel_unix_new(natnet_data->socket_in); + g_io_add_watch(sk, G_IO_IN | G_IO_NVAL | G_IO_HUP, + sample_data, NULL); + + // Run the main loop + g_main_loop_run(ml); + + return 0; +}