Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/SITL/SIM_JSBSim.cpp

485 lines
13 KiB
C++
Raw Blame History

/*
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 JSBSim
*/
#include "SIM_JSBSim.h"
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <AP_HAL/AP_HAL.h>
extern const AP_HAL::HAL& hal;
namespace SITL {
// the asprintf() calls are not worth checking for SITL
#pragma GCC diagnostic ignored "-Wunused-result"
#define DEBUG_JSBSIM 1
JSBSim::JSBSim(const char *frame_str) :
Aircraft(frame_str),
sock_control(false),
sock_fgfdm(true),
initialised(false),
jsbsim_script(nullptr),
jsbsim_fgout(nullptr),
created_templates(false),
started_jsbsim(false),
opened_control_socket(false),
opened_fdm_socket(false),
frame(FRAME_NORMAL)
{
if (strstr(frame_str, "elevon")) {
frame = FRAME_ELEVON;
} else if (strstr(frame_str, "vtail")) {
frame = FRAME_VTAIL;
} else {
frame = FRAME_NORMAL;
}
const char *model_name = strchr(frame_str, ':');
if (model_name != nullptr) {
jsbsim_model = model_name + 1;
}
control_port = 5505 + instance*10;
fdm_port = 5504 + instance*10;
num_motors = 2;
printf("JSBSim backend started: control_port=%u fdm_port=%u\n",
control_port, fdm_port);
}
/*
create template files
*/
bool JSBSim::create_templates(void)
{
if (created_templates) {
return true;
}
asprintf(&jsbsim_script, "%s/jsbsim_start_%u.xml", autotest_dir, instance);
asprintf(&jsbsim_fgout, "%s/jsbsim_fgout_%u.xml", autotest_dir, instance);
printf("JSBSim_script: '%s'\n", jsbsim_script);
printf("JSBSim_fgout: '%s'\n", jsbsim_fgout);
FILE *f = fopen(jsbsim_script, "w");
if (f == nullptr) {
AP_HAL::panic("Unable to create jsbsim script %s", 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 %s\">\n"
"\n"
" <description>\n"
" test ArduPlane using %s and JSBSim\n"
" </description>\n"
"\n"
" <use aircraft=\"%s\" 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=\"%.6f\">\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"
"",
jsbsim_model,
jsbsim_model,
jsbsim_model,
control_port,
1.0/rate_hz);
fclose(f);
f = fopen(jsbsim_fgout, "w");
if (f == nullptr) {
AP_HAL::panic("Unable to create jsbsim fgout script %s", jsbsim_fgout);
}
fprintf(f, "<?xml version=\"1.0\"?>\n"
"<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"UDP\" rate=\"%f\">\n"
" <time type=\"simulation\" resolution=\"1e-6\"/>\n"
"</output>",
fdm_port, rate_hz);
fclose(f);
char *jsbsim_reset;
asprintf(&jsbsim_reset, "%s/aircraft/%s/reset.xml", autotest_dir, jsbsim_model);
printf("JSBSim_reset: '%s'\n", jsbsim_reset);
f = fopen(jsbsim_reset, "w");
if (f == nullptr) {
AP_HAL::panic("Unable to create jsbsim reset script %s", jsbsim_reset);
}
float r, p, y;
dcm.to_euler(&r, &p, &y);
fprintf(f,
"<?xml version=\"1.0\"?>\n"
"<initialize name=\"Start up location\">\n"
" <latitude unit=\"DEG\" type=\"geodetic\"> %f </latitude>\n"
" <longitude unit=\"DEG\"> %f </longitude>\n"
" <altitude unit=\"M\"> 1.3 </altitude>\n"
" <vt unit=\"FT/SEC\"> 0.0 </vt>\n"
" <gamma unit=\"DEG\"> 0.0 </gamma>\n"
" <phi unit=\"DEG\"> 0.0 </phi>\n"
" <theta unit=\"DEG\"> 13.0 </theta>\n"
" <psi unit=\"DEG\"> %f </psi>\n"
"</initialize>\n",
home.lat*1.0e-7,
home.lng*1.0e-7,
degrees(y));
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|O_CLOEXEC);
if (pipe(p) != 0) {
AP_HAL::panic("Unable to create pipe");
}
pid_t child_pid = fork();
if (child_pid == 0) {
// in child
setsid();
dup2(devnull, 0);
dup2(p[1], 1);
close(p[0]);
for (uint8_t i=3; i<100; i++) {
close(i);
}
char *logdirective;
char *script;
char *nice;
char *rate;
asprintf(&logdirective, "--logdirectivefile=%s", jsbsim_fgout);
asprintf(&script, "--script=%s", jsbsim_script);
asprintf(&nice, "--nice=%.8f", 10*1e-9);
asprintf(&rate, "--simulation-rate=%f", rate_hz);
if (chdir(autotest_dir) != 0) {
perror(autotest_dir);
exit(1);
}
int ret = execlp("JSBSim",
"JSBSim",
"--suspend",
rate,
nice,
logdirective,
script,
nullptr);
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) {
AP_HAL::panic("Unable to start JSBSim");
}
if (!expect("JSBSim Execution beginning")) {
AP_HAL::panic("Failed to start JSBSim");
}
if (!open_control_socket()) {
AP_HAL::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();
close(devnull);
return true;
}
/*
check for stdout from JSBSim
*/
void JSBSim::check_stdout(void) const
{
char line[100];
ssize_t ret = ::read(jsbsim_stdout, line, sizeof(line));
if (ret > 0) {
#if DEBUG_JSBSIM
write(1, line, ret);
#endif
}
}
/*
a simple function to wait for a string on jsbsim_stdout
*/
bool JSBSim::expect(const char *str) const
{
const char *basestr = str;
while (*str) {
char c;
if (read(jsbsim_stdout, &c, 1) != 1) {
return false;
}
if (c == *str) {
str++;
} else {
str = basestr;
}
#if DEBUG_JSBSIM
write(1, &c, 1);
#endif
}
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\n"
"resume\n"
"iterate 1\n"
"set atmosphere/turb-type 4\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 = nullptr;
float aileron = filtered_servo_angle(input, 0);
float elevator = filtered_servo_angle(input, 1);
float throttle = filtered_servo_range(input, 2);
float rudder = filtered_servo_angle(input, 3);
if (frame == FRAME_ELEVON) {
// fake an elevon plane
float ch1 = aileron;
float ch2 = elevator;
aileron = (ch2-ch1)/2.0f;
// the minus does away with the need for RC2_REVERSED=-1
elevator = -(ch2+ch1)/2.0f;
} else if (frame == FRAME_VTAIL) {
// fake a vtail plane
float ch1 = elevator;
float ch2 = rudder;
// this matches VTAIL_OUTPUT==2
elevator = (ch2-ch1)/2.0f;
rudder = (ch2+ch1)/2.0f;
}
float wind_speed_fps = input.wind.speed / FEET_TO_METERS;
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"
"set atmosphere/psiw-rad %f\n"
"set atmosphere/wind-mag-fps %f\n"
"set atmosphere/turbulence/milspec/windspeed_at_20ft_AGL-fps %f\n"
"set atmosphere/turbulence/milspec/severity %f\n"
"iterate 1\n",
aileron, elevator, rudder, throttle,
radians(input.wind.direction),
wind_speed_fps,
wind_speed_fps/3,
input.wind.turbulence);
ssize_t buflen = strlen(buf);
ssize_t sent = sock_control.send(buf, buflen);
free(buf);
if (sent < 0) {
if (errno != EAGAIN) {
fprintf(stderr, "Fatal: Failed to send on control socket: %s\n",
strerror(errno));
exit(1);
}
}
if (sent < buflen) {
fprintf(stderr, "Failed to send all bytes on control socket\n");
}
}
/* nasty hack ....
JSBSim sends in little-endian
*/
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]);
}
// fixup the 3 doubles
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;
check_stdout();
do {
while (sock_fgfdm.recv(&fdm, sizeof(fdm), 100) != sizeof(fdm)) {
send_servos(input);
check_stdout();
}
fdm.ByteSwap();
} while (fdm.cur_time == time_now_us);
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.agl*100 + home.alt;
dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
airspeed = fdm.vcas * KNOTS_TO_METERS_PER_SECOND;
airspeed_pitot = airspeed;
// update magnetic field
update_mag_field_bf();
rpm[0] = fdm.rpm[0];
rpm[1] = fdm.rpm[1];
time_now_us = fdm.cur_time;
}
void JSBSim::drain_control_socket()
{
const uint16_t buflen = 1024;
char buf[buflen];
ssize_t received;
do {
received = sock_control.recv(buf, buflen, 0);
} while (received > 0);
}
/*
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(rate_hz);
sync_frame_time();
drain_control_socket();
}
} // namespace SITL
Reference in New Issue View Git Blame Copy Permalink