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

SITL: SIM_JSON: update

Browse Source
This commit is contained in:
Iampete1 2020-05-31 23:10:41 +01:00 committed by Andrew Tridgell
parent 44f727cfd2
commit fbb4df97a3
2 changed files with 35 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/SITL/SIM_JSON.cpp
View File

@ -99,8 +99,8 @@ void JSON::set_interface_ports(const char* address, const int port_in, const int
void JSON::output_servos(const struct sitl_input &input)
{
servo_packet pkt;
pkt.frame_rate = rate_hz;
pkt.frame_count = frame_counter;
pkt.speedup = get_speedup();
for (uint8_t i=0; i<16; i++) {
pkt.pwm[i] = input.servos[i];
}
@ -149,7 +149,7 @@ bool JSON::parse_sensors(const char *json)
p += strlen(key.key)+2;
switch (key.type) {
case DATA_UINT64:
*((uint64_t *)key.ptr) = atof(p); // using atof rather than strtoul means we support scientific notation
*((uint64_t *)key.ptr) = strtoull(p, nullptr, 10);
//printf("%s/%s = %lu\n", key.section, key.key, *((uint64_t *)key.ptr));
break;
@ -241,24 +241,25 @@ void JSON::recv_fdm(const struct sitl_input &input)
// Convert from a meters from origin physics to a lat long alt
update_position();
if (last_timestamp) {
int deltat;
if (state.timestamp < last_timestamp) {
// Physics time has gone backwards, don't reset AP, assume an average size timestep
printf("Detected physics reset\n");
deltat = average_frame_time;
} else {
deltat = state.timestamp - last_timestamp;
}
time_now_us += deltat;
if (deltat > 0 && deltat < 100000) {
if (average_frame_time < 1) {
average_frame_time = deltat;
}
average_frame_time = average_frame_time * 0.98 + deltat * 0.02;
}
double deltat;
if (state.timestamp_s < last_timestamp_s) {
// Physics time has gone backwards, don't reset AP, assume an average size timestep
printf("Detected physics reset\n");
deltat = 0;
} else {
deltat = state.timestamp_s - last_timestamp_s;
}
time_now_us += deltat * 1.0e6;
if (deltat > 0 && deltat < 0.1) {
// time in us to hz
adjust_frame_time(1.0 / deltat);
// match actual frame rate with desired speedup
time_advance();
}
last_timestamp_s = state.timestamp_s;
frame_counter++;
#if 0
// @LoggerMessage: JSN1
@ -315,8 +316,6 @@ void JSON::recv_fdm(const struct sitl_input &input)
velocity_ef.z);
#endif
last_timestamp = state.timestamp;
frame_counter++;
}
/*
@ -333,4 +332,14 @@ void JSON::update(const struct sitl_input &input)
// update magnetic field
// as the model does not provide mag feild we calculate it from position and attitude
update_mag_field_bf();
// allow for changes in physics step
adjust_frame_time(constrain_float(sitl->loop_rate_hz, rate_hz-1, rate_hz+1));
#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
}
#endif
}

10
libraries/SITL/SIM_JSON.h
View File

@ -37,8 +37,9 @@ public:
private:
struct servo_packet {
uint16_t magic = 18458; // constant magic value
uint16_t frame_rate;
uint32_t frame_count;
float speedup;
uint16_t pwm[16];
};
@ -50,9 +51,8 @@ private:
SocketAPM sock;
double average_frame_time;
uint32_t frame_counter;
uint64_t last_timestamp;
double last_timestamp_s;
void output_servos(const struct sitl_input &input);
void recv_fdm(const struct sitl_input &input);
@ -71,7 +71,7 @@ private:
};
struct {
uint64_t timestamp;
double timestamp_s;
struct {
Vector3f gyro;
Vector3f accel_body;
@ -88,7 +88,7 @@ private:
void *ptr;
enum data_type type;
} keytable[6] = {
{ "", "timestamp", &state.timestamp, DATA_UINT64 },
{ "", "timestamp", &state.timestamp_s, DATA_DOUBLE },
{ "imu", "gyro", &state.imu.gyro, DATA_VECTOR3F },
{ "imu", "accel_body", &state.imu.accel_body, DATA_VECTOR3F },
{ "", "position", &state.position, DATA_VECTOR3F },