Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: SIM_JSON: add no time sync to JSON format

Browse Source
This commit is contained in:
Iampete1 2021-04-25 01:32:27 +01:00 committed by Andrew Tridgell
parent 5ff0f42372
commit 4505dce64d
2 changed files with 20 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/SITL/SIM_JSON.cpp
View File

@ -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
*/
uint16_t JSON::parse_sensors(const char *json)
uint32_t JSON::parse_sensors(const char *json)
{
uint16_t received_bitmask = 0;
uint32_t received_bitmask = 0;
//printf("%s\n", json);
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
@ -203,6 +203,11 @@ uint16_t JSON::parse_sensors(const char *json)
break;
}
case BOOLEAN:
*((bool *)key.ptr) = strtoull(p, nullptr, 10) != 0;
//printf("%s/%s = %i\n", key.section, key.key, *((unit8_t *)key.ptr));
break;
}
}
@ -246,7 +251,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
return;
}
const uint16_t received_bitmask = parse_sensors((const char *)(p1+1));
const uint32_t received_bitmask = parse_sensors((const char *)(p1+1));
if (received_bitmask == 0) {
// did not receve one of the mandatory fields
printf("Did not contain all mandatory fields\n");
@ -285,6 +290,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
velocity_ef = state.velocity;
position = state.position;
position.xy() += origin.get_distance_NE_double(home);
use_time_sync = !state.no_time_sync;
// deal with euler or quaternion attitude
if ((received_bitmask & QUAT_ATT) != 0) {
@ -342,8 +348,9 @@ void JSON::recv_fdm(const struct sitl_input &input)
if (is_positive(deltat) && deltat < 0.1) {
// time in us to hz
adjust_frame_time(1.0 / deltat);
if (use_time_sync) {
adjust_frame_time(1.0 / deltat);
}
// match actual frame rate with desired speedup
time_advance();
}
@ -437,7 +444,7 @@ void JSON::update(const struct sitl_input &input)
#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
printf("FPS %.2f\n", rate_hz); // this is instantaneous rather than any clever average
}
#endif
}

10
libraries/SITL/SIM_JSON.h
View File

@ -57,7 +57,7 @@ private:
void output_servos(const struct sitl_input &input);
void recv_fdm(const struct sitl_input &input);
uint16_t parse_sensors(const char *json);
uint32_t parse_sensors(const char *json);
// buffer for parsing pose data in JSON format
uint8_t sensor_buffer[65000];
@ -70,6 +70,7 @@ private:
DATA_VECTOR3F,
DATA_VECTOR3D,
QUATERNION,
BOOLEAN,
};
struct {
@ -88,6 +89,7 @@ private:
float speed;
} wind_vane_apparent;
float airspeed;
bool no_time_sync;
} state;
// table to aid parsing of JSON sensor data
@ -97,7 +99,7 @@ private:
void *ptr;
enum data_type type;
bool required;
} keytable[16] = {
} keytable[17] = {
{ "", "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 },
@ -114,6 +116,7 @@ private:
{"windvane","direction", &state.wind_vane_apparent.direction, DATA_FLOAT, false},
{"windvane","speed", &state.wind_vane_apparent.speed, DATA_FLOAT, false},
{"", "airspeed", &state.airspeed, DATA_FLOAT, false},
{"", "no_time_sync", &state.no_time_sync, BOOLEAN, false},
};
// Enum coresponding to the ordering of keys in the keytable.
@ -134,8 +137,9 @@ private:
WIND_DIR = 1U << 13,
WIND_SPD = 1U << 14,
AIRSPEED = 1U << 15,
TIME_SYNC = 1U << 16,
};
uint16_t last_received_bitmask;
uint32_t last_received_bitmask;
};
}