diff --git a/libraries/SITL/SIM_JSON.cpp b/libraries/SITL/SIM_JSON.cpp index 5c30b65c4f..8f9e95e786 100644 --- a/libraries/SITL/SIM_JSON.cpp +++ b/libraries/SITL/SIM_JSON.cpp @@ -255,7 +255,11 @@ void JSON::recv_fdm(const struct sitl_input &input) if ((received_bitmask & 1U << i) == 0) { continue; } - printf("\t%s\n",key.key); + if (strcmp(key.section, "") == 0) { + printf("\t%s\n",key.key); + } else { + printf("\t%s: %s\n",key.section,key.key); + } } printf("\n"); } @@ -297,6 +301,14 @@ void JSON::recv_fdm(const struct sitl_input &input) rangefinder_m[i-7] = state.rng[i-7]; } + // update wind vane + if ((received_bitmask & WIND_DIR) != 0) { + wind_vane_apparent.direction = state.wind_vane_apparent.direction; + } + if ((received_bitmask & WIND_SPD) != 0) { + wind_vane_apparent.speed = state.wind_vane_apparent.speed; + } + double deltat; if (state.timestamp_s < last_timestamp_s) { // Physics time has gone backwards, don't reset AP diff --git a/libraries/SITL/SIM_JSON.h b/libraries/SITL/SIM_JSON.h index 9f3b13e6fd..ee743127e0 100644 --- a/libraries/SITL/SIM_JSON.h +++ b/libraries/SITL/SIM_JSON.h @@ -82,6 +82,10 @@ private: Quaternion quaternion; Vector3f velocity; float rng[6]; + struct { + float direction; + float speed; + } wind_vane_apparent; } state; // table to aid parsing of JSON sensor data @@ -91,7 +95,7 @@ private: void *ptr; enum data_type type; bool required; - } keytable[13] = { + } keytable[15] = { { "", "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 }, @@ -105,6 +109,8 @@ private: { "", "rng_4", &state.rng[3], DATA_FLOAT, false }, { "", "rng_5", &state.rng[4], DATA_FLOAT, false }, { "", "rng_6", &state.rng[5], DATA_FLOAT, false }, + {"windvane","direction", &state.wind_vane_apparent.direction, DATA_FLOAT, false}, + {"windvane","speed", &state.wind_vane_apparent.speed, DATA_FLOAT, false}, }; // Enum coresponding to the ordering of keys in the keytable. @@ -122,6 +128,8 @@ private: RNG_4 = 1U << 10, RNG_5 = 1U << 11, RNG_6 = 1U << 12, + WIND_DIR = 1U << 13, + WIND_SPD = 1U << 14, }; uint16_t last_received_bitmask; }; diff --git a/libraries/SITL/examples/JSON/readme.md b/libraries/SITL/examples/JSON/readme.md index 1cec7071aa..5a56e60ce6 100644 --- a/libraries/SITL/examples/JSON/readme.md +++ b/libraries/SITL/examples/JSON/readme.md @@ -2,7 +2,8 @@ The JSON SITL backend allows software to easily interface with ArduPilot using a To launch the JSON backend run SITL with ```-f json:127.0.0.1``` where 127.0.0.1 is replaced with the IP the physics backend is running at. -Connection to SITL is made via a UDP link on port 9002. +Connection to SITL is made via a UDP link. The physics backend should listen for incoming messages on port 9002 it should then reply to the IP and port the messages were received from. This removes the need to configure the a target +IP and port for SITL in the physics backend. SITL will send a output message every 10 seconds allowing the physics backend to auto detect. SITL output Data is output from SITL in a binary format: @@ -57,6 +58,14 @@ rangefinder distances corresponding to driver instances: rng_6 (m) ``` +Apparent wind: +``` + windvane: + direction (radians) clockwise relative to the front, i.e. 0 = head to wind + speed (m/s) +``` +for example:```"windvane":{"direction":0,"speed":0}``` + When first connecting you will see a message reporting what fields were successfully received. If any of the mandatory fields are missing SITL will stop, however it will run without the optional fields. This message can be used to double check SITL is receiving everything being sent by the physics backend. For example: