[misc] Add GPSD to IVY functionalities for follow me (#2499)

.travis.yml

@@ -11,7 +11,7 @@ before_install:
   - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
   - sudo apt-get update -q
 install:
-  - sudo apt-get install -y --force-yes paparazzi-dev paparazzi-jsbsim gcc-arm-embedded gcc-arm-linux-gnueabi g++-arm-linux-gnueabi libipc-run-perl rustc cargo libgazebo9-dev
+  - sudo apt-get install -y --force-yes paparazzi-dev paparazzi-jsbsim gcc-arm-embedded gcc-arm-linux-gnueabi g++-arm-linux-gnueabi libipc-run-perl rustc cargo libgazebo9-dev libgps-dev
 script:
   - arm-none-eabi-gcc --version
   - if [ "${COVERITY_SCAN_BRANCH}" != 1 ]; then if [ "${TRAVIS_EVENT_TYPE}" == "cron" ]; then make test_all_confs; else make test; fi; fi

sw/ground_segment/tmtc/Makefile

@@ -38,7 +38,7 @@ SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo parse_m
 SERVERCMX = $(SERVERCMO:.cmo=.cmx)
 
 
-all: link server messages settings ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy2serial ivy_serial_bridge app_server ivy2nmea
+all: link server messages settings ivy_tcp_aircraft ivy_tcp_controller broadcaster ivy2udp ivy2serial ivy_serial_bridge app_server ivy2nmea gpsd2ivy
 
 opt: server.opt
 
@@ -134,7 +134,11 @@ app_server: app_server.c
 	$(Q)$(CC) $(shell pkg-config libxml-2.0 gio-2.0 ivy-glib --cflags) -o $@ $^ -lm -lz $(shell pkg-config libxml-2.0 gio-2.0 ivy-glib libpcre --libs)
 
 gpsd2ivy: gpsd2ivy.c
+ifeq (, $(shell which gpsd))
+	$(info GPSD not installed)
+else
 	$(CC) $(GLIBIVY_CFLAGS) -o $@ $< $(GLIBIVY_LDFLAGS) -lgps
+endif
 
 ivy2nmea: ivy2nmea.c
 	@echo OL $@

sw/ground_segment/tmtc/gpsd2ivy.c

@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2009  Martin Mueller
+ *               2019  Freek van Tienen <freek.v.tienen@gmail.com>
  *
  * This file is part of paparazzi.
  *
@@ -66,13 +67,15 @@
 #define MSG_NAME    "FLIGHT_PARAM"
 #define MSG_ID		"GCS"
 
-#define TIMEOUT_PERIOD 200
+#define TIMEOUT_PERIOD 10
 
 struct gps_data_t *gpsdata;
 gboolean verbose;
 char* server;
 char* port;
 char* ivy_bus;
+char* ac;
+char* wp;
 
 static void update_gps(struct gps_data_t *gpsdata,
                        char *message,
@@ -104,7 +107,7 @@ static void update_gps(struct gps_data_t *gpsdata,
         }
 
         if (verbose)
-            printf("sending gps info viy Ivy: lat %g, lon %g, speed %g, course %g, alt %g, climb %g\n",
+            printf("sending gps info viy Ivy: lat %f, lon %f, speed %g, course %g, alt %g, climb %g\n",
                    gpsdata->fix.latitude, gpsdata->fix.longitude, fix_speed, fix_track, fix_altitude, fix_climb);
 
         IvySendMsg("%s %s %s %f %f %f %f %f %f %f %f %f %f %f %d %f",
@@ -125,6 +128,18 @@ static void update_gps(struct gps_data_t *gpsdata,
                 0, // itow
                 0.0); // airspeed
 
+
+        if(strcmp(ac, "NONE") != 0) {
+            IvySendMsg("%s TARGET_POS %s %s %d %d %d %f %f %f", "0", "0", ac, (int)(gpsdata->fix.latitude * 1e7), (int)(gpsdata->fix.longitude * 1e7), (int)(fix_altitude* 1000), fix_speed, fix_climb, fix_track);
+            if (verbose)
+                printf("sending TARGET_POS for aircraft %s\n", ac);
+        }
+        if(strcmp(ac, "NONE") != 0 && strcmp(wp, "NONE") != 0) {
+		    IvySendMsg("%s MOVE_WP %s %s %d %d %d", "0", wp, ac, (int)(gpsdata->fix.latitude * 1e7), (int)(gpsdata->fix.longitude * 1e7), (int)(20. * 1000));
+            if (verbose)
+                printf("sending waypoint %s for aircraft %s\n", wp, ac);
+        }
+
         fix_time = gpsdata->fix.time;
     }
     else
@@ -137,7 +152,7 @@ static void update_gps(struct gps_data_t *gpsdata,
 
 static gboolean gps_periodic(gpointer data __attribute__ ((unused)))
 {
-    if (gps_waiting (gpsdata, 500)) {
+    if (gps_waiting (gpsdata, TIMEOUT_PERIOD)) {
         if (gps_read (gpsdata) == -1) {
             perror("gps read error");
         } else {
@@ -152,6 +167,8 @@ gboolean parse_args(int argc, char** argv)
     verbose = FALSE;
     server = "localhost";
     port = DEFAULT_GPSD_PORT;
+    ac = "NONE";
+    wp = "NONE";
 #ifdef __APPLE__
     ivy_bus = "224.255.255.255";
 #else
@@ -165,7 +182,9 @@ gboolean parse_args(int argc, char** argv)
         "   -v --verbose                           Print verbose information\n"
         "   --server <gpsd server>                 e.g. localhost\n"
         "   --port <gpsd port>                     e.g. 2947\n"
-        "   --ivy_bus <ivy bus>                    e.g. 127.255.255.255\n";
+        "   --ivy_bus <ivy bus>                    e.g. 127.255.255.255\n"
+        "   --ac <ac_id>                           e.g. 17\n"
+        "   --wp <wp_id>                           e.g. 2\n";
 
     while (1) {
 
@@ -173,6 +192,8 @@ gboolean parse_args(int argc, char** argv)
             {"ivy_bus", 1, NULL, 0},
             {"server", 1, NULL, 0},
             {"port", 1, NULL, 0},
+            {"ac", 1, NULL, 0},
+            {"wp", 1, NULL, 0},
             {"help", 0, NULL, 'h'},
             {"verbose", 0, NULL, 'v'},
             {0, 0, 0, 0}
@@ -192,6 +213,10 @@ gboolean parse_args(int argc, char** argv)
                         server = strdup(optarg); break;
                     case 2:
                         port = strdup(optarg); break;
+                    case 3:
+                        ac = strdup(optarg); break;
+                    case 4:
+                        wp = strdup(optarg); break;
                     default:
                         break;
                 }