Merge pull request #2 from GaloisInc/jsbsim-ext-rate-multiplier

SITL: allow setting jsbsim_ext sim rate multiplier

I noticed recently that the OpenSUT autopilot is completely non-functional - instead of initializing sensors in a few seconds and letting the user start the mission, it slowly initializes them over the course of several hours (I ran it for 3+ hours and it never reached the "pre-arm good" state) while constantly dropping the connection to MAVproxy.  After several days of debugging and a bit of archeology, I found this commit on my old laptop, which apparently I forgot to push when creating #1.  Adding this commit fixes the bug.

The actual change in this commit is to allow the JSBSim update rate to be an integer multiple of the autopilot's update rate.  The performance overhead of the OpenSUT's nested VM setup means the autopilot update rate can't exceed ~300 Hz, but JSBSim's update rate must be at least 400 Hz (ideally much higher) or else the simulation becomes unstable.  To work around this, we run JSBSim at 2500 Hz and run the autopilot at 250 Hz with a sim rate multiplier of 10x.  The autopilot needs to be aware of the sim rate multiplier because the autopilot itself is responsible for driving the simulation - it periodically sends commands to JSBSim instructing it to advance the simulation by a certain amount.

I haven't fully investigated the previous bug, but my guess is that the autopilot was timing out waiting for updates from JSBSim.  The autopilot was expecting an update after each step, but JSBSim was sending updates only every 10 steps.

libraries/SITL/SIM_JSBSimExt.cpp

@@ -131,7 +131,7 @@ bool JSBSimExt::create_templates(void)
             // listen on a different port instead.  `jsbsim_proxy` will forward
             // traffic between the two (after starting JSBSim).
             control_port + 10000,
-            1.0/rate_hz);
+            1.0/(rate_hz * jsbsim_rate_multiplier));
     fclose(f);
 
     f = fopen(jsbsim_fgout, "w");
@@ -256,12 +256,13 @@ void JSBSimExt::send_servos(const struct sitl_input &input)
              "set atmosphere/wind-mag-fps %f\n"
              "set atmosphere/turbulence/milspec/windspeed_at_20ft_AGL-fps %f\n"
              "set atmosphere/turbulence/milspec/severity %f\n"
-             "iterate 1\n",
+             "iterate %d\n",
              aileron, elevator, rudder, throttle,
              radians(input.wind.direction),
              wind_speed_fps,
              wind_speed_fps/3,
-             input.wind.turbulence);
+             input.wind.turbulence,
+             jsbsim_rate_multiplier);
     ssize_t buflen = strlen(buf);
     ssize_t sent = sock_control.send(buf, buflen);
     free(buf);
@@ -347,6 +348,19 @@ void JSBSimExt::update(const struct sitl_input &input)
     drain_control_socket();
 }
 
+void JSBSimExt::set_config(const char* config) {
+    if (*config == '\0') {
+        jsbsim_rate_multiplier = 1;
+        return;
+    }
+
+    jsbsim_rate_multiplier = atoi(config);
+    if (jsbsim_rate_multiplier <= 0) {
+        printf("bad JSBSim rate multiplier: %s\n", config);
+        exit(1);
+    }
+}
+
 void JSBSimExt::set_interface_ports(const char* address, const int port_in, const int port_out) {
     control_address = address;
     control_port = port_out;

libraries/SITL/SIM_JSBSimExt.h

@@ -47,6 +47,7 @@ class JSBSimExt : public Aircraft {
         return new JSBSimExt(frame_str);
     }
 
+    void set_config(const char* config) override;
     void set_interface_ports(const char* address, const int port_in, const int port_out) override;
 
 private:
@@ -65,6 +66,7 @@ class JSBSimExt : public Aircraft {
 
     // default JSBSim model
     const char *jsbsim_model = "Rascal";
+    int jsbsim_rate_multiplier = 1;
 
     bool created_templates;
     bool opened_control_socket;