Paparazzi UAS  v5.15_devel-81-gd13dafb
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  uint32_t rx_msgs = 0;
218  uint32_t rx_commands = 0;
219  uint32_t rx_motor_mixing = 0;
220  uint8_t show_stats = 1;
221 
222  while (TRUE) {
223  if ((rx_msgs % 100 == 0) && show_stats && (rx_msgs > 0) ) {
224  printf("AP data loop received %i messages.\n", rx_msgs);
225  printf("From those, %i were COMMANDS messages, and %i were MOTOR_MIXING messages.\n",
226  rx_commands, rx_motor_mixing);
227  show_stats = 0;
228  }
229 
230  // receive messages from the autopilot
231  // Note: read function might wait until the buffer is full, in which case
232  // it could contain several messages. That might lead to delay in processing,
233  // for example if we send COMMANDS at 100Hz, and each read() will hold 10 COMMANDS
234  // it means a delay of 100ms before the message is processed.
235  rdlen = read(fd, buf, sizeof(buf) - 1);
236 
237  for (int i = 0; i < rdlen; i++) {
238  // parse data
239  parse_pprz(&pprz_tp_logger, buf[i]);
240 
241  // if msg_available then read
242  if (pprz_tp_logger.trans_rx.msg_received) {
243  rx_msgs++;
244  for (int k = 0; k < pprz_tp_logger.trans_rx.payload_len; k++) {
245  buf[k] = pprz_tp_logger.trans_rx.payload[k];
246  }
247  //Parse message
248  uint8_t sender_id = SenderIdOfPprzMsg(buf);
249  uint8_t msg_id = IdOfPprzMsg(buf);
250 
251  // parse telemetry messages coming from other AC
252  if (sender_id != AC_ID) {
253  switch (msg_id) {
254  default: {
255  break;
256  }
257  }
258  } else {
259  /* parse telemetry messages coming from the correct AC_ID */
260  switch (msg_id) {
261  case DL_COMMANDS:
262  // parse commands message
263  rx_commands++;
264  cmd_len = DL_COMMANDS_values_length(buf);
265  memcpy(&cmd_buf, DL_COMMANDS_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  // hack: invert pitch to fit most JSBSim models
272  nps_autopilot.commands[COMMAND_PITCH] = -(double)cmd_buf[COMMAND_PITCH] / MAX_PPRZ;
273  pthread_mutex_unlock(&fdm_mutex);
274  break;
275  case DL_MOTOR_MIXING:
276  // parse actuarors message
277  rx_motor_mixing++;
278  cmd_len = DL_MOTOR_MIXING_values_length(buf);
279  // check for out-of-bounds access
280  if (cmd_len > NPS_COMMANDS_NB) {
281  cmd_len = NPS_COMMANDS_NB;
282  }
283  memcpy(&cmd_buf, DL_MOTOR_MIXING_values(buf), cmd_len * sizeof(int16_t));
284  pthread_mutex_lock(&fdm_mutex);
285  // update commands
286  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
287  nps_autopilot.commands[i] = (double)cmd_buf[i] / MAX_PPRZ;
288  }
289  pthread_mutex_unlock(&fdm_mutex);
290  break;
291  default:
292  break;
293  }
294  }
295  pprz_tp_logger.trans_rx.msg_received = false;
296  }
297  }
298  }
299  return(NULL);
300 }
301 
302 
303 
304 
305 void *nps_main_loop(void *data __attribute__((unused)))
306 {
307  struct timespec requestStart;
308  struct timespec requestEnd;
309  struct timespec waitFor;
310  long int period_ns = SIM_DT * 1000000000L; // thread period in nanoseconds
311  long int task_ns = 0; // time it took to finish the task in nanoseconds
312 
313  // check the sim time difference from the realtime
314  // fdm.time - simulation time
315  struct timespec startTime;
316  struct timespec realTime;
317  clock_get_current_time(&startTime);
318  double start_secs = ntime_to_double(&startTime);
319  double real_secs = 0;
320  double real_time = 0;
321  static int guard;
322 
323  while (TRUE) {
324  clock_get_current_time(&requestStart);
325 
326  pthread_mutex_lock(&fdm_mutex);
327 
328  // check the current simulation time
329  clock_get_current_time(&realTime);
330  real_secs = ntime_to_double(&realTime);
331  real_time = real_secs - start_secs; // real time elapsed
332 
333  guard = 0;
334  while ((real_time - fdm.time) > SIM_DT) {
337  guard++;
338  if (guard > 2) {
339  //If we are too much behind, catch up incrementaly
340  break;
341  }
342  }
343  pthread_mutex_unlock(&fdm_mutex);
344 
345  clock_get_current_time(&requestEnd);
346 
347  // Calculate time it took
348  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
349 
350  // task took less than one period, sleep for the rest of time
351  if (task_ns < period_ns) {
352  waitFor.tv_sec = 0;
353  waitFor.tv_nsec = period_ns - task_ns;
354  nanosleep(&waitFor, NULL);
355  } else {
356  // task took longer than the period
357 #ifdef PRINT_TIME
358  printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
359  (double)task_ns / 1E6, (double)period_ns / 1E6);
360 #endif
361  }
362  }
363  return(NULL);
364 }
365 
366 
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:42
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:70
#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
unsigned long uint32_t
Definition: types.h:18
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:47
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)