diff --git a/sw/airborne/sim/jsbsim_gps.c b/sw/airborne/sim/jsbsim_gps.c new file mode 100644 index 0000000000..b4c770d056 --- /dev/null +++ b/sw/airborne/sim/jsbsim_gps.c @@ -0,0 +1,78 @@ +/* OCaml binding to link the simulator to autopilot functions. */ + +#include +#include +#include + +/** From airborne/autopilot/ */ +#include "airframe.h" +#include "flight_plan.h" +#include "autopilot.h" +#include "gps.h" +#include "estimator.h" +#include "latlong.h" +#include "common_nav.h" + +uint8_t gps_mode; +uint32_t gps_itow; /* ms */ +int32_t gps_alt; /* cm */ +uint16_t gps_gspeed; /* cm/s */ +int16_t gps_climb; /* cm/s */ +int16_t gps_course; /* decideg */ +int32_t gps_utm_east, gps_utm_north; +uint8_t gps_utm_zone; +int32_t gps_lat, gps_lon; +struct svinfo gps_svinfos[GPS_NB_CHANNELS]; +uint8_t gps_nb_channels = 0; +uint16_t gps_PDOP; +uint32_t gps_Pacc, gps_Sacc; +uint8_t gps_numSV; +uint16_t gps_reset; + + +void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time) { + + gps_mode = 3; // Mode 3D + gps_course = DegOfRad(course) * 10.; + gps_alt = alt * 100.; + gps_gspeed = gspeed * 100.; + gps_climb = climb * 100.; + gps_itow = time * 1000.; + + gps_lat = DegOfRad(lat)*1e7; + gps_lon = DegOfRad(lon)*1e7; + latlong_utm_of(lat, lon, nav_utm_zone0); + gps_utm_east = latlong_utm_x * 100; + gps_utm_north = latlong_utm_y * 100; + gps_utm_zone = nav_utm_zone0; + +} + +/** Space vehicle info simulation */ +void sim_update_sv(void) { + + gps_nb_channels=7; + int i; + static int time; + time++; + for(i = 0; i < gps_nb_channels; i++) { + gps_svinfos[i].svid = 7 + i; + gps_svinfos[i].elev = (cos(((100*i)+time)/100.) + 1) * 45; + gps_svinfos[i].azim = (time/gps_nb_channels + 50 * i) % 360; + gps_svinfos[i].cno = 40 + sin((time+i*10)/100.) * 10.; + gps_svinfos[i].flags = ((time/10) % (i+1) == 0 ? 0x00 : 0x01); + gps_svinfos[i].qi = (int)((time / 1000.) + i) % 8; + } + gps_PDOP = gps_Sacc = gps_Pacc = 500+200*sin(time/100.); + gps_numSV = 7; + + gps_verbose_downlink = !launch; + UseGpsPosNoSend(estimator_update_state_gps); + gps_downlink(); + +} + +void ubxsend_cfg_rst(uint16_t a __attribute__ ((unused)), uint8_t b __attribute__ ((unused))) { + return; +} + diff --git a/sw/airborne/sim/jsbsim_hw.c b/sw/airborne/sim/jsbsim_hw.c new file mode 100644 index 0000000000..f466675fc7 --- /dev/null +++ b/sw/airborne/sim/jsbsim_hw.c @@ -0,0 +1,35 @@ +/* Definitions and declarations required to compile autopilot code on a + i386 architecture */ + +#include "jsbsim_hw.h" + +#include +#include +#include +#include +#include +#include + + +/* Dummy definitions to replace the ones from the files not compiled in the simulator */ +//uint8_t ir_estim_mode; +//uint8_t vertical_mode; +//uint8_t inflight_calib_mode; +//bool_t rc_event_1, rc_event_2; +uint8_t gps_nb_ovrn, modem_nb_ovrn, link_fbw_fbw_nb_err, link_fbw_nb_err; +//float alt_roll_pgain; +//float roll_rate_pgain; +//bool_t gpio1_status; +uint16_t adc_generic_val1; +uint16_t adc_generic_val2; +uint16_t ppm_pulses[ PPM_NB_PULSES ]; +volatile bool_t ppm_valid; + +uint8_t ac_id; + +void update_bat(double bat) { + fbw_vsupply_decivolt = (int) (bat * 10.); +} + +void adc_generic_init( void ) {} +void adc_generic_periodic( void ) {} diff --git a/sw/airborne/sim/jsbsim_hw.h b/sw/airborne/sim/jsbsim_hw.h new file mode 100644 index 0000000000..e13d7ad6ab --- /dev/null +++ b/sw/airborne/sim/jsbsim_hw.h @@ -0,0 +1,62 @@ +/* + * + * Copyright (C) 2009 Enac + * + * 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. + * + */ + +/** \file jsbsim_hw.h + */ + +#ifndef JSBSIM_HW_H +#define JSBSIM_HW_H + +#include +#include "std.h" +#include "inter_mcu.h" +#include "autopilot.h" +#include "estimator.h" +#include "gps.h" +#include "traffic_info.h" +#include "flight_plan.h" +#include "airframe.h" +#include "settings.h" +#include "nav.h" +#include "fw_h_ctl.h" +#include "fw_v_ctl.h" +#include "infrared.h" +#include "cam.h" +#include "commands.h" +#include "main_ap.h" +#include "ap_downlink.h" +#include "sim_uart.h" +#include "latlong.h" +#include "datalink.h" +#include "adc_generic.h" +#include "ppm.h" + + +void sim_use_gps_pos(double lat, double lon, double alt, double course, double gspeed, double climb, double time); +void sim_update_sv(void); + +void set_ir(double roll, double pitch); + +void update_bat(double bat); + +#endif diff --git a/sw/airborne/sim/jsbsim_ir.c b/sw/airborne/sim/jsbsim_ir.c new file mode 100644 index 0000000000..d1d84d37d1 --- /dev/null +++ b/sw/airborne/sim/jsbsim_ir.c @@ -0,0 +1,28 @@ +/** + * \brief Regroup functions to simulate autopilot/infrared.c + * + * Infrared soft simulation. + */ + + +#include "jsbsim_hw.h" +#include + +void set_ir(double roll, double pitch) { + + double ir_contrast = 150; //FIXME + double roll_sensor = roll + ir_roll_neutral; + double pitch_sensor = pitch + ir_pitch_neutral; +#ifdef INFRARED + ir_roll = sin(roll_sensor) * ir_contrast; + ir_pitch = sin(pitch_sensor) * ir_contrast; + ir_top = cos(roll_sensor) * cos(pitch_sensor) * ir_contrast; +#endif + +} + + +void ir_gain_calib(void) {} + +/** Required by infrared.c:ir_init() */ +void adc_buf_channel(uint8_t adc_channel __attribute__ ((unused)), struct adc_buf* s __attribute__ ((unused)), uint8_t av_nb_sample __attribute__ ((unused))) {} diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c index 275bc259e2..34695141e5 100644 --- a/sw/simulator/sim_ac_fw.c +++ b/sw/simulator/sim_ac_fw.c @@ -26,12 +26,10 @@ #include "sim_ac_jsbsim.h" #include "main_ap.h" #include "main_fbw.h" +#include "jsbsim_hw.h" using namespace JSBSim; -//static void sim_gps_feed_data(void); -//static void sim_ir_feed_data(void); - void autopilot_init(void) { init_fbw(); init_ap(); @@ -47,36 +45,70 @@ void autopilot_event_task(void) { event_task_fbw(); } +void print(FGFDMExec* FDMExec) { + FGPropertyManager* cur_node; + double cur_value, factor=1; + const char* state[] = {"sim-time-sec", + "position/lat-gc-deg","position/long-gc-deg","position/h-sl-meters",/* + "ic/lat-gc-deg","ic/long-gc-deg","ic/h-sl-ft", + "velocities/v-north-fps","velocities/v-east-fps","velocities/v-down-fps","velocities/vg-fps", + "attitude/roll-rad","attitude/pitch-rad","attitude/heading-true-rad", + "velocities/p-rad_sec","velocities/q-rad_sec","velocities/r-rad_sec",*/ + "forces/fbx-gear-lbs","forces/fby-gear-lbs","forces/fbz-gear-lbs"}; + int i=0; + + cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec"); + cur_value = cur_node->getDoubleValue(); + cout << state[i] << " " << cur_value << endl; + + for (i=1; i<6+1; i++) { + if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG; + if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M; + cur_node = FDMExec->GetPropertyManager()->GetNode(state[i]); + cur_value = factor*(cur_node->getDoubleValue()); + cout << string(state[i]) << " " << cur_value << endl; + factor = 1; + } +} + void copy_inputs_to_jsbsim(FGFDMExec* FDMExec) { } -void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) { - - FGPropertyManager* cur_node; - double cur_value, factor=1; - char buf[64]; - const char* state[] = {"sim-time-sec", - "lat-gc-deg","long-gc-deg","h-sl-ft", - "u-fps","v-fps","w-fps", - "theta-deg","phi-deg","psi-true-deg", - "p-rad_sec","q-rad_sec","r-rad_sec"}; - int i=0; +double get_value(FGFDMExec* FDMExec, string name) { + return FDMExec->GetPropertyManager()->GetNode(name)->getDoubleValue(); +} - cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec"); - cur_value = cur_node->getDoubleValue(); - cout << state[i] << cur_value << endl; - - for (i=1; i<12+1; i++) { - sprintf(buf,"ic/%s",state[i]); - if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG; - if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M; - cur_node = FDMExec->GetPropertyManager()->GetNode(buf); - cur_value = factor*(cur_node->getDoubleValue()); - cout << state[i] << " " << cur_value << endl; - factor = 1; +#define GPS_PERIOD (1./4.) + +void copy_outputs_from_jsbsim(FGFDMExec* FDMExec) { + static double gps_period = 0.; + + // copy GPS pos + gps_period += DT; + if (gps_period > GPS_PERIOD) { + double lat = get_value(FDMExec, "position/lat-gc-rad"); + double lon = get_value(FDMExec, "position/long-gc-rad"); + double alt = get_value(FDMExec, "position/h-sl-meters"); + double course = get_value(FDMExec, "attitude/heading-true-rad"); + double gspeed = get_value(FDMExec, "velocities/vg-fps") * FT2M; + double climb = get_value(FDMExec, "velocities/v-down-fps") * (-FT2M); + double time = get_value(FDMExec, "sim-time-sec"); + sim_use_gps_pos(lat, lon, alt, course, gspeed, climb, time); + sim_update_sv(); + gps_period = 0.; } + print(FDMExec); + // copy IR + double roll = get_value(FDMExec, "attitude/roll-rad"); + double pitch = get_value(FDMExec, "attitude/pitch-rad"); + set_ir(roll, pitch); + + // copy Bat level + double bat = 12.; // get_value(FDMExec, "propulsion/total-fuel-lbs"); + update_bat(bat); + } diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index 2599fcaeb1..8427e1e204 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -24,6 +24,7 @@ #include "sim_ac_jsbsim.h" +#include #include #include @@ -34,16 +35,15 @@ GLOBAL DATA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/ -string RootDir = ""; string ICName; string AircraftName; -string LogOutputName; JSBSim::FGFDMExec* FDMExec; static void sim_parse_options(int argc, char** argv); static void sim_init(void); static gboolean sim_periodic(gpointer data); +string ivyBus = "127.255.255.255"; static void ivy_transport_init(void); //static void on_DL_SETTING(IvyClientPtr app __attribute__ ((unused)), // void *user_data __attribute__ ((unused)), @@ -51,7 +51,7 @@ static void ivy_transport_init(void); static void sim_init(void) { - + jsbsim_init(); // init sensors ? or discribe them in jSBSim @@ -69,19 +69,21 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { copy_inputs_to_jsbsim(FDMExec); /* run JSBSim flight model */ - bool result = FDMExec->Run(); - - //sim_time += DT; - + bool result = true; + //if (check_crash_jsbsim(FDMExec)) { + result = FDMExec->Run(); + //} /* check if still flying */ - result = check_crash_jsbsim(FDMExec); + //result = check_crash_jsbsim(FDMExec); /* read outputs from model state (and display ?) */ copy_outputs_from_jsbsim(FDMExec); /* run the airborne code */ - airborne_run_one_step(); +// airborne_run_one_step(); + autopilot_event_task(); + autopilot_periodic_task(); return result; } @@ -89,11 +91,7 @@ static gboolean sim_periodic(gpointer data __attribute__ ((unused))) { int main ( int argc, char** argv) { - ICName = "reset00"; - AircraftName = "quad"; - LogOutputName = "out.csv"; - - //sim_parse_options(argc, argv); + sim_parse_options(argc, argv); sim_init(); @@ -103,21 +101,20 @@ int main ( int argc, char** argv) { g_main_loop_run(ml); - //delete FDMExec; + delete FDMExec; return 0; } static void ivy_transport_init(void) { IvyInit ("Paparazzi sim " + AC_ID, "READY", NULL, NULL, NULL, NULL); - IvyStart("127.255.255.255"); + IvyStart(ivyBus.c_str()); } void print_help() { - //cout << "Usage: " << argv[0] << " [options]" << endl; cout << "Usage: simsitl [options]" << endl; cout << " Options :" << endl; - cout << " -a " << endl; + cout << " -a \tUnused, only for compatibility" << endl; cout << " -b \tdefault is 127.255.255.255:2010" << endl; cout << " -fg " << endl; cout << " -h --help show this help" << endl; @@ -125,26 +122,34 @@ void print_help() { static void sim_parse_options(int argc, char** argv) { - if (argc == 1) { - print_help(); - exit(0); - } + if (argc == 1) return; int i; - for (i = 1; i < argc; ++i) { string argument = string(argv[i]); if (argument == "--help" || argument == "-h") { print_help(); exit(0); - } else if (argument == "-a") { - - } else if (argument == "-b") { - - } else if (argument == "-fg") { - - } else { + } + else if (argument == "-a") { + // Compatibility with ocaml + i++; + } + else if (argument == "-boot") { + // Compatibility with ocaml + } + else if (argument == "-norc") { + // Compatibility with ocaml + } + else if (argument == "-b") { + ivyBus = string(argv[++i]); + } + else if (argument == "-fg") { + // TODO + i++; + } + else { cerr << "Unknown argument" << endl; print_help(); exit(0); @@ -155,30 +160,37 @@ static void sim_parse_options(int argc, char** argv) { void jsbsim_init(void) { - JSBSim::FGState* State; - bool result; + // *** SET UP JSBSIM *** // + + char* root = getenv("PAPARAZZI_HOME"); + if (root == NULL) { + cerr << "PAPARAZZI_HOME is not defined" << endl; + exit(0); + } + string pprzRoot = string(root); + +#ifdef JSBSIM_MODEL + AircraftName = JSBSIM_MODEL; +#endif +#ifdef JSBSIM_INIT + ICName = JSBSIM_INIT; +#endif FDMExec = new JSBSim::FGFDMExec(); - State = FDMExec->GetState(); - State->Setsim_time(0.); - State->Setdt(DT); - - cout << "Simulation elapsed time: " << FDMExec->GetSimTime() << endl; + /* Set simulation time step */ + FDMExec->GetState()->Setsim_time(0.); + FDMExec->GetState()->Setdt(DT); cout << "Simulation delta " << FDMExec->GetDeltaT() << endl; -#ifdef JSBSIM_ROOT_DIR - RootDir = JSBSIM_ROOT_DIR; -#endif + FDMExec->DisableOutput(); + FDMExec->SetDebugLevel(0); // No DEBUG messages if (!AircraftName.empty()) { - - FDMExec->DisableOutput(); - FDMExec->SetDebugLevel(0); // No DEBUG messages - - if ( ! FDMExec->LoadModel( RootDir + "aircraft", - RootDir + "engine", - RootDir + "systems", + + if ( ! FDMExec->LoadModel( pprzRoot + "/conf/simulator", + pprzRoot + "/conf/simulator", + pprzRoot + "/conf/simulator", AircraftName)){ cerr << " JSBSim could not be started" << endl << endl; delete FDMExec; @@ -186,11 +198,27 @@ void jsbsim_init(void) { } JSBSim::FGInitialCondition *IC = FDMExec->GetIC(); - if ( ! IC->Load(ICName)) { - delete FDMExec; - cerr << "Initialization unsuccessful" << endl; - exit(-1); + if(!ICName.empty()) { + if (!IC->Load(ICName)) { + delete FDMExec; + cerr << "Initialization from file unsuccessful" << endl; + exit(-1); + } } + else { + // Use flight plan initial conditions + IC->SetLatitudeDegIC(NAV_LAT0 / 1e7); + IC->SetLongitudeDegIC(NAV_LON0 / 1e7); + IC->SetAltitudeFtIC((GROUND_ALT+1) / FT2M); + IC->SetTerrainAltitudeFtIC(GROUND_ALT / FT2M); + IC->SetPsiDegIC(QFU); + IC->SetVgroundFpsIC(15./FT2M); + if (!FDMExec->RunIC()) { + cerr << "Initialization from flight plan unsuccessful" << endl; + exit(-1); + } + } + //FDMExec->GetGroundReactions()->InitModel(); } else { cerr << " No Aircraft given" << endl << endl; @@ -198,8 +226,11 @@ void jsbsim_init(void) { exit(-1); } - result = FDMExec->Run(); - if (result) cout << "Made Initial Run" << endl; + //if (FDMExec->Run()) cout << "Made Initial Run" << endl; + //else { + // cerr << "Initial run failed " << endl; + // exit(-1); + //} } @@ -208,9 +239,9 @@ bool check_crash_jsbsim(JSBSim::FGFDMExec* FDMExec) { JSBSim::FGPropertyManager* cur_node; double cur_value; - cur_node = FDMExec->GetPropertyManager()->GetNode("ic/h-agl-ft"); + cur_node = FDMExec->GetPropertyManager()->GetNode("position/h-agl-ft"); cur_value = cur_node->getDoubleValue(); - if (cur_value>0) return true; + if (cur_value>=0) return true; else return false; } diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h index de86a60e78..db3ae16cef 100644 --- a/sw/simulator/sim_ac_jsbsim.h +++ b/sw/simulator/sim_ac_jsbsim.h @@ -29,6 +29,7 @@ #include #include +#include "std.h" #include "airframe.h" #include "flight_plan.h"