SITL: JSBSim: Use rate_hz to specify timestep

libraries/SITL/SIM_JSBSim.cpp

@@ -103,7 +103,7 @@ bool JSBSim::create_templates(void)
 "       interface on TCP 5124 -->\n"
 "  <input port=\"%u\"/>\n"
 "\n"
-"  <run start=\"0\" end=\"10000000\" dt=\"0.001\">\n"
+"  <run start=\"0\" end=\"10000000\" dt=\"%.6f\">\n"
 "    <property value=\"0\"> simulation/notify-time-trigger </property>\n"
 "\n"
 "    <event name=\"start engine\">\n"
@@ -124,7 +124,8 @@ bool JSBSim::create_templates(void)
             jsbsim_model,
             jsbsim_model,
             jsbsim_model,
-            control_port);
+            control_port,
+            1.0/rate_hz);
     fclose(f);
 
     f = fopen(jsbsim_fgout, "w");
@@ -132,8 +133,8 @@ bool JSBSim::create_templates(void)
         AP_HAL::panic("Unable to create jsbsim fgout script %s", jsbsim_fgout);
     }
     fprintf(f, "<?xml version=\"1.0\"?>\n"
-            "<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"UDP\" rate=\"1000\"/>\n",
-            fdm_port);
+            "<output name=\"127.0.0.1\" type=\"FLIGHTGEAR\" port=\"%u\" protocol=\"UDP\" rate=\"%f\"/>\n",
+            fdm_port, rate_hz);
     fclose(f);
 
     char *jsbsim_reset;
@@ -199,10 +200,12 @@ bool JSBSim::start_JSBSim(void)
         char *logdirective;
         char *script;
         char *nice;
+        char *rate;
 
         asprintf(&logdirective, "--logdirectivefile=%s", jsbsim_fgout);
         asprintf(&script, "--script=%s", jsbsim_script);
         asprintf(&nice, "--nice=%.8f", 10*1e-9);
+        asprintf(&rate, "--simulation-rate=%f", rate_hz);
 
         if (chdir(autotest_dir) != 0) {
             perror(autotest_dir);
@@ -212,7 +215,7 @@ bool JSBSim::start_JSBSim(void)
         int ret = execlp("JSBSim",
                          "JSBSim",
                          "--suspend",
-                         "--simulation-rate=1000",
+                         rate,
                          nice,
                          logdirective,
                          script,
@@ -439,7 +442,7 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
     rpm2 = fdm.rpm[1];
     
     // assume 1kHz for now
-    time_now_us += 1000;
+    time_now_us += 1e6/rate_hz;
 }
 
 void JSBSim::drain_control_socket()
@@ -468,7 +471,7 @@ void JSBSim::update(const struct sitl_input &input)
     }
     send_servos(input);
     recv_fdm(input);
-    adjust_frame_time(1000);
+    adjust_frame_time(rate_hz);
     sync_frame_time();
     drain_control_socket();
 }