diff --git a/conf/messages.xml b/conf/messages.xml
index 0678892a30..04307f9269 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -663,6 +663,19 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -843,6 +856,7 @@
+
diff --git a/sw/airborne/datalink.c b/sw/airborne/datalink.c
index 58b4e62e95..d3c3f35e56 100644
--- a/sw/airborne/datalink.c
+++ b/sw/airborne/datalink.c
@@ -65,7 +65,8 @@ void dl_parse_msg(void) {
float a = MOfCm(DL_ACINFO_alt(dl_buffer));
float c = RadOfDeg(((float)DL_ACINFO_course(dl_buffer))/ 10.);
float s = MOfCm(DL_ACINFO_speed(dl_buffer));
- SetAcInfo(id, ux, uy, c, a, s);
+ uint32_t t = DL_ACINFO_itow(dl_buffer);
+ SetAcInfo(id, ux, uy, c, a, s, t);
} else
#endif
#ifdef NAV
diff --git a/sw/airborne/sim/sim_ap.c b/sw/airborne/sim/sim_ap.c
index b079de93b7..d05f3b9dff 100644
--- a/sw/airborne/sim/sim_ap.c
+++ b/sw/airborne/sim/sim_ap.c
@@ -104,17 +104,17 @@ value get_commands(value val_commands) {
return Val_int(commands[COMMAND_THROTTLE]);
}
-value set_ac_info_native(value ac_id __attribute__ ((unused)), value ux __attribute__ ((unused)), value uy __attribute__ ((unused)), value course __attribute__ ((unused)), value alt __attribute__ ((unused)), value gspeed __attribute__ ((unused))) {
+value set_ac_info_native(value ac_id __attribute__ ((unused)), value ux __attribute__ ((unused)), value uy __attribute__ ((unused)), value course __attribute__ ((unused)), value alt __attribute__ ((unused)), value gspeed __attribute__ ((unused)), value itow __attribute__ ((unused))) {
#ifdef TRAFFIC_INFO
SetAcInfo(Int_val(ac_id), Double_val(ux), Double_val(uy),
- Double_val(course), Double_val(alt), Double_val(gspeed));
+ Double_val(course), Double_val(alt), Double_val(gspeed), Int32_val(itow));
#endif
return Val_unit;
}
value set_ac_info(value * argv, int argn) {
- assert (argn == 6);
- return set_ac_info_native(argv[0], argv[1], argv[2], argv[3],argv[4], argv[5]);
+ assert (argn == 7);
+ return set_ac_info_native(argv[0], argv[1], argv[2], argv[3],argv[4], argv[5], argv[6]);
}
diff --git a/sw/airborne/traffic_info.h b/sw/airborne/traffic_info.h
index 07c5a9c600..29c2da2f13 100644
--- a/sw/airborne/traffic_info.h
+++ b/sw/airborne/traffic_info.h
@@ -37,21 +37,22 @@ struct ac_info_ {
float course; /* rad (CW) */
float alt; /* m */
float gspeed; /* m/s */
+ uint32_t itow; /* ms */
};
extern struct ac_info_ the_acs[NB_ACS];
-#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/) { \
+#define SetAcInfo(_id, _utm_x /*m*/, _utm_y /*m*/, _course/*rad(CW)*/, _alt/*m*/,_gspeed/*m/s*/, _itow) { \
if (_id < NB_ACS) { \
the_acs[_id].east = _utm_x - nav_utm_east0; \
the_acs[_id].north = _utm_y - nav_utm_north0; \
the_acs[_id].course = _course; \
the_acs[_id].alt = _alt; \
the_acs[_id].gspeed = _gspeed; \
+ the_acs[_id].itow = (uint32_t)_itow; \
} \
}
-
struct ac_info_ *
get_ac_info(uint8_t id);
diff --git a/sw/ground_segment/tmtc/aircraft.ml b/sw/ground_segment/tmtc/aircraft.ml
index 76c4ef31f7..e079d33fc3 100644
--- a/sw/ground_segment/tmtc/aircraft.ml
+++ b/sw/ground_segment/tmtc/aircraft.ml
@@ -79,6 +79,7 @@ type aircraft = {
airframe : Xml.xml;
mutable pos : Latlong.utm;
mutable unix_time : float;
+ mutable itow : int32; (* ms *)
mutable roll : float;
mutable pitch : float;
mutable nav_ref : Latlong.utm option;
@@ -139,7 +140,7 @@ let new_aircraft = fun id name fp airframe ->
desired_altitude = 0.;
desired_climb = 0.;
pos = { Latlong.utm_x = 0.; utm_y = 0.; utm_zone = 0 };
- unix_time = 0.;
+ unix_time = 0.; itow = Int32.of_int 0;
nav_ref = None;
cam = { phi = 0.; theta = 0. ; target=(0.,0.)};
inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.};
diff --git a/sw/ground_segment/tmtc/aircraft.mli b/sw/ground_segment/tmtc/aircraft.mli
index e3a823ed5a..6b011d8440 100644
--- a/sw/ground_segment/tmtc/aircraft.mli
+++ b/sw/ground_segment/tmtc/aircraft.mli
@@ -65,6 +65,7 @@ type aircraft = {
airframe : Xml.xml;
mutable pos : Latlong.utm;
mutable unix_time : float;
+ mutable itow : int32;
mutable roll : float;
mutable pitch : float;
mutable nav_ref : Latlong.utm option;
diff --git a/sw/ground_segment/tmtc/link.ml b/sw/ground_segment/tmtc/link.ml
index d4d1f100e9..fe3d9451d1 100644
--- a/sw/ground_segment/tmtc/link.ml
+++ b/sw/ground_segment/tmtc/link.ml
@@ -314,18 +314,21 @@ let get_fp = fun device _sender vs ->
Debug.trace 'b' (sprintf "ACINFO %d for %d" ac_id dest_id);
let ac_device = airborne_device dest_id airframes device.transport in
let f = fun a -> Pprz.float_assoc a vs in
+ let i32 = fun a -> Pprz.int32_assoc a vs in
let lat = (Deg>>Rad) (f "lat")
and long = (Deg>>Rad) (f "long")
and course = f "course"
and alt = f "alt"
- and gspeed = f "speed" in
+ and gspeed = f "speed"
+ and itow = i32 "itow" in
let utm = Latlong.utm_of WGS84 {posn_lat=lat; posn_long=long} in
let vs = ["ac_id", Pprz.Int ac_id;
"utm_east", cm_of_m utm.utm_x;
"utm_north", cm_of_m utm.utm_y;
"course", Pprz.Int (truncate (10. *. course));
"alt", cm_of_m alt;
- "speed", cm_of_m gspeed] in
+ "speed", cm_of_m gspeed;
+ "itow", Pprz.Int32 itow] in
let msg_id, _ = Dl_Pprz.message_of_name "ACINFO" in
let s = Dl_Pprz.payload_of_values msg_id my_id vs in
send dest_id device ac_device s Low
diff --git a/sw/ground_segment/tmtc/server.ml b/sw/ground_segment/tmtc/server.ml
index c3649b044a..7dcab58c23 100644
--- a/sw/ground_segment/tmtc/server.ml
+++ b/sw/ground_segment/tmtc/server.ml
@@ -244,9 +244,10 @@ let log_and_parse = fun logging ac_name (a:Aircraft.aircraft) msg values ->
utm_y = fvalue "utm_north" /. 100.;
utm_zone = ivalue "utm_zone" };
a.unix_time <- Latlong.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
+ a.itow <- Int32.of_float (fvalue "itow");
+ (*Printf.fprintf stderr "itow %lu %ld\n" a.itow a.itow;*)
a.gspeed <- fvalue "speed" /. 100.;
a.course <- norm_course ((Deg>>Rad)(fvalue "course" /. 10.));
-
a.agl <- a.alt -. float (try Srtm.of_utm a.pos with _ -> 0);
a.gps_mode <- check_index (ivalue "mode") gps_modes "GPS_MODE";
if a.gspeed > 3. && a.ap_mode = _AUTO2 then
@@ -553,6 +554,7 @@ let send_aircraft_msg = fun ac ->
"lat", f ((Rad>>Deg)wgs84.posn_lat);
"long", f ((Rad>>Deg) wgs84.posn_long);
"unix_time", f a.unix_time;
+ "itow", Pprz.Int32 a.itow;
"speed", f a.gspeed;
"course", f (Geometry_2d.rad2deg a.course);
"alt", f a.alt;