Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_main.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  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
23 #include <stdio.h>
24 #include <stdlib.h>
25 #include <string.h>
26 #include <signal.h>
27 #include <glib.h>
28 #include <sys/time.h>
29 #include <getopt.h>
30 
31 #include "nps_main.h"
32 #include "nps_fdm.h"
33 #include "nps_sensors.h"
34 #include "nps_atmosphere.h"
35 #include "nps_autopilot.h"
36 #include "nps_ivy.h"
37 #include "nps_flightgear.h"
38 #include "mcu_periph/sys_time.h"
39 #define SIM_DT (1./SYS_TIME_FREQUENCY)
40 #define DISPLAY_DT (1./30.)
41 #define HOST_TIMEOUT_MS 40
42 
43 
44 static struct {
45  double real_initial_time;
46  double scaled_initial_time;
47  double host_time_factor;
48  double sim_time;
49  double display_time;
50  char *fg_host;
51  unsigned int fg_port;
52  unsigned int fg_port_in;
53  unsigned int fg_time_offset;
54  int fg_fdm;
55  char *js_dev;
56  char *spektrum_dev;
57  int rc_script;
58  char *ivy_bus;
59 } nps_main;
60 
61 static bool nps_main_parse_options(int argc, char **argv);
62 static void nps_main_init(void);
63 static void nps_main_display(void);
64 static void nps_main_run_sim_step(void);
65 static gboolean nps_main_periodic(gpointer data __attribute__((unused)));
66 
67 int pauseSignal = 0;
68 
69 void tstp_hdl(int n __attribute__((unused)))
70 {
71  if (pauseSignal) {
72  pauseSignal = 0;
73  signal(SIGTSTP, SIG_DFL);
74  raise(SIGTSTP);
75  } else {
76  pauseSignal = 1;
77  }
78 }
79 
80 void cont_hdl(int n __attribute__((unused)))
81 {
82  signal(SIGCONT, cont_hdl);
83  signal(SIGTSTP, tstp_hdl);
84  printf("Press <enter> to continue.\n");
85 }
86 
87 double time_to_double(struct timeval *t)
88 {
89  return ((double)t->tv_sec + (double)(t->tv_usec * 1e-6));
90 }
91 
92 int main(int argc, char **argv)
93 {
94 
95  if (!nps_main_parse_options(argc, argv)) { return 1; }
96 
97  /* disable buffering for stdout,
98  * so it properly works in paparazzi center
99  * where it is not detected as interactive
100  * and hence fully buffered instead of line buffered
101  */
102  setbuf(stdout, NULL);
103 
104  nps_main_init();
105 
106  signal(SIGCONT, cont_hdl);
107  signal(SIGTSTP, tstp_hdl);
108  printf("Time factor is %f. (Press Ctrl-Z to change)\n", nps_main.host_time_factor);
109 
110  GMainLoop *ml = g_main_loop_new(NULL, FALSE);
111  g_timeout_add(HOST_TIMEOUT_MS, nps_main_periodic, NULL);
112  g_main_loop_run(ml);
113 
114  return 0;
115 }
116 
117 
118 static void nps_main_init(void)
119 {
120 
121  nps_main.sim_time = 0.;
122  nps_main.display_time = 0.;
123  struct timeval t;
124  gettimeofday(&t, NULL);
125  nps_main.real_initial_time = time_to_double(&t);
126  nps_main.scaled_initial_time = time_to_double(&t);
127 
128  nps_ivy_init(nps_main.ivy_bus);
131  nps_sensors_init(nps_main.sim_time);
132  printf("Simulating with dt of %f\n", SIM_DT);
133 
134  enum NpsRadioControlType rc_type;
135  char *rc_dev = NULL;
136  if (nps_main.js_dev) {
137  rc_type = JOYSTICK;
138  rc_dev = nps_main.js_dev;
139  } else if (nps_main.spektrum_dev) {
140  rc_type = SPEKTRUM;
141  rc_dev = nps_main.spektrum_dev;
142  } else {
143  rc_type = SCRIPT;
144  }
145  nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
146 
147  if (nps_main.fg_host)
148  nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset);
149 
150 
151 #if DEBUG_NPS_TIME
152  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");
153 #endif
154 
155 }
156 
157 
158 
159 static void nps_main_run_sim_step(void)
160 {
161  // printf("sim at %f\n", nps_main.sim_time);
162 
164 
166 
168 
169  nps_sensors_run_step(nps_main.sim_time);
170 
172 
173 }
174 
175 
176 static void nps_main_display(void)
177 {
178  // printf("display at %f\n", nps_main.display_time);
179  nps_ivy_display();
180 
181  if (nps_main.fg_host) {
182  if (nps_main.fg_fdm) {
184  } else {
186  }
187  }
189 }
190 
191 
192 void nps_set_time_factor(float time_factor)
193 {
194  if (time_factor < 0.0 || time_factor > 100.0) {
195  return;
196  }
197  if (fabs(nps_main.host_time_factor - time_factor) < 0.01) {
198  return;
199  }
200 
201  struct timeval tv_now;
202  double t_now, t_elapsed;
203 
204  gettimeofday(&tv_now, NULL);
205  t_now = time_to_double(&tv_now);
206 
207  /* "virtual" elapsed time with old time factor */
208  t_elapsed = (t_now - nps_main.scaled_initial_time) * nps_main.host_time_factor;
209 
210  /* set new time factor */
211  nps_main.host_time_factor = time_factor;
212  printf("Time factor is %f\n", nps_main.host_time_factor);
213  fflush(stdout);
214 
215  /* set new "virtual" scaled initial time using new time factor*/
216  nps_main.scaled_initial_time = t_now - t_elapsed / nps_main.host_time_factor;
217 }
218 
219 
220 static gboolean nps_main_periodic(gpointer data __attribute__((unused)))
221 {
222  struct timeval tv_now;
223  double host_time_now;
224 
225  if (pauseSignal) {
226  char line[128];
227  double tf = 1.0;
228  double t1, t2, irt;
229 
230  gettimeofday(&tv_now, NULL);
231  t1 = time_to_double(&tv_now);
232  /* unscale to initial real time*/
233  irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor;
234 
235  printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ",
236  nps_main.host_time_factor);
237  fflush(stdout);
238  if (fgets(line, 127, stdin)) {
239  if ((sscanf(line, " %le ", &tf) == 1)) {
240  if (tf > 0 && tf < 1000) {
241  nps_main.host_time_factor = tf;
242  }
243  }
244  printf("Time factor is %f\n", nps_main.host_time_factor);
245  }
246  gettimeofday(&tv_now, NULL);
247  t2 = time_to_double(&tv_now);
248  /* add the pause to initial real time */
249  irt += t2 - t1;
250  nps_main.real_initial_time += t2 - t1;
251  /* convert to scaled initial real time */
252  nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor;
253  pauseSignal = 0;
254  }
255 
256  gettimeofday(&tv_now, NULL);
257  host_time_now = time_to_double(&tv_now);
258  double host_time_elapsed = nps_main.host_time_factor * (host_time_now - nps_main.scaled_initial_time);
259 
260 #if DEBUG_NPS_TIME
261  printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time,
262  nps_main.sim_time, nps_main.display_time);
263 #endif
264 
265  int cnt = 0;
266  static int prev_cnt = 0;
267  static int grow_cnt = 0;
268  while (nps_main.sim_time <= host_time_elapsed) {
270  nps_main.sim_time += SIM_DT;
271  if (nps_main.display_time < (host_time_now - nps_main.real_initial_time)) {
273  nps_main.display_time += DISPLAY_DT;
274  }
275  cnt++;
276  }
277 
278  /* Check to make sure the simulation doesn't get too far behind real time looping */
279  if (cnt > (prev_cnt)) {grow_cnt++;}
280  else { grow_cnt--;}
281  if (grow_cnt < 0) {grow_cnt = 0;}
282  prev_cnt = cnt;
283 
284  if (grow_cnt > 10) {
285  printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
286  }
287 
288 #if DEBUG_NPS_TIME
289  printf("%f,%f\n", nps_main.sim_time, nps_main.display_time);
290 #endif
291 
292  return TRUE;
293 
294 }
295 
296 
297 static bool nps_main_parse_options(int argc, char **argv)
298 {
299 
300  nps_main.fg_host = NULL;
301  nps_main.fg_port = 5501;
302  nps_main.fg_port_in = 5502;
303  nps_main.fg_time_offset = 0;
304  nps_main.js_dev = NULL;
305  nps_main.spektrum_dev = NULL;
306  nps_main.rc_script = 0;
307  nps_main.ivy_bus = NULL;
308  nps_main.host_time_factor = 1.0;
309  nps_main.fg_fdm = 0;
310 
311  static const char *usage =
312  "Usage: %s [options]\n"
313  " Options :\n"
314  " -h Display this help\n"
315  " --fg_host <flight gear host> e.g. 127.0.0.1\n"
316  " --fg_port <flight gear port> e.g. 5501\n"
317  " --fg_port_in <flight gear in port> e.g. 5502\n"
318  " --fg_time_offset <offset in seconds> e.g. 21600 for 6h\n"
319  " -j --js_dev <optional joystick index> e.g. 1 (default 0)\n"
320  " --spektrum_dev <spektrum device> e.g. /dev/ttyUSB0\n"
321  " --rc_script <number> e.g. 0\n"
322  " --ivy_bus <ivy bus> e.g. 127.255.255.255\n"
323  " --time_factor <factor> e.g. 2.5\n"
324  " --fg_fdm";
325 
326 
327  while (1) {
328 
329  static struct option long_options[] = {
330  {"fg_host", 1, NULL, 0},
331  {"fg_port", 1, NULL, 0},
332  {"fg_time_offset", 1, NULL, 0},
333  {"js_dev", 2, NULL, 0},
334  {"spektrum_dev", 1, NULL, 0},
335  {"rc_script", 1, NULL, 0},
336  {"ivy_bus", 1, NULL, 0},
337  {"time_factor", 1, NULL, 0},
338  {"fg_fdm", 0, NULL, 0},
339  {"fg_port_in", 1, NULL, 0},
340  {0, 0, 0, 0}
341  };
342  int option_index = 0;
343  int c = getopt_long(argc, argv, "jh",
344  long_options, &option_index);
345  if (c == -1) {
346  break;
347  }
348 
349  switch (c) {
350  case 0:
351  switch (option_index) {
352  case 0:
353  nps_main.fg_host = strdup(optarg); break;
354  case 1:
355  nps_main.fg_port = atoi(optarg); break;
356  case 2:
357  nps_main.fg_time_offset = atoi(optarg); break;
358  case 3:
359  if (optarg == NULL) {nps_main.js_dev = strdup("0");}
360  else {nps_main.js_dev = strdup(optarg);}
361  break;
362  case 4:
363  nps_main.spektrum_dev = strdup(optarg); break;
364  case 5:
365  nps_main.rc_script = atoi(optarg); break;
366  case 6:
367  nps_main.ivy_bus = strdup(optarg); break;
368  case 7:
369  nps_main.host_time_factor = atof(optarg); break;
370  case 8:
371  nps_main.fg_fdm = 1; break;
372  case 9:
373  nps_main.fg_port_in = atoi(optarg); break;
374  }
375  break;
376 
377  case 'j':
378  if (optarg == NULL) {nps_main.js_dev = strdup("0");}
379  else {nps_main.js_dev = strdup(optarg);}
380  break;
381 
382  case 'h':
383  fprintf(stderr, usage, argv[0]);
384  exit(0);
385 
386  default:
387  printf("?? getopt returned character code 0%o ??\n", c);
388  fprintf(stderr, usage, argv[0]);
389  exit(EXIT_FAILURE);
390  }
391  }
392  return TRUE;
393 }
static gboolean nps_main_periodic(gpointer data)
Definition: nps_main.c:220
void nps_flightgear_send_fdm()
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc)...
void nps_ivy_init(char *ivy_bus)
Definition: nps_ivy.c:37
void nps_sensors_run_step(double time)
Definition: nps_sensors.c:27
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:22
void tstp_hdl(int n)
Definition: nps_main.c:69
#define HOST_TIMEOUT_MS
Definition: nps_main.c:41
static void nps_main_display(void)
Definition: nps_main.c:176
void cont_hdl(int n)
Definition: nps_main.c:80
void nps_ivy_display(void)
Definition: nps_ivy.c:144
NpsRadioControlType
#define FALSE
Definition: std.h:5
#define TRUE
Definition: std.h:4
static void nps_main_run_sim_step(void)
Definition: nps_main.c:159
#define SIM_DT
Definition: nps_main.c:39
Architecture independent timing functions.
void nps_autopilot_run_step(double time)
void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
int main(int argc, char **argv)
Definition: nps_main.c:92
struct NpsAutopilot autopilot
static void nps_main_init(void)
Definition: nps_main.c:118
void nps_autopilot_run_systime_step(void)
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
int pauseSignal
Definition: nps_main.c:67
void nps_fdm_init(double dt)
void nps_flightgear_send()
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with ...
void nps_set_time_factor(float time_factor)
Definition: nps_main.c:192
void nps_atmosphere_update(double dt)
#define DISPLAY_DT
Definition: nps_main.c:40
void nps_flightgear_receive()
Receive Flight Gear environment messages.
double time_to_double(struct timeval *t)
Definition: nps_main.c:87
static bool nps_main_parse_options(int argc, char **argv)
Definition: nps_main.c:297
void nps_atmosphere_init(void)
Atmosphere model (pressure, wind) for NPS.
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:28
static struct @328 nps_main
void(* rc_script)(double)
void nps_sensors_init(double time)
Definition: nps_sensors.c:8
void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char *js_dev)