SITL: JSON add rangefinder support

libraries/SITL/SIM_JSON.cpp

@@ -124,9 +124,9 @@ void JSON::output_servos(const struct sitl_input &input)
     This parser does not do any syntax checking, and is not at all
     general purpose
 */
-uint8_t JSON::parse_sensors(const char *json)
+uint16_t JSON::parse_sensors(const char *json)
 {
-    uint8_t received_bitmask = 0;
+    uint16_t received_bitmask = 0;
 
     //printf("%s\n", json);
     for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
@@ -234,9 +234,10 @@ void JSON::recv_fdm(const struct sitl_input &input)
         return;
     }
 
-    const uint8_t received_bitmask = parse_sensors((const char *)(p1+1));
+    const uint16_t received_bitmask = parse_sensors((const char *)(p1+1));
     if (received_bitmask == 0) {
-        // did not receve one of the required fields
+        // did not receve one of the mandatory fields
+        printf("Did not contain all mandatory fields\n");
         return;
     }
 
@@ -246,6 +247,20 @@ void JSON::recv_fdm(const struct sitl_input &input)
         return;
     }
 
+    if (received_bitmask != last_received_bitmask) {
+        // some change in the message we have received, print what we got
+        printf("\nJSON received:\n");
+        for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
+            struct keytable &key = keytable[i];
+            if ((received_bitmask &  1U << i) == 0) {
+                continue;
+            }
+            printf("\t%s\n",key.key);
+        }
+        printf("\n");
+    }
+    last_received_bitmask = received_bitmask;
+
     memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
     sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
 
@@ -274,11 +289,20 @@ void JSON::recv_fdm(const struct sitl_input &input)
     // Convert from a meters from origin physics to a lat long alt
     update_position();
 
+    // update range finder distances
+    for (uint8_t i=7; i<13; i++) {
+        if ((received_bitmask &  1U << i) == 0) {
+            continue;
+        }
+        rangefinder_m[i-7] = state.rng[i-7];
+    }
+
     double deltat;
     if (state.timestamp_s < last_timestamp_s) {
-        // Physics time has gone backwards, don't reset AP, assume an average size timestep
+        // Physics time has gone backwards, don't reset AP
         printf("Detected physics reset\n");
         deltat = 0;
+        last_received_bitmask = 0;
     } else {
         deltat = state.timestamp_s - last_timestamp_s;
     }

libraries/SITL/SIM_JSON.h

@@ -57,7 +57,7 @@ private:
     void output_servos(const struct sitl_input &input);
     void recv_fdm(const struct sitl_input &input);
 
-    uint8_t parse_sensors(const char *json);
+    uint16_t parse_sensors(const char *json);
 
     // buffer for parsing pose data in JSON format
     uint8_t sensor_buffer[65000];
@@ -81,6 +81,7 @@ private:
         Vector3f attitude;
         Quaternion quaternion;
         Vector3f velocity;
+        float rng[6];
     } state;
 
     // table to aid parsing of JSON sensor data
@@ -90,7 +91,7 @@ private:
         void *ptr;
         enum data_type type;
         bool required;
-    } keytable[7] = {
+    } keytable[13] = {
         { "", "timestamp", &state.timestamp_s, DATA_DOUBLE, true },
         { "imu", "gyro",    &state.imu.gyro, DATA_VECTOR3F, true },
         { "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F, true },
@@ -98,6 +99,12 @@ private:
         { "", "attitude", &state.attitude, DATA_VECTOR3F, false },
         { "", "quaternion", &state.quaternion, QUATERNION, false },
         { "", "velocity", &state.velocity, DATA_VECTOR3F, true },
+        { "", "rng_1", &state.rng[0], DATA_FLOAT, false },
+        { "", "rng_2", &state.rng[1], DATA_FLOAT, false },
+        { "", "rng_3", &state.rng[2], DATA_FLOAT, false },
+        { "", "rng_4", &state.rng[3], DATA_FLOAT, false },
+        { "", "rng_5", &state.rng[4], DATA_FLOAT, false },
+        { "", "rng_6", &state.rng[5], DATA_FLOAT, false },
     };
 
     // Enum coresponding to the ordering of keys in the keytable.
@@ -109,7 +116,14 @@ private:
         EULER_ATT   = 1U << 4,
         QUAT_ATT    = 1U << 5,
         VELOCITY    = 1U << 6,
+        RNG_1       = 1U << 7,
+        RNG_2       = 1U << 8,
+        RNG_3       = 1U << 9,
+        RNG_4       = 1U << 10,
+        RNG_5       = 1U << 11,
+        RNG_6       = 1U << 12,
     };
+    uint16_t last_received_bitmask;
 };
 
 }