added flightgear visualisation for fixed wing jsbsim simulation

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`

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
 

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 <sys/socket.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <stdio.h>
+#include <math.h>
+#include "std.h"
+#include <FGFDMExec.h>
+#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");
+
+}

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 */

sw/simulator/sim_ac_jsbsim.c

@@ -22,18 +22,23 @@
  *
  */
 
-#include "sim_ac_jsbsim.h"
+
 
 #include <stdlib.h>
 #include <stdio.h>
 #include <glib.h>
 #include <getopt.h>
 
 #include <iostream>
+
+#include <FGFDMExec.h>
+#include "sim_ac_flightgear.h"
+
 using namespace std;
 
 //#include <Ivy/ivy.h>
 #include <Ivy/ivyglibloop.h>
+#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 *** //