diff --git a/conf/Makefile.nps b/conf/Makefile.nps index 2fcebaa429..2c6fc6eabf 100644 --- a/conf/Makefile.nps +++ b/conf/Makefile.nps @@ -50,7 +50,7 @@ CXXFLAGS += -O$(OPT) CXXFLAGS += $(DEBUG_FLAGS) CXXFLAGS += $(shell pkg-config --cflags-only-I ivy-glib) -LDFLAGS += $($(TARGET).LDFLAGS) -pthread +LDFLAGS += $($(TARGET).LDFLAGS) LDFLAGS += $(BOARD_LDFLAGS) # x86/64 and armv7 allow unaligned access diff --git a/conf/firmwares/subsystems/shared/nps.makefile b/conf/firmwares/subsystems/shared/nps.makefile index af06073b9a..6a3179b22f 100644 --- a/conf/firmwares/subsystems/shared/nps.makefile +++ b/conf/firmwares/subsystems/shared/nps.makefile @@ -17,7 +17,14 @@ nps.ARCHDIR = sim nps.MAKEFILE = nps nps.CFLAGS += -DSITL -DUSE_NPS -nps.LDFLAGS += -lm -livy $(shell pcre-config --libs) -lgsl -lgslcblas -lrt +nps.LDFLAGS += -lm -livy $(shell pcre-config --libs) -lgsl -lgslcblas + +# detect system arch and include rt and pthread library only on linux +UNAME_S := $(shell uname -s) +ifeq ($(UNAME_S),Linux) + nps.LDFLAGS += -lrt -pthread +endif + nps.CFLAGS += -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I$(PAPARAZZI_SRC)/sw/simulator -I$(PAPARAZZI_SRC)/sw/simulator/nps -I$(PAPARAZZI_HOME)/conf/simulator/nps # sdl needed for joystick input @@ -52,10 +59,8 @@ nps.srcs += \ $(NPSDIR)/nps_main_common.c ifeq ($(USE_HITL),1) -$(info USE_HITL defined) include $(CFG_SHARED)/nps_hitl.makefile else -$(info USE_HITL undefined) include $(CFG_SHARED)/nps_sitl.makefile endif diff --git a/sw/simulator/nps/nps_main.h b/sw/simulator/nps/nps_main.h index c6c9c2f4d4..56dbeea00b 100644 --- a/sw/simulator/nps/nps_main.h +++ b/sw/simulator/nps/nps_main.h @@ -9,6 +9,14 @@ #include "nps_sensors.h" #include "nps_autopilot.h" +#ifdef __MACH__ // OS X does not have clock_gettime, use clock_get_time +#include +#include +void clock_get_current_time(struct timespec *ts); +#else // Linux +#define clock_get_current_time(_x) clock_gettime(CLOCK_REALTIME, _x) +#endif // #ifdef __MACH__ + #define SIM_DT (1./SYS_TIME_FREQUENCY) #define DISPLAY_DT (1./30.) #define HOST_TIMEOUT_MS 40 diff --git a/sw/simulator/nps/nps_main_common.c b/sw/simulator/nps/nps_main_common.c index bf470eac52..3c2d401ba8 100644 --- a/sw/simulator/nps/nps_main_common.c +++ b/sw/simulator/nps/nps_main_common.c @@ -30,6 +30,22 @@ #include "nps_ivy.h" +#ifdef __MACH__ +pthread_mutex_t clock_mutex; // mutex for clock +void clock_get_current_time(struct timespec *ts) +{ + pthread_mutex_lock(&clock_mutex); + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + ts->tv_sec = mts.tv_sec; + ts->tv_nsec = mts.tv_nsec; + pthread_mutex_unlock(&clock_mutex); +} +#endif + void tstp_hdl(int n __attribute__((unused))) { if (pauseSignal) { @@ -239,7 +255,7 @@ void *nps_flight_gear_loop(void *data __attribute__((unused))) nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset); while (TRUE) { - clock_gettime(CLOCK_REALTIME, &requestStart); + clock_get_current_time(&requestStart); pthread_mutex_lock(&fdm_mutex); if (nps_main.fg_host) { @@ -251,7 +267,7 @@ void *nps_flight_gear_loop(void *data __attribute__((unused))) } pthread_mutex_unlock(&fdm_mutex); - clock_gettime(CLOCK_REALTIME, &requestEnd); + clock_get_current_time(&requestEnd); // Calculate time it took task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); @@ -287,7 +303,7 @@ void *nps_main_display(void *data __attribute__((unused))) nps_ivy_init(nps_main.ivy_bus); while (TRUE) { - clock_gettime(CLOCK_REALTIME, &requestStart); + clock_get_current_time(&requestStart); pthread_mutex_lock(&fdm_mutex); memcpy(&fdm_ivy, &fdm, sizeof(fdm)); @@ -296,7 +312,7 @@ void *nps_main_display(void *data __attribute__((unused))) nps_ivy_display(&fdm_ivy, &sensors_ivy); - clock_gettime(CLOCK_REALTIME, &requestEnd); + clock_get_current_time(&requestEnd); // Calculate time it took task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); diff --git a/sw/simulator/nps/nps_main_hitl.c b/sw/simulator/nps/nps_main_hitl.c index 6177629f68..6f40547960 100644 --- a/sw/simulator/nps/nps_main_hitl.c +++ b/sw/simulator/nps/nps_main_hitl.c @@ -138,7 +138,7 @@ void *nps_ins_data_loop(void *data __attribute__((unused))) pthread_mutex_lock(&fdm_mutex); // start timing - clock_gettime(CLOCK_REALTIME, &requestStart); + clock_get_current_time(&requestStart); // make a copy of fdm struct to speed things up memcpy(&fdm_ins, &fdm, sizeof(fdm)); @@ -159,7 +159,7 @@ void *nps_ins_data_loop(void *data __attribute__((unused))) printf("INS THREAD: Warning - sent only %u bytes to the autopilot, instead of expected %u\n", wlen, idx); } - clock_gettime(CLOCK_REALTIME, &requestEnd); + clock_get_current_time(&requestEnd); // Calculate time it took task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); @@ -285,19 +285,19 @@ void *nps_main_loop(void *data __attribute__((unused))) // fdm.time - simulation time struct timespec startTime; struct timespec realTime; - clock_gettime(CLOCK_REALTIME, &startTime); + clock_get_current_time(&startTime); double start_secs = ntime_to_double(&startTime); double real_secs = 0; double real_time = 0; static int guard; while (TRUE) { - clock_gettime(CLOCK_REALTIME, &requestStart); + clock_get_current_time(&requestStart); pthread_mutex_lock(&fdm_mutex); // check the current simulation time - clock_gettime(CLOCK_REALTIME, &realTime); + clock_get_current_time(&realTime); real_secs = ntime_to_double(&realTime); real_time = real_secs - start_secs; // real time elapsed @@ -312,7 +312,7 @@ void *nps_main_loop(void *data __attribute__((unused))) } pthread_mutex_unlock(&fdm_mutex); - clock_gettime(CLOCK_REALTIME, &requestEnd); + clock_get_current_time(&requestEnd); // Calculate time it took task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec); diff --git a/sw/simulator/nps/nps_main_sitl.c b/sw/simulator/nps/nps_main_sitl.c index bf2b03fb84..ebf28b7058 100644 --- a/sw/simulator/nps/nps_main_sitl.c +++ b/sw/simulator/nps/nps_main_sitl.c @@ -127,7 +127,7 @@ void *nps_main_loop(void *data __attribute__((unused))) pauseSignal = 0; } - clock_gettime(CLOCK_REALTIME, &requestStart); // init measurement (after the pause signal) + clock_get_current_time(&requestStart); // init measurement (after the pause signal) gettimeofday(&tv_now, NULL); host_time_now = time_to_double(&tv_now); @@ -163,7 +163,7 @@ void *nps_main_loop(void *data __attribute__((unused))) printf("%f,%f\n", nps_main.sim_time, nps_main.display_time); #endif - clock_gettime(CLOCK_REALTIME, &requestEnd); // end measurement + clock_get_current_time(&requestEnd); // end measurement // Calculate time it took task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);