41pthread_mutex_t clock_mutex;
44 pthread_mutex_lock(&clock_mutex);
47 host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
48 clock_get_time(cclock, &mts);
49 mach_port_deallocate(mach_task_self(), cclock);
50 ts->tv_sec = mts.tv_sec;
51 ts->tv_nsec = mts.tv_nsec;
52 pthread_mutex_unlock(&clock_mutex);
60 signal(SIGTSTP, SIG_DFL);
72 printf(
"Press <enter> to continue.\n");
78 return ((
double)t->tv_sec + (
double)(t->tv_usec * 1e-6));
83 return ((
double)t->tv_sec + (
double)(t->tv_nsec * 1e-9));
103 gettimeofday(&t, NULL);
110 printf(
"Simulating with dt of %f\n",
SIM_DT);
115 printf(
"host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n");
120 printf(
"Time factor is %f. (Press Ctrl-Z to change)\n",
nps_main.host_time_factor);
128 if (time_factor < 0.0 || time_factor > 100.0) {
131 if (fabs(
nps_main.host_time_factor - time_factor) < 0.01) {
135 struct timeval tv_now;
136 double t_now, t_elapsed;
138 gettimeofday(&tv_now, NULL);
142 t_elapsed = (t_now -
nps_main.scaled_initial_time) *
nps_main.host_time_factor;
145 nps_main.host_time_factor = time_factor;
146 printf(
"Time factor is %f\n",
nps_main.host_time_factor);
150 nps_main.scaled_initial_time = t_now - t_elapsed /
nps_main.host_time_factor;
170 static const char *
usage =
171 "Usage: %s [options]\n"
173 " -h Display this help\n"
174 " --fg_host <flight gear host> e.g. 127.0.0.1\n"
175 " --fg_port <flight gear port> e.g. 5501\n"
176 " --fg_port_in <flight gear in port> e.g. 5502\n"
177 " --fg_time_offset <offset in seconds> e.g. 21600 for 6h\n"
178 " -j --js_dev <optional joystick index> e.g. 1 (default 0)\n"
179 " --spektrum_dev <spektrum device> e.g. /dev/ttyUSB0\n"
180 " --rc_script <number> e.g. 0\n"
181 " --norc e.g. disable RC\n"
182 " --ivy_bus <ivy bus> e.g. 127.255.255.255\n"
183 " --time_factor <factor> e.g. 2.5\n"
184 " --nodisplay e.g. disable NPS ivy messages\n"
190 static struct option long_options[] = {
191 {
"fg_host", 1, NULL, 0},
192 {
"fg_port", 1, NULL, 0},
193 {
"fg_time_offset", 1, NULL, 0},
194 {
"js_dev", 2, NULL, 0},
195 {
"spektrum_dev", 1, NULL, 0},
196 {
"rc_script", 1, NULL, 0},
197 {
"norc", 0, NULL, 0},
198 {
"ivy_bus", 1, NULL, 0},
199 {
"time_factor", 1, NULL, 0},
200 {
"fg_fdm", 0, NULL, 0},
201 {
"fg_port_in", 1, NULL, 0},
202 {
"nodisplay", 0, NULL, 0},
205 int option_index = 0;
206 int c = getopt_long(argc, argv,
"jh",
207 long_options, &option_index);
214 switch (option_index) {
216 nps_main.fg_host = strdup(optarg);
break;
218 nps_main.fg_port = atoi(optarg);
break;
220 nps_main.fg_time_offset = atoi(optarg);
break;
222 if (optarg == NULL) {
nps_main.js_dev = strdup(
"0");}
223 else {
nps_main.js_dev = strdup(optarg);}
226 nps_main.spektrum_dev = strdup(optarg);
break;
228 nps_main.rc_script = atoi(optarg);
break;
232 nps_main.ivy_bus = strdup(optarg);
break;
234 nps_main.host_time_factor = atof(optarg);
break;
238 nps_main.fg_port_in = atoi(optarg);
break;
247 if (optarg == NULL) {
nps_main.js_dev = strdup(
"0");}
248 else {
nps_main.js_dev = strdup(optarg);}
252 fprintf(stderr,
usage, argv[0]);
256 printf(
"?? getopt returned character code 0%o ??\n", c);
257 fprintf(stderr,
usage, argv[0]);
267 struct timespec requestStart;
268 struct timespec requestEnd;
269 struct timespec waitFor;
270 long int period_ns =
DISPLAY_DT * 1000000000L;
271 long int task_ns = 0;
292 task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
295 if (task_ns < period_ns) {
297 waitFor.tv_nsec = period_ns - task_ns;
298 nanosleep(&waitFor, NULL);
302 printf(
"FG THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
303 (
double)task_ns / 1
E6, (
double)period_ns / 1
E6);
static void usage(BaseSequentialStream *lchp, char *p)
void nps_atmosphere_init(void)
void nps_fdm_init(double dt)
NPS FDM rover init.
void nps_flightgear_send(void)
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with –...
void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
void nps_flightgear_send_fdm(void)
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc).
void nps_ivy_init(char *ivy_bus, bool nodisplay)
#define clock_get_current_time(_x)
void nps_radio_and_autopilot_init(void)
pthread_mutex_t fdm_mutex
int nps_main_init(int argc, char **argv)
bool nps_main_parse_options(int argc, char **argv)
void * nps_main_display(void *data)
void nps_set_time_factor(float time_factor)
void * nps_flight_gear_loop(void *data)
double time_to_double(struct timeval *t)
double ntime_to_double(struct timespec *t)
void nps_sensors_init(double time)