/*
receive an update from the FDM
This is a blocking function
*/
void JSBSim::recv_fdm(const struct sitl_input &input)
{
FGNetFDM fdm;
check_stdout();
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.agl*100 + home.alt;
dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
airspeed = fdm.vcas * FEET_TO_METERS;
// assume 1kHz for now
time_now_us += 1000;
}
/*
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;
}
int
main (void)
{
mpc_t z, x;
mp_prec_t prec;
test_start ();
mpc_init2 (z, 1000);
mpc_init2 (x, 1000);
check_file ("inp_str.dat");
for (prec = 2; prec <= 1000; prec+=7)
{
mpc_set_prec (z, prec);
mpc_set_prec (x, prec);
mpc_set_si_si (x, 1, 1, MPC_RNDNN);
check_io_str (z, x);
mpc_set_si_si (x, -1, 1, MPC_RNDNN);
check_io_str (z, x);
mpfr_set_inf (MPC_RE(x), -1);
mpfr_set_inf (MPC_IM(x), +1);
check_io_str (z, x);
test_default_random (x, -1024, 1024, 128, 25);
check_io_str (z, x);
}
#ifndef NO_STREAM_REDIRECTION
mpc_set_si_si (x, 1, -4, MPC_RNDNN);
mpc_div_ui (x, x, 3, MPC_RNDDU);
check_stdout(z, x);
#endif
mpc_clear (z);
mpc_clear (x);
test_end ();
return 0;
}
int
main (int argc, char *argv[])
{
int opt;
char const *prog;
exit_failure = EXIT_TROUBLE;
initialize_main (&argc, &argv);
program_name = argv[0];
setlocale (LC_ALL, "");
bindtextdomain (PACKAGE, LOCALEDIR);
textdomain (PACKAGE);
c_stack_action (cleanup);
prog = getenv ("EDITOR");
if (prog)
editor_program = prog;
diffarg (DEFAULT_DIFF_PROGRAM);
/* parse command line args */
while ((opt = getopt_long (argc, argv, "abBdEHiI:lo:stvw:W", longopts, 0))
!= -1)
{
switch (opt)
{
case 'a':
diffarg ("-a");
break;
case 'b':
diffarg ("-b");
break;
case 'B':
diffarg ("-B");
break;
case 'd':
diffarg ("-d");
break;
case 'E':
diffarg ("-E");
break;
case 'H':
diffarg ("-H");
break;
case 'i':
diffarg ("-i");
break;
case 'I':
diffarg ("-I");
diffarg (optarg);
break;
case 'l':
diffarg ("--left-column");
break;
case 'o':
output = optarg;
break;
case 's':
suppress_common_lines = true;
break;
case 't':
diffarg ("-t");
break;
case 'v':
version_etc (stdout, "sdiff", PACKAGE_NAME, PACKAGE_VERSION,
"Thomas Lord", (char *) 0);
check_stdout ();
return EXIT_SUCCESS;
case 'w':
diffarg ("-W");
diffarg (optarg);
break;
case 'W':
diffarg ("-w");
break;
case DIFF_PROGRAM_OPTION:
diffargv[0] = optarg;
break;
case HELP_OPTION:
usage ();
check_stdout ();
return EXIT_SUCCESS;
case STRIP_TRAILING_CR_OPTION:
diffarg ("--strip-trailing-cr");
break;
case TABSIZE_OPTION:
diffarg ("--tabsize");
diffarg (optarg);
break;
default:
try_help (0, 0);
}
}
if (argc - optind != 2)
{
if (argc - optind < 2)
try_help ("missing operand after `%s'", argv[argc - 1]);
else
try_help ("extra operand `%s'", argv[optind + 2]);
}
if (! output)
{
/* easy case: diff does everything for us */
if (suppress_common_lines)
diffarg ("--suppress-common-lines");
diffarg ("-y");
diffarg ("--");
diffarg (argv[optind]);
diffarg (argv[optind + 1]);
diffarg (0);
execvp (diffargv[0], (char **) diffargv);
perror_fatal (diffargv[0]);
}
else
{
char const *lname, *rname;
FILE *left, *right, *out, *diffout;
bool interact_ok;
struct line_filter lfilt;
struct line_filter rfilt;
struct line_filter diff_filt;
bool leftdir = diraccess (argv[optind]);
bool rightdir = diraccess (argv[optind + 1]);
if (leftdir & rightdir)
fatal ("both files to be compared are directories");
lname = expand_name (argv[optind], leftdir, argv[optind + 1]);
left = ck_fopen (lname, "r");
rname = expand_name (argv[optind + 1], rightdir, argv[optind]);
right = ck_fopen (rname, "r");
out = ck_fopen (output, "w");
diffarg ("--sdiff-merge-assist");
diffarg ("--");
diffarg (argv[optind]);
diffarg (argv[optind + 1]);
diffarg (0);
trapsigs ();
#if ! (HAVE_WORKING_FORK || HAVE_WORKING_VFORK)
{
size_t cmdsize = 1;
char *p, *command;
int i;
for (i = 0; diffargv[i]; i++)
cmdsize += quote_system_arg (0, diffargv[i]) + 1;
command = p = xmalloc (cmdsize);
for (i = 0; diffargv[i]; i++)
{
p += quote_system_arg (p, diffargv[i]);
*p++ = ' ';
}
p[-1] = 0;
errno = 0;
diffout = popen (command, "r");
if (! diffout)
perror_fatal (command);
free (command);
}
#else
{
int diff_fds[2];
# if HAVE_WORKING_VFORK
sigset_t procmask;
sigset_t blocked;
# endif
if (pipe (diff_fds) != 0)
perror_fatal ("pipe");
# if HAVE_WORKING_VFORK
/* Block SIGINT and SIGPIPE. */
sigemptyset (&blocked);
sigaddset (&blocked, SIGINT);
sigaddset (&blocked, SIGPIPE);
sigprocmask (SIG_BLOCK, &blocked, &procmask);
# endif
diffpid = vfork ();
if (diffpid < 0)
perror_fatal ("fork");
if (! diffpid)
{
/* Alter the child's SIGINT and SIGPIPE handlers;
this may munge the parent.
The child ignores SIGINT in case the user interrupts the editor.
The child does not ignore SIGPIPE, even if the parent does. */
if (initial_handler (handler_index_of_SIGINT) != SIG_IGN)
signal_handler (SIGINT, SIG_IGN);
signal_handler (SIGPIPE, SIG_DFL);
# if HAVE_WORKING_VFORK
/* Stop blocking SIGINT and SIGPIPE in the child. */
sigprocmask (SIG_SETMASK, &procmask, 0);
# endif
close (diff_fds[0]);
if (diff_fds[1] != STDOUT_FILENO)
{
dup2 (diff_fds[1], STDOUT_FILENO);
close (diff_fds[1]);
}
execvp (diffargv[0], (char **) diffargv);
_exit (errno == ENOENT ? 127 : 126);
}
# if HAVE_WORKING_VFORK
/* Restore the parent's SIGINT and SIGPIPE behavior. */
if (initial_handler (handler_index_of_SIGINT) != SIG_IGN)
signal_handler (SIGINT, catchsig);
if (initial_handler (handler_index_of_SIGPIPE) != SIG_IGN)
signal_handler (SIGPIPE, catchsig);
else
signal_handler (SIGPIPE, SIG_IGN);
/* Stop blocking SIGINT and SIGPIPE in the parent. */
sigprocmask (SIG_SETMASK, &procmask, 0);
# endif
close (diff_fds[1]);
diffout = fdopen (diff_fds[0], "r");
if (! diffout)
perror_fatal ("fdopen");
}
#endif
lf_init (&diff_filt, diffout);
lf_init (&lfilt, left);
lf_init (&rfilt, right);
interact_ok = interact (&diff_filt, &lfilt, lname, &rfilt, rname, out);
ck_fclose (left);
ck_fclose (right);
ck_fclose (out);
{
int wstatus;
int werrno = 0;
#if ! (HAVE_WORKING_FORK || HAVE_WORKING_VFORK)
wstatus = pclose (diffout);
if (wstatus == -1)
werrno = errno;
#else
ck_fclose (diffout);
while (waitpid (diffpid, &wstatus, 0) < 0)
if (errno == EINTR)
checksigs ();
else
perror_fatal ("waitpid");
diffpid = 0;
#endif
if (tmpname)
{
unlink (tmpname);
tmpname = 0;
}
if (! interact_ok)
exiterr ();
check_child_status (werrno, wstatus, EXIT_FAILURE, diffargv[0]);
untrapsig (0);
checksigs ();
exit (WEXITSTATUS (wstatus));
}
}
return EXIT_SUCCESS; /* Fool `-Wall'. */
}
/*
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) {
AP_HAL::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) {
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();
return true;
}
