diff --git a/.gitignore b/.gitignore
index d2b0ab9013..1d6e7da3c1 100644
--- a/.gitignore
+++ b/.gitignore
@@ -71,8 +71,6 @@ paparazzi.sublime-workspace
# /sw/ground_segment/lpc21iap/
/sw/ground_segment/lpc21iap/lpc21iap
-/sw/ground_segment/misc/ivy2serial
-
# /sw/ground_segment/multimon/
/sw/ground_segment/multimon/costabf.c
/sw/ground_segment/multimon/mkcostab
@@ -148,6 +146,8 @@ paparazzi.sublime-workspace
/sw/ground_segment/misc/natnet2ivy
/sw/ground_segment/misc/sbp2ivy
/sw/ground_segment/misc/video_synchronizer
+/sw/ground_segment/misc/ivy2serial
+/sw/ground_segment/misc/sbs2ivy
# /sw/airborne/arch/lpc21/test/bootloader
/sw/airborne/arch/lpc21/test/bootloader/bl.dmp
diff --git a/conf/control_panel_example.xml b/conf/control_panel_example.xml
index f74793987d..a9a481c5c2 100644
--- a/conf/control_panel_example.xml
+++ b/conf/control_panel_example.xml
@@ -57,7 +57,9 @@
-
+
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 696a5f69f0..f396474e92 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -2818,6 +2818,18 @@
+
+
+
+
+
+ altitude above WGS84 reference ellipsoid
+
+
+
+
+
+
diff --git a/sw/airborne/modules/multi/tcas.c b/sw/airborne/modules/multi/tcas.c
index b231319b73..4c23707915 100644
--- a/sw/airborne/modules/multi/tcas.c
+++ b/sw/airborne/modules/multi/tcas.c
@@ -206,7 +206,8 @@ void tcas_periodic_task_1Hz(void)
}
}
// Downlink alert
- DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice, &tcas_ac_RA, &tcas_resolve);
+ uint8_t resolve = tcas_resolve;
+ DOWNLINK_SEND_TCAS_RA(DefaultChannel, DefaultDevice, &tcas_ac_RA, &resolve);
} else { tcas_ac_RA = AC_ID; } // no conflict
#ifdef TCAS_DEBUG
if (tcas_status == TCAS_RA) { DOWNLINK_SEND_TCAS_DEBUG(DefaultChannel, DefaultDevice, &ac_id_close, &tau_min); }
diff --git a/sw/ground_segment/cockpit/Makefile b/sw/ground_segment/cockpit/Makefile
index af6373b352..15a92792db 100644
--- a/sw/ground_segment/cockpit/Makefile
+++ b/sw/ground_segment/cockpit/Makefile
@@ -55,7 +55,7 @@ PP_SRC = gcs.ml
PP_CMO = $(PP_SRC:.ml=.cmo)
PP_CMX = $(PP_SRC:.ml=.cmx)
-ML= gtk_setting_time.ml gtk_strip.ml horizon.ml strip.ml gtk_save_settings.ml saveSettings.ml page_settings.ml pages.ml speech.ml plugin.ml sectors.ml map2d.ml editFP.ml live.ml particules.ml papgets.ml gcs.ml
+ML= gtk_setting_time.ml gtk_strip.ml horizon.ml strip.ml gtk_save_settings.ml saveSettings.ml page_settings.ml pages.ml speech.ml plugin.ml sectors.ml map2d.ml editFP.ml intruders.ml live.ml particules.ml papgets.ml gcs.ml
MAIN=gcs
CMO=$(ML:.ml=.cmo)
CMX=$(ML:.ml=.cmx)
diff --git a/sw/ground_segment/cockpit/intruders.ml b/sw/ground_segment/cockpit/intruders.ml
new file mode 100644
index 0000000000..c7d520cca1
--- /dev/null
+++ b/sw/ground_segment/cockpit/intruders.ml
@@ -0,0 +1,61 @@
+(*
+ * Copyright (C) 2015 Gautier Hattenberger
+ *
+ * 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
+ * .
+ *
+ *)
+
+(*open Latlong*)
+
+type intruder = {
+ intruder_track : MapTrack.track;
+ mutable last_update : float
+}
+
+(*let intruders = (string, intruder) Hashtbl.t*)
+let intruders = Hashtbl.create 1
+
+let new_intruder = fun id name time geomap ->
+ let track = new MapTrack.track ~size:200 ~icon:"intruder" ~name ~show_carrot:false id geomap in
+ let intruder = { intruder_track = track; last_update = time } in
+ Hashtbl.add intruders id intruder
+
+let remove_intruder = fun id ->
+ try
+ let intruder = Hashtbl.find intruders id in
+ intruder.intruder_track#destroy ();
+ Hashtbl.remove intruders id
+ with _ -> () (* no intruder *)
+
+let update_intruder = fun id wgs84 heading alt speed climb time ->
+ try
+ let intruder = Hashtbl.find intruders id in
+ intruder.intruder_track#move_icon wgs84 heading alt speed climb;
+ intruder.last_update <- time;
+ with _ -> () (* no intruder, add a new one ? *)
+
+let intruder_exist = fun id ->
+ Hashtbl.mem intruders id
+
+(* remove old intruders after 10s *)
+let remove_old_intruders = fun () ->
+ Hashtbl.iter
+ (fun id i ->
+ if (Unix.gettimeofday () -. i.last_update) > 10.0 then
+ remove_intruder id
+ ) intruders
+
diff --git a/sw/ground_segment/cockpit/live.ml b/sw/ground_segment/cockpit/live.ml
index a1f4cbc688..5b6427b40e 100644
--- a/sw/ground_segment/cockpit/live.ml
+++ b/sw/ground_segment/cockpit/live.ml
@@ -1460,6 +1460,23 @@ let listen_tcas = fun a timestamp ->
tele_bind "TCAS_TA" (get_alarm_tcas a "tcas TA") timestamp;
tele_bind "TCAS_RA" (get_alarm_tcas a "TCAS RA") timestamp
+let get_intruders = fun (geomap:G.widget) _sender vs ->
+ let f = fun s -> Pprz.float_assoc s vs in
+ let i = fun s -> float (Pprz.int_assoc s vs) in
+ let name = Pprz.string_assoc "name" vs
+ and id = Pprz.string_assoc "id" vs
+ and time = Unix.gettimeofday ()
+ and lat = (i "lat") /. 1e7
+ and lon = (i "lon") /. 1e7 in
+ let pos = { posn_lat=(Deg>>Rad)lat; posn_long=(Deg>>Rad)lon } in
+ if not (Intruders.intruder_exist id) then
+ Intruders.new_intruder id name time geomap;
+ Intruders.update_intruder id pos (f "course") ((i "alt") /. 1000.) (f "speed") (f "climb") time
+
+let listen_intruders = fun (geomap:G.widget) ->
+ safe_bind "INTRUDER" (get_intruders geomap)
+
+
let listen_acs_and_msgs = fun geomap ac_notebook strips my_alert auto_center_new_ac alt_graph timestamp ->
(** Probe live A/Cs *)
let probe = fun () ->
@@ -1484,6 +1501,7 @@ let listen_acs_and_msgs = fun geomap ac_notebook strips my_alert auto_center_new
listen_autopilot_version_msg my_alert timestamp;
listen_tcas my_alert timestamp;
listen_dcshot geomap timestamp;
+ listen_intruders geomap;
(** Select the active aircraft on notebook page selection *)
let callback = fun i ->
@@ -1504,4 +1522,7 @@ let listen_acs_and_msgs = fun geomap ac_notebook strips my_alert auto_center_new
match GdkEvent.Key.keyval ev with
| k when (k = GdkKeysyms._c) || (k = GdkKeysyms._C) -> center_active () ; true
| _ -> false in
- ignore (geomap#canvas#event#connect#after#key_press key_press)
+ ignore (geomap#canvas#event#connect#after#key_press key_press);
+
+ (* call periodic_handle_intruders every second *)
+ ignore (Glib.Timeout.add 1000 (fun () -> Intruders.remove_old_intruders (); true));
diff --git a/sw/ground_segment/misc/Makefile b/sw/ground_segment/misc/Makefile
index c217b237ea..b8184dcdb3 100644
--- a/sw/ground_segment/misc/Makefile
+++ b/sw/ground_segment/misc/Makefile
@@ -32,13 +32,16 @@ ifeq ("$(UNAME)","Darwin")
LIBRARYS = $(shell if test -d /opt/paparazzi/lib; then echo "-L/opt/paparazzi/lib"; elif test -d /opt/local/lib; then echo "-L/opt/local/lib"; fi)
INCLUDES = $(shell if test -d /opt/paparazzi/include; then echo "-I/opt/paparazzi/include"; elif test -d /opt/local/include; then echo "-I/opt/local/include"; fi)
else
- LIBRARYS = -s
+ LIBRARYS =
endif
-CFLAGS += -Wall -fPIC
+CFLAGS += -Wall -fPIC -g
GTK_CFLAGS = $(shell pkg-config gtk+-2.0 --cflags)
GTK_LDFLAGS = $(shell pkg-config gtk+-2.0 --libs) $(shell pkg-config --libs ivy-glib) $(shell pcre-config --libs) -fPIC
+GLIBIVY_CFLAGS = $(shell pkg-config --cflags ivy-glib)
+GLIBIVY_LDFLAGS = $(shell pkg-config --libs ivy-glib) $(shell pcre-config --libs)
+
# fallback to ivy-glib pkg-config info if there is no ivy-c.pc
IVY_INC = $(shell pkg-config --cflags-only-I ivy-c 2> /dev/null)
IVY_LDFLAGS = $(shell pkg-config --libs ivy-c 2> /dev/null)
@@ -54,10 +57,10 @@ GLIB_LDFLAGS = $(shell pkg-config glib-2.0 --libs) -lglibivy -lm $(shell pcre-co
INCLUDES += $(shell pkg-config glib-2.0 --cflags) -I$(PAPARAZZI_SRC)/sw/airborne/ -I$(PAPARAZZI_SRC)/sw/include/ $(IVY_INC)
INCLUDES += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include/ -I$(PAPARAZZI_SRC)/sw/airborne/arch/linux/
-all: davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer
+all: davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy
clean:
- $(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer
+ $(Q)rm -f *.o davis2ivy kestrel2ivy natnet2ivy sbp2ivy video_synchronizer sbs2ivy
davis2ivy: davis2ivy.o
@echo CC $@
@@ -79,12 +82,19 @@ video_synchronizer: video_synchronizer.c
@echo CC $@
$(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) $(LIBRARYS) $(INCLUDES) -o $@ $< $(GTK_LDFLAGS)
+sbs2ivy: sbs2ivy.o pprz_geodetic_double.o pprz_geodetic_float.o sbs_parser.o
+ @echo CC $@
+ $(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) -o $@ $^ $(LIBRARYS) $(GLIBIVY_LDFLAGS) $(GTK_LDFLAGS) -lm
+
pprz_algebra_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_algebra_double.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
pprz_geodetic_double.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_double.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
+pprz_geodetic_float.o : $(PAPARAZZI_SRC)/sw/airborne/math/pprz_geodetic_float.c
+ $(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
+
udp_socket.o : $(PAPARAZZI_SRC)/sw/airborne/arch/linux/udp_socket.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
@@ -97,6 +107,12 @@ sbp.o : $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c
edc.o : $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c
$(Q)$(CC) $(CFLAGS) -c -std=c99 -O2 -Wall $(INCLUDES) $<
+sbs2ivy.o : sbs2ivy.c
+ $(Q)$(CC) $(CFLAGS) $(GTK_CFLAGS) -c -std=gnu99 -O2 -Wall $(INCLUDES) $<
+
+sbs_parser.o : sbs_parser.c
+ $(Q)$(CC) $(CFLAGS) -c -std=gnu99 -O2 -Wall $(INCLUDES) $<
+
%.o : %.c
$(Q)$(CC) $(CFLAGS) -c -O2 -Wall $(INCLUDES) $<
diff --git a/sw/ground_segment/misc/sbs2ivy.c b/sw/ground_segment/misc/sbs2ivy.c
new file mode 100644
index 0000000000..638a545715
--- /dev/null
+++ b/sw/ground_segment/misc/sbs2ivy.c
@@ -0,0 +1,819 @@
+/*
+ * Copyright (C) 2013 Marc Schwarzbach
+ * 2015 Felix Ruess
+ *
+ * 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
+ * .
+ *
+ */
+
+/**
+ * @file sbs2ivy.c
+ * Parser for the SBS-1 protocol (ADS-B data).
+ *
+ *
+ * ## Message Types
+ * // Generated when the user changes the selected aircraft in
+ * // BaseStation.
+ * SELECTION_CHANGE: 'SEL',
+ * // Generated when an aircraft being tracked sets or changes its
+ * // callsign.
+ * NEW_ID: 'ID',
+ * // Generated when the SBS picks up a signal for an aircraft that it
+ * // isn't currently tracking,
+ * NEW_AIRCRAFT: 'AIR',
+ * // Generated when an aircraft's status changes according to the
+ * // time-out values in the SBS1 Data Settings menu.
+ * STATUS_CHANGE: 'STA',
+ * // Generated when the user double-clicks (or presses return) on an
+ * // aircraft (i.e. to bring up the aircraft details window).
+ * CLICK: 'CLK',
+ * // Generated by the aircraft. There are eight different MSG
+ * // transmission types, see `TransmissionType`.
+ * TRANSMISSION: 'MSG'
+ *
+ * ## Transmission Types
+ *
+ * Transmission messages (MSG) from aircraft may be one of eight types
+ * (ES = Extended Squitter, DF = Downlink Format, BDS = B-Definition
+ * Subfield).
+ *
+ * |Type|Description |Spec |
+ * |----|--------------------------------|--------------|
+ * | 1 | ES identification and category | DF17 BDS 0,8 |
+ * | 2 | ES surface position message | DF17 BDS 0,6 |
+ * | 3 | ES airborne position message | DF17 BDS 0,5 |
+ * | 4 | ES airborne velocity message | DF17 BDS 0,9 |
+ * | 5 | Surveillance alt message | DF4, DF20 |
+ * | 6 | Surveillance ID message | DF5, DF21 |
+ * | 7 | Air-to-air message | DF16 |
+ * | 8 | All call reply | DF11 |
+ *
+ */
+
+
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+
+#include
+
+#include
+#include
+#include
+#include
+//#include
+
+#include "math/pprz_geodetic_int.h"
+#include "math/pprz_geodetic_float.h"
+#include "math/pprz_geodetic_double.h"
+#include "math/pprz_algebra_double.h"
+
+#include "sbs_parser.h"
+
+#define DEBUG_INPUT 0
+#define DEBUG_INTR 0
+
+
+
+#if DEBUG_INPUT == 1
+#define INPUT_PRINT(...) { printf( __VA_ARGS__);};
+#else
+#define INPUT_PRINT(...) { };
+#endif
+
+
+#if DEBUG_INTR == 1
+#define INTR_PRINT(...) { printf( __VA_ARGS__);};
+#else
+#define INTR_PRINT(...) { };
+#endif
+
+
+static void send_intruders(void);
+void utm_i_of_lla_d(struct UtmCoor_i *utmi, struct LlaCoor_d *lla);
+
+void close_port(void);
+int open_port(char *host, unsigned int port);
+void read_port(void);
+
+struct SbsMsgData in_data;
+
+
+unsigned int timer = 100;
+unsigned int lastivyrcv = 0;
+unsigned int lastivytrx = 0;
+
+int portstat = 0;
+
+static float dist_to_uav(struct UtmCoor_i *utmi);
+
+
+//TCP Port variables
+int sockfd;
+struct sockaddr_in serv_addr;
+char buffer[256];
+fd_set readSet;
+struct timeval selTimeout;
+
+
+/** Ivy Bus default */
+#ifdef __APPLE__
+#define DEFAULT_IVY_BUS "224.255.255.255"
+#else
+#define DEFAULT_IVY_BUS "127.255.255.255:2010"
+#endif
+
+struct Opts {
+ uint8_t ac_id;
+ char *host;
+ unsigned int port;
+ unsigned int enable_remote_uav;
+ char *ivy_bus;
+};
+
+struct Opts options;
+
+
+//////////////////////////////////////////////////////////////////////////////////
+// SETTINGS
+//////////////////////////////////////////////////////////////////////////////////
+
+// Serial Repeat Rate
+long delay = 1000;
+
+GtkWidget *status_ivy;
+GtkWidget *status_out_ivy;
+GtkWidget *status;
+GtkWidget *status_sbs;
+
+char status_str[256];
+char status_ivy_str[256];
+char status_ivy_out[256];
+char status_sbs_str[256];
+
+long int count_ivy = 0;
+long int count_serial = 0;
+
+//////////////////////////////////////////////////////////////////////////////////
+// local_uav DATA
+//////////////////////////////////////////////////////////////////////////////////
+
+struct _uav_type_ {
+ // Header
+ unsigned char header;
+
+ // Data
+ unsigned char ac_id;
+ int16_t phi, theta, psi;
+ uint16_t speed;
+ struct LlaCoor_i lla_i;
+ int utm_east, utm_north, utm_z;
+ unsigned char utm_zone;
+ unsigned char pprz_mode;
+ float desired_alt;
+ int16_t climb;
+ int16_t course;
+ unsigned char block;
+
+ // Footer
+ unsigned char footer;
+}
+__attribute__((packed))
+
+local_uav;
+
+
+//////////////////////////////////////////////////////////////////////////////////
+// IVY Reader
+//////////////////////////////////////////////////////////////////////////////////
+
+static void on_Attitude(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+ /*
+
+
+
+
+
+ */
+
+ local_uav.phi = (short int)(atof(argv[0]) * 1000.0);
+ local_uav.psi = (short int)(atof(argv[1]) * 1000.0);
+ local_uav.theta = (short int)(atof(argv[2]) * 1000.0);
+
+}
+
+static void on_Estimator(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+ /*
+
+
+
+
+ */
+
+ local_uav.utm_z = ((atof(argv[0])) * 1000.0f);
+ local_uav.climb = atof(argv[1]);
+
+}
+
+static void on_Navigation(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+ /*
+
+
+
+
+
+
+
+
+
+
+ */
+
+ local_uav.block = atoi(argv[0]);
+}
+
+static void on_Desired(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+ /*
+
+
+
+
+
+
+
+
+
+
+
+ */
+ local_uav.desired_alt = atof(argv[5]);
+}
+
+static void on_Gps(IvyClientPtr app, void *user_data, int argc, char *argv[])
+{
+ /*
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ */
+ local_uav.utm_east = atoi(argv[1]);
+ local_uav.utm_north = atoi(argv[2]);
+ local_uav.utm_zone = atoi(argv[9]);
+ local_uav.speed = atoi(argv[5]);
+
+ count_ivy++;
+ lastivyrcv = timer;
+
+ //Print to window
+ sprintf(status_ivy_str, "Received from IVY: [%ld]", count_ivy);
+ gtk_label_set_text(GTK_LABEL(status_ivy), status_ivy_str);
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// IVY Writer
+//////////////////////////////////////////////////////////////////////////////////
+
+void send_intruder_msg(struct Intruder *intruder)
+{
+ /*
+
+
+
+
+
+ altitude above WGS84 reference ellipsoid
+
+
+
+
+
+ */
+
+ struct LlaCoor_i lla_i;
+ LLA_BFP_OF_REAL(lla_i, intruder->lla);
+
+ // FIXME: using WGS84 ellipsoid alt, it is probably hmsl???
+ IvySendMsg("ground INTRUDER %d %s %d %d %d %f %f %f %d\n", intruder->id, intruder->name,
+ lla_i.lat, lla_i.lon, lla_i.alt, intruder->course,
+ intruder->gspeed, intruder->climb, 0);
+
+ count_serial++;
+ lastivytrx = timer;
+
+ sprintf(status_ivy_out, "Sending Intruder ID: %d [%ld]", intruder->id, count_serial);
+ gtk_label_set_text(GTK_LABEL(status_out_ivy), status_ivy_out);
+}
+
+
+void send_remote_uav(struct Intruder *intruder)
+{
+ struct _uav_type_ remote_uav;
+
+ remote_uav.ac_id = 254;
+ remote_uav.phi = 0;
+ LLA_BFP_OF_REAL(remote_uav.lla_i, intruder->lla);
+
+ struct UtmCoor_i utmi;
+ utm_i_of_lla_d(&utmi, &intruder->lla);
+
+ remote_uav.utm_east = utmi.east;
+ remote_uav.utm_north = utmi.north;
+ remote_uav.utm_z = utmi.alt;
+ remote_uav.utm_zone = utmi.zone;
+
+ // uav speed/climb are in in cm/s
+ remote_uav.speed = intruder->gspeed * 100;
+ remote_uav.climb = intruder->climb * 100;
+ // course in decideg
+ remote_uav.course = intruder->course * 10;
+
+
+ IvySendMsg("%d ALIVE 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0\n", remote_uav.ac_id);
+
+ // ATTITUDE: phi, psi, theta
+ float psi = RadOfDeg(intruder->course);
+ IvySendMsg("%d ATTITUDE %f %f %f\n", remote_uav.ac_id, 0.0f, psi, 0.0f);
+
+
+ /*
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ IvySendMsg("%d GPS 3 %d %d %d %d %d %d 0 0 %d 0\n", remote_uav.ac_id, remote_uav.utm_east, remote_uav.utm_north,
+ (int)remote_uav.course, remote_uav.utm_z, remote_uav.speed, (int)remote_uav.climb, remote_uav.utm_zone);
+
+ */
+
+ /*
+
+
+
+ altitude above WGS84 reference ellipsoid
+ Height above Mean Sea Level (geoid)
+
+
+
+
+
+
+
+
+ */
+
+ // FIXME: using alt from ADS-B as ellipsoid and geoid alt...
+ IvySendMsg("%d GPS_LLA %d %d %d %d %d %d %d 0 0 3 0\n", remote_uav.ac_id,
+ remote_uav.lla_i.lat, remote_uav.lla_i.lon, remote_uav.lla_i.alt,
+ remote_uav.lla_i.alt, remote_uav.course, remote_uav.speed, remote_uav.climb);
+
+ /*
+
+
+
+
+
+
+
+ */
+
+ IvySendMsg("%d FBW_STATUS 2 0 1 81 0 \n", remote_uav.ac_id);
+
+ float z = ((float)remote_uav.utm_z) / 1000.0f;
+ float zdot = remote_uav.climb / 100.0f;
+ IvySendMsg("%d ESTIMATOR %f %f\n", remote_uav.ac_id, z, zdot);
+
+ count_serial++;
+ lastivytrx = timer;
+
+ sprintf(status_ivy_out, "Intruder ID: %d; forwarding to IVY [%ld]", remote_uav.ac_id, count_serial);
+ gtk_label_set_text(GTK_LABEL(status_out_ivy), status_ivy_out);
+}
+
+
+
+//////////////////////////////////////////////////////////////////////////////////
+// TIMER
+//////////////////////////////////////////////////////////////////////////////////
+
+// Main functions called every 1/4s
+gboolean timeout_callback(gpointer data)
+{
+ static unsigned char dispatch = 0;
+
+ // Every Time
+ if (portstat == 1) {
+ read_port();
+ }
+ // send intruder info
+ send_intruders();
+ timer++;
+
+ // One out of 4
+ if (dispatch > 2) {
+ dispatch = 0;
+
+ if ((timer - lastivyrcv) > 10) {
+ sprintf(status_ivy_str, "--");
+ gtk_label_set_text(GTK_LABEL(status_ivy), status_ivy_str);
+ }
+ if ((timer - lastivytrx) > 10) {
+ sprintf(status_ivy_out, "--");
+ gtk_label_set_text(GTK_LABEL(status_out_ivy), status_ivy_out);
+ }
+ if (portstat == 0) {
+ portstat = open_port(options.host, options.port);
+ }
+
+ } else {
+ dispatch ++;
+ }
+ return TRUE;
+}
+
+//////////////////////////////////////////////////////////////////////////////////
+// MAIN
+//////////////////////////////////////////////////////////////////////////////////
+
+gint delete_event(GtkWidget *widget,
+ GdkEvent *event,
+ gpointer data)
+{
+ g_print("CLEAN STOP\n");
+
+ close_port();
+ IvyStop();
+
+ exit(0);
+
+ return (FALSE); // false = delete window, FALSE = keep active
+}
+
+
+static bool_t parse_options(int argc, char **argv, struct Opts *opts)
+{
+ opts->ac_id = 0;
+ opts->host = "localhost";
+ opts->port = 30003;
+ opts->enable_remote_uav = 0;
+ opts->ivy_bus = DEFAULT_IVY_BUS;
+
+ static const char *usage =
+ "Usage: %s [options]\n"
+ " Options :\n"
+ " -h Display this help\n"
+ " --ac e.g. 23\n"
+ " --host e.g. localhost\n"
+ " --port e.g. 30002\n"
+ " --enable_remote_uav\n"
+ " --ivy_bus e.g. 127.255.255.255\n";
+
+ while (1) {
+
+ static struct option long_options[] = {
+ {"ac", 1, NULL, 0},
+ {"host", 1, NULL, 0},
+ {"port", 1, NULL, 0},
+ {"enable_remote_uav", 0, NULL, 0},
+ {"ivy_bus", 1, NULL, 0},
+ {0, 0, 0, 0}
+ };
+ int option_index = 0;
+ int c = getopt_long(argc, argv, "h", long_options, &option_index);
+ if (c == -1) {
+ break;
+ }
+
+ switch (c) {
+ case 0:
+ switch (option_index) {
+ case 0:
+ opts->ac_id = atoi(optarg); break;
+ case 1:
+ opts->host = strdup(optarg); break;
+ case 2:
+ opts->port = atoi(optarg); break;
+ case 3:
+ opts->enable_remote_uav = 1; break;
+ case 4:
+ opts->ivy_bus = strdup(optarg); break;
+ default:
+ break;
+ }
+ break;
+
+ case 'h':
+ fprintf(stderr, usage, argv[0]);
+ exit(0);
+
+ default: /* ’?’ */
+ printf("?? getopt returned character code 0%o ??\n", c);
+ fprintf(stderr, usage, argv[0]);
+ exit(EXIT_FAILURE);
+ }
+ }
+ return TRUE;
+}
+
+int main(int argc, char **argv)
+{
+
+ if (!parse_options(argc, argv, &options)) { return 1; }
+
+ gtk_init(&argc, &argv);
+
+ local_uav.ac_id = options.ac_id;
+
+ sprintf(status_str, "Listening to AC=%d", local_uav.ac_id);
+ sprintf(status_ivy_str, "--");
+ sprintf(status_ivy_out, "--");
+ printf("%s\n", status_str);
+
+ printf("Listening for SBS-1 messages on %s:%d\n", options.host, options.port);
+ portstat = open_port(options.host, options.port);
+
+ // Start IVY
+ IvyInit("SBS2Ivy", "SBS2Ivy READY", NULL, NULL, NULL, NULL);
+ IvyBindMsg(on_Desired, NULL, "^%d DESIRED (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)", local_uav.ac_id);
+ IvyBindMsg(on_Estimator, NULL, "^%d ESTIMATOR (\\S*) (\\S*)", local_uav.ac_id);
+ IvyBindMsg(on_Navigation, NULL, "^%d NAVIGATION (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",
+ local_uav.ac_id);
+ IvyBindMsg(on_Attitude, NULL, "^%d ATTITUDE (\\S*) (\\S*) (\\S*)", local_uav.ac_id);
+ IvyBindMsg(on_Gps, NULL, "^%d GPS (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*) (\\S*)",
+ local_uav.ac_id);
+ IvyStart("127.255.255.255");
+
+ // Add Timer
+ gtk_timeout_add(delay / 4, timeout_callback, NULL);
+
+ // GTK Window
+ GtkWidget *window = gtk_window_new(GTK_WINDOW_TOPLEVEL);
+ gtk_window_set_title(GTK_WINDOW(window), "SBS2Ivy");
+
+ gtk_signal_connect(GTK_OBJECT(window), "delete_event",
+ GTK_SIGNAL_FUNC(delete_event), NULL);
+
+ GtkWidget *box = gtk_vbox_new(TRUE, 1);
+ gtk_container_add(GTK_CONTAINER(window), box);
+
+ GtkWidget *hbox = gtk_hbox_new(FALSE, 1);
+ gtk_container_add(GTK_CONTAINER(box), hbox);
+ status = gtk_label_new("Status:");
+ gtk_box_pack_start(GTK_BOX(hbox), status, FALSE, FALSE, 1);
+ gtk_label_set_justify((GtkLabel *) status, GTK_JUSTIFY_LEFT);
+ status = gtk_label_new(status_str);
+ gtk_box_pack_start(GTK_BOX(hbox), status, FALSE, FALSE, 1);
+ gtk_label_set_justify((GtkLabel *) status, GTK_JUSTIFY_LEFT);
+
+ hbox = gtk_hbox_new(FALSE, 1);
+ gtk_container_add(GTK_CONTAINER(box), hbox);
+ status_ivy = gtk_label_new("From IVY:");
+ gtk_box_pack_start(GTK_BOX(hbox), status_ivy, FALSE, FALSE, 1);
+ gtk_label_set_justify((GtkLabel *) status_ivy, GTK_JUSTIFY_LEFT);
+ status_ivy = gtk_label_new(status_ivy_str);
+ gtk_box_pack_start(GTK_BOX(hbox), status_ivy, FALSE, FALSE, 1);
+ gtk_label_set_justify((GtkLabel *) status_ivy, GTK_JUSTIFY_LEFT);
+
+ hbox = gtk_hbox_new(FALSE, 1);
+ gtk_container_add(GTK_CONTAINER(box), hbox);
+ status_out_ivy = gtk_label_new("To IVY:");
+ gtk_box_pack_start(GTK_BOX(hbox), status_out_ivy, FALSE, FALSE, 1);
+ gtk_label_set_justify((GtkLabel *) status_out_ivy, GTK_JUSTIFY_LEFT);
+ status_out_ivy = gtk_label_new(status_ivy_out);
+ gtk_label_set_justify(GTK_LABEL(status_out_ivy), GTK_JUSTIFY_LEFT);
+ gtk_box_pack_start(GTK_BOX(hbox), status_out_ivy, FALSE, FALSE, 1);
+
+ hbox = gtk_hbox_new(FALSE, 1);
+ gtk_container_add(GTK_CONTAINER(box), hbox);
+ status_sbs = gtk_label_new("SBS:");
+ gtk_box_pack_start(GTK_BOX(hbox), status_sbs, FALSE, FALSE, 1);
+ gtk_label_set_justify((GtkLabel *) status_sbs, GTK_JUSTIFY_LEFT);
+ status_sbs = gtk_label_new(status_sbs_str);
+ gtk_label_set_justify(GTK_LABEL(status_sbs), GTK_JUSTIFY_LEFT);
+ gtk_box_pack_start(GTK_BOX(hbox), status_sbs, FALSE, FALSE, 1);
+
+
+ gtk_widget_show_all(window);
+
+ gtk_main();
+
+ // Clean up
+ fprintf(stderr, "Stopping\n");
+
+ return 0;
+}
+
+///
+//Subroutines
+/////
+
+//////////////////////////////////////////////////////////////////////////////////
+// TCP PORT
+//////////////////////////////////////////////////////////////////////////////////
+
+/// Open
+int open_port(char *host, unsigned int port)
+{
+
+ sockfd = socket(AF_INET, SOCK_STREAM, 0);
+ if ((sockfd < 0) && DEBUG_INPUT) {
+ perror("ERROR opening socket");
+ }
+
+ struct hostent *server;
+ server = gethostbyname(host);
+
+ bzero((char *) &serv_addr, sizeof(serv_addr));
+ serv_addr.sin_family = AF_INET;
+ bcopy((char *)server->h_addr,
+ (char *)&serv_addr.sin_addr.s_addr,
+ server->h_length);
+ serv_addr.sin_port = htons(port);
+ if ((connect(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) && DEBUG_INPUT) {
+ perror("ERROR connecting socket");
+ return 0;
+ }
+
+ //set values for select()
+ FD_ZERO(&readSet);
+ FD_SET(sockfd, &readSet);//tcp socket
+
+ selTimeout.tv_sec = 0; /* timeout (secs.) */
+ selTimeout.tv_usec = 10; /* 10 microseconds */
+
+ printf("Connected to %s, port %i\n", host, port);
+
+ return 1;
+}
+
+///Close
+void close_port(void)
+{
+ close(sockfd);
+}
+
+/////////////////////////////////////
+//Read ADSB data from TCP port
+////////////////////////////////////////////
+
+void read_port(void)
+{
+ unsigned char readbuf;
+ int sel_ret;
+
+ FD_SET(sockfd, &readSet);//set tcp socket for select
+
+ sel_ret = select(sockfd + 1, &readSet, NULL, NULL, &selTimeout);
+
+ if (sel_ret == -1) {
+ perror("select"); // error occurred in select()
+ } else if (sel_ret == 0) {
+ INPUT_PRINT("."); //Debug for no message
+ fflush(stdout);
+ } else {
+ FD_SET(sockfd, &readSet);//re-set
+ while (select(sockfd + 1, &readSet, NULL, NULL, &selTimeout) > 0) {
+ FD_SET(sockfd, &readSet);//re-set
+ if (read(sockfd, &readbuf, 1) == 1) {
+ sbs_parse_char(&in_data, readbuf);
+ } else { //port closed at server, go to reconnect
+ portstat = 0;
+ close_port();
+ break;
+ }
+ if (in_data.msg_available) {
+ sbs_parse_msg(&in_data);
+ }
+ }
+ }
+
+}
+
+
+
+static void send_intruders(void)
+{
+ int num_intr = 0;
+ int closest_intr = -1;
+ float min_dist = 0.0;
+
+ // make sure intruder info is up-to-date (old ones deleted)
+ update_intruders(intruders);
+
+ /* count and send all valid intruders and find closest one */
+ for (int i = 0; i < MAX_INTRUDERS; i++) {
+ if (intruders[i].used == 1) {
+ num_intr++;
+
+ send_intruder_msg(&intruders[i]);
+
+ struct UtmCoor_i utmi;
+ utm_i_of_lla_d(&utmi, &intruders[i].lla);
+
+ float dist = dist_to_uav(&utmi);
+ if (min_dist <= 0.0f || dist < min_dist) {
+ min_dist = dist;
+ closest_intr = i;
+ }
+ }
+ }
+
+ if (options.enable_remote_uav && closest_intr >= 0) {
+ send_remote_uav(&intruders[closest_intr]);
+ }
+
+ //show on window
+ if (portstat == 1) {
+ sprintf(status_sbs_str, "Connected; Intruders: %i Min.Dist: %4.1f km", num_intr, min_dist / 1000.0);
+ gtk_label_set_text(GTK_LABEL(status_sbs), status_sbs_str);
+ } else {
+ sprintf(status_sbs_str, "No SBS Data, Port closed");
+ gtk_label_set_text(GTK_LABEL(status_sbs), status_sbs_str);
+ }
+ return;
+}
+
+//calculate distance intruder to local UAV in meters
+static float dist_to_uav(struct UtmCoor_i *utmi)
+{
+ float dn = (float)(utmi->north - local_uav.utm_north) / 100.0;
+ float de = (float)(utmi->east - local_uav.utm_east) / 100.0;
+ float da = (float)(utmi->alt - local_uav.utm_z) / 1000.0;
+ return sqrtf(dn * dn + de * de + da * da);
+}
+
+void utm_i_of_lla_d(struct UtmCoor_i *utmi, struct LlaCoor_d *lla)
+{
+ // FIXME, LLA should not be stored as float!
+ struct LlaCoor_f lla_f;
+ LLA_COPY(lla_f, *lla);
+ struct LlaCoor_i lla_i;
+ LLA_BFP_OF_REAL(lla_i, *lla);
+
+ struct UtmCoor_f utmf;
+
+ //Calculations for UTM zone of local UAV so that distance calc works even for zone borders
+ utmf.zone = local_uav.utm_zone;
+ utm_of_lla_f(&utmf, &lla_f);
+
+ /* copy results of utm conversion */
+ utmi->east = utmf.east * 100;
+ utmi->north = utmf.north * 100;
+ utmi->alt = lla_i.alt;
+ utmi->zone = utmf.zone;
+
+ INPUT_PRINT("UTMI E N Z = %d, %d, %d\n\r", utmi->east, utmi->north, utmi->zone);
+}
diff --git a/sw/ground_segment/misc/sbs_parser.c b/sw/ground_segment/misc/sbs_parser.c
new file mode 100644
index 0000000000..860b79a400
--- /dev/null
+++ b/sw/ground_segment/misc/sbs_parser.c
@@ -0,0 +1,457 @@
+/*
+ * Copyright (C) 2013 Marc Schwarzbach
+ * 2015 Felix Ruess
+ *
+ * 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
+ * .
+ *
+ */
+
+/**
+ * @file sbs_parser.c
+ * Parser for the SBS-1 protocol (ADS-B data).
+ */
+
+#include "sbs_parser.h"
+#include
+#include
+
+#define DEBUG_INPUT 0
+
+#if DEBUG_INPUT == 1
+#define INPUT_PRINT(...) { printf( __VA_ARGS__);};
+#else
+#define INPUT_PRINT(...) { };
+#endif
+
+
+struct Intruder intruders[MAX_INTRUDERS];
+
+static void parse_msg3(struct SbsMsgData *data);
+static void parse_msg4(struct SbsMsgData *data);
+
+static void mark_old_intruders(struct Intruder *intr);
+static void sort_intruders(struct Intruder *intr);
+
+
+/**
+ * This is the actual parser.
+ * It reads one character at a time
+ * setting in_data.msg_available to TRUE
+ * after a full line.
+ */
+void sbs_parse_char(struct SbsMsgData *data, unsigned char c)
+{
+ //reject empty lines
+ if (data->msg_len == 0) {
+ if (c == '\r' || c == '\n' || c == '$') {
+ return;
+ }
+ }
+
+ // fill the buffer, unless it's full
+ if (data->msg_len < SBS_INPUT_MAXLEN - 1) {
+
+ // messages end with a linefeed
+ //AD: TRUNK: if (c == '\r' || c == '\n')
+ if (c == '\r' || c == '\n') {
+ data->msg_available = 1;
+ } else {
+ data->msg_buf[data->msg_len] = c;
+ data->msg_len++;
+ data->msg_available = 0;
+ }
+ }
+
+ if (data->msg_len >= SBS_INPUT_MAXLEN - 1) {
+ data->msg_available = 1;
+ }
+}
+
+/**
+ * sbs_parse_char() has a complete line.
+ * Find out what type of message it is and
+ * hand it to the parser for that type.
+MSG,4,,,495224,,,,,,,,404,54,,,64,,0,0,0,0
+MSG,1,,,495224,,,,,,TAP594 ,,,,,,,,0,0,0,0
+MSG,8,,,495224,,,,,,,,,,,,,,,,,
+MSG,3,,,495224,,,,,,,37000,,,48.27750,11.68840,,,0,0,0,0
+ */
+void sbs_parse_msg(struct SbsMsgData *data)
+{
+
+ if (data->msg_len > 5 && !strncmp(data->msg_buf , "MSG,4", 5)) {
+ data->msg_buf[data->msg_len] = 0;
+ INPUT_PRINT("parsing MSG 4: \"%s\" \n\r", data->msg_buf);
+ parse_msg4(data);
+ } else {
+ if (data->msg_len > 5 && !strncmp(data->msg_buf , "MSG,3", 5)) {
+ data->msg_buf[data->msg_len] = 0;
+ INPUT_PRINT("parsing MSG 3: \"%s\" \n\r", data->msg_buf);
+ parse_msg3(data);
+ } else {
+ data->msg_buf[data->msg_len] = 0;
+ INPUT_PRINT("ignoring: len=%i \"%s\" \n\r", data->msg_len, data->msg_buf);
+ }
+ }
+
+ // reset message-buffer
+ data->msg_len = 0;
+}
+
+
+static inline void read_until_sep(struct SbsMsgData *data, int *i)
+{
+ while (data->msg_buf[(*i)++] != ',') {
+ if (*i >= data->msg_len) {
+ return;
+ }
+ }
+}
+
+
+/**
+ * parse MSG 4 SBS airborne velocity message
+ *
+ * MSG,4,0,1019,3C65A3,1119,2015/08/14,18:19:17.913,2015/08/14,18:19:17.913,,,511.6,48.6,,,128,,,,,
+ */
+static void parse_msg4(struct SbsMsgData *data)
+{
+ int i = 6; // current position in the message, start after: MSG,4,
+ char *endptr; // end of parsed substrings
+
+ // transmission type
+ //int type = atoi(&data->msg_buf[4]);
+ //if (type != 4) { return; }
+
+ // aircraft ID
+ read_until_sep(data, &i);
+ int aircraft_id = atoi(&data->msg_buf[i]);
+ INPUT_PRINT("MSG4 aircraft_id = %i \n\r", aircraft_id);
+
+ // hex ident
+ read_until_sep(data, &i);
+ // Reading Hex number
+ char hex_ident[10] = "foo";
+ for (int j=0; j < 10; j++) {
+ if (data->msg_buf[i+j] == ',') {
+ hex_ident[j] = '\0';
+ break;
+ }
+ hex_ident[j] = data->msg_buf[i+j];
+ }
+ hex_ident[9] = '\0';
+ INPUT_PRINT("MSG4 hex_ident = %s\n\r", hex_ident);
+ data->msg_buf[i - 2] = '0';
+ data->msg_buf[i - 1] = 'x';
+ //get Flarm intruder ID
+ int __attribute__((unused)) flarm_id = strtod(&data->msg_buf[i - 2], &endptr);
+ INPUT_PRINT("MSG4 flarm_id = %i \n\r", flarm_id);
+
+ // flight ID
+ read_until_sep(data, &i);
+ int flight_id = atoi(&data->msg_buf[i]);
+ INPUT_PRINT("MSG4 flight_id = %i\n\r", flight_id);
+
+ // skip some fields
+ // generated_date, generated_time, logged_date, logged_time, ?, ?
+ int j;
+ for (j = 0; j < 6; j++) {
+ read_until_sep(data, &i);
+ }
+
+ //get intruder Ground speed (knots) and convert to m/s
+ read_until_sep(data, &i);
+ double speed = strtod(&data->msg_buf[i], &endptr) * 0.5144444;
+ INPUT_PRINT("Speed = %f m/s\n\r", speed);
+
+ //get intruder ground track
+ read_until_sep(data, &i);
+ double track = strtod(&data->msg_buf[i], &endptr);
+ INPUT_PRINT("Track = %f deg\n\r", track);
+
+ // next field: empty
+ read_until_sep(data, &i);
+
+ // next field: empty
+ read_until_sep(data, &i);
+
+ // next field: Climb rate
+ read_until_sep(data, &i);
+
+ //get intruder climb rate (feet per minute) and convert to m/s
+ double climb = strtod(&data->msg_buf[i], &endptr) * 0.00508;
+ INPUT_PRINT("Climb = %f m/s\n\r", climb);
+
+
+
+ //Build table of current intruder situation
+
+ //Check if already in list, then replace by new values
+ int z ;
+ int newflag = 1;
+
+ for (z = 0; z < MAX_INTRUDERS; z++) {
+ if (intruders[z].id == aircraft_id) {
+ newflag = 0;
+ //Check for positive or negative vertical speed (not signed :-( )
+ if (intruders[z].lla.alt < intruders[z].lastalt) {
+ //sinking
+ climb = -climb;
+ } else if (intruders[z].lla.alt == intruders[z].lastalt) {
+ //equal.. take last
+ if (intruders[z].climb < 0) {
+ climb = -climb;
+ }
+ }
+
+ intruders[z].gspeed = speed;
+ intruders[z].course = track;
+ intruders[z].climb = climb;
+ gettimeofday(&intruders[z].time4, NULL);
+ }
+
+ }
+
+ //New intruder
+ if (newflag) {
+ intruders[MAX_INTRUDERS-1].id = aircraft_id;
+ intruders[MAX_INTRUDERS-1].flight_id = flight_id;
+ strcpy(intruders[MAX_INTRUDERS-1].name, hex_ident);
+ intruders[MAX_INTRUDERS-1].gspeed = speed;
+ intruders[MAX_INTRUDERS-1].course = track;
+ intruders[MAX_INTRUDERS-1].climb = climb;
+ intruders[MAX_INTRUDERS-1].used = 4;
+ gettimeofday(&intruders[MAX_INTRUDERS-1].time4, NULL);
+ INPUT_PRINT("new = %i \n\r", intruders[MAX_INTRUDERS-1].id);
+ }
+
+ update_intruders(intruders);
+}
+
+
+/**
+ * parse MSG 3 SBS Message (airborne position message)
+ *
+ * MSG,3,0,1019,3C65A3,1119,2015/08/14,18:18:21.714,2015/08/14,18:18:21.714,,35000,,,52.81230,13.39689,,,,0,0,0
+ *
+ * message_type
+ * transmission_type
+ * session_id
+ * aircraft_id = parts[3];
+ * hex_ident = parts[4];
+ * flight_id = parts[5];
+ * generated_date = parts[6];
+ * generated_time = parts[7];
+ * logged_date = parts[8];
+ * logged_time = parts[9];
+ * callsign = parts[10];
+ * altitude = sbs1_value_to_int(parts[11]);
+ * ground_speed = sbs1_value_to_int(parts[12]);
+ * track = sbs1_value_to_int(parts[13]);
+ * lat = sbs1_value_to_float(parts[14]);
+ * lon = sbs1_value_to_float(parts[15]);
+ * vertical_rate = sbs1_value_to_int(parts[16]);
+ * squawk = parts[17];
+ * alert = sbs1_value_to_bool(parts[18]);
+ * emergency = sbs1_value_to_bool(parts[19]);
+ * spi = sbs1_value_to_bool(parts[20]);
+ * is_on_ground = sbs1_value_to_bool(parts[21]);
+ */
+static void parse_msg3(struct SbsMsgData *data)
+{
+
+ int i = 6; // current position in the message, start after: MSG,3,
+ char *endptr; // end of parsed substrings
+ struct LlaCoor_d lla;
+
+ // transmission type
+ //int type = atoi(&data->msg_buf[4]);
+ //if (type != 3) { return; }
+
+ // aircraft ID
+ read_until_sep(data, &i);
+ int aircraft_id = atoi(&data->msg_buf[i]);
+ INPUT_PRINT("MSG3 aircraft_id = %i \n\r", aircraft_id);
+
+ // hex ident
+ read_until_sep(data, &i);
+ //Reading Hex number
+ char hex_ident[10] = "foo";
+ for (int j=0; j < 10; j++) {
+ if (data->msg_buf[i+j] == ',') {
+ hex_ident[j] = '\0';
+ break;
+ }
+ hex_ident[j] = data->msg_buf[i+j];
+ }
+ hex_ident[9] = '\0';
+ INPUT_PRINT("MSG3 hex_ident = %s\n\r", hex_ident);
+ data->msg_buf[i - 2] = '0';
+ data->msg_buf[i - 1] = 'x';
+ //get Flarm intruder ID
+ int __attribute__((unused)) flarm_id = strtod(&data->msg_buf[i - 2], &endptr);
+ INPUT_PRINT("MSG3 flarm_id = %i \n\r", flarm_id);
+
+ // flight ID
+ read_until_sep(data, &i);
+ int flight_id = atoi(&data->msg_buf[i]);
+ INPUT_PRINT("MSG3 flight_id = %i\n\r", flight_id);
+
+ // skip some fields
+ // generated_date, generated_time, logged_date, logged_time, callsign
+ int j;
+ for (j = 0; j < 5; j++) {
+ read_until_sep(data, &i);
+ }
+
+ //get intruder alt (in feet)
+ read_until_sep(data, &i);
+ lla.alt = strtod(&data->msg_buf[i], &endptr) * 0.3048; //feet to m
+ INPUT_PRINT("Alt = %f m\n\r", lla.alt);
+
+ // ground_speed
+ read_until_sep(data, &i);
+
+ // track
+ read_until_sep(data, &i);
+
+ // latitude
+ read_until_sep(data, &i);
+ double lat = strtod(&data->msg_buf[i], &endptr);
+ INPUT_PRINT("Lat = %f deg\n\r", lat);
+ lla.lat = RadOfDeg(lat);
+
+ // longitude
+ read_until_sep(data, &i);
+ double lon = strtod(&data->msg_buf[i], &endptr);
+ INPUT_PRINT("Lon = %f deg\n\r", lon);
+ lla.lon = RadOfDeg(lon);
+
+ // vertical rate
+ read_until_sep(data, &i);
+
+ // not using the rest of the fields atm
+
+
+ //Build table of current intruder situation
+
+ //Check if already in list, then replace by new values
+ int z ;
+ int newflag = 1;
+
+ for (z = 0; z < MAX_INTRUDERS; z++) {
+ if (intruders[z].id == aircraft_id) {
+ newflag = 0;
+ intruders[z].lastalt = intruders[z].lla.alt;
+ intruders[z].lla = lla;
+ gettimeofday(&intruders[z].time3, NULL);
+ }
+
+ }
+
+ //New intruder
+ //If Relative East is empty (value 0), it is a mode C/S transponder, so no position.
+ if (newflag) {
+ intruders[MAX_INTRUDERS-1].id = aircraft_id;
+ intruders[MAX_INTRUDERS-1].flight_id = flight_id;
+ strcpy(intruders[MAX_INTRUDERS-1].name, hex_ident);
+ intruders[MAX_INTRUDERS-1].lla = lla;
+ intruders[MAX_INTRUDERS-1].used = 3;
+ gettimeofday(&intruders[MAX_INTRUDERS-1].time3, NULL);
+ INPUT_PRINT("new = %i \n\r", intruders[MAX_INTRUDERS-1].id);
+ }
+
+ update_intruders(intruders);
+}
+
+void update_intruders(struct Intruder *intr)
+{
+ mark_old_intruders(intr);
+ sort_intruders(intr);
+}
+
+
+/** elapsed time in milliseconds between two timevals */
+static inline unsigned int time_elapsed_ms(struct timeval *prev, struct timeval *now)
+{
+ time_t d_sec = now->tv_sec - prev->tv_sec;
+ long d_usec = now->tv_usec - prev->tv_usec;
+ /* wrap if negative microseconds */
+ if (d_usec < 0) {
+ d_sec -= 1;
+ d_usec += 1000000L;
+ }
+ return d_sec * 1000 + d_usec / 1000;
+}
+
+
+#define MAX_AGE_INTR 5000 ///< max age in ms before intruder is marked as unused
+
+/// mark intruders as unused if data too old
+static void mark_old_intruders(struct Intruder *intr)
+{
+
+ struct timeval now;
+ gettimeofday(&now, NULL);
+
+ //Analyse if Data ok
+ for (int i = 0; i < MAX_INTRUDERS; i++) {
+ //If data too old, mark unused
+ if ((time_elapsed_ms(&intr[i].time3, &now) < MAX_AGE_INTR) &&
+ (time_elapsed_ms(&intr[i].time4, &now) < MAX_AGE_INTR))
+ {
+ intr[i].used = 1 ;
+ } else if (time_elapsed_ms(&intr[i].time3, &now) < MAX_AGE_INTR) {
+ intr[i].used = 3 ;
+ } else if (time_elapsed_ms(&intr[i].time4, &now) < MAX_AGE_INTR) {
+ intr[i].used = 4 ;
+ } else {
+ intr[i].used = 0;
+ }
+ }
+
+ for (int i = 0; i < MAX_INTRUDERS; i++) {
+ INPUT_PRINT("Nr:%i u:%d\n", i, intr[i].used);
+ }
+ INPUT_PRINT("fin \n\r");
+}
+
+
+#define timercmp(a, b, CMP) \
+ (((a)->tv_sec == (b)->tv_sec) ? \
+ ((a)->tv_usec CMP (b)->tv_usec) : \
+ ((a)->tv_sec CMP (b)->tv_sec))
+
+
+/// Sort intruder array for used and last updated
+static void sort_intruders(struct Intruder *intr)
+{
+ //Sort for used and last updated
+ struct Intruder temp_int;
+ //Bubble sort
+ for (int i = 0; i < MAX_INTRUDERS-1; i++) {
+ for (int j = 0; j < MAX_INTRUDERS-1 - i; j++) {
+ if (((intr[j].used == 0) && (intr[j + 1].used != 0)) ||
+ (timercmp(&intr[j].time4, &intr[j + 1].time4, >) && (intr[j + 1].used == 1)))
+ {
+ temp_int = intr[j];
+ intr[j] = intr[j + 1];
+ intr[j + 1] = temp_int;
+ }
+ }
+ }
+}
diff --git a/sw/ground_segment/misc/sbs_parser.h b/sw/ground_segment/misc/sbs_parser.h
new file mode 100644
index 0000000000..c3e7f6a7b5
--- /dev/null
+++ b/sw/ground_segment/misc/sbs_parser.h
@@ -0,0 +1,107 @@
+/*
+ * Copyright (C) 2013 Marc Schwarzbach
+ * 2015 Felix Ruess
+ *
+ * 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
+ * .
+ */
+/**
+ * @file sbs_parser.h
+ * Parser for the SBS-1 protocol (ADS-B data).
+ *
+ *
+ * ## Message Types
+ * // Generated when the user changes the selected aircraft in
+ * // BaseStation.
+ * SELECTION_CHANGE: 'SEL',
+ * // Generated when an aircraft being tracked sets or changes its
+ * // callsign.
+ * NEW_ID: 'ID',
+ * // Generated when the SBS picks up a signal for an aircraft that it
+ * // isn't currently tracking,
+ * NEW_AIRCRAFT: 'AIR',
+ * // Generated when an aircraft's status changes according to the
+ * // time-out values in the SBS1 Data Settings menu.
+ * STATUS_CHANGE: 'STA',
+ * // Generated when the user double-clicks (or presses return) on an
+ * // aircraft (i.e. to bring up the aircraft details window).
+ * CLICK: 'CLK',
+ * // Generated by the aircraft. There are eight different MSG
+ * // transmission types, see `TransmissionType`.
+ * TRANSMISSION: 'MSG'
+ *
+ * ## Transmission Types
+ *
+ * Transmission messages (MSG) from aircraft may be one of eight types
+ * (ES = Extended Squitter, DF = Downlink Format, BDS = B-Definition
+ * Subfield).
+ *
+ * |Type|Description |Spec |
+ * |----|--------------------------------|--------------|
+ * | 1 | ES identification and category | DF17 BDS 0,8 |
+ * | 2 | ES surface position message | DF17 BDS 0,6 |
+ * | 3 | ES airborne position message | DF17 BDS 0,5 |
+ * | 4 | ES airborne velocity message | DF17 BDS 0,9 |
+ * | 5 | Surveillance alt message | DF4, DF20 |
+ * | 6 | Surveillance ID message | DF5, DF21 |
+ * | 7 | Air-to-air message | DF16 |
+ * | 8 | All call reply | DF11 |
+ *
+ */
+
+#ifndef SBS_PARSER_H_
+#define SBS_PARSER_H_
+
+#include "math/pprz_geodetic_double.h"
+
+#include
+
+#define SBS_INPUT_MAXLEN 255
+#define INTRUDER_NAME_MAXLEN 20
+
+struct SbsMsgData {
+ int msg_available;
+ int pos_available;
+ int nb_ovrn; // number if incomplete messages
+ char msg_buf[SBS_INPUT_MAXLEN]; ///< buffer for storing one line
+ int msg_len;
+};
+
+/// data structure for intruders
+struct Intruder {
+ int id;
+ int flight_id;
+ char name[INTRUDER_NAME_MAXLEN];
+ struct LlaCoor_d lla;
+ int lastalt;
+ float gspeed;
+ float course;
+ float climb;
+ struct timeval time4;
+ struct timeval time3;
+ int used;
+};
+
+#define MAX_INTRUDERS 20
+extern struct Intruder intruders[MAX_INTRUDERS];
+
+void sbs_parse_msg(struct SbsMsgData *data);
+void sbs_parse_char(struct SbsMsgData *data, unsigned char c);
+
+void update_intruders(struct Intruder *intr);
+
+
+#endif /* SBS_PARSER_H_ */
diff --git a/sw/ground_segment/tmtc/Makefile b/sw/ground_segment/tmtc/Makefile
index 78bb3642f3..660b86fafc 100644
--- a/sw/ground_segment/tmtc/Makefile
+++ b/sw/ground_segment/tmtc/Makefile
@@ -37,7 +37,7 @@ XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib
LIBMULTIMONCMA=../multimon/multimon.cma
LIBMULTIMONDLL= multimon.cma -dllpath $(PAPARAZZI_SRC)/sw/ground_segment/multimon
-SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_server.ml rotorcraft_server.ml server.cmo
+SERVERCMO = server_globals.cmo aircraft.cmo wind.cmo airprox.cmo kml.cmo fw_server.ml rotorcraft_server.ml intruder.cmo server.cmo
SERVERCMX = $(SERVERCMO:.cmo=.cmx)
diff --git a/sw/ground_segment/tmtc/intruder.ml b/sw/ground_segment/tmtc/intruder.ml
new file mode 100644
index 0000000000..fc9697add0
--- /dev/null
+++ b/sw/ground_segment/tmtc/intruder.ml
@@ -0,0 +1,50 @@
+(*
+ * Copyright (C) 2015 Felix Ruess
+ *
+ * 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.
+ *
+ *)
+
+open Latlong
+
+type intruder = {
+ id : string;
+ name : string;
+ mutable pos : Latlong.geographic;
+ mutable unix_time : float;
+ mutable itow : int64; (* ms *)
+ mutable roll : float;
+ mutable pitch : float;
+ mutable heading : float; (* rad, CW 0=N *)
+ mutable gspeed : float; (* m/s *)
+ mutable airspeed : float; (* m/s *)
+ mutable course : float; (* rad *)
+ mutable alt : float;
+ mutable agl : float;
+ mutable climb : float
+}
+
+let new_intruder = fun id name ->
+ {
+ id = id; name = name;
+ pos = { Latlong.posn_lat = 0.; posn_long = 0. };
+ unix_time = 0.; itow = Int64.of_int 0;
+ roll=0.; pitch=0.; heading=0.;
+ gspeed=0.; airspeed= -1.; course = 0.;
+ alt=0.; climb=0.; agl = 0.
+ }
diff --git a/sw/ground_segment/tmtc/intruder.mli b/sw/ground_segment/tmtc/intruder.mli
new file mode 100644
index 0000000000..902bf8250e
--- /dev/null
+++ b/sw/ground_segment/tmtc/intruder.mli
@@ -0,0 +1,42 @@
+(*
+ * Copyright (C) 2015 Felix Ruess
+ *
+ * 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.
+ *
+ *)
+
+(** State of an intruder (external aircraft) handled by the server *)
+
+type intruder = {
+ id : string;
+ name : string;
+ mutable pos : Latlong.geographic;
+ mutable unix_time : float;
+ mutable itow : int64; (* ms *)
+ mutable roll : float;
+ mutable pitch : float;
+ mutable heading : float; (* rad, CW 0=N *)
+ mutable gspeed : float; (* m/s *)
+ mutable airspeed : float; (* m/s *)
+ mutable course : float; (* rad *)
+ mutable alt : float;
+ mutable agl : float;
+ mutable climb : float
+}
+
+val new_intruder : string -> string -> intruder
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index 5a1a792661..20bb843d9a 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -31,6 +31,7 @@ open Printf
open Latlong
open Server_globals
open Aircraft
+(*open Intruder*)
module U = Unix
module LL = Latlong
@@ -475,6 +476,9 @@ let replayed = fun ac_id ->
(* Store of unknown received A/C ids. To be able to report an error only once *)
let unknown_aircrafts = Hashtbl.create 5
+(* Intruders (external aircrafts), e.g. received via ADS-B *)
+let intruders = Hashtbl.create 3
+
let get_conf = fun real_id id conf_xml ->
try
ExtXml.child conf_xml "aircraft" ~select:(fun x -> ExtXml.attrib x "ac_id" = id)
@@ -649,6 +653,60 @@ let listen_acs = fun log timestamp ->
if !replay_old_log then
ignore (Tm_Pprz.message_bind "PPRZ_MODE" (ident_msg log timestamp))
+let send_intruder_acinfo = fun id intruder ->
+ let cm_of_m_32 = fun f -> Pprz.Int32 (Int32.of_int (truncate (100. *. f))) in
+ let cm_of_m = fun f -> Pprz.Int (truncate (100. *. f)) in
+ let pos = LL.utm_of WGS84 intruder.Intruder.pos in
+ (* TODO: find a better way to map intruders to AC_IDs *)
+ let ac_id = 200 + ((int_of_string id) mod 50) in
+ let ac_info = ["ac_id", Pprz.Int ac_id;
+ "utm_east", cm_of_m_32 pos.utm_x;
+ "utm_north", cm_of_m_32 pos.utm_y;
+ "course", Pprz.Int (truncate (10. *. (Geometry_2d.rad2deg intruder.Intruder.course)));
+ "alt", cm_of_m_32 intruder.Intruder.alt;
+ "speed", cm_of_m intruder.Intruder.gspeed;
+ "climb", cm_of_m intruder.Intruder.climb;
+ "itow", Pprz.Int64 intruder.Intruder.itow] in
+ Dl_Pprz.message_send my_id "ACINFO" ac_info
+
+let periodic_handle_intruders = fun () ->
+ (* remove old intruders after 10s *)
+ Hashtbl.iter
+ (fun id i ->
+ if (U.gettimeofday () -. i.Intruder.unix_time) > 10.0 then begin
+ (*prerr_endline (sprintf "remove intruder %s" id);*)
+ Hashtbl.remove intruders id
+ end;
+ ) intruders;
+ (* send ACINFO for each active intruder *)
+ Hashtbl.iter (send_intruder_acinfo) intruders
+
+let add_intruder = fun vs ->
+ let id = Pprz.string_assoc "id" vs in
+ let name = Pprz.string_assoc "name" vs in
+ let intruder = Intruder.new_intruder id name in
+ Hashtbl.add intruders id intruder
+
+let update_intruder = fun logging _sender vs ->
+ let id = Pprz.string_assoc "id" vs in
+ (*prerr_endline (sprintf "update_intruder %s" id);*)
+ if not (Hashtbl.mem intruders id) then
+ add_intruder vs;
+ let i = Hashtbl.find intruders id in
+ let lat = Pprz.int_assoc "lat" vs
+ and lon = Pprz.int_assoc "lon" vs in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
+ i.Intruder.pos <- geo;
+ i.Intruder.alt <- float (Pprz.int_assoc "alt" vs) /. 1000.;
+ i.Intruder.course <- Pprz.float_assoc "course" vs;
+ i.Intruder.gspeed <- Pprz.float_assoc "speed" vs;
+ i.Intruder.climb <- Pprz.float_assoc "climb" vs;
+ i.Intruder.unix_time <- U.gettimeofday ();
+ log logging "ground" "INTRUDER" vs
+
+(* listen for intruders and log them *)
+let listen_intruders = fun log ->
+ ignore(Ground_Pprz.message_bind "INTRUDER" (update_intruder log))
let send_config = fun http _asker args ->
let ac_id' = Pprz.string_assoc "ac_id" args in
@@ -824,9 +882,15 @@ let () =
(* Waits for new aircrafts *)
listen_acs logging !timestamp;
+ (* wait for new external vehicles/intruders *)
+ listen_intruders logging;
+
(* Forward messages from ground agents to vehicles *)
ground_to_uplink logging;
+ (* call periodic_handle_intruders every second *)
+ ignore (Glib.Timeout.add 1000 (fun () -> periodic_handle_intruders (); true));
+
(* Waits for client configurations requests on the Ivy bus *)
ivy_server !http;
diff --git a/sw/lib/ocaml/acIcon.ml b/sw/lib/ocaml/acIcon.ml
index fb97ec554a..d959924ac3 100644
--- a/sw/lib/ocaml/acIcon.ml
+++ b/sw/lib/ocaml/acIcon.ml
@@ -167,6 +167,17 @@ let icon_home_template = {
width = 3;
}
+let icon_intruder_template = {
+ lines = [
+ [| 0.; 0.; 0.; -24. |];
+ [| 6.; -15.; 0.; -24.; -6.; -15.|]; (** Front Marker **)
+ ];
+ ellipse = [
+ [| -8.; -8.; 8.; 8.|];
+ ];
+ width = 1
+}
+
class widget = fun ?(color="red") ?(icon_template=icon_fixedwing_template) (group:GnoCanvas.group) ->
let new_line width color points =
GnoCanvas.line ~fill_color:color ~props:[`WIDTH_PIXELS width; `CAP_STYLE `ROUND] ~points:points group in
diff --git a/sw/lib/ocaml/acIcon.mli b/sw/lib/ocaml/acIcon.mli
index e906d3a83d..5158563d4f 100644
--- a/sw/lib/ocaml/acIcon.mli
+++ b/sw/lib/ocaml/acIcon.mli
@@ -38,6 +38,7 @@ val icon_quadrotor_x_template : icon
val icon_hexarotor_x_template : icon
val icon_octorotor_x_template : icon
val icon_home_template : icon
+val icon_intruder_template : icon
class widget :
?color : string ->
diff --git a/sw/lib/ocaml/mapTrack.ml b/sw/lib/ocaml/mapTrack.ml
index 47e2b680fe..4eacf9c52d 100644
--- a/sw/lib/ocaml/mapTrack.ml
+++ b/sw/lib/ocaml/mapTrack.ml
@@ -53,7 +53,7 @@ type desired =
| DesiredCircle of LL.geographic*float*GnoCanvas.ellipse
| DesiredSegment of LL.geographic*LL.geographic*GnoCanvas.line
-class track = fun ?(name="Noname") ?(icon="fixedwing") ?(size = 500) ?(color="red") (ac_id:string) (geomap:MapCanvas.widget) ->
+class track = fun ?(name="Noname") ?(icon="fixedwing") ?(size = 500) ?(color="red") ?(show_carrot=true) (ac_id:string) (geomap:MapCanvas.widget) ->
let group = GnoCanvas.group geomap#canvas#root in
let empty = ({LL.posn_lat=0.; LL.posn_long=0.}, GnoCanvas.line group) in
let v_empty = ({LL.posn_lat=0.; LL.posn_long=0.}, 0.0) in
@@ -70,6 +70,7 @@ class track = fun ?(name="Noname") ?(icon="fixedwing") ?(size = 500) ?(color="re
| "hexarotor_x" -> ACI.icon_hexarotor_x_template
| "octorotor_x" -> ACI.icon_octorotor_x_template
| "flyingwing" -> ACI.icon_flyingwing_template
+ | "intruder" -> ACI.icon_intruder_template
| "fixedwing" | _ -> ACI.icon_fixedwing_template
in
let _ac_icon = new ACI.widget ~color ~icon_template aircraft in
@@ -77,7 +78,10 @@ class track = fun ?(name="Noname") ?(icon="fixedwing") ?(size = 500) ?(color="re
let carrot = GnoCanvas.group group in
let _ac_carrot =
- ignore (GnoCanvas.polygon ~points:[|0.;0.;-5.;-10.;5.;-10.|] ~props:[`WIDTH_UNITS 1.;`FILL_COLOR "orange"; `OUTLINE_COLOR "orange"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] carrot) in
+ if show_carrot then
+ ignore (GnoCanvas.polygon ~points:[|0.;0.;-5.;-10.;5.;-10.|] ~props:[`WIDTH_UNITS 1.;`FILL_COLOR "orange"; `OUTLINE_COLOR "orange"; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] carrot)
+ else ()
+ in
let cam = GnoCanvas.group group in
@@ -125,11 +129,14 @@ object (self)
val zone = GnoCanvas.rect group
val mutable ac_cam_cover = GnoCanvas.rect ~fill_color:"grey" ~props:[`WIDTH_PIXELS 1 ; `FILL_STIPPLE (Gdk.Bitmap.create_from_data ~width:2 ~height:2 "\002\001")] cam
val mutable event_cb = None
+ val mutable destroyed = false
method color = color
method set_color c = color <- c
method track = track
method v_path = v_path
method aircraft = aircraft
+ method id = ac_id
+ method name = name
method set_label = fun s ->
ac_label#set_name s
method clear_one = fun i ->
@@ -358,6 +365,13 @@ object (self)
method set_event_cb = fun (cb: string -> unit) -> event_cb <- Some cb
initializer
- ignore(geomap#zoom_adj#connect#value_changed
- (fun () -> self#zoom geomap#zoom_adj#value))
+ (* could not properly disconnect adjustment signal, so only calling zoom method if group is still displayed *)
+ ignore(geomap#zoom_adj#connect#value_changed (fun () -> if not destroyed then self#zoom geomap#zoom_adj#value));
+ ignore(group#connect#destroy (fun () -> destroyed <- true))
+
+ (* destroy method *)
+ method destroy = fun () -> group#destroy ()
+
+ initializer
+ Gc.finalise (fun self -> self#destroy ()) self
end
diff --git a/sw/lib/ocaml/mapTrack.mli b/sw/lib/ocaml/mapTrack.mli
index 7ced8b47b9..aed95ad31e 100644
--- a/sw/lib/ocaml/mapTrack.mli
+++ b/sw/lib/ocaml/mapTrack.mli
@@ -27,11 +27,14 @@ class track :
?icon:string ->
?size:int ->
?color:string ->
+ ?show_carrot:bool ->
string ->
MapCanvas.widget ->
object
method add_point : Latlong.geographic -> float -> unit
method aircraft : GnoCanvas.group
+ method id : string
+ method name : string
method clear : unit -> unit
method clear_map2D : unit
method clear_one : int -> unit
@@ -67,4 +70,5 @@ class track :
method zoom : float -> unit
method event : GnoCanvas.item_event -> bool
method set_event_cb : (string -> unit) -> unit
+ method destroy : unit -> unit
end