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_flightgear.c
Go to the documentation of this file.
1 #include "nps_flightgear.h"
2 
3 #include <sys/socket.h>
4 #include <sys/time.h>
5 #include <errno.h>
6 #include <time.h>
7 #include <netdb.h>
8 #include <netinet/in.h>
9 #include <arpa/inet.h>
10 #include <stdio.h>
11 #include <math.h>
12 #include <stdlib.h>
13 #include <pthread.h>
14 
15 #include "std.h"
16 #include "../flight_gear.h"
17 #include "nps_main.h"
18 #include "nps_fdm.h"
19 #include "nps_atmosphere.h"
20 
21 static struct {
22  int socket;
23  struct sockaddr_in addr;
24  int socket_in;
25  unsigned int initial_time;
26  unsigned int time_offset;
27 } flightgear;
28 
29 pthread_t th_fg_rx; // fligh gear receive thread
30 
31 void* nps_flightgear_receive(void* data __attribute__((unused)));
32 
33 static double htond(double x)
34 {
35  int *p = (int *)&x;
36  int tmp = p[0];
37  p[0] = htonl(p[1]);
38  p[1] = htonl(tmp);
39 
40  return x;
41 }
42 
43 
44 static float htonf(float x)
45 {
46  int *p = (int *)&x;
47  *p = htonl(*p);
48  return x;
49 }
50 
51 
52 void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
53 {
54  int so_reuseaddr = 1;
55  struct protoent *pte = getprotobyname("UDP");
56  flightgear.socket = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
57  if (flightgear.socket < 0){
58  perror("nps_flightgear_init flightgear.socket socket()");
59  exit(errno);
60  }
61  if ( setsockopt(flightgear.socket, SOL_SOCKET, SO_REUSEADDR,
62  &so_reuseaddr, sizeof(so_reuseaddr)) == -1) {
63  perror("nps_flightgear_init flightgear.socket setsockopt()");
64  exit(errno);
65  }
66 
67  flightgear.addr.sin_family = PF_INET;
68  flightgear.addr.sin_port = htons(port);
69  flightgear.addr.sin_addr.s_addr = inet_addr(host);
70 
71  // incoming flight gear socket
72  // only bind to socket if port_in is not zero
73  if (port_in > 0) {
74  struct sockaddr_in addr_in;
75  flightgear.socket_in = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
76  if (flightgear.socket_in < 0) {
77  perror("nps_flightgear_init flightgear.socket_in socket()");
78  exit(errno);
79  }
80  if ( setsockopt(flightgear.socket_in, SOL_SOCKET, SO_REUSEADDR,
81  &so_reuseaddr, sizeof(so_reuseaddr)) == -1) {
82  perror("nps_flightgear_init flightgear.socket_in setsockopt()");
83  exit(errno);
84  }
85  addr_in.sin_family = PF_INET;
86  addr_in.sin_port = htons(port_in);
87  addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
88 
89  if (bind(flightgear.socket_in, (struct sockaddr*)&addr_in, sizeof(addr_in)) == -1) {
90  perror("nps_flightgear_init bind()");
91  exit(errno);
92  }
93  else{
94  printf("Bount to port %u to receive from\n", port_in);
95  }
96  }
97  else {
98  flightgear.socket_in = -1;
99  }
100 
101  // get current time to use as inital when computing cur_time for FG
102  time_t t = time(NULL);
103  flightgear.initial_time = t;
104  flightgear.time_offset = time_offset;
105 
106  // launch rx thread
107  pthread_create(&th_fg_rx, NULL, nps_flightgear_receive, NULL);
108 }
109 
116 {
117  struct FGNetFDM fgfdm;
118 
119  memset(&fgfdm, 0, sizeof(fgfdm));
120  fgfdm.version = htonl(FG_NET_FDM_VERSION);
121 
122  fgfdm.latitude = htond(fdm.lla_pos.lat);
123  fgfdm.longitude = htond(fdm.lla_pos.lon);
124  fgfdm.altitude = htond(fdm.lla_pos.alt);
125 
126 
127  fgfdm.agl = htonf((float)fdm.agl);
128 
129  fgfdm.phi = htonf((float)fdm.ltp_to_body_eulers.phi);
130  fgfdm.theta = htonf((float)fdm.ltp_to_body_eulers.theta);
131  fgfdm.psi = htonf((float)fdm.ltp_to_body_eulers.psi);
132 
133  fgfdm.vcas = htonf(0.);
134  fgfdm.climb_rate = htonf(0.);
135 
136  fgfdm.num_tanks = htonl(1);
137  fgfdm.fuel_quantity[0] = htonf(0.);
138 
139  fgfdm.cur_time = htonl(flightgear.initial_time + rint(fdm.time));
140  // if cur_time is zero, flightgear would take the real current time
141  //gui.cur_time = 0;
142  // warp is used as an offset to the current time in seconds
143  fgfdm.warp = htonl(flightgear.time_offset);
144 
145  // Engine
146  fgfdm.num_engines = htonl(fdm.num_engines);
147  for (int k = 0; k < FG_NET_FDM_MAX_ENGINES; k++) {
148  // Temprary hack to clearly show when the engine is running
149  if (fdm.eng_state[k] == 1) {
150  fgfdm.rpm[k] = htonf(fdm.rpm[k]);
151  } else {
152  fgfdm.rpm[k] = htonf(0.0);
153  }
154  fgfdm.eng_state[k] = htonl(fdm.eng_state[k]);
155  }
156 
157  //control surfaces
158  fgfdm.elevator = htonf(fdm.elevator);
161  fgfdm.rudder = htonf(fdm.rudder);
162  fgfdm.left_flap = htonf(fdm.flap);
163  fgfdm.right_flap = htonf(fdm.flap);
164 
165  if (sendto(flightgear.socket, (char *)(&fgfdm), sizeof(fgfdm), 0,
166  (struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
167  fprintf(stderr, "error sending to FlightGear\n");
168  fflush(stderr);
169  }
170 }
171 
180 {
181 
182  struct FGNetGUI gui;
183 
185  gui.padding1 = 0; // initialize the padding variable to zero
186 
187  gui.latitude = fdm.lla_pos.lat;
188  gui.longitude = fdm.lla_pos.lon;
189  gui.altitude = fdm.lla_pos.alt;
190  // printf("%f %f %f\n", gui.latitude, gui.longitude, gui.altitude);
191 
192  gui.agl = 1.111652;
193 
197 
198  gui.vcas = 0.;
199  gui.climb_rate = 0.;
200 
201  gui.num_tanks = 1;
202  gui.fuel_quantity[0] = 0.;
203 
204  gui.cur_time = flightgear.initial_time + rint(fdm.time);
205  // if cur_time is zero, flightgear would take the real current time
206  //gui.cur_time = 0;
207  // warp is used as an offset to the current time in seconds
208  gui.warp = flightgear.time_offset;
209 
210  gui.ground_elev = 0.;
211 
212  gui.tuned_freq = 125.65;
213  gui.nav_radial = 90.;
214  gui.in_range = 1;
215  gui.dist_nm = 10.;
216  gui.course_deviation_deg = 0.;
217  gui.gs_deviation_deg = 0.;
218 
219  if (sendto(flightgear.socket, (char *)(&gui), sizeof(gui), 0,
220  (struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
221  fprintf(stderr, "error sending to FlightGear\n");
222  fflush(stderr);
223  }
224 
225 }
226 
227 
231 void* nps_flightgear_receive(void* data __attribute__((unused)))
232 {
233 
234  if (flightgear.socket_in != -1) {
235  // socket is correctly opened
236 
237  struct FGEnvironment env;
238  size_t s_env = sizeof(env);
239  int bytes_read;
240 
241  while(TRUE)
242  {
243  //read first message
244  memset(&env, 0, s_env);
245  bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_WAITALL, NULL, NULL);
246  while (bytes_read != -1) { // while we read a message (empty buffer)
247  if (bytes_read == (int)s_env){
248  // Update wind info
249  pthread_mutex_lock(&fdm_mutex);
251  (double)env.wind_from_north,
252  (double)env.wind_from_east,
253  (double)env.wind_from_down);
254  pthread_mutex_unlock(&fdm_mutex);
255  }
256  else {
257  //error
258  printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)s_env);
259  }
260 
261  //read next message
262  memset(&env, 0, s_env);
263  bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_WAITALL, NULL, NULL);
264  }
265 
266  if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) {
267  perror("nps_flightgear_receive recvfrom()");
268  }
269  }
270  }
271  return NULL;
272 }
273 
274 
float climb_rate
Definition: flight_gear.h:267
double agl
Definition: nps_fdm.h:61
void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double wind_down)
float theta
Definition: flight_gear.h:130
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:127
double time
Definition: nps_fdm.h:46
uint32_t num_tanks
Definition: flight_gear.h:270
float fuel_quantity[FG_NET_FDM_MAX_TANKS]
Definition: flight_gear.h:173
float left_aileron
Definition: flight_gear.h:193
pthread_mutex_t fdm_mutex
Definition: nps_main.h:28
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
float left_flap
Definition: flight_gear.h:191
void nps_flightgear_send(void)
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with ...
double phi
in radians
float left_aileron
Definition: nps_fdm.h:121
float agl
Definition: flight_gear.h:128
uint32_t cur_time
Definition: flight_gear.h:183
float altitude
Definition: flight_gear.h:259
double psi
in radians
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
float wind_from_north
Definition: flight_gear.h:299
double latitude
Definition: flight_gear.h:126
double theta
in radians
float dist_nm
Definition: flight_gear.h:283
float vcas
Definition: flight_gear.h:266
uint32_t cur_time
Definition: flight_gear.h:274
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
float elevator
Definition: flight_gear.h:189
float phi
Definition: flight_gear.h:261
#define FG_NET_FDM_MAX_ENGINES
Definition: flight_gear.h:113
#define FG_NET_GUI_VERSION
Definition: flight_gear.h:231
float phi
Definition: flight_gear.h:129
uint32_t warp
Definition: flight_gear.h:276
float nav_radial
Definition: flight_gear.h:281
float vcas
Definition: flight_gear.h:139
uint32_t version
Definition: flight_gear.h:253
#define TRUE
Definition: std.h:4
void nps_flightgear_send_fdm(void)
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc)...
float fuel_quantity[FG_NET_GUI_MAX_TANKS]
Definition: flight_gear.h:271
float ground_elev
Definition: flight_gear.h:277
float elevator
Definition: nps_fdm.h:119
float theta
Definition: flight_gear.h:262
float right_aileron
Definition: nps_fdm.h:122
uint32_t num_engines
Definition: nps_fdm.h:126
float agl
Definition: flight_gear.h:260
float tuned_freq
Definition: flight_gear.h:280
void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
float psi
Definition: flight_gear.h:263
double longitude
Definition: flight_gear.h:125
uint32_t padding1
Definition: flight_gear.h:254
float wind_from_down
Definition: flight_gear.h:301
float climb_rate
Definition: flight_gear.h:140
pthread_t th_fg_rx
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
uint32_t num_tanks
Definition: flight_gear.h:172
float gs_deviation_deg
Definition: flight_gear.h:285
uint32_t in_range
Definition: flight_gear.h:282
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
Definition: flight_gear.h:160
void * nps_flightgear_receive(void *data)
Receive Flight Gear environment messages.
float rpm[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:128
int32_t warp
Definition: flight_gear.h:185
static float htonf(float x)
double lon
in radians
float psi
Definition: flight_gear.h:131
double latitude
Definition: flight_gear.h:258
static float p[2][2]
float rudder
Definition: nps_fdm.h:123
uint32_t version
Definition: flight_gear.h:121
double altitude
Definition: flight_gear.h:127
float rudder
Definition: flight_gear.h:195
float right_flap
Definition: flight_gear.h:192
uint32_t num_engines
Definition: flight_gear.h:159
float right_aileron
Definition: flight_gear.h:194
float flap
Definition: nps_fdm.h:120
Atmosphere model (pressure, wind) for NPS.
float rpm[FG_NET_FDM_MAX_ENGINES]
Definition: flight_gear.h:161
double longitude
Definition: flight_gear.h:257
float wind_from_east
Definition: flight_gear.h:300
#define FG_NET_FDM_VERSION
Definition: flight_gear.h:112
float course_deviation_deg
Definition: flight_gear.h:284
static double htond(double x)
static struct @337 flightgear