From 4673d710f82f88ed26fe86359c4bc4eefbc7c5ce Mon Sep 17 00:00:00 2001
From: Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
Date: Mon, 26 Nov 2018 12:42:41 -0800
Subject: [PATCH] Indicate when HITL is not getting AP data (#2353)

---
 sw/simulator/nps/nps_main_hitl.c | 18 ++++++++++++++++++
 1 file changed, 18 insertions(+)

diff --git a/sw/simulator/nps/nps_main_hitl.c b/sw/simulator/nps/nps_main_hitl.c
index a50f79dc96..fc67a0a7f1 100644
--- a/sw/simulator/nps/nps_main_hitl.c
+++ b/sw/simulator/nps/nps_main_hitl.c
@@ -214,9 +214,24 @@ void *nps_ap_data_loop(void *data __attribute__((unused)))
   struct pprz_transport pprz_tp_logger;
 
   pprz_transport_init(&pprz_tp_logger);
+  uint32_t rx_msgs = 0;
+  uint32_t rx_commands = 0;
+  uint32_t rx_motor_mixing = 0;
+  uint8_t show_stats = 1;
 
   while (TRUE) {
+    if ((rx_msgs % 100 == 0) && show_stats && (rx_msgs > 0) ) {
+      printf("AP data loop received %i messages.\n", rx_msgs);
+      printf("From those, %i were COMMANDS messages, and %i were MOTOR_MIXING messages.\n",
+          rx_commands, rx_motor_mixing);
+      show_stats = 0;
+    }
+
     // receive messages from the autopilot
+    // Note: read function might wait until the buffer is full, in which case
+    // it could contain several messages. That might lead to delay in processing,
+    // for example if we send COMMANDS at 100Hz, and each read() will hold 10 COMMANDS
+    // it means a delay of 100ms before the message is processed.
     rdlen = read(fd, buf, sizeof(buf) - 1);
 
     for (int i = 0; i < rdlen; i++) {
@@ -225,6 +240,7 @@ void *nps_ap_data_loop(void *data __attribute__((unused)))
 
       // if msg_available then read
       if (pprz_tp_logger.trans_rx.msg_received) {
+        rx_msgs++;
         for (int k = 0; k < pprz_tp_logger.trans_rx.payload_len; k++) {
           buf[k] = pprz_tp_logger.trans_rx.payload[k];
         }
@@ -244,6 +260,7 @@ void *nps_ap_data_loop(void *data __attribute__((unused)))
           switch (msg_id) {
             case DL_COMMANDS:
               // parse commands message
+              rx_commands++;
               cmd_len = DL_COMMANDS_values_length(buf);
               memcpy(&cmd_buf, DL_COMMANDS_values(buf), cmd_len * sizeof(int16_t));
               pthread_mutex_lock(&fdm_mutex);
@@ -257,6 +274,7 @@ void *nps_ap_data_loop(void *data __attribute__((unused)))
               break;
             case DL_MOTOR_MIXING:
               // parse actuarors message
+              rx_motor_mixing++;
               cmd_len = DL_MOTOR_MIXING_values_length(buf);
               // check for out-of-bounds access
               if (cmd_len > NPS_COMMANDS_NB) {