SITL: SIM_JSON: update

libraries/SITL/SIM_JSON.cpp

@@ -99,8 +99,8 @@ void JSON::set_interface_ports(const char* address, const int port_in, const int
 void JSON::output_servos(const struct sitl_input &input)
 {
     servo_packet pkt;
+    pkt.frame_rate = rate_hz;
     pkt.frame_count = frame_counter;
-    pkt.speedup = get_speedup();
     for (uint8_t i=0; i<16; i++) {
         pkt.pwm[i] = input.servos[i];
     }
@@ -149,7 +149,7 @@ bool JSON::parse_sensors(const char *json)
         p += strlen(key.key)+2;
         switch (key.type) {
             case DATA_UINT64:
-                *((uint64_t *)key.ptr) = atof(p); // using atof rather than strtoul means we support scientific notation
+                *((uint64_t *)key.ptr) = strtoull(p, nullptr, 10);
                 //printf("%s/%s = %lu\n", key.section, key.key, *((uint64_t *)key.ptr));
                 break;
 
@@ -241,24 +241,25 @@ void JSON::recv_fdm(const struct sitl_input &input)
     // Convert from a meters from origin physics to a lat long alt
     update_position();
 
-    if (last_timestamp) {
-        int deltat;
-        if (state.timestamp < last_timestamp) {
-            // Physics time has gone backwards, don't reset AP, assume an average size timestep
-            printf("Detected physics reset\n");
-            deltat = average_frame_time;
-        } else {
-            deltat = state.timestamp - last_timestamp;
-        }
-        time_now_us += deltat;
+    double deltat;
+    if (state.timestamp_s < last_timestamp_s) {
+        // Physics time has gone backwards, don't reset AP, assume an average size timestep
+        printf("Detected physics reset\n");
+        deltat = 0;
+    } else {
+        deltat = state.timestamp_s - last_timestamp_s;
+    }
+    time_now_us += deltat * 1.0e6;
 
-        if (deltat > 0 && deltat < 100000) {
-            if (average_frame_time < 1) {
-                average_frame_time = deltat;
-            }
-            average_frame_time = average_frame_time * 0.98 + deltat * 0.02;
-        }
+    if (deltat > 0 && deltat < 0.1) {
+        // time in us to hz
+        adjust_frame_time(1.0 / deltat);
+
+        // match actual frame rate with desired speedup
+        time_advance();
     }
+    last_timestamp_s = state.timestamp_s;
+    frame_counter++;
 
 #if 0
 // @LoggerMessage: JSN1
@@ -315,8 +316,6 @@ void JSON::recv_fdm(const struct sitl_input &input)
                        velocity_ef.z);
 #endif
 
-    last_timestamp = state.timestamp;
-    frame_counter++;
 }
 
 /*
@@ -333,4 +332,14 @@ void JSON::update(const struct sitl_input &input)
     // update magnetic field
     // as the model does not provide mag feild we calculate it from position and attitude
     update_mag_field_bf();
+
+    // allow for changes in physics step
+    adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
+
+#if 0
+    // report frame rate
+    if (frame_counter % 1000 == 0) {
+        printf("FPS %.2f\n", achieved_rate_hz); // this is instantaneous rather than any clever average
+    }
+#endif
 }

libraries/SITL/SIM_JSON.h

@@ -37,8 +37,9 @@ public:
 private:
 
     struct servo_packet {
+        uint16_t magic = 18458; // constant magic value
+        uint16_t frame_rate;
         uint32_t frame_count;
-        float speedup;
         uint16_t pwm[16];
     };
 
@@ -50,9 +51,8 @@ private:
 
     SocketAPM sock;
 
-    double average_frame_time;
     uint32_t frame_counter;
-    uint64_t last_timestamp;
+    double last_timestamp_s;
 
     void output_servos(const struct sitl_input &input);
     void recv_fdm(const struct sitl_input &input);
@@ -71,7 +71,7 @@ private:
     };
 
     struct {
-        uint64_t timestamp;
+        double timestamp_s;
         struct {
             Vector3f gyro;
             Vector3f accel_body;
@@ -88,7 +88,7 @@ private:
         void *ptr;
         enum data_type type;
     } keytable[6] = {
-        { "", "timestamp", &state.timestamp, DATA_UINT64 },
+        { "", "timestamp", &state.timestamp_s, DATA_DOUBLE },
         { "imu", "gyro",    &state.imu.gyro, DATA_VECTOR3F },
         { "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F },
         { "", "position", &state.position, DATA_VECTOR3F },