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

148
libraries/SITL/SIM_Webots.cpp
View File

@ -83,9 +83,9 @@ static const struct {
Webots::Webots(const char *frame_str) :
Aircraft(frame_str)
{
use_time_sync = true;
rate_hz = 4000;
use_time_sync = false;
use_smoothing = false;
last_state.timestamp = 0.0l;
char *saveptr = nullptr;
char *s = strdup(frame_str);
char *frame_option = strtok_r(s, ":", &saveptr);
@ -97,6 +97,7 @@ Webots::Webots(const char *frame_str) :
*/
if (args1) {
webots_ip = args1;
printf("Simulation Port %s\n",args1);
}
if (args2) {
webots_sensors_port = atoi(args2);
@ -157,8 +158,7 @@ Webots::Webots(const char *frame_str) :
bool Webots::parse_sensors(const char *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];
//printf("search %s/%s\n", key.section, key.key);
// look for section header
@ -220,7 +220,6 @@ bool Webots::parse_sensors(const char *json)
v->length = n+1;
}
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);
return false;
}
@ -278,7 +277,7 @@ bool Webots::parse_sensors(const char *json)
}
}
}
// */
socket_frame_counter++;
return true;
@ -305,6 +304,8 @@ bool Webots::connect_sockets(void)
sim_sock = nullptr;
return false;
}
// wait for Webots packets
sim_sock->set_blocking(true);
sim_sock->reuseaddress();
printf("Sensors connected\n");
}
@ -431,21 +432,21 @@ void Webots::output_quad(const struct sitl_input &input)
*/
void Webots::output_pwm(const struct sitl_input &input)
{
char buf[200];
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",
char buf[2000];
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[4], input.servos[5], input.servos[6], input.servos[7],
input.servos[8], input.servos[9], input.servos[10], input.servos[11],
input.servos[12], input.servos[13], input.servos[14], input.servos[15],
input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
buf[len ] = 0;
buf[len] = 0;
sim_sock->send(buf, len);
}
void Webots::output (const struct sitl_input &input)
{
switch (output_type) {
case OUTPUT_ROVER:
output_rover(input);
@ -467,73 +468,37 @@ void Webots::output (const struct sitl_input &input)
*/
void Webots::update(const struct sitl_input &input)
{
static bool first = true;
if (!connect_sockets()) {
return;
}
last_state = state;
const bool valid = sensors_receive();
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;
if (!valid)
{
return ;
}
//printf("%lf %lf\n", state.timestamp, state.timestamp * 1.0e6f);
// printf("state.timestamp %lf\n", state.timestamp);
// 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;
//time frame from simulator
frame_time_us = ((state.timestamp - last_state.timestamp) * 1.0e6f); //HERE
if ((!first) && (frame_time_us ==0))
{
first = false;
printf("frame_time_us Zero \n");
output(input);
return ;
}
if (dt_s < 0.00001f) {
float delta_time = 0.001;
// 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) {
// dont extrapolate anymore untill there is valid data with long-enough dt_s
usleep(1000);
return;
}
// extrapolated_s duration is safe. keep on extrapolation.
time_now_us += delta_time * 1.0e6;
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;
}
//printf ("state.timestamp %lf last_state.timestamp %lf frame_time_us %ld\n", state.timestamp, last_state.timestamp, frame_time_us);
time_now_us += frame_time_us;
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
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;
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_mag_field_bf();
time_advance();
update_wind (input);
output(input);
report_FPS();
//report_FPS();
}
output(input);
last_state = state;
}
@ -594,16 +554,16 @@ void Webots::update(const struct sitl_input &input)
*/
void Webots::report_FPS(void)
{
if (frame_counter++ % 1000 == 0) {
if (!is_zero(last_frame_count_s)) {
uint64_t frames = socket_frame_counter - last_socket_frame_counter;
last_socket_frame_counter = socket_frame_counter;
double dt = state.timestamp - last_frame_count_s;
printf("%.2f/%.2f FPS avg=%.2f\n",
frames / dt, 1000 / dt, 1.0/average_frame_time_s);
} else {
printf("Initial position %f %f %f\n", position.x, position.y, position.z);
}
last_frame_count_s = state.timestamp;
}
// if (frame_counter++ % 1000 == 0) {
// if (!is_zero(last_frame_count_s)) {
// uint64_t frames = socket_frame_counter - last_socket_frame_counter;
// last_socket_frame_counter = socket_frame_counter;
// double dt = state.timestamp - last_frame_count_s;
// printf("%.2f/%.2f FPS avg=%.2f\n",
// frames / dt, 1000 / dt, 1.0/average_frame_time_s);
// } else {
// printf("Initial position %f %f %f\n", position.x, position.y, position.z);
// }
// last_frame_count_s = state.timestamp;
// }
}

5
libraries/SITL/SIM_Webots.h
View File

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