diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index ed43366ab6c0b4..8a98f48abc2052 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -35,10 +35,7 @@ #include #include #include -#if USE_PICOJSON -#include "picojson.h" -#include -#endif +#include using namespace SITL; diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 8b40a4eba281fa..1d79437e9065d2 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -38,10 +38,6 @@ #include #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 { /* diff --git a/libraries/SITL/SIM_Frame.cpp b/libraries/SITL/SIM_Frame.cpp index a9b49040427371..dac0ec8e50e289 100644 --- a/libraries/SITL/SIM_Frame.cpp +++ b/libraries/SITL/SIM_Frame.cpp @@ -25,6 +25,8 @@ #include #include +#include + using namespace SITL; static Motor quad_plus_motors[] = @@ -322,10 +324,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_JSONPARSER_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 +348,20 @@ 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); + + AP_JSONParser json_parser; + if (!json_parser.init(50)) { + ::printf("failed to init json\n"); + return; + } + if (!json_parser.parse_filepath(model_json)) { + free(fname); + ::printf("failed to parse json %s\n", model_json); + return; } enum class VarType { + UNKNOWN, FLOAT, VECTOR3F, }; @@ -384,63 +399,98 @@ void Frame::load_frame_params(const char *model_json) FRAME_VAR(num_motors), }; - for (uint8_t i=0; iget(vars[i].label); - if (v.is()) { - // 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) + uint16_t count; + if (!json_parser.shift_token_object(count)) { + ::printf("No base object in %s\n", model_json); + return; + } + bool failed = false; + while (count--) { + char param_name[20] {}; + if (!json_parser.shift_token_string(param_name, ARRAY_SIZE(param_name))) { + failed = true; + break; } - if (vars[i].t == VarType::FLOAT) { - parse_float(v, vars[i].label, *((float *)vars[i].ptr)); - - } 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; iget(label_name); - if (v.is()) { - // use default value + // check static list of known parameter names first: + for (const auto &var : vars) { + if (!streq(param_name, var.label)) { continue; } - if (per_motor_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()) { - 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 + failed = true; + break; + } + float ¶m = *((float*)param_ptr); + if (!json_parser.shift_token_float(param)) { + failed = true; + break; + } + printf("%s=%f\n", param_name, param); + break; + } + case VarType::VECTOR3F: { + if (param_ptr == nullptr) { + // should not happen + return; + } + Vector3f ¶m = *((Vector3f*)param_ptr); + if (!json_parser.shift_token_vector3f(param)) { + failed = true; + break; + } + printf("%s=[%f,%f,%f]\n", param_name, param[0], param[1], param[2]); + } + } } - param = val.get(); -} -void Frame::parse_vector3(picojson::value val, const char* label, Vector3f ¶m) { - if (!val.is() || !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]); + if (failed) { + ::printf("Failed to load model params from %s\n", model_json); + return; } + + ::printf("Loaded model params from %s\n", model_json); } #endif @@ -455,7 +505,7 @@ void Frame::init(const char *frame_str, Battery *_battery) model = default_model; battery = _battery; -#if USE_PICOJSON +#if AP_JSONPARSER_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) { diff --git a/libraries/SITL/SIM_Frame.h b/libraries/SITL/SIM_Frame.h index 21ba0c9fd75074..e1b00eaea78b4c 100644 --- a/libraries/SITL/SIM_Frame.h +++ b/libraries/SITL/SIM_Frame.h @@ -20,10 +20,7 @@ #include "SIM_Aircraft.h" #include "SIM_Motor.h" - -#if USE_PICOJSON -#include "picojson.h" -#endif +#include namespace SITL { @@ -146,7 +143,7 @@ class Frame { } default_model; protected: -#if USE_PICOJSON +#if AP_JSONPARSER_ENABLED // load frame parameters from a json model file void load_frame_params(const char *model_json); #endif @@ -164,11 +161,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 ¶m); - void parse_vector3(picojson::value val, const char* label, Vector3f ¶m); -#endif }; } diff --git a/libraries/SITL/SIM_XPlane.cpp b/libraries/SITL/SIM_XPlane.cpp index e0a75091843238..63f2aa1e08a67e 100644 --- a/libraries/SITL/SIM_XPlane.cpp +++ b/libraries/SITL/SIM_XPlane.cpp @@ -30,7 +30,6 @@ #include #include #include -#include "picojson.h" #include // ignore cast errors in this case to keep complexity down @@ -121,72 +120,222 @@ XPlane::XPlane(const char *frame_str) : } } +#ifndef streq +#define streq(a,b) !strcmp(a, b) +#endif + /* - add one DRef to list + add one joystick axis to list */ -void XPlane::add_dref(const char *name, DRefType type, const picojson::value &dref) +bool XPlane::handle_axis(const char *label, class ::AP_JSONParser &json_parser) { - struct DRef *d = new struct DRef; - if (d == nullptr) { - AP_HAL::panic("out of memory for DRef %s", name); + struct JoyInput *j = new struct JoyInput; + if (j == nullptr) { + AP_HAL::panic("out of memory for JoyInput %s", label); } - d->name = strdup(name); - d->type = type; - if (d->name == nullptr) { - AP_HAL::panic("out of memory for DRef %s", name); + j->type = JoyType::AXIS; + j->axis = atoi(label+4); + + uint16_t n_object_kv_pairs; + if (!json_parser.shift_token_object(n_object_kv_pairs)) { + return false; } - if (d->type == DRefType::FIXED) { - d->fixed_value = dref.get("value").get(); - } else { - d->range = dref.get("range").get(); - d->channel = dref.get("channel").get(); + + for (uint8_t i=0;ichannel = value; + continue; + } + if (streq(n, "input_min")) { + float value; + if (!json_parser.shift_token_float(value)) { + return false; + } + j->input_min = value; + continue; + } + if (streq(n, "input_max")) { + float value; + if (!json_parser.shift_token_float(value)) { + return false; + } + j->input_max = value; + continue; + } + if (streq(n, "type")) { + char unused[50] {}; + if (!json_parser.shift_token_string(unused, ARRAY_SIZE(unused))) { + return false; + } + continue; + } + ::printf("Unknown axis nv %s\n", n); + return false; } - // add to linked list - d->next = drefs; - drefs = d; -} -/* - add one joystick axis to list - */ -void XPlane::add_joyinput(const char *label, JoyType type, const picojson::value &d) + j->next = joyinputs; + joyinputs = j; + + return true; +} +bool XPlane::handle_button(const char *label, class ::AP_JSONParser &json_parser) { - if (strncmp(label, "axis", 4) == 0) { - struct JoyInput *j = new struct JoyInput; - if (j == nullptr) { - AP_HAL::panic("out of memory for JoyInput %s", label); - } - j->axis = atoi(label+4); - j->type = JoyType::AXIS; - j->channel = d.get("channel").get(); - j->input_min = d.get("input_min").get(); - j->input_max = d.get("input_max").get(); - j->next = joyinputs; - joyinputs = j; + struct JoyInput *j = new struct JoyInput; + if (j == nullptr) { + AP_HAL::panic("out of memory for JoyInput %s", label); + } + j->type = JoyType::BUTTON; + + uint16_t n_object_kv_pairs; + if (!json_parser.shift_token_object(n_object_kv_pairs)) { + return false; } - if (strncmp(label, "button", 6) == 0) { - struct JoyInput *j = new struct JoyInput; - if (j == nullptr) { - AP_HAL::panic("out of memory for JoyInput %s", label); + + for (uint8_t i=0;itype = JoyType::BUTTON; - j->channel = d.get("channel").get(); - j->mask = d.get("mask").get(); - j->next = joyinputs; - joyinputs = j; + if (streq(n, "channel")) { + int32_t value; + if (!json_parser.shift_token_int32_t(value)) { + return false; + } + j->channel = value; + continue; + } + if (streq(n, "mask")) { + int32_t value; + if (!json_parser.shift_token_int32_t(value)) { + return false; + } + j->channel = value; + continue; + } + if (streq(n, "type")) { + char unused[50] {}; + if (!json_parser.shift_token_string(unused, ARRAY_SIZE(unused))) { + return false; + } + continue; + } + ::printf("Unknown button nv %s\n", n); + return false; } + + j->next = joyinputs; + joyinputs = j; + + return true; } /* handle a setting */ -void XPlane::handle_setting(const picojson::value &d) + +bool XPlane::handle_settings(class ::AP_JSONParser &json_parser) { - if (d.contains("debug")) { - dref_debug = d.get("debug").get(); + uint16_t n_object_kv_pairs; + if (!json_parser.shift_token_object(n_object_kv_pairs)) { + return false; + } + for (auto i = 0; iname = strdup(name); + if (d->name == nullptr) { + AP_HAL::panic("out of memory for DRef %s", name); + } + + char type_s[6] {}; + for (uint8_t i=0;itype = DRefType::ANGLE; + } else if (strcmp(type_s, "range") == 0) { + d->type = DRefType::RANGE; + } else if (strcmp(type_s, "fixed") == 0) { + d->type = DRefType::FIXED; + } else { + ::printf("Invalid dref type %s for %s", type_s, name); + } + + continue; + } + if (streq(n, "range")) { + float range; + if (!json_parser.shift_token_float(range)) { + return false; + } + d->range = range; + continue; + } + if (streq(n, "channel")) { + int32_t channel; + if (!json_parser.shift_token_int32_t(channel)) { + return false; + } + d->channel = channel; + continue; + } + if (streq(n, "value")) { + float value; + if (!json_parser.shift_token_float(value)) { + return false; + } + d->fixed_value = value; + continue; + } + } + + ::printf("%s t=%s range=%f fv=%f channel=%u\n", d->name, type_s, d->range, d->fixed_value, (unsigned)d->channel); + +// add to linked list + d->next = drefs; + drefs = d; + + return true; +} /* load mapping of channels to datarefs from a json file @@ -205,8 +354,12 @@ bool XPlane::load_dref_map(const char *map_json) if (fname == nullptr) { return false; } - picojson::value *obj = (picojson::value *)load_json(fname); - if (obj == nullptr) { + + AP_JSONParser json_parser; + if (!json_parser.init(400)) { + return false; + } + if (!json_parser.parse_filepath(fname)) { return false; } @@ -227,39 +380,42 @@ bool XPlane::load_dref_map(const char *map_json) delete joyinputs; joyinputs = j; } - - uint32_t count = 0; - // obtain a const reference to the map, and print the contents - const picojson::value::object& o = obj->get(); - for (picojson::value::object::const_iterator i = o.begin(); - i != o.end(); - ++i) { - const char *label = i->first.c_str(); - const auto &d = i->second; + + uint16_t count = 0; + if (!json_parser.shift_token_object(count)) { + return false; + } + + uint16_t i = count; + while (i--) { + char label[50] {}; // dref name + if (!json_parser.shift_token_string(label, ARRAY_SIZE(label))) { + return true; + } + if (strchr(label, '/') != nullptr) { - const char *type_s = d.get("type").to_str().c_str(); - if (strcmp(type_s, "angle") == 0) { - add_dref(label, DRefType::ANGLE, d); - } else if (strcmp(type_s, "range") == 0) { - add_dref(label, DRefType::RANGE, d); - } else if (strcmp(type_s, "fixed") == 0) { - add_dref(label, DRefType::FIXED, d); - } else { - ::printf("Invalid dref type %s for %s in %s", type_s, label, map_filename); + if (!handle_dref(label, json_parser)) { + return false; } - } else if (strcmp(label, "settings") == 0) { - handle_setting(d); - } else if (strncmp(label, "axis", 4) == 0) { - add_joyinput(label, JoyType::AXIS, d); - } else if (strncmp(label, "button", 6) == 0) { - add_joyinput(label, JoyType::BUTTON, d); - } else { - ::printf("Invalid json type %s in %s", label, map_json); continue; } - count++; + if (streq(label, "settings")) { + if (!handle_settings(json_parser)) { + return false; + } + continue; + } + if (strncmp(label, "axis", 4) == 0) { + handle_axis(label, json_parser); + continue; + } + if (strncmp(label, "button", 6) == 0) { + handle_button(label, json_parser); + continue; + } + ::printf("Invalid json type %s in %s", label, map_json); + return false; } - delete obj; ::printf("Loaded %u DRefs from %s\n", unsigned(count), map_filename); return true; diff --git a/libraries/SITL/SIM_XPlane.h b/libraries/SITL/SIM_XPlane.h index 8c90ea6c2172e3..abe298eb23a90d 100644 --- a/libraries/SITL/SIM_XPlane.h +++ b/libraries/SITL/SIM_XPlane.h @@ -28,9 +28,9 @@ #include #include +#include #include "SIM_Aircraft.h" -#include "picojson.h" namespace SITL { @@ -126,9 +126,10 @@ class XPlane : public Aircraft { struct stat map_st; bool load_dref_map(const char *map_json); - void add_dref(const char *name, DRefType type, const picojson::value &dref); - void add_joyinput(const char *name, JoyType type, const picojson::value &d); - void handle_setting(const picojson::value &d); + bool handle_axis(const char *name, class AP_JSONParser &json_parser); + bool handle_button(const char *name, class AP_JSONParser &json_parser); + bool handle_settings(class AP_JSONParser &json_parser); + bool handle_dref(const char *name, class ::AP_JSONParser &json_parser); void check_reload_dref(void);