Paparazzi UAS  v5.15_devel-110-g5cf27ee
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_main_common.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
3  * Copyright (C) 2012 The Paparazzi Team
4  * Copyright (C) 2016 Michal Podhradsky <http://github.com/podhrmic>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #include "nps_main.h"
25 #include <signal.h>
26 #include <stdio.h>
27 #include <getopt.h>
28 
29 #include "nps_flightgear.h"
30 
31 #include "nps_ivy.h"
32 
33 #ifdef __MACH__
34 pthread_mutex_t clock_mutex; // mutex for clock
35 void clock_get_current_time(struct timespec *ts)
36 {
37  pthread_mutex_lock(&clock_mutex);
38  clock_serv_t cclock;
39  mach_timespec_t mts;
40  host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock);
41  clock_get_time(cclock, &mts);
42  mach_port_deallocate(mach_task_self(), cclock);
43  ts->tv_sec = mts.tv_sec;
44  ts->tv_nsec = mts.tv_nsec;
45  pthread_mutex_unlock(&clock_mutex);
46 }
47 #endif
48 
49 void tstp_hdl(int n __attribute__((unused)))
50 {
51  if (pauseSignal) {
52  pauseSignal = 0;
53  signal(SIGTSTP, SIG_DFL);
54  raise(SIGTSTP);
55  } else {
56  pauseSignal = 1;
57  }
58 }
59 
60 
61 void cont_hdl(int n __attribute__((unused)))
62 {
63  signal(SIGCONT, cont_hdl);
64  signal(SIGTSTP, tstp_hdl);
65  printf("Press <enter> to continue.\n");
66 }
67 
68 
69 double time_to_double(struct timeval *t)
70 {
71  return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
72 }
73 
74 double ntime_to_double(struct timespec *t)
75 {
76  return ((double)t->tv_sec + (double)(t->tv_nsec * 1e-9));
77 }
78 
79 int nps_main_init(int argc, char **argv)
80 {
81  pauseSignal = 0;
82 
83  if (!nps_main_parse_options(argc, argv)) { return 1; }
84 
85  /* disable buffering for stdout,
86  * so it properly works in paparazzi center
87  * where it is not detected as interactive
88  * and hence fully buffered instead of line buffered
89  */
90  setbuf(stdout, NULL);
91 
92 
93  nps_main.sim_time = 0.;
95  struct timeval t;
96  gettimeofday(&t, NULL);
99 
103  printf("Simulating with dt of %f\n", SIM_DT);
104 
106 
107 #if DEBUG_NPS_TIME
108  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");
109 #endif
110 
111  signal(SIGCONT, cont_hdl);
112  signal(SIGTSTP, tstp_hdl);
113  printf("Time factor is %f. (Press Ctrl-Z to change)\n", nps_main.host_time_factor);
114 
115  return 0;
116 }
117 
118 
119 void nps_set_time_factor(float time_factor)
120 {
121  if (time_factor < 0.0 || time_factor > 100.0) {
122  return;
123  }
124  if (fabs(nps_main.host_time_factor - time_factor) < 0.01) {
125  return;
126  }
127 
128  struct timeval tv_now;
129  double t_now, t_elapsed;
130 
131  gettimeofday(&tv_now, NULL);
132  t_now = time_to_double(&tv_now);
133 
134  /* "virtual" elapsed time with old time factor */
135  t_elapsed = (t_now - nps_main.scaled_initial_time) * nps_main.host_time_factor;
136 
137  /* set new time factor */
138  nps_main.host_time_factor = time_factor;
139  printf("Time factor is %f\n", nps_main.host_time_factor);
140  fflush(stdout);
141 
142  /* set new "virtual" scaled initial time using new time factor*/
144 }
145 
146 
147 bool nps_main_parse_options(int argc, char **argv)
148 {
149 
150  nps_main.fg_host = NULL;
151  nps_main.fg_port = 5501;
152  nps_main.fg_port_in = 5502;
154  nps_main.js_dev = NULL;
155  nps_main.spektrum_dev = NULL;
156  nps_main.rc_script = 0;
157  nps_main.norc = false;
158  nps_main.ivy_bus = NULL;
160  nps_main.fg_fdm = 0;
161  nps_main.nodisplay = false;
162 
163  static const char *usage =
164  "Usage: %s [options]\n"
165  " Options :\n"
166  " -h Display this help\n"
167  " --fg_host <flight gear host> e.g. 127.0.0.1\n"
168  " --fg_port <flight gear port> e.g. 5501\n"
169  " --fg_port_in <flight gear in port> e.g. 5502\n"
170  " --fg_time_offset <offset in seconds> e.g. 21600 for 6h\n"
171  " -j --js_dev <optional joystick index> e.g. 1 (default 0)\n"
172  " --spektrum_dev <spektrum device> e.g. /dev/ttyUSB0\n"
173  " --rc_script <number> e.g. 0\n"
174  " --norc e.g. disable RC\n"
175  " --ivy_bus <ivy bus> e.g. 127.255.255.255\n"
176  " --time_factor <factor> e.g. 2.5\n"
177  " --nodisplay e.g. disable NPS ivy messages\n"
178  " --fg_fdm";
179 
180 
181  while (1) {
182 
183  static struct option long_options[] = {
184  {"fg_host", 1, NULL, 0},
185  {"fg_port", 1, NULL, 0},
186  {"fg_time_offset", 1, NULL, 0},
187  {"js_dev", 2, NULL, 0},
188  {"spektrum_dev", 1, NULL, 0},
189  {"rc_script", 1, NULL, 0},
190  {"norc", 0, NULL, 0},
191  {"ivy_bus", 1, NULL, 0},
192  {"time_factor", 1, NULL, 0},
193  {"fg_fdm", 0, NULL, 0},
194  {"fg_port_in", 1, NULL, 0},
195  {"nodisplay", 0, NULL, 0},
196  {0, 0, 0, 0}
197  };
198  int option_index = 0;
199  int c = getopt_long(argc, argv, "jh",
200  long_options, &option_index);
201  if (c == -1) {
202  break;
203  }
204 
205  switch (c) {
206  case 0:
207  switch (option_index) {
208  case 0:
209  nps_main.fg_host = strdup(optarg); break;
210  case 1:
211  nps_main.fg_port = atoi(optarg); break;
212  case 2:
213  nps_main.fg_time_offset = atoi(optarg); break;
214  case 3:
215  if (optarg == NULL) {nps_main.js_dev = strdup("0");}
216  else {nps_main.js_dev = strdup(optarg);}
217  break;
218  case 4:
219  nps_main.spektrum_dev = strdup(optarg); break;
220  case 5:
221  nps_main.rc_script = atoi(optarg); break;
222  case 6:
223  nps_main.norc = true; break;
224  case 7:
225  nps_main.ivy_bus = strdup(optarg); break;
226  case 8:
227  nps_main.host_time_factor = atof(optarg); break;
228  case 9:
229  nps_main.fg_fdm = 1; break;
230  case 10:
231  nps_main.fg_port_in = atoi(optarg); break;
232  case 11:
233  nps_main.nodisplay = true; break;
234  default:
235  break;
236  }
237  break;
238 
239  case 'j':
240  if (optarg == NULL) {nps_main.js_dev = strdup("0");}
241  else {nps_main.js_dev = strdup(optarg);}
242  break;
243 
244  case 'h':
245  fprintf(stderr, usage, argv[0]);
246  exit(0);
247 
248  default:
249  printf("?? getopt returned character code 0%o ??\n", c);
250  fprintf(stderr, usage, argv[0]);
251  exit(EXIT_FAILURE);
252  }
253  }
254  return TRUE;
255 }
256 
257 
258 void *nps_flight_gear_loop(void *data __attribute__((unused)))
259 {
260  struct timespec requestStart;
261  struct timespec requestEnd;
262  struct timespec waitFor;
263  long int period_ns = DISPLAY_DT * 1000000000L; // thread period in nanoseconds
264  long int task_ns = 0; // time it took to finish the task in nanoseconds
265 
266 
268 
269  while (TRUE) {
270  clock_get_current_time(&requestStart);
271 
272  pthread_mutex_lock(&fdm_mutex);
273  if (nps_main.fg_host) {
274  if (nps_main.fg_fdm) {
276  } else {
278  }
279  }
280  pthread_mutex_unlock(&fdm_mutex);
281 
282  clock_get_current_time(&requestEnd);
283 
284  // Calculate time it took
285  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
286 
287  // task took less than one period, sleep for the rest of time
288  if (task_ns < period_ns) {
289  waitFor.tv_sec = 0;
290  waitFor.tv_nsec = period_ns - task_ns;
291  nanosleep(&waitFor, NULL);
292  } else {
293  // task took longer than the period
294 #ifdef PRINT_TIME
295  printf("FG THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
296  (double)task_ns / 1E6, (double)period_ns / 1E6);
297 #endif
298  }
299  }
300 
301  return(NULL);
302 }
303 
304 
305 
306 void *nps_main_display(void *data __attribute__((unused)))
307 {
308  struct timespec requestStart;
309  struct timespec requestEnd;
310  struct timespec waitFor;
311  long int period_ns = 3 * DISPLAY_DT * 1000000000L; // thread period in nanoseconds
312  long int task_ns = 0; // time it took to finish the task in nanoseconds
313 
314  struct NpsFdm fdm_ivy;
315  struct NpsSensors sensors_ivy;
316 
318 
319  // start the loop only if no_display is false
320  if (!nps_main.nodisplay) {
321  while (TRUE) {
322  clock_get_current_time(&requestStart);
323 
324  pthread_mutex_lock(&fdm_mutex);
325  memcpy(&fdm_ivy, &fdm, sizeof(fdm));
326  memcpy(&sensors_ivy, &sensors, sizeof(sensors));
327  pthread_mutex_unlock(&fdm_mutex);
328 
329  nps_ivy_display(&fdm_ivy, &sensors_ivy);
330 
331  clock_get_current_time(&requestEnd);
332 
333  // Calculate time it took
334  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
335 
336  // task took less than one period, sleep for the rest of time
337  if (task_ns < period_ns) {
338  waitFor.tv_sec = 0;
339  waitFor.tv_nsec = period_ns - task_ns;
340  nanosleep(&waitFor, NULL);
341  } else {
342  // task took longer than the period
343  #ifdef PRINT_TIME
344  printf("IVY DISPLAY THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
345  (double)task_ns / 1E6, (double)period_ns / 1E6);
346  #endif
347  }
348  }
349  }
350  return(NULL);
351 }
char * fg_host
Definition: nps_main.h:57
char * ivy_bus
Definition: nps_main.h:66
bool norc
Definition: nps_main.h:65
double real_initial_time
Definition: nps_main.h:52
pthread_mutex_t fdm_mutex
Definition: nps_main.h:28
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_ivy_init(char *ivy_bus)
Definition: nps_ivy.c:59
void nps_flightgear_send(void)
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with ...
void cont_hdl(int n)
void tstp_hdl(int n)
unsigned int fg_port_in
Definition: nps_main.h:59
char * js_dev
Definition: nps_main.h:62
#define SIM_DT
Definition: nps_main.h:20
int nps_main_init(int argc, char **argv)
Definition: nps_fdm.h:44
bool nps_main_parse_options(int argc, char **argv)
int pauseSignal
Definition: nps_main.h:30
unsigned int fg_port
Definition: nps_main.h:58
struct NpsMain nps_main
Definition: nps_main.h:70
#define TRUE
Definition: std.h:4
#define clock_get_current_time(_x)
Definition: nps_main.h:17
void nps_flightgear_send_fdm(void)
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc)...
double ntime_to_double(struct timespec *t)
double scaled_initial_time
Definition: nps_main.h:53
char * spektrum_dev
Definition: nps_main.h:63
unsigned int fg_time_offset
Definition: nps_main.h:60
void nps_ivy_display(struct NpsFdm *fdm_data, struct NpsSensors *sensors_data)
Definition: nps_ivy.c:205
void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
struct NpsSensors sensors
Definition: nps_sensors.c:6
void nps_fdm_init(double dt)
Initialize actuator dynamics, set unused fields in fdm.
double sim_time
Definition: nps_main.h:55
double host_time_factor
Definition: nps_main.h:54
#define DISPLAY_DT
Definition: nps_main.h:21
void * nps_main_display(void *data)
bool nodisplay
Definition: nps_main.h:67
int rc_script
Definition: nps_main.h:64
void nps_radio_and_autopilot_init(void)
Definition: nps_main_hitl.c:75
void nps_set_time_factor(float time_factor)
int fg_fdm
Definition: nps_main.h:61
double time_to_double(struct timeval *t)
double display_time
Definition: nps_main.h:56
void * nps_flight_gear_loop(void *data)
void nps_atmosphere_init(void)
void nps_sensors_init(double time)
Definition: nps_sensors.c:8