Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
21static struct {
22 int socket;
23 struct sockaddr_in addr;
24 int socket_in;
25 unsigned int initial_time;
26 unsigned int time_offset;
28
29pthread_t th_fg_rx; // fligh gear receive thread
30
31void* nps_flightgear_receive(void* data __attribute__((unused)));
32
33static 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
44static float htonf(float x)
45{
46 int *p = (int *)&x;
47 *p = htonl(*p);
48 return x;
49}
50
51
52void 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);
159 fgfdm.left_aileron = htonf(fdm.left_aileron);
160 fgfdm.right_aileron = htonf(fdm.right_aileron);
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
194 gui.phi = fdm.ltp_to_body_eulers.phi;
195 gui.theta = fdm.ltp_to_body_eulers.theta;
196 gui.psi = fdm.ltp_to_body_eulers.psi;
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
231void* 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
double latitude
float agl
float climb_rate
float wind_from_north
float wind_from_east
double longitude
float phi
double latitude
float elevator
float climb_rate
float course_deviation_deg
float left_flap
float vcas
float left_aileron
uint32_t cur_time
#define FG_NET_FDM_VERSION
uint32_t warp
uint32_t num_engines
float ground_elev
int32_t warp
float right_aileron
uint32_t num_tanks
uint32_t version
uint32_t padding1
float rpm[FG_NET_FDM_MAX_ENGINES]
#define FG_NET_FDM_MAX_ENGINES
float dist_nm
float psi
float theta
double altitude
float altitude
double longitude
float fuel_quantity[FG_NET_FDM_MAX_TANKS]
uint32_t num_tanks
float agl
float fuel_quantity[FG_NET_GUI_MAX_TANKS]
float phi
float theta
float vcas
float right_flap
float nav_radial
#define FG_NET_GUI_VERSION
uint32_t cur_time
float wind_from_down
float rudder
uint32_t version
float gs_deviation_deg
float tuned_freq
float psi
uint32_t in_range
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
static float p[2][2]
void nps_atmosphere_set_wind_ned(double wind_north, double wind_east, double wind_down)
Atmosphere model (pressure, wind) for NPS.
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
void * nps_flightgear_receive(void *data)
Receive Flight Gear environment messages.
void nps_flightgear_send(void)
Send FlightGear GUI packet For visualization of airplane position and attitude only start fgfs with –...
void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset)
static double htond(double x)
static float htonf(float x)
pthread_t th_fg_rx
static struct @125225171014250235336336056257217374010263115315 flightgear
void nps_flightgear_send_fdm(void)
Send FlightGear FDM packet For visualization with moving surfaces (elevator, propeller etc).
pthread_mutex_t fdm_mutex
#define TRUE
Definition std.h:4