Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-18 08:03:44 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Merge pull request #1538 from rmeertens/natnetloggingmerged

Browse Source 
logging natnet messages
This commit is contained in:
Gautier Hattenberger
2016-02-24 11:06:35 +01:00
parent b5b7d7cd80 100dab1bd0
commit 8674fee5de
1 changed files with 47 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/ground_segment/misc/natnet2ivy.c
+47 -1
View File
@@ -57,6 +57,12 @@ uint16_t natnet_data_port = 1511;
uint8_t natnet_major = 2;
uint8_t natnet_minor = 7;
/** Logging */
FILE *fp;
char *nameOfLogfile = "natnet_log.dat";
bool_t log_exists = 0;
bool_t must_log = 0;
/** Ivy Bus default */
#ifdef __APPLE__
char *ivy_bus = "224.255.255.255";
@@ -588,6 +594,39 @@ gboolean timeout_transmit_callback(gpointer data)
0,
(int)(heading * 10000000.0)); //int32 Course in rad*1e7
}
if (must_log) {
if (log_exists == 0) {
fp = fopen(nameOfLogfile, "w");
log_exists = 1;
}
if (fp == NULL) {
printf("I couldn't open file for writing.\n");
exit(0);
} else {
struct timeval cur_time;
gettimeofday(&cur_time, NULL);
fprintf(fp, "%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", aircrafts[rigidBodies[i].id].ac_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)(DegOfRad(lla_pos.lat) * 1e7), //int32 LLA latitude in deg*1e7
(int)(DegOfRad(lla_pos.lon) * 1e7), //int32 LLA longitude in deg*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 cm/s
(int)(rigidBodies[i].ecef_vel.y * 100.0), //int32 ECEF velocity Y in cm/s
(int)(rigidBodies[i].ecef_vel.z * 100.0), //int32 ECEF velocity Z in cm/s
(int)(heading * 10000000.0), //int32 Course in rad*1e7
(int)cur_time.tv_sec,
(int)cur_time.tv_usec);
}
}
// Reset the velocity differentiator if we calculated the velocity
if (rigidBodies[i].nVelocitySamples >= min_velocity_samples) {
@@ -634,7 +673,7 @@ void print_help(char *filename)
" -v, --verbose <level> Verbosity level 0-2 (0)\n\n"
" -ac <rigid_id> <ac_id> Use rigid ID for GPS of ac_id (multiple possible)\n\n"
" -log <name of file> Log to a file\n\n"
" -multicast_addr <ip> NatNet server multicast address (239.255.42.99)\n"
" -server <ip> NatNet server IP address (255.255.255.255)\n"
" -version <id> NatNet server version (2.5)\n"
@@ -696,6 +735,13 @@ static void parse_options(int argc, char **argv)
aircrafts[rigid_id].ac_id = ac_id;
count_ac++;
}
// See if we want to log to a file
else if (strcmp(argv[i], "-log") == 0) {
check_argcount(argc, argv, i, 1);
nameOfLogfile = argv[++i];
must_log = 1;
}
// Set the NatNet multicast address
else if (strcmp(argv[i], "-multicast_addr") == 0) {