/// -*- 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 #include extern const AP_HAL::HAL& hal; // the asprintf() calls are not worth checking for SITL #pragma GCC diagnostic ignored "-Wunused-result" #define DEBUG_JSBSIM 1 #define FEET_TO_METERS 0.3048f /* 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), frame(FRAME_NORMAL) { if (strstr(frame_str, "elevon")) { frame = FRAME_ELEVON; } else if (strstr(frame_str, "vtail")) { frame = FRAME_VTAIL; } else { frame = FRAME_NORMAL; } } /* create template files */ bool JSBSim::create_templates(void) { if (created_templates) { return true; } control_port = 5505 + instance*10; fdm_port = 5504 + instance*10; 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); char *jsbsim_reset; asprintf(&jsbsim_reset, "%s/aircraft/Rascal/reset.xml", autotest_dir); f = fopen(jsbsim_reset, "w"); if (f == NULL) { hal.scheduler->panic("Unable to create jsbsim Rascal reset script"); } float r, p, y; dcm.to_euler(&r, &p, &y); fprintf(f, "\n" "\n" " %f \n" " %f \n" " 1.3 \n" " 0.0 \n" " 0.0 \n" " 0.0 \n" " 13.0 \n" " %f \n" "\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); 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]); for (uint8_t i=3; i<100; i++) { close(i); } 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) { #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 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\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; 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_REV=-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; } 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" "step\n", aileron, elevator, rudder, throttle, radians(input.wind.direction), input.wind.speed / FEET_TO_METERS); 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 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(1000); sync_frame_time(); drain_control_socket(); } #endif // CONFIG_HAL_BOARD