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_hitl.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 <time.h>
28 
29 #include "termios.h"
30 #include <fcntl.h>
31 #include <unistd.h>
32 
33 
34 #include "paparazzi.h"
35 #include "pprzlink/messages.h"
36 #include "pprzlink/dl_protocol.h"
37 #include "pprzlink/pprz_transport.h"
38 #include "generated/airframe.h"
39 
40 
41 #include "nps_main.h"
42 #include "nps_sensors.h"
43 #include "nps_atmosphere.h"
44 #include "nps_autopilot.h"
45 #include "nps_ivy.h"
46 #include "nps_flightgear.h"
47 #include "mcu_periph/sys_time.h"
48 
49 #include "nps_ins.h"
50 
51 void *nps_ins_data_loop(void *data __attribute__((unused)));
52 void *nps_ap_data_loop(void *data __attribute__((unused)));
53 
54 pthread_t th_ins_data; // sends INS packets to the autopilot
55 pthread_t th_ap_data; // receives commands from the autopilot
56 
57 #define NPS_MAX_MSG_SIZE 512
58 
59 int main(int argc, char **argv)
60 {
61  nps_main_init(argc, argv);
62 
63  if (nps_main.fg_host) {
64  pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL);
65  }
66  pthread_create(&th_display_ivy, NULL, nps_main_display, NULL);
67  pthread_create(&th_main_loop, NULL, nps_main_loop, NULL);
68  pthread_create(&th_ins_data, NULL, nps_ins_data_loop, NULL);
69  pthread_create(&th_ap_data, NULL, nps_ap_data_loop, NULL);
70  pthread_join(th_main_loop, NULL);
71 
72  return 0;
73 }
74 
76 {
77  enum NpsRadioControlType rc_type;
78  char *rc_dev = NULL;
79  if (nps_main.norc) {
80  rc_type = NORC;
81  } else if (nps_main.js_dev) {
82  rc_type = JOYSTICK;
83  rc_dev = nps_main.js_dev;
84  } else if (nps_main.spektrum_dev) {
85  rc_type = SPEKTRUM;
86  rc_dev = nps_main.spektrum_dev;
87  } else {
88  rc_type = SCRIPT;
89  }
90  nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev);
91 }
92 
94 {
95  nps_autopilot.launch = value;
96  printf("Launch value=%u\n",nps_autopilot.launch);
97 }
98 
100 {
102 
104 
106 }
107 
108 void *nps_ins_data_loop(void *data __attribute__((unused)))
109 {
110  struct timespec requestStart;
111  struct timespec requestEnd;
112  struct timespec waitFor;
113  long int period_ns = (1. / INS_FREQUENCY) * 1000000000L; // thread period in nanoseconds
114  long int task_ns = 0; // time it took to finish the task in nanoseconds
115 
116  nps_ins_init(); // initialize ins variables and pointers
117 
118  // configure port
119  int fd = open(INS_DEV, O_WRONLY | O_NOCTTY | O_SYNC);//open(INS_DEV, O_RDWR | O_NOCTTY);
120  if (fd < 0) {
121  printf("INS THREAD: data loop error opening port %i\n", fd);
122  return(NULL);
123  }
124 
125  struct termios new_settings;
126  tcgetattr(fd, &new_settings);
127  memset(&new_settings, 0, sizeof(new_settings));
128  new_settings.c_iflag = 0;
129  new_settings.c_cflag = 0;
130  new_settings.c_lflag = 0;
131  new_settings.c_cc[VMIN] = 0;
132  new_settings.c_cc[VTIME] = 0;
133  cfsetispeed(&new_settings, (speed_t)INS_BAUD);
134  cfsetospeed(&new_settings, (speed_t)INS_BAUD);
135  tcsetattr(fd, TCSANOW, &new_settings);
136 
137  struct NpsFdm fdm_ins;
138 
139  while (TRUE) {
140  // lock mutex
141  pthread_mutex_lock(&fdm_mutex);
142 
143  // start timing
144  clock_get_current_time(&requestStart);
145 
146  // make a copy of fdm struct to speed things up
147  memcpy(&fdm_ins, &fdm, sizeof(fdm));
148 
149  // unlock mutex
150  pthread_mutex_unlock(&fdm_mutex);
151 
152  // fetch data
153  nps_ins_fetch_data(&fdm_ins);
154 
155  // send ins data here
156  static uint16_t idx;
157  idx = nps_ins_fill_buffer();
158 
159  static int wlen;
160  wlen = write(fd, ins_buffer, idx);
161  if (wlen != idx) {
162  printf("INS THREAD: Warning - sent only %u bytes to the autopilot, instead of expected %u\n", wlen, idx);
163  }
164 
165  clock_get_current_time(&requestEnd);
166 
167  // Calculate time it took
168  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
169 
170  // task took less than one period, sleep for the rest of time
171  if (task_ns < period_ns) {
172  waitFor.tv_sec = 0;
173  waitFor.tv_nsec = period_ns - task_ns;
174  nanosleep(&waitFor, NULL);
175  } else {
176  // task took longer than the period
177 #ifdef PRINT_TIME
178  printf("INS THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
179  (double)task_ns / 1E6, (double)period_ns / 1E6);
180 #endif
181  }
182  }
183  return(NULL);
184 }
185 
186 
187 
188 void *nps_ap_data_loop(void *data __attribute__((unused)))
189 {
190  // configure port
191  int fd = open(AP_DEV, O_RDONLY | O_NOCTTY);
192  if (fd < 0) {
193  printf("AP data loop error opening port %i\n", fd);
194  return(NULL);
195  }
196 
197  struct termios new_settings;
198  tcgetattr(fd, &new_settings);
199  memset(&new_settings, 0, sizeof(new_settings));
200  new_settings.c_iflag = 0;
201  new_settings.c_cflag = 0;
202  new_settings.c_lflag = 0;
203  new_settings.c_cc[VMIN] = 1;
204  new_settings.c_cc[VTIME] = 5;
205  cfsetispeed(&new_settings, (speed_t)AP_BAUD);
206  cfsetospeed(&new_settings, (speed_t)AP_BAUD);
207  tcsetattr(fd, TCSANOW, &new_settings);
208 
209  int rdlen;
211  uint8_t cmd_len;
213 
214  struct pprz_transport pprz_tp_logger;
215 
216  pprz_transport_init(&pprz_tp_logger);
217 
218  while (TRUE) {
219  // receive messages from the autopilot
220  rdlen = read(fd, buf, sizeof(buf) - 1);
221 
222  for (int i = 0; i < rdlen; i++) {
223  // parse data
224  parse_pprz(&pprz_tp_logger, buf[i]);
225 
226  // if msg_available then read
227  if (pprz_tp_logger.trans_rx.msg_received) {
228  for (int k = 0; k < pprz_tp_logger.trans_rx.payload_len; k++) {
229  buf[k] = pprz_tp_logger.trans_rx.payload[k];
230  }
231  //Parse message
232  uint8_t sender_id = SenderIdOfPprzMsg(buf);
233  uint8_t msg_id = IdOfPprzMsg(buf);
234 
235  // parse telemetry messages coming from other AC
236  if (sender_id != AC_ID) {
237  switch (msg_id) {
238  default: {
239  break;
240  }
241  }
242  } else {
243  /* parse telemetry messages coming from the correct AC_ID */
244  switch (msg_id) {
245  case DL_COMMANDS:
246  // parse commands message
247  cmd_len = DL_COMMANDS_values_length(buf);
248  memcpy(&cmd_buf, DL_COMMANDS_values(buf), cmd_len * sizeof(int16_t));
249  pthread_mutex_lock(&fdm_mutex);
250  // update commands
251  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
252  nps_autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ;
253  }
254  // hack: invert pitch to fit most JSBSim models
255  nps_autopilot.commands[COMMAND_PITCH] = -(double)cmd_buf[COMMAND_PITCH] / MAX_PPRZ;
256  pthread_mutex_unlock(&fdm_mutex);
257  break;
258  case DL_MOTOR_MIXING:
259  // parse actuarors message
260  cmd_len = DL_MOTOR_MIXING_values_length(buf);
261  // check for out-of-bounds access
262  if (cmd_len > NPS_COMMANDS_NB) {
263  cmd_len = NPS_COMMANDS_NB;
264  }
265  memcpy(&cmd_buf, DL_MOTOR_MIXING_values(buf), cmd_len * sizeof(int16_t));
266  pthread_mutex_lock(&fdm_mutex);
267  // update commands
268  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
269  nps_autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ;
270  }
271  pthread_mutex_unlock(&fdm_mutex);
272  break;
273  default:
274  break;
275  }
276  }
277  pprz_tp_logger.trans_rx.msg_received = false;
278  }
279  }
280  }
281  return(NULL);
282 }
283 
284 
285 
286 
287 void *nps_main_loop(void *data __attribute__((unused)))
288 {
289  struct timespec requestStart;
290  struct timespec requestEnd;
291  struct timespec waitFor;
292  long int period_ns = SIM_DT * 1000000000L; // thread period in nanoseconds
293  long int task_ns = 0; // time it took to finish the task in nanoseconds
294 
295  // check the sim time difference from the realtime
296  // fdm.time - simulation time
297  struct timespec startTime;
298  struct timespec realTime;
299  clock_get_current_time(&startTime);
300  double start_secs = ntime_to_double(&startTime);
301  double real_secs = 0;
302  double real_time = 0;
303  static int guard;
304 
305  while (TRUE) {
306  clock_get_current_time(&requestStart);
307 
308  pthread_mutex_lock(&fdm_mutex);
309 
310  // check the current simulation time
311  clock_get_current_time(&realTime);
312  real_secs = ntime_to_double(&realTime);
313  real_time = real_secs - start_secs; // real time elapsed
314 
315  guard = 0;
316  while ((real_time - fdm.time) > SIM_DT) {
319  guard++;
320  if (guard > 2) {
321  //If we are too much behind, catch up incrementaly
322  break;
323  }
324  }
325  pthread_mutex_unlock(&fdm_mutex);
326 
327  clock_get_current_time(&requestEnd);
328 
329  // Calculate time it took
330  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
331 
332  // task took less than one period, sleep for the rest of time
333  if (task_ns < period_ns) {
334  waitFor.tv_sec = 0;
335  waitFor.tv_nsec = period_ns - task_ns;
336  nanosleep(&waitFor, NULL);
337  } else {
338  // task took longer than the period
339 #ifdef PRINT_TIME
340  printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
341  (double)task_ns / 1E6, (double)period_ns / 1E6);
342 #endif
343  }
344  }
345  return(NULL);
346 }
347 
348 
char * fg_host
Definition: nps_main.h:57
unsigned short uint16_t
Definition: types.h:16
pthread_t th_ap_data
Definition: nps_main_hitl.c:55
char cmd_buf[64]
double time
Definition: nps_fdm.h:46
static uint32_t idx
int main(int argc, char **argv)
Definition: nps_main_hitl.c:59
void * nps_main_display(void *data)
bool norc
Definition: nps_main.h:65
pthread_mutex_t fdm_mutex
Definition: nps_main.h:28
struct NpsAutopilot nps_autopilot
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
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:18
void nps_update_launch_from_dl(uint8_t value)
Definition: nps_main_hitl.c:93
int16_t pprz_t
Definition: paparazzi.h:6
NpsRadioControlType
#define INS_FREQUENCY
Definition: nps_ins.h:34
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
Definition: nps_fdm.h:44
uint8_t * ins_buffer
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
Architecture independent timing functions.
void nps_ins_init(void)
char * spektrum_dev
Definition: nps_main.h:63
signed short int16_t
Definition: types.h:17
void * nps_main_loop(void *data)
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Update the simulation state.
void * nps_flight_gear_loop(void *data)
void * nps_ap_data_loop(void *data)
void * nps_ins_data_loop(void *data)
double sim_time
Definition: nps_main.h:55
pthread_t th_ins_data
Definition: nps_main_hitl.c:54
void nps_ins_fetch_data(struct NpsFdm *fdm_ins)
Fetch data from FDM and store them into vectornav packet NOTE: some noise is being added...
void nps_atmosphere_update(double dt)
#define NPS_MAX_MSG_SIZE
Definition: nps_main_hitl.c:57
unsigned char uint8_t
Definition: types.h:14
double ntime_to_double(struct timespec *t)
int fd
Definition: serial.c:26
pthread_t th_display_ivy
Definition: nps_main.h:25
void nps_radio_and_autopilot_init(void)
Definition: nps_main_hitl.c:75
uint16_t nps_ins_fill_buffer(void)
int rc_script
Definition: nps_main.h:64
#define MAX_PPRZ
Definition: paparazzi.h:8
pthread_t th_main_loop
Definition: nps_main.h:26
Atmosphere model (pressure, wind) for NPS.
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:23
void nps_main_run_sim_step(void)
Definition: nps_main_hitl.c:99
void nps_autopilot_init(enum NpsRadioControlType type, int num_script, char *js_dev)