Paparazzi UAS  v5.12_stable-4-g9b43e9b
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 
162  static const char *usage =
163  "Usage: %s [options]\n"
164  " Options :\n"
165  " -h Display this help\n"
166  " --fg_host <flight gear host> e.g. 127.0.0.1\n"
167  " --fg_port <flight gear port> e.g. 5501\n"
168  " --fg_port_in <flight gear in port> e.g. 5502\n"
169  " --fg_time_offset <offset in seconds> e.g. 21600 for 6h\n"
170  " -j --js_dev <optional joystick index> e.g. 1 (default 0)\n"
171  " --spektrum_dev <spektrum device> e.g. /dev/ttyUSB0\n"
172  " --rc_script <number> e.g. 0\n"
173  " --norc e.g. disable RC\n"
174  " --ivy_bus <ivy bus> e.g. 127.255.255.255\n"
175  " --time_factor <factor> e.g. 2.5\n"
176  " --fg_fdm";
177 
178 
179  while (1) {
180 
181  static struct option long_options[] = {
182  {"fg_host", 1, NULL, 0},
183  {"fg_port", 1, NULL, 0},
184  {"fg_time_offset", 1, NULL, 0},
185  {"js_dev", 2, NULL, 0},
186  {"spektrum_dev", 1, NULL, 0},
187  {"rc_script", 1, NULL, 0},
188  {"norc", 0, NULL, 0},
189  {"ivy_bus", 1, NULL, 0},
190  {"time_factor", 1, NULL, 0},
191  {"fg_fdm", 0, NULL, 0},
192  {"fg_port_in", 1, NULL, 0},
193  {0, 0, 0, 0}
194  };
195  int option_index = 0;
196  int c = getopt_long(argc, argv, "jh",
197  long_options, &option_index);
198  if (c == -1) {
199  break;
200  }
201 
202  switch (c) {
203  case 0:
204  switch (option_index) {
205  case 0:
206  nps_main.fg_host = strdup(optarg); break;
207  case 1:
208  nps_main.fg_port = atoi(optarg); break;
209  case 2:
210  nps_main.fg_time_offset = atoi(optarg); break;
211  case 3:
212  if (optarg == NULL) {nps_main.js_dev = strdup("0");}
213  else {nps_main.js_dev = strdup(optarg);}
214  break;
215  case 4:
216  nps_main.spektrum_dev = strdup(optarg); break;
217  case 5:
218  nps_main.rc_script = atoi(optarg); break;
219  case 6:
220  nps_main.norc = true; break;
221  case 7:
222  nps_main.ivy_bus = strdup(optarg); break;
223  case 8:
224  nps_main.host_time_factor = atof(optarg); break;
225  case 9:
226  nps_main.fg_fdm = 1; break;
227  case 10:
228  nps_main.fg_port_in = atoi(optarg); break;
229  }
230  break;
231 
232  case 'j':
233  if (optarg == NULL) {nps_main.js_dev = strdup("0");}
234  else {nps_main.js_dev = strdup(optarg);}
235  break;
236 
237  case 'h':
238  fprintf(stderr, usage, argv[0]);
239  exit(0);
240 
241  default:
242  printf("?? getopt returned character code 0%o ??\n", c);
243  fprintf(stderr, usage, argv[0]);
244  exit(EXIT_FAILURE);
245  }
246  }
247  return TRUE;
248 }
249 
250 
251 void *nps_flight_gear_loop(void *data __attribute__((unused)))
252 {
253  struct timespec requestStart;
254  struct timespec requestEnd;
255  struct timespec waitFor;
256  long int period_ns = DISPLAY_DT * 1000000000L; // thread period in nanoseconds
257  long int task_ns = 0; // time it took to finish the task in nanoseconds
258 
259 
261 
262  while (TRUE) {
263  clock_get_current_time(&requestStart);
264 
265  pthread_mutex_lock(&fdm_mutex);
266  if (nps_main.fg_host) {
267  if (nps_main.fg_fdm) {
269  } else {
271  }
272  }
273  pthread_mutex_unlock(&fdm_mutex);
274 
275  clock_get_current_time(&requestEnd);
276 
277  // Calculate time it took
278  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
279 
280  // task took less than one period, sleep for the rest of time
281  if (task_ns < period_ns) {
282  waitFor.tv_sec = 0;
283  waitFor.tv_nsec = period_ns - task_ns;
284  nanosleep(&waitFor, NULL);
285  } else {
286  // task took longer than the period
287 #ifdef PRINT_TIME
288  printf("FG THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
289  (double)task_ns / 1E6, (double)period_ns / 1E6);
290 #endif
291  }
292  }
293 
294  return(NULL);
295 }
296 
297 
298 
299 void *nps_main_display(void *data __attribute__((unused)))
300 {
301  struct timespec requestStart;
302  struct timespec requestEnd;
303  struct timespec waitFor;
304  long int period_ns = 3 * DISPLAY_DT * 1000000000L; // thread period in nanoseconds
305  long int task_ns = 0; // time it took to finish the task in nanoseconds
306 
307  struct NpsFdm fdm_ivy;
308  struct NpsSensors sensors_ivy;
309 
311 
312  while (TRUE) {
313  clock_get_current_time(&requestStart);
314 
315  pthread_mutex_lock(&fdm_mutex);
316  memcpy(&fdm_ivy, &fdm, sizeof(fdm));
317  memcpy(&sensors_ivy, &sensors, sizeof(sensors));
318  pthread_mutex_unlock(&fdm_mutex);
319 
320  nps_ivy_display(&fdm_ivy, &sensors_ivy);
321 
322  clock_get_current_time(&requestEnd);
323 
324  // Calculate time it took
325  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
326 
327  // task took less than one period, sleep for the rest of time
328  if (task_ns < period_ns) {
329  waitFor.tv_sec = 0;
330  waitFor.tv_nsec = period_ns - task_ns;
331  nanosleep(&waitFor, NULL);
332  } else {
333  // task took longer than the period
334 #ifdef PRINT_TIME
335  printf("IVY DISPLAY THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
336  (double)task_ns / 1E6, (double)period_ns / 1E6);
337 #endif
338  }
339 
340  }
341  return(NULL);
342 }
char * fg_host
Definition: nps_main.h:57
char * ivy_bus
Definition: nps_main.h:66
void nps_flightgear_send_fdm()
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc)...
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 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:69
#define TRUE
Definition: std.h:4
#define clock_get_current_time(_x)
Definition: nps_main.h:17
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:204
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)
Set JSBsim specific fields that are not used for Gazebo.
void nps_flightgear_send()
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with â€...
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)
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