diff --git a/sw/simulator/fg_environment.xml b/sw/simulator/fg_environment.xml new file mode 100644 index 0000000000..f845afcb2b --- /dev/null +++ b/sw/simulator/fg_environment.xml @@ -0,0 +1,53 @@ + + + + + + true + host + + + time (s) + double + /sim/time/elapsed-sec + + + + wind-from-north (m/s) + float + /environment/wind-from-north-fps + 0.3048 + + + + wind-from-east (m/s) + float + /environment/wind-from-east-fps + 0.3048 + + + + wind-from-down (m/s) + float + /environment/wind-from-down-fps + 0.3048 + + + + wind-from-heading (deg) + float + /environment/wind-from-heading-deg + + + + wind-speed (m/s) + float + 0.514444444 + /environment/wind-speed-kt + + + magic,0x12345678 + + + + diff --git a/sw/simulator/flight_gear.h b/sw/simulator/flight_gear.h index aff9e3350d..e0247eebdd 100644 --- a/sw/simulator/flight_gear.h +++ b/sw/simulator/flight_gear.h @@ -292,6 +292,20 @@ struct FGNetGUI { #endif /*FG_2_4*/ +#define FG_ENVIRONMENT_FOOTER_MAGIC 0x12345678 +// described in file fg_environment.xml (generic protocol) +struct FGEnvironment{ + double elapsed_sec; // elapsed sim seconds + float wind_from_north; // wind from north in m/s + float wind_from_east; // wind from east in m/s + float wind_from_down; // wind from down in m/s + float wind_from_heading; // wind N-E heading in degrees + float wind_speed; // wind N-E speed in m/s + uint32_t footer_magic; // magic footer 0x12345678 +}; + + + extern void net_fdm_dump (struct FGNetFDM* fdm); extern void net_fdm_ntoh (struct FGNetFDM* fdm); extern void net_fdm_init (struct FGNetFDM* fdm); diff --git a/sw/simulator/nps/nps_flightgear.c b/sw/simulator/nps/nps_flightgear.c index 3cd2895189..461f5dc180 100644 --- a/sw/simulator/nps/nps_flightgear.c +++ b/sw/simulator/nps/nps_flightgear.c @@ -2,12 +2,14 @@ #include #include +#include #include #include #include #include #include #include +#include #include "std.h" @@ -17,6 +19,7 @@ static struct { int socket; struct sockaddr_in addr; + int socket_in; unsigned int initial_time; unsigned int time_offset; } flightgear; @@ -41,17 +44,40 @@ float htonf(float x) } -void nps_flightgear_init(const char *host, unsigned int port, unsigned int time_offset) +void nps_flightgear_init(const char *host, unsigned int port, unsigned int port_in, unsigned int time_offset) { int so_reuseaddr = 1; struct protoent *pte = getprotobyname("UDP"); flightgear.socket = socket(PF_INET, SOCK_DGRAM, pte->p_proto); setsockopt(flightgear.socket, SOL_SOCKET, SO_REUSEADDR, - &so_reuseaddr, sizeof(so_reuseaddr)); + &so_reuseaddr, sizeof(so_reuseaddr)); flightgear.addr.sin_family = PF_INET; flightgear.addr.sin_port = htons(port); flightgear.addr.sin_addr.s_addr = inet_addr(host); + // incoming flight gear socket + // only bind to socket if port_in is not zero + if (port_in > 0) { + struct sockaddr_in addr_in; + flightgear.socket_in = socket(PF_INET, SOCK_DGRAM, pte->p_proto); + setsockopt(flightgear.socket_in, SOL_SOCKET, SO_REUSEADDR, + &so_reuseaddr, sizeof(so_reuseaddr)); + addr_in.sin_family = PF_INET; + addr_in.sin_port = htons(port_in); + addr_in.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(flightgear.socket_in, (struct sockaddr*)&addr_in, sizeof(addr_in)) == -1) { + perror("nps_flightgear_init bind()"); + exit(errno); + } + else{ + printf("Bount to port %u to receive from\n", port_in); + } + } + else { + flightgear.socket_in = -1; + } + // get current time to use as inital when computing cur_time for FG time_t t = time(NULL); flightgear.initial_time = t; @@ -166,7 +192,6 @@ void nps_flightgear_send() gui.course_deviation_deg = 0.; gui.gs_deviation_deg = 0.; - if (sendto(flightgear.socket, (char *)(&gui), sizeof(gui), 0, (struct sockaddr *)&flightgear.addr, sizeof(flightgear.addr)) == -1) { fprintf(stderr, "error sending to FlightGear\n"); @@ -174,3 +199,46 @@ void nps_flightgear_send() } } + + +/** + * Receive Flight Gear environment messages + */ +void nps_flightgear_receive() { + + if (flightgear.socket_in != -1) { + // socket is correctly opened + + struct FGEnvironment env; + size_t s_env = sizeof(env); + int bytes_read; + + //read first message + memset(&env, 0, s_env); + bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL); + while (bytes_read != -1) { // while we read a message (empty buffer) + if (bytes_read == (int)s_env){ + // Update wind info + nps_atmosphere_set_wind_ned( + (double)env.wind_from_north, + (double)env.wind_from_east, + (double)env.wind_from_down); + } + else { + //error + printf("WARNING : ignoring packet with size %d (%d expected)", bytes_read, (int)sbuf); + } + + //read next message + memset(&env, 0, s_env); + bytes_read = recvfrom(flightgear.socket_in, (char*)(&env), s_env, MSG_DONTWAIT, NULL, NULL); + } + + if ((errno & (EAGAIN | EWOULDBLOCK)) == 0) { + perror("nps_flightgear_receive recvfrom()"); + } + } + +} + + diff --git a/sw/simulator/nps/nps_flightgear.h b/sw/simulator/nps/nps_flightgear.h index 5cab6cb4f5..d463d49cc4 100644 --- a/sw/simulator/nps/nps_flightgear.h +++ b/sw/simulator/nps/nps_flightgear.h @@ -2,8 +2,10 @@ #define NPS_FLIGHTGEAR_H -extern void nps_flightgear_init(const char *host, unsigned int port, unsigned int time_offset); +extern void nps_flightgear_init(const char* host, unsigned int port, unsigned int port_in, unsigned int time_offset); extern void nps_flightgear_send(); extern void nps_flightgear_send_fdm(); +extern void nps_flightgear_receive(); + #endif /* NPS_FLIGHTGEAR_H */ diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c index 0c92fdc9b4..ad1c5f2391 100644 --- a/sw/simulator/nps/nps_main.c +++ b/sw/simulator/nps/nps_main.c @@ -49,6 +49,7 @@ static struct { double display_time; char *fg_host; unsigned int fg_port; + unsigned int fg_port_in; unsigned int fg_time_offset; int fg_fdm; char *js_dev; @@ -143,9 +144,9 @@ static void nps_main_init(void) } nps_autopilot_init(rc_type, nps_main.rc_script, rc_dev); - if (nps_main.fg_host) { - nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_time_offset); - } + if (nps_main.fg_host) + nps_flightgear_init(nps_main.fg_host, nps_main.fg_port, nps_main.fg_port_in, nps_main.fg_time_offset); + #if DEBUG_NPS_TIME printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n"); @@ -176,6 +177,7 @@ static void nps_main_display(void) { // printf("display at %f\n", nps_main.display_time); nps_ivy_display(); + if (nps_main.fg_host) { if (nps_main.fg_fdm) { nps_flightgear_send_fdm(); @@ -183,6 +185,7 @@ static void nps_main_display(void) nps_flightgear_send(); } } + nps_flightgear_receive(); } @@ -296,6 +299,7 @@ static bool_t nps_main_parse_options(int argc, char **argv) nps_main.fg_host = NULL; nps_main.fg_port = 5501; + nps_main.fg_port_in = 5502; nps_main.fg_time_offset = 0; nps_main.js_dev = NULL; nps_main.spektrum_dev = NULL; @@ -310,6 +314,7 @@ static bool_t nps_main_parse_options(int argc, char **argv) " -h Display this help\n" " --fg_host e.g. 127.0.0.1\n" " --fg_port e.g. 5501\n" + " --fg_port_in e.g. 5502\n" " --fg_time_offset e.g. 21600 for 6h\n" " -j --js_dev e.g. 1 (default 0)\n" " --spektrum_dev e.g. /dev/ttyUSB0\n" @@ -331,6 +336,7 @@ static bool_t nps_main_parse_options(int argc, char **argv) {"ivy_bus", 1, NULL, 0}, {"time_factor", 1, NULL, 0}, {"fg_fdm", 0, NULL, 0}, + {"fg_port_in", 1, NULL, 0}, {0, 0, 0, 0} }; int option_index = 0; @@ -362,8 +368,9 @@ static bool_t nps_main_parse_options(int argc, char **argv) case 7: nps_main.host_time_factor = atof(optarg); break; case 8: - nps_main.fg_fdm = 1; - break; + nps_main.fg_fdm = 1; break; + case 9: + nps_main.fg_port_in = atoi(optarg); break; } break; diff --git a/sw/simulator/pprzsim-launch b/sw/simulator/pprzsim-launch index 82b77f3e85..38cffbd8c6 100755 --- a/sw/simulator/pprzsim-launch +++ b/sw/simulator/pprzsim-launch @@ -66,6 +66,9 @@ def main(): nps_opts.add_option("-p", "--fg_port", dest="fg_port", metavar="PORT", type="int", default=5501, action="store", help="Port on FlightGear host to connect to (Default: %default)") + nps_opts.add_option("-P", "--fg_port_in", dest="fg_port_in", metavar="PORT_IN", + type="int", default=5502, action="store", + help="Port to receive from FlightGear host (Default: %default)") nps_opts.add_option("--fg_time_offset", type="int", action="store", metavar="SEC", help="FlightGear time offset in seconds (e.g. 21600 for 6h)") nps_opts.add_option("-j", "--js_dev", dest="js_dev", metavar="IDX", @@ -135,6 +138,8 @@ def main(): simargs.append(options.fg_host) simargs.append("--fg_port") simargs.append(str(options.fg_port)) + simargs.append("--fg_port_in") + simargs.append(str(options.fg_port_in)) if options.fg_time_offset: simargs.append("--fg_time_offset") simargs.append(str(options.fg_time_offset))