/* 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; }