From c136d65c257a8f9ee32a315ba298a464e9ec7f30 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 10 May 2015 21:01:49 +1000 Subject: [PATCH] SITL: added initial JSBSim simulator backend --- libraries/SITL/SIM_Aircraft.h | 8 + libraries/SITL/SIM_JSBSim.cpp | 355 ++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_JSBSim.h | 171 ++++++++++++++++ 3 files changed, 534 insertions(+) create mode 100644 libraries/SITL/SIM_JSBSim.cpp create mode 100644 libraries/SITL/SIM_JSBSim.h diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 69b76a31c3..c1dc6ee5d5 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -45,6 +45,13 @@ public: */ void set_speedup(float speedup); + /* + set instance number + */ + void set_instance(uint8_t _instance) { + instance = _instance; + } + /* step the FDM by one time step */ @@ -76,6 +83,7 @@ protected: uint64_t frame_time_us; float scaled_frame_time_us; uint64_t last_wall_time_us; + uint8_t instance; bool on_ground(const Vector3f &pos) const; diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp new file mode 100644 index 0000000000..15142d461f --- /dev/null +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -0,0 +1,355 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + This program 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 3 of the License, or + (at your option) any later version. + + This program 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 this program. If not, see . + */ +/* + simulator connector for ardupilot version of JSBSim +*/ + +#include +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include "SIM_JSBSim.h" +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +#pragma GCC diagnostic ignored "-Wunused-result" + +/* + constructor + */ +JSBSim::JSBSim(const char *home_str, const char *frame_str) : + Aircraft(home_str, frame_str), + sock_control(false), + sock_fgfdm(true), + initialised(false), + jsbsim_script(NULL), + jsbsim_fgout(NULL), + created_templates(false), + started_jsbsim(false), + opened_control_socket(false), + opened_fdm_socket(false) +{ +} + +/* + create template files + */ +bool JSBSim::create_templates(void) +{ + if (created_templates) { + return true; + } + control_port = 5505 + instance*10; + fdm_port = 5504 + instance*10; + + + asprintf(&autotest_dir, SKETCHBOOK "/Tools/autotest"); + asprintf(&jsbsim_script, "%s/jsbsim_start_%u.xml", autotest_dir, instance); + asprintf(&jsbsim_fgout, "%s/jsbsim_fgout_%u.xml", autotest_dir, instance); + + FILE *f = fopen(jsbsim_script, "w"); + if (f == NULL) { + hal.scheduler->panic("Unable to create jsbsim script"); + } + fprintf(f, +"\n" +"\n" +"\n" +"\n" +" \n" +" test ArduPlane using Rascal110 and JSBSim\n" +" \n" +"\n" +" \n" +"\n" +" \n" +" \n" +"\n" +" \n" +" simulation/notify-time-trigger \n" +"\n" +" \n" +" simulation/sim-time-sec le 0.01 \n" +" \n" +" \n" +" \n" +"\n" +" \n" +" simulation/sim-time-sec ge 0.01\n" +" \n" +" \n" +" \n" +" \n" +"\n" +"\n" +"", control_port); + fclose(f); + + f = fopen(jsbsim_fgout, "w"); + if (f == NULL) { + hal.scheduler->panic("Unable to create jsbsim fgout script"); + } + fprintf(f, "\n" + "\n", + fdm_port); + fclose(f); + + created_templates = true; + return true; +} + + +/* + start JSBSim child + */ +bool JSBSim::start_JSBSim(void) +{ + if (started_jsbsim) { + return true; + } + if (!open_fdm_socket()) { + return false; + } + + int p[2]; + int devnull = open("/dev/null", O_RDWR); + if (pipe(p) != 0) { + hal.scheduler->panic("Unable to create pipe"); + } + pid_t child_pid = fork(); + if (child_pid == 0) { + // in child + dup2(devnull, 0); + dup2(p[1], 1); + close(p[0]); + char *logdirective; + char *script; + + asprintf(&logdirective, "--logdirectivefile=%s", jsbsim_fgout); + asprintf(&script, "--script=%s", jsbsim_script); + + if (chdir(autotest_dir) != 0) { + perror(autotest_dir); + exit(1); + } + + int ret = execlp("JSBSim", + "JSBSim", + "--realtime", + "--suspend", + "--nice", + "--simulation-rate=1000", + logdirective, + script, + NULL); + if (ret != 0) { + perror("JSBSim"); + } + exit(1); + } + close(p[1]); + jsbsim_stdout = p[0]; + + // read startup to be sure it is running + char c; + if (read(jsbsim_stdout, &c, 1) != 1) { + hal.scheduler->panic("Unable to start JSBSim"); + } + + if (!expect("JSBSim Execution beginning")) { + hal.scheduler->panic("Failed to start JSBSim"); + } + if (!open_control_socket()) { + hal.scheduler->panic("Failed to open JSBSim control socket"); + } + + fcntl(jsbsim_stdout, F_SETFL, fcntl(jsbsim_stdout, F_GETFL, 0) | O_NONBLOCK); + + started_jsbsim = true; + check_stdout(); + return true; +} + +/* + check for stdout from JSBSim + */ +void JSBSim::check_stdout(void) +{ + char line[100]; + ssize_t ret = ::read(jsbsim_stdout, line, sizeof(line)); + if (ret > 0) { + write(1, line, ret); + } +} + +/* + a simple function to wait for a string on jsbsim_stdout + */ +bool JSBSim::expect(const char *str) +{ + const char *basestr = str; + while (*str) { + char c; + if (read(jsbsim_stdout, &c, 1) != 1) { + return false; + } + if (c == *str) { + str++; + } else { + str = basestr; + } + write(1, &c, 1); + } + return true; +} + +/* + open control socket to JSBSim + */ +bool JSBSim::open_control_socket(void) +{ + if (opened_control_socket) { + return true; + } + if (!sock_control.connect("127.0.0.1", control_port)) { + return false; + } + printf("Opened JSBSim control socket\n"); + sock_control.set_blocking(false); + opened_control_socket = true; + + char startup[] = "info\nresume\nstep\n"; + sock_control.send(startup, strlen(startup)); + return true; +} + +/* + open fdm socket from JSBSim + */ +bool JSBSim::open_fdm_socket(void) +{ + if (opened_fdm_socket) { + return true; + } + if (!sock_fgfdm.bind("127.0.0.1", fdm_port)) { + check_stdout(); + return false; + } + sock_fgfdm.set_blocking(false); + sock_fgfdm.reuseaddress(); + opened_fdm_socket = true; + return true; +} + + +/* + decode and send servos +*/ +void JSBSim::send_servos(const struct sitl_input &input) +{ + char *buf = NULL; + float aileron = (input.servos[0]-1500)/500.0f; + float elevator = (input.servos[1]-1500)/500.0f; + float throttle = (input.servos[2]-1000)/1000.0; + float rudder = (input.servos[3]-1500)/500.0f; + asprintf(&buf, + "set fcs/aileron-cmd-norm %f\n" + "set fcs/elevator-cmd-norm %f\n" + "set fcs/rudder-cmd-norm %f\n" + "set fcs/throttle-cmd-norm %f\n" + "step\n", + aileron, elevator, rudder, throttle); + sock_control.send(buf, strlen(buf)); + free(buf); +} + +#define FEET_TO_METERS 0.3048f + +// nasty hack .... +void FGNetFDM::ByteSwap(void) { + uint32_t *buf = (uint32_t *)this; + for (uint16_t i=0; i. + */ +/* + simulator connection for ardupilot version of JSBSim +*/ + +#ifndef _SIM_JSBSIM_H +#define _SIM_JSBSIM_H + +#include "SIM_Aircraft.h" +#include + +/* + a Jsbsim simulator + */ +class JSBSim : public Aircraft +{ +public: + JSBSim(const char *home_str, const char *frame_str); + + /* update model by one time step */ + void update(const struct sitl_input &input); + + /* static object creator */ + static Aircraft *create(const char *home_str, const char *frame_str) { + return new JSBSim(home_str, frame_str); + } + +private: + // tcp input control socket to JSBSIm + SocketAPM sock_control; + + // UDP packets from JSBSim in fgFDM format + SocketAPM sock_fgfdm; + + bool initialised; + + uint16_t control_port; + uint16_t fdm_port; + char *autotest_dir; + char *jsbsim_script; + char *jsbsim_fgout; + int jsbsim_stdout; + + bool created_templates; + bool started_jsbsim; + bool opened_control_socket; + bool opened_fdm_socket; + + bool create_templates(void); + bool start_JSBSim(void); + bool open_control_socket(void); + bool open_fdm_socket(void); + void send_servos(const struct sitl_input &input); + void recv_fdm(const struct sitl_input &input); + void check_stdout(void); + bool expect(const char *str); +}; + +/* + FGNetFDM class from JSBSim + */ +class FGNetFDM { +public: + + enum { + FG_MAX_ENGINES = 4, + FG_MAX_WHEELS = 3, + FG_MAX_TANKS = 4 + }; + + uint32_t version; // increment when data values change + uint32_t padding; // padding + + // Positions + double longitude; // geodetic (radians) + double latitude; // geodetic (radians) + double altitude; // above sea level (meters) + float agl; // above ground level (meters) + float phi; // roll (radians) + float theta; // pitch (radians) + float psi; // yaw or true heading (radians) + float alpha; // angle of attack (radians) + float beta; // side slip angle (radians) + + // Velocities + float phidot; // roll rate (radians/sec) + float thetadot; // pitch rate (radians/sec) + float psidot; // yaw rate (radians/sec) + float vcas; // calibrated airspeed + float climb_rate; // feet per second + float v_north; // north velocity in local/body frame, fps + float v_east; // east velocity in local/body frame, fps + float v_down; // down/vertical velocity in local/body frame, fps + float v_wind_body_north; // north velocity in local/body frame + // relative to local airmass, fps + float v_wind_body_east; // east velocity in local/body frame + // relative to local airmass, fps + float v_wind_body_down; // down/vertical velocity in local/body + // frame relative to local airmass, fps + + // Accelerations + float A_X_pilot; // X accel in body frame ft/sec^2 + float A_Y_pilot; // Y accel in body frame ft/sec^2 + float A_Z_pilot; // Z accel in body frame ft/sec^2 + + // Stall + float stall_warning; // 0.0 - 1.0 indicating the amount of stall + float slip_deg; // slip ball deflection + + // Pressure + + // Engine status + uint32_t num_engines; // Number of valid engines + uint32_t eng_state[FG_MAX_ENGINES];// Engine state (off, cranking, running) + float rpm[FG_MAX_ENGINES]; // Engine RPM rev/min + float fuel_flow[FG_MAX_ENGINES]; // Fuel flow gallons/hr + float fuel_px[FG_MAX_ENGINES]; // Fuel pressure psi + float egt[FG_MAX_ENGINES]; // Exhuast gas temp deg F + float cht[FG_MAX_ENGINES]; // Cylinder head temp deg F + float mp_osi[FG_MAX_ENGINES]; // Manifold pressure + float tit[FG_MAX_ENGINES]; // Turbine Inlet Temperature + float oil_temp[FG_MAX_ENGINES]; // Oil temp deg F + float oil_px[FG_MAX_ENGINES]; // Oil pressure psi + + // Consumables + uint32_t num_tanks; // Max number of fuel tanks + float fuel_quantity[FG_MAX_TANKS]; + + // Gear status + uint32_t num_wheels; + uint32_t wow[FG_MAX_WHEELS]; + float gear_pos[FG_MAX_WHEELS]; + float gear_steer[FG_MAX_WHEELS]; + float gear_compression[FG_MAX_WHEELS]; + + // Environment + uint32_t cur_time; // current unix time + // FIXME: make this uint64_t before 2038 + int32_t warp; // offset in seconds to unix time + float visibility; // visibility in meters (for env. effects) + + // Control surface positions (normalized values) + float elevator; + float elevator_trim_tab; + float left_flap; + float right_flap; + float left_aileron; + float right_aileron; + float rudder; + float nose_wheel; + float speedbrake; + float spoilers; + + void ByteSwap(void); +}; + +#endif // _SIM_JSBSIM_H