zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: added initial JSBSim simulator backend

mission-4.1.18
Andrew Tridgell 10 years ago
parent
5241c66b57
commit
c136d65c25
3 changed files with 534 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      libraries/SITL/SIM_Aircraft.h
  2. 355
      libraries/SITL/SIM_JSBSim.cpp
  3. 171
      libraries/SITL/SIM_JSBSim.h

8
libraries/SITL/SIM_Aircraft.h
Unescape View File

@ -45,6 +45,13 @@ public: @@ -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: @@ -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;

355
libraries/SITL/SIM_JSBSim.cpp
Unescape View File

@ -0,0 +1,355 @@ @@ -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 <http://www.gnu.org/licenses/>.
*/
/*
simulator connector for ardupilot version of JSBSim
*/
#include <AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include "SIM_JSBSim.h"
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <arpa/inet.h>
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,
"<?xml version=\"1.0\" encoding=\"utf-8\"?>\n"
"<?xml-stylesheet type=\"text/xsl\" href=\"http://jsbsim.sf.net/JSBSimScript.xsl\"?>\n"
"<runscript xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"\n"
" xsi:noNamespaceSchemaLocation=\"http://jsbsim.sf.net/JSBSimScript.xsd\"\n"
" name=\"Testing Rascal110\">\n"
"\n"
" <description>\n"
" test ArduPlane using Rascal110 and JSBSim\n"
" </description>\n"
"\n"
" <use aircraft=\"Rascal\" initialize=\"reset\"/>\n"
"\n"
" <!-- we control the servos via the jsbsim console\n"
" interface on TCP 5124 -->\n"
" <input port=\"%u\"/>\n"
"\n"
" <run start=\"0\" end=\"10000000\" dt=\"0.001\">\n"
" <property value=\"0\"> simulation/notify-time-trigger </property>\n"
"\n"
" <event name=\"start engine\">\n"
" <condition> simulation/sim-time-sec le 0.01 </condition>\n"
" <set name=\"propulsion/engine[0]/set-running\" value=\"1\"/>\n"
" <notify/>\n"
" </event>\n"
"\n"
" <event name=\"Trim\">\n"
" <condition>simulation/sim-time-sec ge 0.01</condition>\n"
" <set name=\"simulation/do_simple_trim\" value=\"2\"/>\n"
" <notify/>\n"
" </event>\n"
" </run>\n"
"\n"
"</runscript>\n"
"", control_port);
fclose(f);
f = fopen(jsbsim_fgout, "w");
if (f == NULL) {
hal.scheduler->panic("Unable to create jsbsim fgout script");
}
fprintf(f, "<?xml version=\"1.0\"?>\n"
"<output name=\"localhost\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"udp\" rate=\"1000\"/>\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<sizeof(*this)/4; i++) {
buf[i] = ntohl(buf[i]);
}
buf = (uint32_t *)&longitude;
uint32_t tmp;
for (uint8_t i=0; i<3; i++) {
tmp = buf[0];
buf[0] = buf[1];
buf[1] = tmp;
buf += 2;
}
}
/*
receive an update from the FDM
This is a blocking function
*/
void JSBSim::recv_fdm(const struct sitl_input &input)
{
FGNetFDM fdm;
while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) {
send_servos(input);
check_stdout();
}
fdm.ByteSwap();
accel_body = Vector3f(fdm.A_X_pilot, fdm.A_Y_pilot, fdm.A_Z_pilot) * FEET_TO_METERS;
double p, q, r;
SITL::convert_body_frame(degrees(fdm.phi), degrees(fdm.theta),
degrees(fdm.phidot), degrees(fdm.thetadot), degrees(fdm.psidot),
&p, &q, &r);
gyro = Vector3f(p, q, r);
velocity_ef = Vector3f(fdm.v_north, fdm.v_east, fdm.v_down) * FEET_TO_METERS;
location.lat = degrees(fdm.latitude) * 1.0e7;
location.lng = degrees(fdm.longitude) * 1.0e7;
location.alt = fdm.altitude*100;
dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
// assume 1kHz for now
time_now_us += 1000;
}
/*
update the JSBSim simulation by one time step
*/
void JSBSim::update(const struct sitl_input &input)
{
while (!initialised) {
if (!create_templates() ||
!start_JSBSim() ||
!open_control_socket() ||
!open_fdm_socket()) {
time_now_us = 1;
return;
}
initialised = true;
}
send_servos(input);
recv_fdm(input);
adjust_frame_time(1000);
sync_frame_time();
}
#endif // CONFIG_HAL_BOARD

171
libraries/SITL/SIM_JSBSim.h
Unescape View File

@ -0,0 +1,171 @@ @@ -0,0 +1,171 @@
/// -*- 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 <http://www.gnu.org/licenses/>.
*/
/*
simulator connection for ardupilot version of JSBSim
*/
#ifndef _SIM_JSBSIM_H
#define _SIM_JSBSIM_H
#include "SIM_Aircraft.h"
#include <utility/Socket.h>
/*
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
Write Preview
Loading…
Cancel
Save