Paparazzi UAS  v5.15_devel-187-g731cc33
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nps_main_sitl.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 <stdio.h>
25 #include <stdlib.h>
26 #include <string.h>
27 #include <sys/time.h>
28 
29 #include "nps_main.h"
30 #include "nps_fdm.h"
31 
32 
33 
34 
35 int main(int argc, char **argv)
36 {
37  if (nps_main_init(argc, argv)) {
38  return 1;
39  }
40 
41  if (nps_main.fg_host) {
42  pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL);
43  }
44  pthread_create(&th_display_ivy, NULL, nps_main_display, NULL);
45  pthread_create(&th_main_loop, NULL, nps_main_loop, NULL);
46  pthread_join(th_main_loop, NULL);
47 
48  return 0;
49 }
50 
51 
52 void nps_update_launch_from_dl(uint8_t value __attribute__((unused))) {}
53 
54 
56 {
57  enum NpsRadioControlType rc_type;
58  char *rc_dev = NULL;
59  if (nps_main.norc) {
60  rc_type = NORC;
61  } else if (nps_main.js_dev) {
62  rc_type = JOYSTICK;
63  rc_dev = nps_main.js_dev;
64  } else if (nps_main.spektrum_dev) {
65  rc_type = SPEKTRUM;
66  rc_dev = nps_main.spektrum_dev;
67  } else {
68  rc_type = SCRIPT;
69  }
70  nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
71 }
72 
73 
75 {
77 
79 
81 
83 
85 
86 }
87 
88 
89 void *nps_main_loop(void *data __attribute__((unused)))
90 {
91  struct timespec requestStart;
92  struct timespec requestEnd;
93  struct timespec waitFor;
94  long int period_ns = HOST_TIMEOUT_MS * 1000000LL; // thread period in nanoseconds
95  long int task_ns = 0; // time it took to finish the task in nanoseconds
96 
97  struct timeval tv_now;
98  double host_time_now;
99 
100  while (TRUE) {
101  if (pauseSignal) {
102  char line[128];
103  double tf = 1.0;
104  double t1, t2, irt;
105 
106  gettimeofday(&tv_now, NULL);
107  t1 = time_to_double(&tv_now);
108  // unscale to initial real time
110 
111  printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ",
113  fflush(stdout);
114  if (fgets(line, 127, stdin)) {
115  if ((sscanf(line, " %le ", &tf) == 1)) {
116  if (tf > 0 && tf < 1000) {
118  }
119  }
120  printf("Time factor is %f\n", nps_main.host_time_factor);
121  }
122  gettimeofday(&tv_now, NULL);
123  t2 = time_to_double(&tv_now);
124  // add the pause to initial real time
125  irt += t2 - t1;
126  nps_main.real_initial_time += t2 - t1;
127  // convert to scaled initial real time
129  pauseSignal = 0;
130  }
131 
132  clock_get_current_time(&requestStart); // init measurement (after the pause signal)
133 
134  gettimeofday(&tv_now, NULL);
135  host_time_now = time_to_double(&tv_now);
136  double host_time_elapsed = nps_main.host_time_factor * (host_time_now - nps_main.scaled_initial_time);
137 
138 #if DEBUG_NPS_TIME
139  printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time,
141 #endif
142 
143  int cnt = 0;
144  static int prev_cnt = 0;
145  static int grow_cnt = 0;
146  while (nps_main.sim_time <= host_time_elapsed) {
147  pthread_mutex_lock(&fdm_mutex);
150  pthread_mutex_unlock(&fdm_mutex);
151  cnt++;
152  }
153 
154  // Check to make sure the simulation doesn't get too far behind real time looping
155  if (cnt > (prev_cnt)) {grow_cnt++;}
156  else { grow_cnt--;}
157  if (grow_cnt < 0) {grow_cnt = 0;}
158  prev_cnt = cnt;
159 
160  if (grow_cnt > 10) {
161  printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
162  }
163 
164 #if DEBUG_NPS_TIME
165  printf("%f,%f\n", nps_main.sim_time, nps_main.display_time);
166 #endif
167 
168  clock_get_current_time(&requestEnd); // end measurement
169 
170  // Calculate time it took
171  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
172 
173  if (task_ns > 0) {
174  waitFor.tv_sec = 0;
175  waitFor.tv_nsec = period_ns - task_ns;
176  nanosleep(&waitFor, NULL);
177  } else {
178  // task took longer than the period
179 #ifdef PRINT_TIME
180  printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
181  (double)task_ns / 1E6, (double)period_ns / 1E6);
182 #endif
183  }
184  }
185  return(NULL);
186 }
char * fg_host
Definition: nps_main.h:57
void * nps_main_display(void *data)
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 NpsAutopilot nps_autopilot
void nps_sensors_run_step(double time)
Definition: nps_sensors.c:28
int nps_main_init(int argc, char **argv)
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:42
NpsRadioControlType
#define HOST_TIMEOUT_MS
Definition: nps_main.h:22
pthread_t th_flight_gear
Definition: nps_main.h:24
char * js_dev
Definition: nps_main.h:62
#define SIM_DT
Definition: nps_main.h:20
int pauseSignal
Definition: nps_main.h:30
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
double scaled_initial_time
Definition: nps_main.h:53
double time_to_double(struct timeval *t)
void nps_autopilot_run_step(double time)
char * spektrum_dev
Definition: nps_main.h:63
void nps_update_launch_from_dl(uint8_t value)
Definition: nps_main_sitl.c:52
void nps_autopilot_run_systime_step(void)
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Update the simulation state.
void nps_main_run_sim_step(void)
Definition: nps_main_sitl.c:74
void * nps_flight_gear_loop(void *data)
double sim_time
Definition: nps_main.h:55
double host_time_factor
Definition: nps_main.h:54
void nps_atmosphere_update(double dt)
unsigned char uint8_t
Definition: types.h:14
pthread_t th_display_ivy
Definition: nps_main.h:25
int main(int argc, char **argv)
Definition: nps_main_sitl.c:35
void nps_radio_and_autopilot_init(void)
Definition: nps_main_sitl.c:55
int rc_script
Definition: nps_main.h:64
pthread_t th_main_loop
Definition: nps_main.h:26
double display_time
Definition: nps_main.h:56
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:47
void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char *js_dev)
void * nps_main_loop(void *data)
Definition: nps_main_sitl.c:89