Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
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 
14 
15 #include "std.h"
16 #include "../flight_gear.h"
17 #include "nps_fdm.h"
18 #include "nps_atmosphere.h"
19 
20 static struct {
21  int socket;
22  struct sockaddr_in addr;
23  int socket_in;
24  unsigned int initial_time;
25  unsigned int time_offset;
26 } flightgear;
27 
28 
29 double htond(double x)
30 {
31  int *p = (int *)&x;
32  int tmp = p[0];
33  p[0] = htonl(p[1]);
34  p[1] = htonl(tmp);
35 
36  return x;
37 }
38 
39 
40 float htonf(float x)
41 {
42  int *p = (int *)&x;
43  *p = htonl(*p);
44  return x;
45 }
46 
47 
48 void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
49 {
50  int so_reuseaddr = 1;
51  struct protoent *pte = getprotobyname("UDP");
52  flightgear.socket = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
53  if (flightgear.socket < 0){
54  perror("nps_flightgear_init flightgear.socket socket()");
55  exit(errno);
56  }
57  if ( setsockopt(flightgear.socket, SOL_SOCKET, SO_REUSEADDR,
58  &so_reuseaddr, sizeof(so_reuseaddr)) == -1) {
59  perror("nps_flightgear_init flightgear.socket setsockopt()");
60  exit(errno);
61  }
62 
63  flightgear.addr.sin_family = PF_INET;
64  flightgear.addr.sin_port = htons(port);
65  flightgear.addr.sin_addr.s_addr = inet_addr(host);
66 
67  // incoming flight gear socket
68  // only bind to socket if port_in is not zero
69  if (port_in > 0) {
70  struct sockaddr_in addr_in;
71  flightgear.socket_in = socket(PF_INET, SOCK_DGRAM, pte->p_proto);
72  if (flightgear.socket_in < 0) {
73  perror("nps_flightgear_init flightgear.socket_in socket()");
74  exit(errno);
75  }
76  if ( setsockopt(flightgear.socket_in, SOL_SOCKET, SO_REUSEADDR,
77  &so_reuseaddr, sizeof(so_reuseaddr)) == -1) {
78  perror("nps_flightgear_init flightgear.socket_in setsockopt()");
79  exit(errno);
80  }
81  addr_in.sin_family = PF_INET;
82  addr_in.sin_port = htons(port_in);
83  addr_in.sin_addr.s_addr = htonl(INADDR_ANY);
84 
85  if (bind(flightgear.socket_in, (struct sockaddr*)&addr_in, sizeof(addr_in)) == -1) {
86  perror("nps_flightgear_init bind()");
87  exit(errno);
88  }
89  else{
90  printf("Bount to port %u to receive from\n", port_in);
91  }
92  }
93  else {
94  flightgear.socket_in = -1;
95  }
96 
97  // get current time to use as inital when computing cur_time for FG
98  time_t t = time(NULL);
99  flightgear.initial_time = t;
100  flightgear.time_offset = time_offset;
101 }
102 
109 {
110  struct FGNetFDM fgfdm;
111 
112  memset(&fgfdm, 0, sizeof(fgfdm));
113  fgfdm.version = htonl(FG_NET_FDM_VERSION);
114 
115  fgfdm.latitude = htond(fdm.lla_pos.lat);
116  fgfdm.longitude = htond(fdm.lla_pos.lon);
117  fgfdm.altitude = htond(fdm.lla_pos.alt);
118 
119 
120  fgfdm.agl = htonf((float)fdm.agl);
121 
122  fgfdm.phi = htonf((float)fdm.ltp_to_body_eulers.phi);
123  fgfdm.theta = htonf((float)fdm.ltp_to_body_eulers.theta);
124  fgfdm.psi = htonf((float)fdm.ltp_to_body_eulers.psi);
125 
126  fgfdm.vcas = htonf(0.);
127  fgfdm.climb_rate = htonf(0.);
128 
129  fgfdm.num_tanks = htonl(1);
130  fgfdm.fuel_quantity[0] = htonf(0.);
131 
132  fgfdm.cur_time = htonl(flightgear.initial_time + rint(fdm.time));
133  // if cur_time is zero, flightgear would take the real current time
134  //gui.cur_time = 0;
135  // warp is used as an offset to the current time in seconds
136  fgfdm.warp = htonl(flightgear.time_offset);
137 
138  // Engine
139  fgfdm.num_engines = htonl(fdm.num_engines);
140  for (int k = 0; k < FG_NET_FDM_MAX_ENGINES; k++) {
141  // Temprary hack to clearly show when the engine is running
142  if (fdm.eng_state[k] == 1) {
143  fgfdm.rpm[k] = htonf(fdm.rpm[k]);
144  } else {
145  fgfdm.rpm[k] = htonf(0.0);
146  }
147  fgfdm.eng_state[k] = htonl(fdm.eng_state[k]);
148  }
149 
150  //control surfaces
151  fgfdm.elevator = htonf(fdm.elevator);
154  fgfdm.rudder = htonf(fdm.rudder);
155  fgfdm.left_flap = htonf(fdm.flap);
156  fgfdm.right_flap = htonf(fdm.flap);
157 
158  if (sendto(flightgear.socket, (char *)(&fgfdm), sizeof(fgfdm), 0,
159  (struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
160  fprintf(stderr, "error sending to FlightGear\n");
161  fflush(stderr);
162  }
163 }
164 
173 {
174 
175  struct FGNetGUI gui;
176 
178  gui.padding1 = 0; // initialize the padding variable to zero
179 
180  gui.latitude = fdm.lla_pos.lat;
181  gui.longitude = fdm.lla_pos.lon;
182  gui.altitude = fdm.lla_pos.alt;
183  // printf("%f %f %f\n", gui.latitude, gui.longitude, gui.altitude);
184 
185  gui.agl = 1.111652;
186 
190 
191  gui.vcas = 0.;
192  gui.climb_rate = 0.;
193 
194  gui.num_tanks = 1;
195  gui.fuel_quantity[0] = 0.;
196 
197  gui.cur_time = flightgear.initial_time + rint(fdm.time);
198  // if cur_time is zero, flightgear would take the real current time
199  //gui.cur_time = 0;
200  // warp is used as an offset to the current time in seconds
201  gui.warp = flightgear.time_offset;
202 
203  gui.ground_elev = 0.;
204 
205  gui.tuned_freq = 125.65;
206  gui.nav_radial = 90.;
207  gui.in_range = 1;
208  gui.dist_nm = 10.;
209  gui.course_deviation_deg = 0.;
210  gui.gs_deviation_deg = 0.;
211 
212  if (sendto(flightgear.socket, (char *)(&gui), sizeof(gui), 0,
213  (struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) {
214  fprintf(stderr, "error sending to FlightGear\n");
215  fflush(stderr);
216  }
217 
218 }
219 
220 
225 
226  if (flightgear.socket_in != -1) {
227  // socket is correctly opened
228 
229  struct FGEnvironment env;
230  size_t s_env = sizeof(env);
231  int bytes_read;
232 
233  //read first message
234  memset(&env, 0, s_env);
235  bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL);
236  while (bytes_read != -1) { // while we read a message (empty buffer)
237  if (bytes_read == (int)s_env){
238  // Update wind info
240  (double)env.wind_from_north,
241  (double)env.wind_from_east,
242  (double)env.wind_from_down);
243  }
244  else {
245  //error
246  printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)s_env);
247  }
248 
249  //read next message
250  memset(&env, 0, s_env);
251  bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL);
252  }
253 
254  if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) {
255  perror("nps_flightgear_receive recvfrom()");
256  }
257  }
258 
259 }
260 
261 
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
static struct @327 flightgear
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:125
double time
Definition: nps_fdm.h:46
void nps_flightgear_send_fdm()
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc)...
float htonf(float x)
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
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
float left_flap
Definition: flight_gear.h:191
double phi
in radians
float left_aileron
Definition: nps_fdm.h:119
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
double htond(double x)
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
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:117
float theta
Definition: flight_gear.h:262
float right_aileron
Definition: nps_fdm.h:120
uint32_t num_engines
Definition: nps_fdm.h:124
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
void nps_flightgear_send()
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with â€...
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()
Receive Flight Gear environment messages.
float rpm[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:126
int32_t warp
Definition: flight_gear.h:185
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:121
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:118
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