Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
35int 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
52void 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
87
88
89void *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 pthread_mutex_lock(&fdm_mutex);
102 pthread_mutex_unlock(&fdm_mutex);
103
104 while (TRUE) {
105 if (pauseSignal) {
106 char line[128];
107 double tf = 1.0;
108 double t1, t2, irt;
109
110 gettimeofday(&tv_now, NULL);
111 t1 = time_to_double(&tv_now);
112 // unscale to initial real time
113 irt = t1 - (t1 - nps_main.scaled_initial_time) * nps_main.host_time_factor;
114
115 printf("Press <enter> to continue (or CTRL-Z to suspend).\nEnter a new time factor if needed (current: %f): ",
116 nps_main.host_time_factor);
117 fflush(stdout);
118 if (fgets(line, 127, stdin)) {
119 if ((sscanf(line, " %le ", &tf) == 1)) {
120 if (tf > 0 && tf < 1000) {
121 nps_main.host_time_factor = tf;
122 }
123 }
124 printf("Time factor is %f\n", nps_main.host_time_factor);
125 }
126 gettimeofday(&tv_now, NULL);
127 t2 = time_to_double(&tv_now);
128 // add the pause to initial real time
129 irt += t2 - t1;
130 nps_main.real_initial_time += t2 - t1;
131 // convert to scaled initial real time
132 nps_main.scaled_initial_time = t2 - (t2 - irt) / nps_main.host_time_factor;
133 pauseSignal = 0;
134 }
135
136 clock_get_current_time(&requestStart); // init measurement (after the pause signal)
137
138 gettimeofday(&tv_now, NULL);
139 host_time_now = time_to_double(&tv_now);
140 double host_time_elapsed = nps_main.host_time_factor * (host_time_now - nps_main.scaled_initial_time);
141
142#if DEBUG_NPS_TIME
143 printf("%f,%f,%f,%f,%f,%f,", nps_main.host_time_factor, host_time_elapsed, host_time_now, nps_main.scaled_initial_time,
144 nps_main.sim_time, nps_main.display_time);
145#endif
146
147 int cnt = 0;
148 static int prev_cnt = 0;
149 static int grow_cnt = 0;
150 while (nps_main.sim_time <= host_time_elapsed) {
151 pthread_mutex_lock(&fdm_mutex);
153 nps_main.sim_time += SIM_DT;
154 pthread_mutex_unlock(&fdm_mutex);
155 cnt++;
156 }
157
158 // Check to make sure the simulation doesn't get too far behind real time looping
159 if (cnt > (prev_cnt)) {grow_cnt++;}
160 else { grow_cnt--;}
161 if (grow_cnt < 0) {grow_cnt = 0;}
162 prev_cnt = cnt;
163
164 if (grow_cnt > 10) {
165 printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
166 }
167
168#if DEBUG_NPS_TIME
169 printf("%f,%f\n", nps_main.sim_time, nps_main.display_time);
170#endif
171
172 clock_get_current_time(&requestEnd); // end measurement
173
174 // Calculate time it took
175 task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
176
177 if (task_ns > 0) {
178 waitFor.tv_sec = 0;
179 waitFor.tv_nsec = period_ns - task_ns;
180 nanosleep(&waitFor, NULL);
181 } else {
182 // task took longer than the period
183#ifdef PRINT_TIME
184 printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
185 (double)task_ns / 1E6, (double)period_ns / 1E6);
186#endif
187 }
188 }
189 return(NULL);
190}
#define E6
static float t1
int main(void)
Definition main_demo1.c:5
void nps_atmosphere_update(double dt)
struct NpsAutopilot nps_autopilot
ifndef NPS_NO_MOTOR_MIXING include "modules/actuators/motor_mixing.h"
void nps_autopilot_run_step(double time)
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
void nps_autopilot_run_systime_step(void)
void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char *js_dev)
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and c...
void nps_fdm_run_init_step(void)
void * nps_main_loop(void *data)
#define clock_get_current_time(_x)
Definition nps_main.h:17
pthread_t th_display_ivy
struct NpsMain nps_main
#define HOST_TIMEOUT_MS
Definition nps_main.h:22
int nps_main_init(int argc, char **argv)
void * nps_main_display(void *data)
int pauseSignal
#define SIM_DT
Definition nps_main.h:20
void * nps_flight_gear_loop(void *data)
double time_to_double(struct timeval *t)
pthread_mutex_t fdm_mutex
pthread_t th_main_loop
pthread_t th_flight_gear
void * nps_main_loop(void *data)
void nps_radio_and_autopilot_init(void)
void nps_update_launch_from_dl(uint8_t value)
void nps_main_run_sim_step(void)
NpsRadioControlType
@ SPEKTRUM
@ SCRIPT
@ JOYSTICK
void nps_sensors_run_step(double time)
Definition nps_sensors.c:25
#define TRUE
Definition std.h:4
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.