aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: SITL-Webots timing is received from Webots only

Browse Source
This commit is contained in:
mhefny 2020-05-14 08:21:41 +02:00 committed by Peter Barker
parent 5437f1e6ee
commit 5a64156862
2 changed files with 55 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

132
libraries/SITL/SIM_Webots.cpp
View File

@ -83,9 +83,9 @@ static const struct {
Webots::Webots(const char *frame_str) : Webots::Webots(const char *frame_str) :
Aircraft(frame_str) Aircraft(frame_str)
{ {
use_time_sync = true; use_time_sync = false;
rate_hz = 4000; use_smoothing = false;
last_state.timestamp = 0.0l;
char *saveptr = nullptr; char *saveptr = nullptr;
char *s = strdup(frame_str); char *s = strdup(frame_str);
char *frame_option = strtok_r(s, ":", &saveptr); char *frame_option = strtok_r(s, ":", &saveptr);
@ -97,6 +97,7 @@ Webots::Webots(const char *frame_str) :
*/ */
if (args1) { if (args1) {
webots_ip = args1; webots_ip = args1;
printf("Simulation Port %s\n",args1);
} }
if (args2) { if (args2) {
webots_sensors_port = atoi(args2); webots_sensors_port = atoi(args2);
@ -157,7 +158,6 @@ Webots::Webots(const char *frame_str) :
bool Webots::parse_sensors(const char *json) bool Webots::parse_sensors(const char *json)
{ {
//printf("%s\n", json); //printf("%s\n", json);
///*
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) { for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
struct keytable &key = keytable[i]; struct keytable &key = keytable[i];
//printf("search %s/%s\n", key.section, key.key); //printf("search %s/%s\n", key.section, key.key);
@ -220,7 +220,6 @@ bool Webots::parse_sensors(const char *json)
v->length = n+1; v->length = n+1;
} }
if (sscanf(p, "[%f, %f, %f]", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) { if (sscanf(p, "[%f, %f, %f]", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) {
//printf("Failed to parse Vector3f for %s %s/%s[%u]\n",p, key.section, key.key, n);
//printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n); //printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n);
return false; return false;
} }
@ -278,7 +277,7 @@ bool Webots::parse_sensors(const char *json)
} }
} }
} }
// */
socket_frame_counter++; socket_frame_counter++;
return true; return true;
@ -305,6 +304,8 @@ bool Webots::connect_sockets(void)
sim_sock = nullptr; sim_sock = nullptr;
return false; return false;
} }
// wait for Webots packets
sim_sock->set_blocking(true);
sim_sock->reuseaddress(); sim_sock->reuseaddress();
printf("Sensors connected\n"); printf("Sensors connected\n");
} }
@ -431,8 +432,8 @@ void Webots::output_quad(const struct sitl_input &input)
*/ */
void Webots::output_pwm(const struct sitl_input &input) void Webots::output_pwm(const struct sitl_input &input)
{ {
char buf[200]; char buf[2000];
const int len = snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u], \"wnd\": [%f, %f, %f, %f]}\n", const int len = snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0], \"wnd\": [%3.3f, %f, %3.3f, %3.3f]}\n",
input.servos[0], input.servos[1], input.servos[2], input.servos[3], input.servos[0], input.servos[1], input.servos[2], input.servos[3],
input.servos[4], input.servos[5], input.servos[6], input.servos[7], input.servos[4], input.servos[5], input.servos[6], input.servos[7],
input.servos[8], input.servos[9], input.servos[10], input.servos[11], input.servos[8], input.servos[9], input.servos[10], input.servos[11],
@ -467,73 +468,37 @@ void Webots::output (const struct sitl_input &input)
*/ */
void Webots::update(const struct sitl_input &input) void Webots::update(const struct sitl_input &input)
{ {
static bool first = true;
if (!connect_sockets()) { if (!connect_sockets()) {
return; return;
} }
last_state = state;
const bool valid = sensors_receive(); const bool valid = sensors_receive();
if (valid) { if (!valid)
// update average frame time used for extrapolation {
double dt = constrain_float(state.timestamp - last_state.timestamp, 0.001, 1.0/50);
if (average_frame_time_s < 1.0e-6) {
//if average is too small take the current dt as a good start.
average_frame_time_s = dt;
}
// this is complementry filter for updating average.
average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02;
}
// again measure dt as dt_s but with no constraints....
double dt_s = state.timestamp - last_state.timestamp;
last_state = state;
if (dt_s < 0 || dt_s > 1) {
// cope with restarting while connected
initial_time_s = time_now_us * 1.0e-6f;
return ; return ;
} }
if (dt_s < 0.00001f) { //printf("%lf %lf\n", state.timestamp, state.timestamp * 1.0e6f);
float delta_time = 0.001; // printf("state.timestamp %lf\n", state.timestamp);
// don't go past the next expected frame
if (delta_time + extrapolated_s > average_frame_time_s) {
delta_time = average_frame_time_s - extrapolated_s;
}
// check if extrapolation_s duration is longer than average_frame_time_s
if (delta_time <= 0) { //time frame from simulator
// dont extrapolate anymore untill there is valid data with long-enough dt_s frame_time_us = ((state.timestamp - last_state.timestamp) * 1.0e6f); //HERE
usleep(1000); if ((!first) && (frame_time_us ==0))
{
first = false;
printf("frame_time_us Zero \n");
output(input);
return ; return ;
} }
// extrapolated_s duration is safe. keep on extrapolation. //printf ("state.timestamp %lf last_state.timestamp %lf frame_time_us %ld\n", state.timestamp, last_state.timestamp, frame_time_us);
time_now_us += delta_time * 1.0e6; time_now_us += frame_time_us;
extrapolate_sensors(delta_time);
update_position();
//update body magnetic field from position and rotation
update_mag_field_bf();
usleep(delta_time * 1.0e6);
extrapolated_s += delta_time;
//output(input);
report_FPS();
return;
}
if (valid) if (valid)
{ {
// reset extrapolation duration.
extrapolated_s = 0;
if (initial_time_s <= 0) {
dt_s = 0.001f;
initial_time_s = state.timestamp - dt_s;
}
// convert from state variables to ardupilot conventions // convert from state variables to ardupilot conventions
dcm.from_euler(state.pose.roll, state.pose.pitch, -state.pose.yaw); dcm.from_euler(state.pose.roll, state.pose.pitch, -state.pose.yaw);
@ -564,28 +529,23 @@ void Webots::update(const struct sitl_input &input)
scanner.ranges = state.scanner.ranges; scanner.ranges = state.scanner.ranges;
update_position(); update_position();
uint64_t new_time_us = (state.timestamp - initial_time_s)*1.0e6;
if (new_time_us < time_now_us) {
uint64_t dt_us = time_now_us - new_time_us;
if (dt_us > 500000) {
// time going backwards
time_now_us = new_time_us;
}
} else {
// update SITL time with webots time.
time_now_us = new_time_us;
}
time_advance();
// update magnetic field // update magnetic field
update_mag_field_bf(); update_mag_field_bf();
time_advance();
update_wind (input); update_wind (input);
//report_FPS();
}
output(input); output(input);
report_FPS(); last_state = state;
}
} }
@ -594,16 +554,16 @@ void Webots::update(const struct sitl_input &input)
*/ */
void Webots::report_FPS(void) void Webots::report_FPS(void)
{ {
if (frame_counter++ % 1000 == 0) { // if (frame_counter++ % 1000 == 0) {
if (!is_zero(last_frame_count_s)) { // if (!is_zero(last_frame_count_s)) {
uint64_t frames = socket_frame_counter - last_socket_frame_counter; // uint64_t frames = socket_frame_counter - last_socket_frame_counter;
last_socket_frame_counter = socket_frame_counter; // last_socket_frame_counter = socket_frame_counter;
double dt = state.timestamp - last_frame_count_s; // double dt = state.timestamp - last_frame_count_s;
printf("%.2f/%.2f FPS avg=%.2f\n", // printf("%.2f/%.2f FPS avg=%.2f\n",
frames / dt, 1000 / dt, 1.0/average_frame_time_s); // frames / dt, 1000 / dt, 1.0/average_frame_time_s);
} else { // } else {
printf("Initial position %f %f %f\n", position.x, position.y, position.z); // printf("Initial position %f %f %f\n", position.x, position.y, position.z);
} // }
last_frame_count_s = state.timestamp; // last_frame_count_s = state.timestamp;
} // }
} }

5
libraries/SITL/SIM_Webots.h
View File

@ -44,6 +44,7 @@ public:
private: private:
const char *webots_ip = "127.0.0.1"; const char *webots_ip = "127.0.0.1";
// assume sensors are streamed on port 5599 // assume sensors are streamed on port 5599
@ -73,10 +74,6 @@ private:
uint32_t connect_counter; uint32_t connect_counter;
double initial_time_s;
double extrapolated_s;
double average_frame_time_s;
uint64_t socket_frame_counter; uint64_t socket_frame_counter;
uint64_t last_socket_frame_counter; uint64_t last_socket_frame_counter;
uint64_t frame_counter; uint64_t frame_counter;