mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 20:38:27 +08:00
Merge pull request #1372 from flixr/intruders
Add ADS-B parser and handling of INTRUDERS
This commit is contained in:
Gautier Hattenberger
+2
-2
@@ -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
|
||||
|
||||
@@ -57,7 +57,9 @@
|
||||
<arg flag="4242" />
|
||||
<arg flag="4252" />
|
||||
</program>
|
||||
|
||||
<program name="ADS-B Intruders receiver" command="sw/ground_segment/misc/sbs2ivy">
|
||||
<arg flag="--ac" constant="@AC_ID"/>
|
||||
</program>
|
||||
</section>
|
||||
|
||||
<section name="sessions">
|
||||
|
||||
@@ -2818,6 +2818,18 @@
|
||||
<field name="ping_time" type="float" format="%.2f" unit="ms"/>
|
||||
</message>
|
||||
|
||||
<message name="INTRUDER" id="37">
|
||||
<field name="id" type="string"/>
|
||||
<field name="name" type="string"/>
|
||||
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
|
||||
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
|
||||
<field name="alt" type="int32" unit="mm" alt_unit="m">altitude above WGS84 reference ellipsoid</field>
|
||||
<field name="course" type="float" unit="deg"/>
|
||||
<field name="speed" type="float" unit="m/s"/>
|
||||
<field name="climb" type="float" unit="m/s"/>
|
||||
<field name="itow" type="uint32" unit="ms"/>
|
||||
</message>
|
||||
|
||||
<message name="PLUMES" id="100">
|
||||
<field name="ids" type="string" format="csv"/>
|
||||
<field name="lats" type="string" format="csv"/>
|
||||
|
||||
@@ -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); }
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -0,0 +1,61 @@
|
||||
(*
|
||||
* Copyright (C) 2015 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* 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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*)
|
||||
|
||||
(*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
|
||||
|
||||
@@ -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));
|
||||
|
||||
@@ -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) $<
|
||||
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,457 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Marc Schwarzbach
|
||||
* 2015 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*
|
||||
*/
|
||||
|
||||
/**
|
||||
* @file sbs_parser.c
|
||||
* Parser for the SBS-1 protocol (ADS-B data).
|
||||
*/
|
||||
|
||||
#include "sbs_parser.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Marc Schwarzbach
|
||||
* 2015 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/**
|
||||
* @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 <sys/time.h>
|
||||
|
||||
#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_ */
|
||||
@@ -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)
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,50 @@
|
||||
(*
|
||||
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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.
|
||||
}
|
||||
@@ -0,0 +1,42 @@
|
||||
(*
|
||||
* Copyright (C) 2015 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* 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
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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 ->
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user