21 #include <arpa/inet.h> 26 #include <sys/types.h> 35 #pragma GCC diagnostic ignored "-Wunused-result" 37 #define DEBUG_JSBSIM 1 44 jsbsim_script(nullptr),
45 jsbsim_fgout(nullptr),
46 created_templates(false),
47 started_jsbsim(false),
48 opened_control_socket(false),
49 opened_fdm_socket(false),
52 if (strstr(frame_str,
"elevon")) {
54 }
else if (strstr(frame_str,
"vtail")) {
59 const char *model_name = strchr(frame_str,
':');
60 if (model_name !=
nullptr) {
84 "<?xml version=\"1.0\" encoding=\"utf-8\"?>\n" 85 "<?xml-stylesheet type=\"text/xsl\" href=\"http://jsbsim.sf.net/JSBSimScript.xsl\"?>\n" 86 "<runscript xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"\n" 87 " xsi:noNamespaceSchemaLocation=\"http://jsbsim.sf.net/JSBSimScript.xsd\"\n" 88 " name=\"Testing %s\">\n" 91 " test ArduPlane using %s and JSBSim\n" 94 " <use aircraft=\"%s\" initialize=\"reset\"/>\n" 96 " <!-- we control the servos via the jsbsim console\n" 97 " interface on TCP 5124 -->\n" 98 " <input port=\"%u\"/>\n" 100 " <run start=\"0\" end=\"10000000\" dt=\"0.001\">\n" 101 " <property value=\"0\"> simulation/notify-time-trigger </property>\n" 103 " <event name=\"start engine\">\n" 104 " <condition> simulation/sim-time-sec le 0.01 </condition>\n" 105 " <set name=\"propulsion/engine[0]/set-running\" value=\"1\"/>\n" 109 " <event name=\"Trim\">\n" 110 " <condition>simulation/sim-time-sec ge 0.01</condition>\n" 111 " <set name=\"simulation/do_simple_trim\" value=\"2\"/>\n" 128 fprintf(f,
"<?xml version=\"1.0\"?>\n" 129 "<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"udp\" rate=\"1000\"/>\n",
135 f =
fopen(jsbsim_reset,
"w");
137 AP_HAL::panic(
"Unable to create jsbsim reset script %s", jsbsim_reset);
142 "<?xml version=\"1.0\"?>\n" 143 "<initialize name=\"Start up location\">\n" 144 " <latitude unit=\"DEG\"> %f </latitude>\n" 145 " <longitude unit=\"DEG\"> %f </longitude>\n" 146 " <altitude unit=\"M\"> 1.3 </altitude>\n" 147 " <vt unit=\"FT/SEC\"> 0.0 </vt>\n" 148 " <gamma unit=\"DEG\"> 0.0 </gamma>\n" 149 " <phi unit=\"DEG\"> 0.0 </phi>\n" 150 " <theta unit=\"DEG\"> 13.0 </theta>\n" 151 " <psi unit=\"DEG\"> %f </psi>\n" 176 int devnull =
open(
"/dev/null", O_RDWR|O_CLOEXEC);
180 pid_t child_pid = fork();
181 if (child_pid == 0) {
187 for (uint8_t i=3; i<100; i++) {
201 int ret = execlp(
"JSBSim",
206 "--simulation-rate=1000",
224 if (!
expect(
"JSBSim Execution beginning")) {
258 const char *basestr = str;
287 printf(
"Opened JSBSim control socket\n");
295 "set atmosphere/turb-type 4\n";
332 float ch2 = elevator;
333 aileron = (ch2-ch1)/2.0
f;
335 elevator = -(ch2+ch1)/2.0
f;
338 float ch1 = elevator;
341 elevator = (ch2-ch1)/2.0
f;
342 rudder = (ch2+ch1)/2.0
f;
346 "set fcs/aileron-cmd-norm %f\n" 347 "set fcs/elevator-cmd-norm %f\n" 348 "set fcs/rudder-cmd-norm %f\n" 349 "set fcs/throttle-cmd-norm %f\n" 350 "set atmosphere/psiw-rad %f\n" 351 "set atmosphere/wind-mag-fps %f\n" 352 "set atmosphere/turbulence/milspec/windspeed_at_20ft_AGL-fps %f\n" 353 "set atmosphere/turbulence/milspec/severity %f\n" 355 aileron, elevator, rudder, throttle,
360 ssize_t
buflen = strlen(buf);
364 if (
errno != EAGAIN) {
365 fprintf(stderr,
"Fatal: Failed to send on control socket: %s\n",
371 fprintf(stderr,
"Failed to send all bytes on control socket\n");
380 uint32_t *buf = (uint32_t *)
this;
381 for (uint16_t i=0; i<
sizeof(*this)/4; i++) {
382 buf[i] = ntohl(buf[i]);
385 buf = (uint32_t *)&longitude;
387 for (uint8_t i=0; i<3; i++) {
403 while (
sock_fgfdm.recv(&fdm,
sizeof(fdm), 100) !=
sizeof(fdm)) {
437 const uint16_t
buflen = 1024;
443 if (
errno != EAGAIN &&
errno != EWOULDBLOCK) {
444 fprintf(stderr,
"error recv on control socket: %s",
450 }
while (received > 0);
bool create_templates(void)
void to_euler(float *roll, float *pitch, float *yaw) const
int printf(const char *fmt,...)
void sync_frame_time(void)
Vector3< float > Vector3f
int open(const char *pathname, int flags)
POSIX Open a file with integer mode flags.
char * strerror(int errnum)
POSIX strerror() - convert POSIX errno to text with user message.
float rpm[FG_MAX_ENGINES]
ssize_t write(int fd, const void *buf, size_t count)
POSIX Write count bytes from *buf to fileno fd.
bool opened_control_socket
float filtered_servo_range(const struct sitl_input &input, uint8_t idx)
void from_euler(float roll, float pitch, float yaw)
void recv_fdm(const struct sitl_input &input)
int32_t lat
param 3 - Latitude * 10**7
static void convert_body_frame(double rollDeg, double pitchDeg, double rollRate, double pitchRate, double yawRate, double *p, double *q, double *r)
void send_servos(const struct sitl_input &input)
void update(const struct sitl_input &input)
void perror(const char *s)
POSIX perror() - convert POSIX errno to text with user message.
ssize_t read(int fd, void *buf, size_t count)
POSIX read count bytes from *buf to fileno fd.
int32_t alt
param 2 - Altitude in centimeters (meters * 100) see LOCATION_ALT_MAX_M
const char * autotest_dir
int close(int fileno)
POSIX Close a file with fileno handel.
bool open_fdm_socket(void)
void drain_control_socket()
const char * jsbsim_model
JSBSim(const char *home_str, const char *frame_str)
bool expect(const char *str)
void adjust_frame_time(float rate)
void update_mag_field_bf(void)
int32_t lng
param 4 - Longitude * 10**7
FILE * fopen(const char *path, const char *mode)
POSIX Open a file with path name and ascii file mode string.
int chdir(const char *pathname)
POSIX change directory.
static constexpr float radians(float deg)
int errno
Note: fdevopen assigns stdin,stdout,stderr.
int fprintf(FILE *fp, const char *fmt,...)
fprintf character write function
const AP_HAL::HAL & hal
-*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
bool open_control_socket(void)
int asprintf(char **strp, const char *fmt,...)
const float FEET_TO_METERS
void panic(const char *errormsg,...) FMT_PRINTF(1
float filtered_servo_angle(const struct sitl_input &input, uint8_t idx)
enum SITL::JSBSim::@206 frame