diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp
index 15142d461f..0d8a6ec619 100644
--- a/libraries/SITL/SIM_JSBSim.cpp
+++ b/libraries/SITL/SIM_JSBSim.cpp
@@ -29,8 +29,11 @@
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
+
/*
constructor
*/
@@ -44,8 +47,16 @@ JSBSim::JSBSim(const char *home_str, const char *frame_str) :
created_templates(false),
started_jsbsim(false),
opened_control_socket(false),
- opened_fdm_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;
+ }
}
/*
@@ -114,6 +125,31 @@ bool JSBSim::create_templates(void)
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;
}
@@ -198,7 +234,9 @@ 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
}
}
@@ -218,7 +256,9 @@ bool JSBSim::expect(const char *str)
} else {
str = basestr;
}
+#if DEBUG_JSBSIM
write(1, &c, 1);
+#endif
}
return true;
}
@@ -272,6 +312,21 @@ void JSBSim::send_servos(const struct sitl_input &input)
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"
@@ -285,12 +340,16 @@ void JSBSim::send_servos(const struct sitl_input &input)
#define FEET_TO_METERS 0.3048f
-// nasty hack ....
-void FGNetFDM::ByteSwap(void) {
+/* nasty hack ....
+ JSBSim sends in little-endian
+ */
+void FGNetFDM::ByteSwap(void)
+{
uint32_t *buf = (uint32_t *)this;
for (uint16_t i=0; i