diff --git a/conf/autopilot/subsystems/fixedwing/autopilot.makefile b/conf/autopilot/subsystems/fixedwing/autopilot.makefile index 676f31890f7..fe9ba0a2755 100644 --- a/conf/autopilot/subsystems/fixedwing/autopilot.makefile +++ b/conf/autopilot/subsystems/fixedwing/autopilot.makefile @@ -198,7 +198,7 @@ jsbsim.CFLAGS += $(fbw_CFLAGS) $(ap_CFLAGS) jsbsim.srcs += $(fbw_srcs) $(ap_srcs) jsbsim.CFLAGS += -DSITL -jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c +jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c $(SIMDIR)/sim_ac_flightgear.c # external libraries jsbsim.CFLAGS += -I$(SIMDIR) -I/usr/include -I$(JSBSIM_INC) -I$(OCAMLLIBDIR) `pkg-config glib-2.0 --cflags` diff --git a/sw/simulator/flight_gear.h b/sw/simulator/flight_gear.h index ecea3debbef..b5515c89cba 100644 --- a/sw/simulator/flight_gear.h +++ b/sw/simulator/flight_gear.h @@ -95,6 +95,7 @@ struct FGNetCtrls { #define FG_NET_FDM_MAX_WHEELS 3 #define FG_NET_FDM_MAX_TANKS 4 +#ifndef _NET_FDM_HXX struct FGNetFDM { @@ -180,6 +181,8 @@ struct FGNetFDM { float spoilers; }; +#endif + struct FGNetMiniFDM { uint32_t version; // increment when data values change diff --git a/sw/simulator/sim_ac_flightgear.c b/sw/simulator/sim_ac_flightgear.c new file mode 100644 index 00000000000..e747fb8a013 --- /dev/null +++ b/sw/simulator/sim_ac_flightgear.c @@ -0,0 +1,103 @@ +/* + * $Id$ + * + * Copyright (C) 2011 Eric Parsonage eric@eparsonage.com + * Mainly based around the equivalent file in nps + * which was written by Antoine + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + * + */ + +#include +#include +#include +#include +#include +#include +#include "std.h" +#include +#include "flight_gear.h" +#include "sim_ac_flightgear.h" + +static struct +{ + int socket; + struct sockaddr_in addr; +} flightgear; + + +void sim_ac_flightgear_init(const char* host, unsigned int port) +{ + 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)); + flightgear.addr.sin_family = PF_INET; + flightgear.addr.sin_port = htons(port); + flightgear.addr.sin_addr.s_addr = inet_addr(host); +} + +static inline double get_value(JSBSim::FGFDMExec* FDMExec, string name) +{ + return FDMExec->GetPropertyManager()->GetNode(name)->getDoubleValue(); +} + +void sim_ac_flightgear_send(JSBSim::FGFDMExec* FDMExec) +{ + + struct FGNetGUI gui; + + gui.version = FG_NET_GUI_VERSION; + + gui.latitude = get_value(FDMExec, "position/lat-gc-rad"); + gui.longitude = get_value(FDMExec, "position/long-gc-rad"); + gui.altitude = get_value(FDMExec, "position/h-sl-meters"); + // printf("%f %f %f\n", gui.latitude, gui.longitude, gui.altitude); + + gui.agl = 1.111652; + + gui.phi = get_value(FDMExec, "attitude/roll-rad"); + gui.theta = get_value(FDMExec, "attitude/pitch-rad"); + gui.psi = get_value(FDMExec, "attitude/heading-true-rad"); + + gui.vcas = 0.; + gui.climb_rate = 0.; + + gui.num_tanks = 1; + gui.fuel_quantity[0] = get_value(FDMExec, "propulsion/total-fuel-lbs");; + + //gui.cur_time = 3198060679ul; + //gui.cur_time = 3198060679ul + rint(fdm.time); + gui.cur_time = 3198101679ul; + gui.warp = 1122474394ul; + + gui.ground_elev = 0.; + + gui.tuned_freq = 125.65; + gui.nav_radial = 90.; + gui.in_range = 1; + gui.dist_nm = 10.; + 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) + printf("error sending\n"); + +} diff --git a/sw/simulator/sim_ac_flightgear.h b/sw/simulator/sim_ac_flightgear.h new file mode 100644 index 00000000000..0139c15b6ba --- /dev/null +++ b/sw/simulator/sim_ac_flightgear.h @@ -0,0 +1,8 @@ +#ifndef SIM_AC_FLIGHTGEAR_H +#define SIM_AC_FLIGHTGEAR_H + + +void sim_ac_flightgear_init(const char* host, unsigned int port); +void sim_ac_flightgear_send(JSBSim::FGFDMExec* FDMExec); + +#endif /* SIM_AC_FLIGHTGEAR_H */ diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index a02d2d5b30c..f9285e4cb55 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -22,7 +22,7 @@ * */ -#include "sim_ac_jsbsim.h" + #include #include @@ -30,10 +30,15 @@ #include #include + +#include +#include "sim_ac_flightgear.h" + using namespace std; //#include #include +#include "sim_ac_jsbsim.h" /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GLOBAL DATA @@ -50,6 +55,8 @@ static void sim_init(void); static gboolean sim_periodic(gpointer data); string ivyBus = "127.255.255.255"; +string fgAddress = "127.0.0.1"; + static void ivy_transport_init(void); @@ -82,8 +89,11 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { /* check if still flying */ result = check_crash_jsbsim(FDMExec); - /* read outputs from model state (and display ?) */ + /* read outputs from model state */ copy_outputs_from_jsbsim(FDMExec); + + /* send outputs to flightgear for visualisation */ + sim_ac_flightgear_send(FDMExec); /* run the airborne code with 60 Hz, even if JSBSim runs with a multiple of this */ @@ -104,6 +114,8 @@ int main ( int argc, char** argv) { sim_parse_options(argc, argv); sim_init(); + + sim_ac_flightgear_init(fgAddress.c_str(), 5501); GMainLoop *ml = g_main_loop_new(NULL, FALSE); @@ -159,8 +171,7 @@ static void sim_parse_options(int argc, char** argv) { ivyBus = string(argv[++i]); } else if (argument == "-fg") { - // TODO - i++; + fgAddress = string(argv[++i]); } else { cerr << "Unknown argument" << endl; @@ -171,6 +182,13 @@ static void sim_parse_options(int argc, char** argv) { } + + + + + + + void jsbsim_init(void) { // *** SET UP JSBSIM *** //