Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #999 from paparazzi/link_report

Browse Source 
[link] improve datalink/telemetry report

- more reported data about up/down-link
- cleanup report messages
- display link page in GCS even in single link mode

closes #442
This commit is contained in:
Felix Ruess
2014-12-03 20:15:20 +01:00
parent e94c4b3cfc c6cbabd931
commit 863e1d4cd2
22 changed files with 291 additions and 202 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+43 -37
View File
@@ -183,25 +183,8 @@
<field name="segment_north_2" type="float" unit="m"/>
</message>
<message name="DOWNLINK_STATUS" id="23">
<field name="link_id" type="int8"/>
<field name="run_time" type="uint32" unit="s"/>
<field name="rx_bytes" type="uint32"/>
<field name="rx_msgs" type="uint32"/>
<field name="rx_err" type="uint32"/>
<field name="rx_bytes_rate" type="float" format="%.1f"/>
<field name="rx_msgs_rate" type="float" format="%.1f"/>
<field name="ping_time" type="float" format="%.2f" unit="ms"/>
</message>
<message name="MODEM_STATUS" id="24">
<field name="detected" type="uint8"/>
<field name="valim" type="float" format="%.1f"/>
<field name="cd" type="uint8"/>
<field name="nb_byte" type="uint32"/>
<field name="nb_msg" type="uint32"/>
<field name="nb_err" type="uint32"/>
</message>
<!-- ID 23 is free -->
<!-- ID 24 is free -->
<message name="SVINFO" id="25">
<field name="chn" type="uint8"/>
@@ -238,10 +221,13 @@
<field name="flying" type="uint8" unit="bool"/>
</message>
<message name="DOWNLINK" id="30">
<field name="nb_ovrn" type="uint8"/>
<field name="rate" type="uint16" unit="bytes/s"/>
<field name="nb_msgs" type="uint16"/>
<message name="DATALINK_REPORT" id="30">
<description>Datalink status reported by an aircraft for the ground</description>
<field name="uplink_lost_time" type="uint16" unit="s"/>
<field name="uplink_nb_msgs" type="uint16"/>
<field name="downlink_nb_msgs" type="uint16"/>
<field name="downlink_rate" type="uint16" unit="bytes/s"/>
<field name="downlink_ovrn" type="uint8"/>
</message>
<!-- Current value of one variable from the dl_settings (airframe.xml) -->
@@ -601,18 +587,6 @@
<field name="duty" type="uint8" unit="%"/>
</message>
<message name="LINK_STATUS" id="71">
<field name="link_id" type="uint8"/>
<field name="time_since_last_msg" type="float" unit="s"/>
<field name="run_time" type="uint32" unit="s"/>
<field name="rx_bytes" type="uint32"/>
<field name="rx_msgs" type="uint32"/>
<field name="rx_err" type="uint32"/>
<field name="rx_bytes_rate" type="float" format="%.1f"/>
<field name="rx_msgs_rate" type="float" format="%.1f"/>
<field name="ping_time" type="float" format="%.2f" unit="ms"/>
</message>
<message name="SUPERBITRF" id="72">
<field name="status" type="uint8" values="UNINIT|INIT_BINDING|INIT_TRANSFER|BINDING|SYNCING_A|SYNCING_B|TRANSFER"/>
<field name="cyrf_status" type="uint8" values="UNINIT|IDLE|GET_MFG_ID|MULTIWRITE|DATA_CODE|CHAN_SOP_DATA_CRC|RX_IRQ_STATUS_PACKET|SEND"/>
@@ -2768,27 +2742,59 @@
</message>
<message name="TELEMETRY_STATUS" id="32">
<description>
Datalink status reported by Server for the GCS
Combines DATLINK_REPORT (telemetry class) and LINK_REPORT (ground class)
</description>
<field name="ac_id" type="string"/>
<field name="link_id" type="string"/>
<field name="time_since_last_msg" type="float" unit="s"/>
<field name="rx_bytes" type="uint32"/>
<field name="rx_msgs" type="uint32"/>
<field name="rx_bytes_rate" type="float" format="%.1f" unit="bytes/s"/>
<field name="tx_msgs" type="uint32"/>
<field name="uplink_lost_time" type="uint32" unit="s"/>
<field name="uplink_msgs" type="uint16"/>
<field name="downlink_msgs" type="uint16"/>
<field name="downlink_rate" type="uint16" unit="bytes/s"/>
<field name="ping_time" type="float" format="%.2f" unit="ms"/>
</message>
<message name="TELEMETRY_ERROR" id="33">
<description>Report a telemetry error</description>
<field name="ac_id" type="string"/>
<field name="message" type="string"/>
</message>
<message name="TELEMETRY_MESSAGE" id="34">
<message name="TELEMETRY_MESSAGE" id="34">
<description>Encapsulated a telemetry class message (when using redundant link)</description>
<field name="ac_id" type="string"/>
<field name="link_id" type="string" />
<field name="message" type="string" format=";sv"/>
</message>
</message>
<message name="DATALINK_MESSAGE" id="35">
<description>Encapsulated a datalink class message (when using redundant link)</description>
<field name="ac_id" type="string"/>
<field name="link_id" type="string"/>
<field name="message" type="string" format=";sv"/>
</message>
<message name="LINK_REPORT" id="36">
<description>Datalink status reported by Link for the Server</description>
<field name="ac_id" type="string"/>
<field name="link_id" type="uint8"/>
<field name="run_time" type="uint32" unit="s"/>
<field name="rx_lost_time" type="uint32" unit="s"/>
<field name="rx_bytes" type="uint32"/>
<field name="rx_msgs" type="uint32"/>
<field name="rx_err" type="uint32"/>
<field name="rx_bytes_rate" type="float" format="%.1f" unit="bytes/s"/>
<field name="rx_msgs_rate" type="float" format="%.1f" unit="msgs/s"/>
<field name="tx_msgs" type="uint32"/>
<field name="ping_time" type="float" format="%.2f" unit="ms"/>
</message>
<message name="PLUMES" id="100">
<field name="ids" type="string" format="csv"/>
<field name="lats" type="string" format="csv"/>
conf/telemetry/default_ardrone.xml
+1
View File
@@ -18,6 +18,7 @@
<message name="INS" period=".25"/>
<message name="I2C_ERRORS" period="4.1"/>
<message name="UART_ERRORS" period="3.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
</mode>
<mode name="ppm">
conf/telemetry/default_fixedwing.xml
+3 -3
View File
@@ -20,7 +20,7 @@
<message name="PPRZ_MODE" period="5."/>
<message name="SETTINGS" period="5."/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="IMU_GYRO" period="1.1"/>
@@ -46,7 +46,7 @@
<message name="NAVIGATION" period="3."/>
<message name="PPRZ_MODE" period="5."/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="IMU_GYRO" period="10.1"/>
@@ -62,7 +62,7 @@
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="STATE_FILTER_STATUS" period="7."/>
<message name="DOWNLINK" period="5.7"/>
<message name="DATALINK_REPORT" period="5.7"/>
</mode>
<mode name="raw_sensors">
<message name="DL_VALUE" period="0.5"/>
conf/telemetry/default_fixedwing_imu.xml
+4 -4
View File
@@ -20,7 +20,7 @@
<message name="PPRZ_MODE" period="4.9"/>
<message name="SETTINGS" period="5."/>
<message name="STATE_FILTER_STATUS" period="2.2"/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="SURVEY" period="2.1"/>
@@ -48,7 +48,7 @@
<message name="NAVIGATION" period="3."/>
<message name="PPRZ_MODE" period="5."/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="SURVEY" period="2.1"/>
@@ -63,7 +63,7 @@
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="7.5"/>
<message name="STATE_FILTER_STATUS" period="8."/>
<message name="DOWNLINK" period="5.7"/>
<message name="DATALINK_REPORT" period="5.7"/>
</mode>
<mode name="raw_sensors">
<message name="DL_VALUE" period="0.5"/>
@@ -90,7 +90,7 @@
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.7"/>
<message name="DATALINK_REPORT" period="5.7"/>
<message name="IMU_ACCEL" period=".5"/>
<message name="IMU_GYRO" period=".5"/>
<message name="IMU_MAG" period=".5"/>
conf/telemetry/default_fixedwing_imu_9k6.xml
+4 -4
View File
@@ -20,7 +20,7 @@
<message name="PPRZ_MODE" period="5."/>
<message name="SETTINGS" period="5."/>
<message name="STATE_FILTER_STATUS" period="2.2"/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.2"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="SURVEY" period="2.1"/>
@@ -46,7 +46,7 @@
<message name="NAVIGATION" period="3."/>
<message name="PPRZ_MODE" period="5."/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="5.2"/>
<message name="SURVEY" period="2.1"/>
@@ -61,7 +61,7 @@
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="7.5"/>
<message name="STATE_FILTER_STATUS" period="8."/>
<message name="DOWNLINK" period="5.7"/>
<message name="DATALINK_REPORT" period="5.7"/>
</mode>
<mode name="raw_sensors">
<message name="DL_VALUE" period="0.5"/>
@@ -88,7 +88,7 @@
<message name="NAVIGATION" period="5.4"/>
<message name="PPRZ_MODE" period="5.5"/>
<message name="STATE_FILTER_STATUS" period="5."/>
<message name="DOWNLINK" period="5.7"/>
<message name="DATALINK_REPORT" period="5.7"/>
<message name="IMU_ACCEL" period=".5"/>
<message name="IMU_GYRO" period=".5"/>
<message name="IMU_MAG" period=".5"/>
conf/telemetry/default_rotorcraft.xml
+1
View File
@@ -20,6 +20,7 @@
<message name="UART_ERRORS" period="3.1"/>
<message name="SUPERBITRF" period="3"/>
<message name="ENERGY" period="2.5"/>
<message name="DATALINK_REPORT" period="5.1"/>
</mode>
<mode name="ppm">
conf/telemetry/fixedwing_control_new.xml
+1 -1
View File
@@ -16,7 +16,7 @@
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="IMU_GYRO" period="0.2"/>
conf/telemetry/hitl_fixedwing.xml
+1 -1
View File
@@ -14,7 +14,7 @@
<message name="CALIBRATION" period="2.1"/>
<message name="NAVIGATION_REF" period="9."/>
<message name="PPRZ_MODE" period="5."/>
<message name="DOWNLINK" period="5.1"/>
<message name="DATALINK_REPORT" period="5.1"/>
<message name="DL_VALUE" period="1.5"/>
<message name="IR_SENSORS" period="1.2"/>
<message name="SURVEY" period="2.1"/>
conf/telemetry/iridium.xml
+1 -1
View File
@@ -20,7 +20,7 @@
<message name="ALIVE" period="16." phase="1"/>
<message name="BAT" period="16." phase="1"/>
<message name="CALIBRATION" period="16." phase="1"/>
<message name="DOWNLINK" period="16." phase="5"/>
<message name="DATALINK_REPORT" period="16." phase="5"/>
<message name="GPS_SOL" period="16." phase="5"/>
<message name="NAVIGATION_REF" period="16." phase="9"/>
<message name="PPRZ_MODE" period="16." phase="9"/>
conf/telemetry/xbee868.xml
+1 -1
View File
@@ -21,7 +21,7 @@
<message name="ALIVE" period="18." phase="0"/>
<message name="CALIBRATION" period="18." phase="0"/>
<message name="DOWNLINK" period="18." phase="0"/>
<message name="DATALINK_REPORT" period="18." phase="0"/>
<message name="GPS_SOL" period="18." phase="0"/>
<message name="NAVIGATION_REF" period="18." phase="0"/>
</mode>
sw/airborne/firmwares/fixedwing/autopilot.c
-9
View File
@@ -136,14 +136,6 @@ static void send_airspeed(struct transport_tx *trans __attribute__((unused)), st
#endif
}
static void send_downlink(struct transport_tx *trans, struct link_device *dev) {
static uint16_t last;
uint16_t rate = (downlink.nb_bytes - last) / PERIOD_DOWNLINK_Ap_0;
last = downlink.nb_bytes;
pprz_msg_send_DOWNLINK(trans, dev, AC_ID,
&downlink.nb_ovrn, &rate, &downlink.nb_msgs);
}
void autopilot_init(void) {
pprz_mode = PPRZ_MODE_AUTO2;
kill_throttle = FALSE;
@@ -163,7 +155,6 @@ void autopilot_init(void) {
/* register some periodic message */
register_periodic_telemetry(DefaultPeriodic, "ALIVE", send_alive);
register_periodic_telemetry(DefaultPeriodic, "PPRZ_MODE", send_mode);
register_periodic_telemetry(DefaultPeriodic, "DOWNLINK", send_downlink);
register_periodic_telemetry(DefaultPeriodic, "ATTITUDE", send_attitude);
register_periodic_telemetry(DefaultPeriodic, "ESTIMATOR", send_estimator);
register_periodic_telemetry(DefaultPeriodic, "AIRSPEED", send_airspeed);
sw/airborne/subsystems/datalink/downlink.c
+29
View File
@@ -30,6 +30,32 @@
struct downlink downlink;
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
#include "subsystems/datalink/datalink.h"
#include "mcu_periph/sys_time.h"
static void send_downlink(struct transport_tx *trans, struct link_device *dev) {
static uint32_t last_nb_bytes = 0;
// timestamp in usec when last message was send
static uint32_t last_ts = 0.;
// current timestamp
uint32_t now_ts = get_sys_time_msec();
// compute downlink byte rate
if (now_ts > last_ts) {
uint16_t rate = (1000 * ((uint32_t)downlink.nb_bytes - last_nb_bytes)) / (now_ts - last_ts);
last_ts = now_ts;
last_nb_bytes = downlink.nb_bytes;
// TODO uplink nb received msg
uint16_t uplink_nb_msgs = 0;
pprz_msg_send_DATALINK_REPORT(trans, dev, AC_ID,
&datalink_time, &uplink_nb_msgs,
&downlink.nb_msgs, &rate, &downlink.nb_ovrn);
}
}
#endif
void downlink_init(void)
{
downlink.nb_ovrn = 0;
@@ -52,5 +78,8 @@ void downlink_init(void)
ivy_transport_init();
#endif
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "DATALINK_REPORT", send_downlink);
#endif
}
sw/airborne/subsystems/datalink/ivy_transport.c
+1
View File
@@ -150,6 +150,7 @@ static void end_message(struct ivy_transport *trans, struct link_device *dev __a
*(--trans->ivy_p) = '\0';
if (trans->ivy_dl_enabled) {
IvySendMsg("%s", trans->ivy_buf);
downlink.nb_msgs++;
}
}
sw/ground_segment/cockpit/live.ml
+21 -33
View File
@@ -841,23 +841,32 @@ let get_fbw_msg = fun alarm _sender vs ->
log_and_say alarm ac.ac_name (sprintf "%s, mayday, AP Failure. Switch to manual." ac.ac_speech_name)
end
let get_link_status_msg = fun alarm sender vs ->
let ac = find_ac sender in
let link_id = Pprz.int_assoc "link_id" vs in
let time_since_last_msg = Pprz.float_assoc "time_since_last_msg" vs in
let rx_msgs_rate = Pprz.float_assoc "rx_msgs_rate" vs in
let get_telemetry_status = fun alarm _sender vs ->
let ac = get_ac vs in
let link_id = Pprz.string_assoc "link_id" vs in
(* Update color and lost time in the strip *)
let time_lost = Pprz.float_assoc "time_since_last_msg" vs in
let (links_up, total_links) = ac.link_page#links_ratio () in
let link_ratio_string =
if ac.link_page#multiple_links () then sprintf "%i/%i" links_up total_links else "" in
ac.strip#set_label "telemetry_status" (if time_lost > 2. then sprintf "%.0f" time_lost else link_ratio_string);
ac.strip#set_color "telemetry_status" (if time_lost > 5. then alert_color else if links_up < total_links then warning_color else ok_color);
(* Update link page *)
let rx_msgs_rate = Pprz.float_assoc "rx_bytes_rate" vs
and downlink_bytes_rate = Pprz.int_assoc "downlink_rate" vs
and uplink_lost_time = Pprz.int_assoc "uplink_lost_time" vs in
let ping_time = Pprz.float_assoc "ping_time" vs in
if (not (ac.link_page#link_exists link_id)) then begin
ac.link_page#add_link link_id;
log_and_say alarm ac.ac_name (sprintf "%s, new link detected: %i" ac.ac_speech_name link_id)
log_and_say alarm ac.ac_name (sprintf "%s, new link detected: %s" ac.ac_speech_name link_id)
end;
let link_changed = ac.link_page#update_link link_id time_since_last_msg rx_msgs_rate ping_time in
let link_changed = ac.link_page#update_link link_id time_lost ping_time rx_msgs_rate downlink_bytes_rate uplink_lost_time in
let (links_up, _) = ac.link_page#links_ratio () in
match (link_changed, links_up) with
(_, 0) -> log_and_say alarm ac.ac_name (sprintf "%s, all links lost" ac.ac_speech_name)
| (Pages.Linkup, _)-> log_and_say alarm ac.ac_name (sprintf "%s, link %i re-connected" ac.ac_speech_name link_id)
| (Pages.Linkup, _)-> log_and_say alarm ac.ac_name (sprintf "%s, link %s re-connected" ac.ac_speech_name link_id)
| (Pages.Nochange, _) -> ()
| (Pages.Linkdown, _) -> log_and_say alarm ac.ac_name (sprintf "%s, link %i lost" ac.ac_speech_name link_id)
| (Pages.Linkdown, _) -> log_and_say alarm ac.ac_name (sprintf "%s, link %s lost" ac.ac_speech_name link_id)
let get_engine_status_msg = fun _sender vs ->
let ac = get_ac vs in
@@ -884,8 +893,8 @@ let listen_engine_status_msg = fun () ->
let listen_if_calib_msg = fun () ->
safe_bind "INFLIGH_CALIB" get_if_calib_msg
let listen_link_status_msg = fun a ->
tele_bind "LINK_STATUS" (get_link_status_msg a)
let listen_telemetry_status = fun a ->
safe_bind "TELEMETRY_STATUS" (get_telemetry_status a)
let list_separator = Str.regexp ","
@@ -1337,26 +1346,6 @@ let listen_svsinfo = fun a -> safe_bind "SVSINFO" (get_svsinfo a)
let message_request = Ground_Pprz.message_req
let get_ts = fun _sender vs ->
let ac = get_ac vs in
let t = Pprz.float_assoc "time_since_last_msg" vs in
if ac.link_page#multiple_links () then
begin
let (links_up, total_links) = ac.link_page#links_ratio () in
let link_ratio_string = sprintf "%i/%i" links_up total_links in
ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.0f" t else link_ratio_string);
ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else if links_up < total_links then warning_color else ok_color)
end
else
begin
ac.strip#set_label "telemetry_status" (if t > 2. then sprintf "%.0f" t else " ");
ac.strip#set_color "telemetry_status" (if t > 5. then alert_color else ok_color)
end
let listen_telemetry_status = fun () ->
safe_bind "TELEMETRY_STATUS" get_ts
let mark_dcshot = fun (geomap:G.widget) _sender vs ->
let ac = find_ac !active_ac in
let photonumber = Pprz.string_assoc "photo_nr" vs in
@@ -1419,11 +1408,10 @@ let listen_acs_and_msgs = fun geomap ac_notebook my_alert auto_center_new_ac alt
listen_wind_msg geomap;
listen_fbw_msg my_alert;
listen_engine_status_msg ();
listen_link_status_msg my_alert;
listen_telemetry_status my_alert;
listen_if_calib_msg ();
listen_waypoint_moved ();
listen_svsinfo my_alert;
listen_telemetry_status ();
listen_alert my_alert;
listen_error my_alert;
listen_info_msg my_alert;
sw/ground_segment/cockpit/pages.ml
+43 -27
View File
@@ -312,57 +312,73 @@ class link ?(visible = fun _ -> true) (widget: GBin.frame) =
ignore (GMisc.label ~text: "Link id:" ~justify: `RIGHT ~packing: (table#attach ~top:0 ~left: 0) ());
ignore (GMisc.label ~text: "Status:" ~justify: `RIGHT ~packing: (table#attach ~top:1 ~left: 0) ());
ignore (GMisc.label ~text: "Ping time [ms]:" ~justify: `RIGHT ~packing: (table#attach ~top:2 ~left: 0) ());
ignore (GMisc.label ~text: "Rx messages/s:" ~justify: `RIGHT ~packing: (table#attach ~top:3 ~left: 0) ());
ignore (GMisc.label ~text: "Link Rx [Byte/s]:" ~justify: `RIGHT ~packing: (table#attach ~top:3 ~left: 0) ());
ignore (GMisc.label ~text: "Downlink [Byte/s]:" ~justify: `RIGHT ~packing: (table#attach ~top:4 ~left: 0) ());
ignore (GMisc.label ~text: "Uplink lost [s]:" ~justify: `RIGHT ~packing: (table#attach ~top:5 ~left: 0) ());
in
object
val table = table
val mutable links = [] (* Stores the GUI elements that need to be updated and whether the link is connected or not*)
val mutable links_up = 0 (* Stores the number of links that are connected*)
method link_exists link_id =
try
let link = List.assoc link_id links in
ignore link;
true
with
Not_found -> false
method link_exists link_id = List.mem_assoc link_id links
method add_link link_id =
let number_of_links = List.length links in
let _ = GMisc.label ~text: (sprintf "%i" link_id) ~packing: (table#attach ~top:0 ~left: (number_of_links+1) ) () in
let link_status_eventbox = GBin.event_box ~width: 50 ~packing: (table#attach ~top:1 ~left: (number_of_links+1) ) () in
let link_status_label = GMisc.label ~text: " " ~packing: link_status_eventbox#add () in
let rx_msgs_rate_label = GMisc.label ~text: " " ~packing: (table#attach ~top:2 ~left: (number_of_links+1) ) () in
let ping_time_label = GMisc.label ~text: " " ~packing: (table#attach ~top:3 ~left: (number_of_links+1) ) () in
let up = true in
ignore (links <- (link_id, (up, link_status_eventbox, link_status_label, rx_msgs_rate_label, ping_time_label)) :: links);
links_up <- links_up + 1;
(* replace the no_id link if needed *)
if List.mem_assoc "no_id" links
then begin
let (_, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6) = List.assoc "no_id" links in
links <- (link_id, (true, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6)) :: (List.remove_assoc "no_id" links)
end
else begin
let number_of_links = List.length links in
let _ = GMisc.label ~text: (sprintf "%s" link_id) ~packing: (table#attach ~top:0 ~left: (number_of_links+1) ) () in
let link_status_eventbox = GBin.event_box ~width: 50 ~packing: (table#attach ~top:1 ~left: (number_of_links+1) ) () in
let link_status_label = GMisc.label ~text: " " ~packing: link_status_eventbox#add () in
let ping_time_label = GMisc.label ~text: " " ~packing: (table#attach ~top:2 ~left: (number_of_links+1) ) () in
let rx_bytes_rate_label = GMisc.label ~text: " " ~packing: (table#attach ~top:3 ~left: (number_of_links+1) ) () in
let downlink_bytes_rate_label = GMisc.label ~text: " " ~packing: (table#attach ~top:4 ~left: (number_of_links+1) ) () in
let uplink_lost_label = GMisc.label ~text: " " ~packing: (table#attach ~top:5 ~left: (number_of_links+1) ) () in
let up = true in
ignore (links <- (link_id, (up, link_status_eventbox, link_status_label, ping_time_label, rx_bytes_rate_label, downlink_bytes_rate_label, uplink_lost_label)) :: links);
links_up <- links_up + 1;
end
method update_link link_id time_since_last_msg rx_msgs_rate ping_time =
let (up, link_status_event_box, link_status_label, rx_msgs_rate_label, ping_time_label) = List.assoc link_id links in
method update_link link_id time_since_last_msg ping_time rx_bytes_rate downlink_bytes_rate uplink_lost_time =
let (up, link_status_event_box, link_status_label, ping_time_label, rx_bytes_rate_label, downlink_bytes_rate_label, uplink_lost_label) = List.assoc link_id links in
begin
if visible widget then begin (* display only if page is visible *)
let link_status_string = sprintf "%.0f" time_since_last_msg in
if link_status_label#text <> link_status_string then (* Updating the link status light*)
begin
link_status_label#set_label (if time_since_last_msg > 2. then link_status_string else " ");
let color = (if time_since_last_msg > 5. then "red" else "green") in
let color = (if time_since_last_msg > 5. then "red" else if uplink_lost_time > 10 then "orange" else "green") in
link_status_event_box#coerce#misc#modify_bg [`NORMAL, `NAME color];
end;
let rx_msgs_rate_string = sprintf "%.1f" rx_msgs_rate in
if rx_msgs_rate_label#text <> rx_msgs_rate_string then (* Updating the rx_msgs_rate field*)
rx_msgs_rate_label#set_label rx_msgs_rate_string;
let ping_time_string = sprintf "%.1f" ping_time in
if ping_time_label#text <> ping_time_string then (* Updating the ping_time field*)
ping_time_label#set_label ping_time_string;
let rx_bytes_rate_string = sprintf "%.1f" rx_bytes_rate in
if rx_bytes_rate_label#text <> rx_bytes_rate_string then (* Updating the rx_bytes_rate field*)
rx_bytes_rate_label#set_label rx_bytes_rate_string;
let downlink_bytes_rate_string = sprintf "%d" downlink_bytes_rate in
if downlink_bytes_rate_label#text <> downlink_bytes_rate_string then (* Updating the downlink_bytes_rate field*)
downlink_bytes_rate_label#set_label downlink_bytes_rate_string;
let uplink_lost_string = sprintf "%d" uplink_lost_time in
if uplink_lost_label#text <> uplink_lost_string then (* Updating the uplink_lost_time field*)
uplink_lost_label#set_label uplink_lost_string;
()
end;
let update_list = fun list_to_update up ->
let (_, dummy1, dummy2, dummy3, dummy4) = List.assoc link_id list_to_update in
(link_id, (up, dummy1, dummy2, dummy3, dummy4)) :: List.remove_assoc link_id list_to_update in
let (_, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6) = List.assoc link_id list_to_update in
(link_id, (up, dummy1, dummy2, dummy3, dummy4, dummy5, dummy6)) :: List.remove_assoc link_id list_to_update in
if up then (* Updating the stored value of whether this link is connected or not*)
if time_since_last_msg > 5. then
begin
@@ -387,6 +403,6 @@ class link ?(visible = fun _ -> true) (widget: GBin.frame) =
(links_up, List.length links)
method multiple_links () =
if (List.length links) > 0 then true else false
(List.length links) > 1
end;;
sw/ground_segment/cockpit/pages.mli
+3 -3
View File
@@ -64,9 +64,9 @@ class rc_settings :
type link_change = Linkup | Nochange | Linkdown
class link : ?visible:(GBin.frame -> bool) -> GBin.frame ->
object
method link_exists : int -> bool
method add_link : int -> unit
method update_link : int -> float -> float -> float -> link_change
method link_exists : string -> bool
method add_link : string -> unit
method update_link : string -> float -> float -> float -> int -> int -> link_change
method links_ratio : unit -> (int * int)
method multiple_links : unit -> bool
end
sw/ground_segment/python/redundant_link/link_combiner.py
+1 -64
View File
@@ -27,8 +27,6 @@ import logging
import sys
import os
import argparse
from time import time
import threading
from ivy.std_api import *
PPRZ_HOME = os.getenv("PAPARAZZI_HOME")
@@ -121,20 +119,9 @@ class Link:
def __init__(self, name, ac_id, buffer_size=10, verbose=0):
self.buffer = Circular_Buffer(buffer_size)
self.name = name
self.time_of_last_message = time()
self.verbose = verbose
self.acs = [ac_id] #Storing a list of the aircrafts that use this link. Usually it's just one.
# The following are stored values from the DOWNLINK_STATUS message:
self.run_time = 0
self.rx_bytes = 0
self.rx_msgs = 0
self.rx_err = 0
self.rx_bytes_rate = 0
self.rx_msgs_rate = 0
self.ping_time = 0
def checkBuffer(self,message):
return self.buffer.contains(message.message())
@@ -147,49 +134,12 @@ class Link:
def removeFromBuffer(self,message):
self.buffer.remove(message.message())
def updateTimeOfLastMessage(self):
self.time_of_last_message = time()
def timeSinceLastMessage(self):
return time() - self.time_of_last_message
def acAc(self, ac_id):
self.acs = self.acs + [ac_id]
def aircrafts(self):
return self.acs
def sendLinkStatusMessage(self):
for ac_id in self.acs:
values = ( self.name,
self.timeSinceLastMessage(),
self.run_time,
self.rx_bytes,
self.rx_msgs,
self.rx_err,
self.rx_bytes_rate,
self.rx_msgs_rate,
self.ping_time)
IvySendMsg("%s LINK_STATUS %s %f %s %s %s %s %s %s %s" % ((ac_id,) + values))
threading.Timer(LINK_STATUS_PERIOD, self.sendLinkStatusMessage).start()
def updateStatus(self, downlink_status_message):
if downlink_status_message.name() != "DOWNLINK_STATUS":
raise(Exception("function called with message of name other than DOWNLINK_STATUS"))
message_values = downlink_status_message.values()
self.run_time = message_values['run_time']
self.rx_bytes = message_values['rx_bytes']
self.rx_msgs = message_values['rx_msgs']
self.rx_err = message_values['rx_err']
self.rx_bytes_rate = message_values['rx_bytes_rate']
self.rx_msgs_rate = message_values['rx_msgs_rate']
self.ping_time = message_values['ping_time']
class Link_Combiner:
@@ -221,16 +171,11 @@ class Link_Combiner:
if message.linkName() not in self.links: #Adding a new link
self.links[message.linkName()] = Link(message.linkName(), message.sender(), BUFFER_SIZE)
# print("NEW LINK DETECTED: %s" %message.linkName(), file=sys.stderr)
self.repeatSendLinkStatusMessage(message)
#Processing messages from an already added link
link = self.links[message.linkName()]
self.sendMessage(message)
self.bufferMessage(message)
if message.name() != "DOWNLINK_STATUS":
link.updateTimeOfLastMessage()
else:
link.updateStatus(message)
if message.sender() not in link.aircrafts():
link.addAc(message.sender())
@@ -273,11 +218,6 @@ class Link_Combiner:
def bufferMessage(self, message):
self.links[message.linkName()].addToBuffer(message)
def repeatSendLinkStatusMessage(self, message):
link_name = message.linkName()
self.links[link_name].sendLinkStatusMessage()
def main():
messages_xml_map.ParseMessages()
@@ -286,17 +226,14 @@ def main():
#Command line options
parser = argparse.ArgumentParser(description="Link_Combiner listens to the ivy messages received from multiple Link agents (set each of their -id options to a unique number), and sends a combined stream of messages to the other agents.")
parser.add_argument("-b", "-buffer_size", "--buffer_size", help="The number of elements messages to be stored in the circular buffer for each link", default=10)
parser.add_argument("-t", "-link_status_period", "--link_status_period", help="The number of miliseconds in between LINK_STATUS messages being sent to the GCS", default=1000)
args = parser.parse_args()
global BUFFER_SIZE
global LINK_STATUS_PERIOD
BUFFER_SIZE = int(args.buffer_size) #The number of elements messages to be stored in the circular buffer for each link.
LINK_STATUS_PERIOD = float(args.link_status_period)/1000 #The number of seconds in between LINK_STATUS messages being sent to the GCS.
link_combiner = Link_Combiner()
if __name__ == '__main__':
main()
sw/ground_segment/tmtc/aircraft.ml
+35
View File
@@ -59,6 +59,38 @@ let svinfo_init = fun () ->
age = 0
}
type datalink_status = {
mutable uplink_lost_time : int;
mutable uplink_msgs : int;
mutable downlink_msgs : int;
mutable downlink_rate : int;
}
type link_status = {
rx_lost_time : int;
rx_bytes : int;
rx_msgs : int;
rx_bytes_rate : float;
tx_msgs : int;
ping_time : float
}
let datalink_status_init = fun () ->
{
uplink_lost_time = 9999;
uplink_msgs = 0;
downlink_msgs = 0;
downlink_rate = 0;
}
let link_status_init = fun () ->
{
rx_lost_time = 9999;
rx_bytes = 0;
rx_msgs = 0;
rx_bytes_rate = 0.;
tx_msgs = 0;
ping_time = 9999.
}
type inflight_calib = {
mutable if_mode : int; (* DOWN|OFF|UP *)
mutable if_val1 : float;
@@ -155,6 +187,8 @@ type aircraft = {
dl_setting_values : float array;
mutable nb_dl_setting_values : int;
mutable survey : (Latlong.geographic * Latlong.geographic) option;
datalink_status : datalink_status;
link_status : (int, link_status) Hashtbl.t;
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
@@ -188,6 +222,7 @@ let new_aircraft = fun id name fp airframe ->
horiz_mode = UnknownHorizMode;
horizontal_mode = 0;
waypoints = Hashtbl.create 3; survey = None; last_msg_date = 0.; dist_to_wp = 0.;
datalink_status = datalink_status_init (); link_status = Hashtbl.create 1;
time_since_last_survey_msg = 1729.;
inflight_calib = { if_mode = 1 ; if_val1 = 0.; if_val2 = 0.}
}
sw/ground_segment/tmtc/aircraft.mli
+20
View File
@@ -48,6 +48,24 @@ type svinfo = {
mutable age : int
}
val svinfo_init : unit -> svinfo
type datalink_status = {
mutable uplink_lost_time : int;
mutable uplink_msgs : int;
mutable downlink_msgs : int;
mutable downlink_rate : int;
}
type link_status = {
rx_lost_time : int;
rx_bytes : int;
rx_msgs : int;
rx_bytes_rate : float;
tx_msgs : int;
ping_time : float
}
val datalink_status_init : unit -> datalink_status
val link_status_init : unit -> link_status
type horiz_mode =
Circle of Latlong.geographic * int
| Segment of Latlong.geographic * Latlong.geographic
@@ -121,6 +139,8 @@ type aircraft = {
dl_setting_values : float array;
mutable nb_dl_setting_values : int;
mutable survey : (Latlong.geographic * Latlong.geographic) option;
datalink_status : datalink_status;
link_status : (int, link_status) Hashtbl.t;
mutable last_msg_date : float;
mutable time_since_last_survey_msg : float;
mutable dist_to_wp : float;
sw/ground_segment/tmtc/fw_server.ml
+5
View File
@@ -350,5 +350,10 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
"resolve", Pprz.Int (ivalue "resolve")
] in
Dl_Pprz.message_send "ground_dl" "TCAS_RESOLVE" vs
| "DATALINK_REPORT" ->
a.datalink_status.uplink_lost_time <- ivalue "uplink_lost_time";
a.datalink_status.uplink_msgs <- ivalue "uplink_nb_msgs";
a.datalink_status.downlink_rate <- ivalue "downlink_rate";
a.datalink_status.downlink_msgs <- ivalue "downlink_nb_msgs"
| _ -> ()
sw/ground_segment/tmtc/link.ml
+23 -5
View File
@@ -79,6 +79,13 @@ let send_message_over_ivy = fun sender name vs ->
else
Tm_Pprz.message_send ?timestamp sender name vs
let send_ground_over_ivy = fun sender name vs ->
let timestamp =
match !add_timestamp with
None -> None
| Some start_time -> Some (Unix.gettimeofday () -. start_time) in
Ground_Pprz.message_send ?timestamp sender name vs
(*********** Monitoring *************************************************)
type status = {
@@ -87,6 +94,7 @@ type status = {
mutable rx_byte : int;
mutable rx_msg : int;
mutable rx_err : int;
mutable tx_msg : int;
mutable ms_since_last_msg : int;
mutable last_ping : float; (* s *)
mutable last_pong : float; (* s *)
@@ -99,6 +107,7 @@ let dead_aircraft_time_ms = 5000
let initial_status = {
last_rx_byte = 0; last_rx_msg = 0;
rx_byte = 0; rx_msg = 0; rx_err = 0;
tx_msg = 0;
ms_since_last_msg = dead_aircraft_time_ms;
last_ping = 0.; last_pong = 0.;
udp_peername = None
@@ -153,16 +162,19 @@ let send_status_msg =
status.last_rx_msg <- status.rx_msg;
status.last_rx_byte <- status.rx_byte;
status.ms_since_last_msg <- status.ms_since_last_msg + status_msg_period;
let vs = ["link_id", Pprz.Int !link_id;
let vs = ["ac_id", Pprz.Int ac_id;
"link_id", Pprz.Int !link_id;
"run_time", Pprz.Int t;
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
"rx_err", Pprz.Int status.rx_err;
"rx_lost_time", Pprz.Int (1000 * status.ms_since_last_msg);
"rx_bytes", Pprz.Int status.rx_byte;
"rx_msgs", Pprz.Int status.rx_msg;
"rx_err", Pprz.Int status.rx_err;
"rx_bytes_rate", Pprz.Float byte_rate;
"rx_msgs_rate", Pprz.Float msg_rate;
"tx_msgs", Pprz.Int 0;
"ping_time", Pprz.Float (1000. *. (status.last_pong -. status.last_ping))
] in
send_message_over_ivy (string_of_int ac_id) "DOWNLINK_STATUS" vs)
send_ground_over_ivy "link" "LINK_REPORT" vs)
statuss
@@ -304,6 +316,11 @@ let udp_send = fun fd payload peername ->
let send = fun ac_id device payload _priority ->
Debug.call 's' (fun f -> fprintf f "%d\n" ac_id);
if live_aircraft ac_id then
let _ = try
let s = Hashtbl.find statuss ac_id in
s.tx_msg <- s.tx_msg + 1;
()
with Not_found -> () in
match udp_peername ac_id with
Some (Unix.ADDR_INET (peername, _port)) ->
udp_send device.fd payload peername
@@ -319,6 +336,7 @@ let send = fun ac_id device payload _priority ->
let broadcast = fun device payload _priority ->
Hashtbl.iter (fun _ s -> s.tx_msg <- s.tx_msg + 1) statuss;
if !udp then
Hashtbl.iter (* Sending to all alive A/C *)
(fun ac_id status ->
sw/ground_segment/tmtc/server.ml
+50 -9
View File
@@ -277,13 +277,35 @@ let send_wind = fun a ->
let send_telemetry_status = fun a ->
let id = a.id in
try
let vs =
["ac_id", Pprz.String id;
"time_since_last_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date)] in
Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
with
_exc -> ()
let tl_payload = fun link_id datalink_status link_status ->
[ "ac_id", Pprz.String id;
"link_id", Pprz.String link_id;
"time_since_last_msg", Pprz.Float (U.gettimeofday () -. a.last_msg_date); (* don't use rx_lost_time from LINK_REPORT so it also works in simulation *)
"rx_bytes", Pprz.Int link_status.rx_bytes;
"rx_msgs", Pprz.Int link_status.rx_msgs;
"rx_bytes_rate", Pprz.Float link_status.rx_bytes_rate;
"tx_msgs", Pprz.Int link_status.tx_msgs;
"uplink_lost_time", Pprz.Int datalink_status.uplink_lost_time;
"uplink_msgs", Pprz.Int datalink_status.uplink_msgs;
"downlink_msgs", Pprz.Int datalink_status.downlink_msgs;
"downlink_rate", Pprz.Int datalink_status.downlink_rate;
"ping_time", Pprz.Float link_status.ping_time]
in
(* if no link send anyway for rx_lost_time with special link id *)
if Hashtbl.length a.link_status = 0 then
begin
let vs = tl_payload "no_id" a.datalink_status (Aircraft.link_status_init ()) in
Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
end
else
(* send telemetry status for each link *)
Hashtbl.iter (fun link_id link_status ->
try
let vs = tl_payload (string_of_int link_id) a.datalink_status link_status in
Ground_Pprz.message_send my_id "TELEMETRY_STATUS" vs
with
_exc -> ()
) a.link_status
let send_moved_waypoints = fun a ->
Hashtbl.iter
@@ -674,7 +696,7 @@ let jump_block = fun logging _sender vs ->
Dl_Pprz.message_send dl_id "BLOCK" vs;
log logging ac_id "BLOCK" vs
(** Got a RAW_DATALINK,send its contents *)
(** Got a RAW_DATALINK, send its contents *)
let raw_datalink = fun logging _sender vs ->
let ac_id = Pprz.string_assoc "ac_id" vs
and m = Pprz.string_assoc "message" vs in
@@ -686,6 +708,24 @@ let raw_datalink = fun logging _sender vs ->
Dl_Pprz.message_send dl_id msg.Pprz.name vs;
log logging ac_id msg.Pprz.name vs
(** Got a LINK_REPORT, update state but don't send (done asynchronously) *)
let link_report = fun logging _sender vs ->
let ac_id = Pprz.string_assoc "ac_id" vs
and link_id = Pprz.int_assoc "link_id" vs in
try
let ac = Hashtbl.find aircrafts ac_id in
let link_status = {
Aircraft.rx_lost_time = Pprz.int_assoc "rx_lost_time" vs;
rx_bytes = Pprz.int_assoc "rx_bytes" vs;
rx_msgs = Pprz.int_assoc "rx_msgs" vs;
rx_bytes_rate = Pprz.float_assoc "rx_bytes_rate" vs;
tx_msgs = Pprz.int_assoc "tx_msgs" vs;
ping_time = Pprz.float_assoc "ping_time" vs;
} in
Hashtbl.replace ac.link_status link_id link_status;
with _ -> ()
(** Get the 'ground' uplink messages, log them and send 'datalink' messages *)
let ground_to_uplink = fun logging ->
let bind_log_and_send = fun name handler ->
@@ -694,7 +734,8 @@ let ground_to_uplink = fun logging ->
bind_log_and_send "DL_SETTING" setting;
bind_log_and_send "GET_DL_SETTING" get_setting;
bind_log_and_send "JUMP_TO_BLOCK" jump_block;
bind_log_and_send "RAW_DATALINK" raw_datalink
bind_log_and_send "RAW_DATALINK" raw_datalink;
bind_log_and_send "LINK_REPORT" link_report
(* main loop *)