diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c new file mode 100644 index 0000000000..146c8cbe80 --- /dev/null +++ b/sw/simulator/sim_ac_fw.c @@ -0,0 +1,59 @@ +/* + * $Id$ + * + * Copyright (C) 2008 Gautier Hattenberger + * + * 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 "sim_ac_jsbsim.hpp" + + +#include "main_ap.h" +#include "main_fbw.h" + +static void sim_gps_feed_data(void); +static void sim_ir_feed_data(void); + +void autopilot_init(void) { + init_fbw(); + init_ap(); +} + +void autopilot_periodic_task(void) { + periodic_task_ap(); + periodic_task_fbw(); +} + +void autopilot_event_task(void) { + event_task_ap(); + event_task_fbw(); +} + + +void copy_inputs_to_jsbsim(JSBSim::FGFDMExec & FDMExec) { +} + +void copy_outputs_from_jsbsim(JSBSim::FGFDMExec & FDMExec) { +} + +#include "gps.h" +static void sim_gps_feed_data(void) { +} + diff --git a/sw/simulator/sim_ac_jsbsim.cpp b/sw/simulator/sim_ac_jsbsim.cpp index 6693a36844..fca2c995dd 100644 --- a/sw/simulator/sim_ac_jsbsim.cpp +++ b/sw/simulator/sim_ac_jsbsim.cpp @@ -22,19 +22,14 @@ * */ +#include "sim_ac_jsbsim.hpp" + #include #include -#include - #include #include -#include "aircraft.h" - -#include "main_ap.h" -#include "main_fbw.h" - /*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ @@ -53,13 +48,10 @@ double sim_time; #define DT_DISPLAY 0.04 double disp_time; -static void sim_gps_feed_data(void); -static void sim_ir_feed_data(void); - static void sim_parse_options(int argc, char** argv); static void sim_init(void); static gboolean sim_periodic(gpointer data); -static void sim_display(void); +//static void sim_display(void); static void ivy_transport_init(void); static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), @@ -71,34 +63,64 @@ static void sim_init(void) { sim_time = 0.; disp_time = 0.; - - // init flight model - - // init sensors - // init environment + // *** SET UP JSBSIM *** // + FDMExec = new JSBSim::FGFDMExec(); + FDMExec->SetAircraftPath(RootDir + "aircraft"); + FDMExec->SetEnginePath(RootDir + "engine"); + FDMExec->SetSystemsPath(RootDir + "systems"); + //FDMExec->GetPropertyManager()->Tie("simulation/frame_start_time", &actual_elapsed_time); + //FDMExec->GetPropertyManager()->Tie("simulation/cycle_duration", &cycle_duration); + + if (!AircraftName.empty()) { + + FDMExec->SetDebugLevel(0); // No DEBUG messages + + if ( ! FDMExec->LoadModel( RootDir + "aircraft", + RootDir + "engine", + RootDir + "systems", + AircraftName)) { + cerr << " JSBSim could not be started" << endl << endl; + delete FDMExec; + exit(-1); + } + + // Initial conditions (from flight_plan.h and aircraft.h ???) + JSBSim::FGInitialCondition *IC = FDMExec->GetIC(); + //if ( ! IC->Load(ResetName)) { + // delete FDMExec; + // cerr << "Initialization unsuccessful" << endl; + // exit(-1); + //} + + } else { + cerr << " No Aircraft given" << endl << endl; + delete FDMExec; + exit(-1); + } + + // init sensors ? or discribe them in jSBSim ivy_transport_init(); - // main init - init_fbw(); - init_ap(); + // main AP init (feed the sensors once before ?) + init_autopilot(); } static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { - /* read actuators positions */ - // wind model + /* read actuators positions and feed JSBSim inputs */ + copy_inputs_to_jsbsim(FDMExec); - // flight model + /* run JSBSim flight model */ + FDMExec->Run(); - // sensors - sim_time += DT; - /* outputs models state */ + /* read outputs from model state (and display ?) */ + copy_outputs_from_jsbsim(FDMExec); sim_display(); /* run the airborne code */ @@ -106,43 +128,16 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { // feed a rc frame and signal event // process it - event_task_ap(); - event_task_fbw(); + autopilot_event_task(); - //if (booz_sensors_model_baro_available()) { - // Booz2BaroISRHandler(bsm.baro); - // booz2_main_event(); - //} - - //if (booz_sensors_model_gyro_available()) { - // booz2_imu_b2_feed_data(); - // booz2_main_event(); - //} - - //if (booz_sensors_model_gps_available()) { - // sim_gps_feed_data(); - // booz2_main_event(); - //} - - //if (booz_sensors_model_mag_available()) { - // sim_mag_feed_data(); - // booz2_main_event(); - //} - - periodic_task_ap(); - periodic_task_fbw(); + autopilot_periodic_task(); return TRUE; } -#include "gps.h" -static void sim_gps_feed_data(void) { -} - -#define RPM_OF_RAD_S(a) ((a)*60./M_PI) -static void sim_display(void) { -} +//static void sim_display(void) { +//} int main ( int argc, char** argv) { @@ -170,48 +165,40 @@ static void ivy_transport_init(void) { IvyStart("127.255.255.255"); } +void print_help(char** argv) { + cout << "Usage: " << argv[0] << " [options]" << endl; + cout << " Options :" << endl; + cout << " -a " << endl; + cout << " -b \tdefault is 127.255.255.255:2010" << endl; + cout << " -fg " << endl; + cout << " -h --help show this help" << endl; +} static void sim_parse_options(int argc, char** argv) { - static const char* usage = -"Usage: %s [options]\n" -" Options :\n"; + if (argc == 1) { + print_help(); + exit(0); + } + int i; + for (i = 1; i < argc; ++i) { + string argument = string(argv[i]); + + if (argument == "--help" || argument == "-h") { + PrintHelp(); + exit(0); + } else if (argument == "-a") { + + } else if (argument == "-b") { + + } else if (argument == "-fg") { + + } else { + cerr << "Unknown argument" << endl; + PrintHelp(); + exit(0); + } + } -// while (1) { -// -// static struct option long_options[] = { -// {"fg_host", 1, NULL, 0}, -// {"fg_port", 1, NULL, 0}, -// {"js_dev", 1, NULL, 0}, -// {0, 0, 0, 0} -// }; -// int option_index = 0; -// int c = getopt_long(argc, argv, "j:", -// long_options, &option_index); -// if (c == -1) -// break; -// -// switch (c) { -// case 0: -// switch (option_index) { -// case 0: -// fg_host = strdup(optarg); break; -// case 1: -// fg_port = atoi(optarg); break; -// case 2: -// joystick_dev = strdup(optarg); break; -// } -// break; -// -// case 'j': -// joystick_dev = strdup(optarg); -// break; -// -// default: /* ’?’ */ -// printf("?? getopt returned character code 0%o ??\n", c); -// fprintf(stderr, usage, argv[0]); -// exit(EXIT_FAILURE); -// } -// } } diff --git a/sw/simulator/sim_ac_jsbsim.hpp b/sw/simulator/sim_ac_jsbsim.hpp new file mode 100644 index 0000000000..b76e1487f1 --- /dev/null +++ b/sw/simulator/sim_ac_jsbsim.hpp @@ -0,0 +1,41 @@ +/* + * $Id$ + * + * Copyright (C) 2008 Gautier Hattenberger + * + * 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. + * + */ + +#ifndef SIM_AC_JSBSIM_HPP +#define SIM_AC_JSBSIM_HPP + +#include +//#include + +#include "aircraft.h" +#include "flight_plan.h" + +extern void autopilot_init(void); +extern void autopilot_periodic_task(void); +extern void autopilot_event_task(void); +extern void copy_inputs_to_jsbsim(JSBSIM::FGFDMExec & FDMExec); +extern void copy_outputs_from_jsbsim(JSBSIM::FGFDMExec & FDMExec); + + +#endif // SIM_AC_JSBSIM_HPP