Nps Ivy fix: Segmentation Fault when multiple aircraft are simulated simultaneously (#3657)

* NPS Ivy simplification

Incorporate NPS interaction with Ivy inside Ivy main loop, instead of multi-thread manipulation

* Bugfix: Offset NPS GPS by startup time-of-day

Initially, it only used time since start, but it makes desynchronised GPS between several simulated aircraft starting at different times (there may still be bugs when changing time scale).

---------

Co-authored-by: Mael FEURGARD <mael.feurgard@laas.fr>

sw/simulator/nps/nps_hitl_sensors.c

@@ -223,7 +223,7 @@ void *nps_sensors_loop(void *data __attribute__((unused)))
       float gps_vx = (float)sensors.gps.ecef_vel.x;
       float gps_vy = (float)sensors.gps.ecef_vel.y;
       float gps_vz = (float)sensors.gps.ecef_vel.z;
-      float gps_time = (float)nps_main.sim_time;
+      float gps_time = (float)nps_main.real_initial_time+nps_main.sim_time;
       uint8_t gps_fix = 3; // GPS fix
       pprz_msg_send_HITL_GPS(&dev.pprz_tp.trans_tx, &dev.device, 0,
           &gps_lat, &gps_lon, &gps_alt, &gps_hmsl,

sw/simulator/nps/nps_ivy.c

@@ -7,7 +7,6 @@
 #include <Ivy/ivy.h>
 
 #include <Ivy/ivyloop.h>
-#include <pthread.h>
 
 #include "generated/airframe.h"
 #include "math/pprz_algebra_float.h"
@@ -27,11 +26,9 @@
 #endif
 
 bool nps_ivy_send_world_env = false;
-pthread_t th_ivy_main; // runs main Ivy loop
 static MsgRcvPtr ivyPtr = NULL;
 static int seq = 1;
 static int ap_launch_index;
-static pthread_mutex_t ivy_mutex; // mutex for ivy send
 
 /* Gaia Ivy functions */
 static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
@@ -43,19 +40,36 @@ static void on_DL_SETTING(IvyClientPtr app __attribute__((unused)),
                           void *user_data __attribute__((unused)),
                           int argc, char *argv[]);
 
-void* ivy_main_loop(void* data __attribute__((unused)));
+static struct nps_ivy_metadata_t {
+  struct timespec requestStart;
+  long int period_ns; // thread period in nanoseconds
+} nps_ivy_metadata;
 
 int find_launch_index(void);
+void nps_ivy_beforeloop(struct nps_ivy_metadata_t * args);
 
-
-void* ivy_main_loop(void* data __attribute__((unused)))
+void nps_ivy_beforeloop(struct nps_ivy_metadata_t * args)
 {
-  IvyMainLoop();
+  struct timespec now;
+  clock_get_current_time(&now);
+  long int task_ns = (now.tv_sec - args->requestStart.tv_sec) * 1000000000L + (now.tv_nsec - args->requestStart.tv_nsec);
 
-  return NULL;
+  if (task_ns < args->period_ns)
+  {
+    return;
+  }
+  else
+  {
+    #ifdef PRINT_TIME
+          printf("IVY DISPLAY: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
+                (double)task_ns / 1E6, (double)args->period_ns / 1E6);
+    #endif
+    clock_get_current_time(&args->requestStart);
+    nps_ivy_display(&fdm,&sensors);
+  }
 }
 
-void nps_ivy_init(char *ivy_bus)
+void nps_ivy_init(char *ivy_bus, bool nodisplay)
 {
   const char *agent_name = AIRFRAME_NAME"_NPS";
   const char *ready_msg = AIRFRAME_NAME"_NPS Ready";
@@ -67,6 +81,11 @@ void nps_ivy_init(char *ivy_bus)
   // to be able to change datalink_enabled setting back on
   IvyBindMsg(on_DL_SETTING, NULL, "^(\\S*) DL_SETTING (\\S*) (\\S*) (\\S*)");
 
+  // Register extra Ivy actions only if no_display is false
+  nps_ivy_metadata.period_ns = 3 * DISPLAY_DT * 1000000000L;
+  if (!nodisplay)
+    IvySetBeforeSelectHook((IvyHookPtr)nps_ivy_beforeloop,&nps_ivy_metadata);
+
 #ifdef __APPLE__
   const char *default_ivy_bus = "224.255.255.255";
 #else
@@ -81,10 +100,6 @@ void nps_ivy_init(char *ivy_bus)
   nps_ivy_send_world_env = false;
 
   ap_launch_index = find_launch_index();
-
-  // Launch separate thread with IvyMainLoop()
-  pthread_create(&th_ivy_main, NULL, ivy_main_loop, NULL);
-
 }
 
 /*
@@ -126,8 +141,6 @@ static void on_WORLD_ENV(IvyClientPtr app __attribute__((unused)),
 
 void nps_ivy_send_WORLD_ENV_REQ(void)
 {
-  pthread_mutex_lock(&ivy_mutex);
-
   // First unbind from previous request if needed
   if (ivyPtr != NULL) {
     IvyUnbindMsg(ivyPtr);
@@ -154,8 +167,6 @@ void nps_ivy_send_WORLD_ENV_REQ(void)
   seq++;
 
   nps_ivy_send_world_env = false;
-
-  pthread_mutex_unlock(&ivy_mutex);
 }
 
 int find_launch_index(void)
@@ -229,9 +240,6 @@ void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
   memcpy(&sensors_ivy, sensors_data, sizeof(sensors));
   pthread_mutex_unlock(&fdm_mutex);
 
-  // protect Ivy thread
-  pthread_mutex_lock(&ivy_mutex);
-
   IvySendMsg("%d NPS_RATE_ATTITUDE %f %f %f %f %f %f",
              AC_ID,
              DegOfRad(fdm_ivy.body_ecef_rotvel.p),
@@ -287,9 +295,12 @@ void nps_ivy_display(struct NpsFdm* fdm_data, struct NpsSensors* sensors_data)
              fdm_ivy.wind.y,
              fdm_ivy.wind.z);
 
-  pthread_mutex_unlock(&ivy_mutex);
-
   if (nps_ivy_send_world_env) {
     nps_ivy_send_WORLD_ENV_REQ();
   }
 }
+
+void nps_ivy_run(void)
+{
+  IvyMainLoop();
+}

sw/simulator/nps/nps_ivy.h

@@ -6,9 +6,10 @@
 
 extern bool nps_ivy_send_world_env;
 
-extern void nps_ivy_init(char *ivy_bus);
+extern void nps_ivy_init(char *ivy_bus, bool nodisplay);
 extern void nps_ivy_hitl(struct NpsSensors* sensors_data);
 extern void nps_ivy_display(struct NpsFdm* fdm_ivy, struct NpsSensors* sensors_ivy);
 extern void nps_ivy_send_WORLD_ENV_REQ(void);
+extern void nps_ivy_run(void);
 
 #endif /* NPS_IVY */

sw/simulator/nps/nps_main_common.c

@@ -312,39 +312,9 @@ void *nps_flight_gear_loop(void *data __attribute__((unused)))
 
 void *nps_main_display(void *data __attribute__((unused)))
 {
-  struct timespec requestStart;
-  struct timespec requestEnd;
-  struct timespec waitFor;
-  long int period_ns = 3 * DISPLAY_DT * 1000000000L; // thread period in nanoseconds
-  long int task_ns = 0; // time it took to finish the task in nanoseconds
+  nps_ivy_init(nps_main.ivy_bus, nps_main.nodisplay);
 
-  nps_ivy_init(nps_main.ivy_bus);
+  nps_ivy_run();
 
-  // start the loop only if no_display is false
-  if (!nps_main.nodisplay) {
-    while (TRUE) {
-      clock_get_current_time(&requestStart);
-
-      nps_ivy_display(&fdm, &sensors);
-
-      clock_get_current_time(&requestEnd);
-
-      // Calculate time it took
-      task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
-
-      // task took less than one period, sleep for the rest of time
-      if (task_ns < period_ns) {
-        waitFor.tv_sec = 0;
-        waitFor.tv_nsec = period_ns - task_ns;
-        nanosleep(&waitFor, NULL);
-      } else {
-        // task took longer than the period
-  #ifdef PRINT_TIME
-        printf("IVY DISPLAY THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
-               (double)task_ns / 1E6, (double)period_ns / 1E6);
-  #endif
-      }
-    }
-  }
   return(NULL);
 }