change JSON parsing from picojson to jsmn

Tools/autotest/models/Callisto.json

@@ -57,5 +57,9 @@
     "mdrag_coef" : 0.10,
 
     # number of motors
-    "num_motors" : 8
+"num_motors" : 8,
+
+    "moment_inertia": [3, 4, 5],
+
+"motor5_position": [17, 18, 19]
 }

libraries/AP_HAL/AP_HAL_Boards.h

@@ -171,6 +171,10 @@
 #define HAL_WITH_IO_MCU 0
 #endif
 
+#ifndef AP_JSON_PARSING_ENABLED
+#define AP_JSON_PARSING_ENABLED 1
+#endif
+
 // this is used as a general mechanism to make a 'small' build by
 // dropping little used features. We use this to allow us to keep
 // FMUv2 going for as long as possible

libraries/AP_HAL_SITL/SITL_cmdline.cpp

@@ -127,7 +127,9 @@ static const struct {
     Aircraft *(*constructor)(const char *frame_str);
 } model_constructors[] = {
     { "quadplane",          QuadPlane::create },
+#if HAL_SIM_XPLANE_ENABLED
     { "xplane",             XPlane::create },
+#endif
     { "firefly",            QuadPlane::create },
     { "+",                  MultiCopter::create },
     { "quad",               MultiCopter::create },

libraries/SITL/SIM_Aircraft.cpp

@@ -37,8 +37,7 @@
 #include <AP_Terrain/AP_Terrain.h>
 #include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
-#if USE_PICOJSON
-#include "picojson.h"
+#if AP_JSON_PARSING_ENABLED
 #include <AP_Filesystem/AP_Filesystem.h>
 #endif
 

libraries/SITL/SIM_Aircraft.h

@@ -37,10 +37,6 @@
 #include <Filter/Filter.h>
 #include "SIM_JSON_Master.h"
 
-#ifndef USE_PICOJSON
-#define USE_PICOJSON (CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX)
-#endif
-
 namespace SITL {
 
 /*

libraries/SITL/SIM_Frame.cpp

@@ -25,6 +25,10 @@
 #include <stdio.h>
 #include <sys/stat.h>
 
+#if AP_JSON_PARSING_ENABLED
+#include <AP_Common/jsmn.h>
+#endif
+
 using namespace SITL;
 
 static Motor quad_plus_motors[] =
@@ -322,10 +326,15 @@ float Frame::get_air_density(float alt_amsl) const
     return air_pressure / (ISA_GAS_CONSTANT * (C_TO_KELVIN(model.refTempC)));
 }
 
-#if USE_PICOJSON
+#if AP_JSON_PARSING_ENABLED
 /*
   load frame specific parameters from a json file if available
  */
+
+#ifndef streq
+#define streq(a,b) !strcmp(a, b)
+#endif
+
 void Frame::load_frame_params(const char *model_json)
 {
     char *fname = nullptr;
@@ -341,12 +350,69 @@ void Frame::load_frame_params(const char *model_json)
     if (fname == nullptr) {
         AP_HAL::panic("%s failed to load\n", model_json);
     }
-    picojson::value *obj = (picojson::value *)load_json(model_json);
-    if (obj == nullptr) {
-        AP_HAL::panic("%s failed to load\n", model_json);
+
+    jsmntok_t t[50];
+
+    assert_storage_size<jsmntok_t, 16> __assert_token_size;
+    (void)__assert_token_size;
+
+    jsmn_parser parser;
+    jsmn_init(&parser);
+
+    // copied in from load_json:
+    int fd = AP::FS().open(model_json, O_RDONLY);
+    if (fd == -1) {
+        ::printf("failed to open json %s\n", model_json);
+        return;
+    }
+    char *buf = (char*)malloc(st.st_size+1);
+    if (buf == nullptr) {
+        ::printf("alloc failed %u\n", unsigned(st.st_size+1));
+        AP::FS().close(fd);
+        return;
+    }
+
+    if (AP::FS().read(fd, buf, st.st_size) != st.st_size) {
+        ::printf("failed to read json %s\n", model_json);
+        AP::FS().close(fd);
+        return;
+    }
+    AP::FS().close(fd);
+
+    char *start = strchr(buf, '{');
+    if (!start) {
+        ::printf("Invalid json %s", model_json);
+        return;
+    }
+
+    /*
+      remove comments, as not allowed by the parser
+     */
+    for (char *p = strchr(buf,'#'); p; p=strchr(p+1, '#')) {
+        // clear to end of line
+        do {
+            *p++ = ' ';
+        } while (*p != '\n' && *p != '\r' && *p);
+    }
+
+    const int r = jsmn_parse(&parser,
+                             buf,
+                             st.st_size,
+                             t,
+                             ARRAY_SIZE(t));
+    if (r < 0) {
+        ::printf("Failed to parse %s", model_json);
+        return;
+    }
+
+    const uint16_t num_used_tokens = uint16_t(r);
+
+    if (t[0].type != JSMN_OBJECT) {
+        return;
     }
 
     enum class VarType {
+        UNKNOWN,
         FLOAT,
         VECTOR3F,
     };
@@ -384,63 +450,111 @@ void Frame::load_frame_params(const char *model_json)
         FRAME_VAR(num_motors),
     };
 
-    for (uint8_t i=0; i<ARRAY_SIZE(vars); i++) {
-        auto v = obj->get(vars[i].label);
-        if (v.is<picojson::null>()) {
-            // use default value
-            continue;
+    // note we consume the "name" token here but the body of the for
+    // loop consumes either a primitive (1 element) or an array (n+1
+    // elements)
+    for (auto token_offset=1; token_offset<num_used_tokens; ) {
+        if (t[token_offset].type != JSMN_STRING) {
+            abort();
         }
-        if (vars[i].t == VarType::FLOAT) {
-            parse_float(v, vars[i].label, *((float *)vars[i].ptr));
+        char tmp_name[20] {};
+        strncpy(tmp_name, &buf[t[token_offset].start], t[token_offset].end-t[token_offset].start);
+        token_offset++;
 
-        } else if (vars[i].t == VarType::VECTOR3F) {
-            parse_vector3(v, vars[i].label, *(Vector3f *)vars[i].ptr);
+        VarType vartype = VarType::UNKNOWN;
+        void *param_ptr = nullptr;
 
-        }
-    }
-
-    json_search per_motor_vars[] = {
-        {"position", &model.motor_pos, VarType::VECTOR3F},
-        {"vector", &model.motor_thrust_vec, VarType::VECTOR3F},
-        {"yaw", &model.yaw_factor, VarType::FLOAT},
-    };
-    char label_name[20];
-    for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars); i++) {
-        for (uint8_t j=0; j<12; j++) {
-            sprintf(label_name, "motor%i_%s", j+1, per_motor_vars[i].label);
-            auto v = obj->get(label_name);
-            if (v.is<picojson::null>()) {
-                // use default value
+        // check static list of known parameter names first:
+        for (auto &var : vars) {
+            if (!streq(tmp_name, var.label)) {
                 continue;
             }
-            if (vars[i].t == VarType::FLOAT) {
-                parse_float(v, label_name, *(((float *)per_motor_vars[i].ptr) + j));
+            vartype = var.t;
+            param_ptr = var.ptr;
+            break;
+        }
 
-            } else if (per_motor_vars[i].t == VarType::VECTOR3F) {
-                parse_vector3(v, label_name, *(((Vector3f *)per_motor_vars[i].ptr) + j));
+        if (vartype == VarType::UNKNOWN) {
+            // not in static list of names; check for dynamically-named variables:
+            json_search per_motor_vars[] = {
+                {"position", &model.motor_pos, VarType::VECTOR3F},
+                {"vector", &model.motor_thrust_vec, VarType::VECTOR3F},
+                {"yaw", &model.yaw_factor, VarType::FLOAT},
+            };
+            char label_name[20];
+            for (uint8_t i=0; i<ARRAY_SIZE(per_motor_vars) && vartype == VarType::UNKNOWN; i++) {
+                for (uint8_t j=0; j<12; j++) {
+                    snprintf(label_name, sizeof(label_name), "motor%i_%s", j+1, per_motor_vars[i].label);
+                    if (!streq(label_name, tmp_name)) {
+                        continue;
+                    }
+                    vartype = per_motor_vars[i].t;
+                    param_ptr = per_motor_vars[i].ptr;
+                    break;
+                }
             }
         }
-    }
 
-    delete obj;
 
-    ::printf("Loaded model params from %s\n", model_json);
-}
+        switch (vartype) {
+        case VarType::UNKNOWN:
+            // unknown parameter; stop attempting to parse file:
+            ::printf("No parameter (%s)", tmp_name);
+            return;
 
-void Frame::parse_float(picojson::value val, const char* label, float &param) {
-    if (!val.is<double>()) {
-        AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
+        case VarType::FLOAT: {
+            if (param_ptr == nullptr) {
+                // should not happen
+                return;
+            }
+            if (token_offset >= num_used_tokens) {
+                return;
+            }
+            if (t[token_offset].type != JSMN_PRIMITIVE) {
+                return;
+            }
+            float &param = *((float*)param_ptr);
+            // do better here; avoid the copy...
+            char tmp_value[20] {};
+            strncpy(tmp_value, &buf[t[token_offset].start], t[token_offset].end-t[token_offset].start);
+            param = atof(tmp_value);
+            printf("%s=%f\n", tmp_name, (float)param);
+            token_offset++;
+            break;
+        }
+        case VarType::VECTOR3F: {
+            if (param_ptr == nullptr) {
+                // should not happen
+                return;
+            }
+            if (t[token_offset].type != JSMN_ARRAY) {
+                return;
+            }
+            if (t[token_offset].size != 3) {
+                return;
+            }
+            if (token_offset+t[token_offset].size >= num_used_tokens) {
+                return;
+            }
+            Vector3f &param = *((Vector3f*)param_ptr);
+            const uint8_t count = t[token_offset].size;
+            token_offset++;  // consume array token
+            for (uint8_t i=0; i<count; i++) {
+                if (t[token_offset].type != JSMN_PRIMITIVE) {
+                    return;
+                }
+                // do better here; avoid the copy...
+                char tmp_value[20] {};
+                strncpy(tmp_value, &buf[t[token_offset].start], t[token_offset].end-t[token_offset].start);
+                param[i] = atof(tmp_value);
+                printf("%s[%u]=%f\n", tmp_name, i, (float)param[i]);
+                token_offset++;  // consume primitive token
+            }
+        }
+        }
     }
-    param = val.get<double>();
-}
 
-void Frame::parse_vector3(picojson::value val, const char* label, Vector3f &param) {
-    if (!val.is<picojson::array>() || !val.contains(2) || val.contains(3)) {
-        AP_HAL::panic("Bad json type for %s: %s", label, val.to_str().c_str());
-    }
-    for (uint8_t j=0; j<3; j++) {
-        parse_float(val.get(j), label, param[j]);
-    }
+    ::printf("Loaded model params from %s\n", model_json);
 }
 
 #endif
@@ -455,7 +569,7 @@ void Frame::init(const char *frame_str, Battery *_battery)
     model = default_model;
     battery = _battery;
 
-#if USE_PICOJSON
+#if AP_JSON_PARSING_ENABLED
     const char *colon = strchr(frame_str, ':');
     size_t slen = strlen(frame_str);
     if (colon != nullptr && slen > 5 && strcmp(&frame_str[slen-5], ".json") == 0) {

libraries/SITL/SIM_Frame.h

@@ -21,10 +21,6 @@
 #include "SIM_Aircraft.h"
 #include "SIM_Motor.h"
 
-#if USE_PICOJSON
-#include "picojson.h"
-#endif
-
 namespace SITL {
 
 /*
@@ -146,7 +142,7 @@ class Frame {
     } default_model;
 
 protected:
-#if USE_PICOJSON
+#if AP_JSON_PARSING_ENABLED
     // load frame parameters from a json model file
     void load_frame_params(const char *model_json);
 #endif
@@ -164,11 +160,5 @@ class Frame {
 #if AP_SIM_ENABLED
     Battery *battery;
 #endif
-
-    // json parsing helpers
-#if USE_PICOJSON
-    void parse_float(picojson::value val, const char* label, float &param);
-    void parse_vector3(picojson::value val, const char* label, Vector3f &param);
-#endif
 };
 }

libraries/SITL/SIM_XPlane.h

@@ -21,7 +21,7 @@
 #include <AP_HAL/AP_HAL_Boards.h>
 
 #ifndef HAL_SIM_XPLANE_ENABLED
-#define HAL_SIM_XPLANE_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#define HAL_SIM_XPLANE_ENABLED 0
 #endif
 
 #if HAL_SIM_XPLANE_ENABLED
@@ -30,7 +30,6 @@
 #include <AP_Filesystem/AP_Filesystem.h>
 
 #include "SIM_Aircraft.h"
-#include "picojson.h"
 
 namespace SITL {