Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "paparazzi.h"
30 #include "generated/airframe.h"
31 
32 
33 #include "nps_main.h"
34 #include "nps_sensors.h"
35 #include "nps_atmosphere.h"
36 #include "nps_ivy.h"
37 #include "nps_flightgear.h"
38 
39 // nps_autopilot.c is not compiled in HITL
41 
42 int main(int argc, char **argv)
43 {
44  nps_main_init(argc, argv);
45 
47 
48  if (nps_main.fg_host) {
49  pthread_create(&th_flight_gear, NULL, nps_flight_gear_loop, NULL);
50  }
51  pthread_create(&th_display_ivy, NULL, nps_main_display, NULL);
52  pthread_create(&th_main_loop, NULL, nps_main_loop, NULL);
53  pthread_join(th_main_loop, NULL);
54 
55  return 0;
56 }
57 
59 {
60 }
61 
63 {
64  nps_autopilot.launch = value;
65  printf("Launch value=%u\n",nps_autopilot.launch);
66 }
67 
69 {
71 
73 
75 }
76 
77 
78 void *nps_main_loop(void *data __attribute__((unused)))
79 {
80  struct timespec requestStart;
81  struct timespec requestEnd;
82  struct timespec waitFor;
83  long int period_ns = SIM_DT * 1000000000L; // thread period in nanoseconds
84  long int task_ns = 0; // time it took to finish the task in nanoseconds
85 
86  // check the sim time difference from the realtime
87  // fdm.time - simulation time
88  struct timespec startTime;
89  struct timespec realTime;
90  clock_get_current_time(&startTime);
91  double start_secs = ntime_to_double(&startTime);
92  double real_secs = 0;
93  double real_time = 0;
94  static int guard;
95 
96  while (TRUE) {
97  clock_get_current_time(&requestStart);
98 
99  pthread_mutex_lock(&fdm_mutex);
100 
101  // check the current simulation time
102  clock_get_current_time(&realTime);
103  real_secs = ntime_to_double(&realTime);
104  real_time = real_secs - start_secs; // real time elapsed
105 
106  guard = 0;
107  while ((real_time - fdm.time) > SIM_DT) {
110  guard++;
111  if (guard > 2) {
112  //If we are too much behind, catch up incrementaly
113  break;
114  }
115  }
116  pthread_mutex_unlock(&fdm_mutex);
117 
118  clock_get_current_time(&requestEnd);
119 
120  // Calculate time it took
121  task_ns = (requestEnd.tv_sec - requestStart.tv_sec) * 1000000000L + (requestEnd.tv_nsec - requestStart.tv_nsec);
122 
123  // task took less than one period, sleep for the rest of time
124  if (task_ns < period_ns) {
125  waitFor.tv_sec = 0;
126  waitFor.tv_nsec = period_ns - task_ns;
127  nanosleep(&waitFor, NULL);
128  } else {
129  // task took longer than the period
130 #ifdef PRINT_TIME
131  printf("MAIN THREAD: task took longer than one period, exactly %f [ms], but the period is %f [ms]\n",
132  (double)task_ns / 1E6, (double)period_ns / 1E6);
133 #endif
134  }
135  }
136  return(NULL);
137 }
138 
139 
void nps_atmosphere_update(double dt)
Atmosphere model (pressure, wind) for NPS.
double commands[NPS_COMMANDS_NB]
Definition: nps_autopilot.h:47
#define NPS_COMMANDS_NB
Number of commands sent to the FDM of NPS.
Definition: nps_autopilot.h:42
double time
Definition: nps_fdm.h:46
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...
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void nps_hitl_impl_init(void)
END pprzlink_dev.
void * nps_flight_gear_loop(void *data)
#define clock_get_current_time(_x)
Definition: nps_main.h:17
pthread_t th_display_ivy
struct NpsMain nps_main
double sim_time
Definition: nps_main.h:57
char * fg_host
Definition: nps_main.h:59
int nps_main_init(int argc, char **argv)
void * nps_main_display(void *data)
#define SIM_DT
Definition: nps_main.h:20
pthread_mutex_t fdm_mutex
pthread_t th_main_loop
pthread_t th_flight_gear
double ntime_to_double(struct timespec *t)
struct NpsAutopilot nps_autopilot
#ifndef NPS_NO_MOTOR_MIXING #include "modules/actuators/motor_mixing.h"
Definition: nps_main_hitl.c:40
int main(int argc, char **argv)
Definition: nps_main_hitl.c:42
void nps_radio_and_autopilot_init(void)
Definition: nps_main_hitl.c:58
void nps_update_launch_from_dl(uint8_t value)
Definition: nps_main_hitl.c:62
void nps_main_run_sim_step(void)
Definition: nps_main_hitl.c:68
void * nps_main_loop(void *data)
Definition: nps_main_hitl.c:78
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.
Definition: vl53l1_types.h:98