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

SITL: sending wind data to Webots controller

Browse Source
This commit is contained in:
mhefny 2019-08-19 15:26:46 +02:00 committed by Andrew Tridgell
parent 8bb6a67b36
commit bfb7e3af3b
1 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
libraries/SITL/SIM_Webots.cpp
View File

@ -359,15 +359,16 @@ void Webots::output_rover(const struct sitl_input &input)
const float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f); const float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
// construct a JSON packet for v and w // construct a JSON packet for v and w
char buf[60]; char buf[200];
snprintf(buf, sizeof(buf)-1, "{\"rover\": [%f, %f]}\n", const int len = snprintf(buf, sizeof(buf)-1, "{\"rover\": [%f, %f], \"wnd\": [%f, %f, %f, %f]}\n",
motor1, motor2); motor1, motor2,
input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
//printf("rover motors m1: %f m2: %f\n", steer_angle, speed_ms); //printf("rover motors m1: %f m2: %f\n", steer_angle, speed_ms);
buf[sizeof(buf)-1] = 0; buf[len] = 0;
sim_sock->send(buf, strlen(buf)); sim_sock->send(buf, len);
} }
/* /*
@ -394,12 +395,13 @@ void Webots::output_quad(const struct sitl_input &input)
// m4: left // m4: left
// construct a JSON packet for motors // construct a JSON packet for motors
char buf[60]; char buf[200];
snprintf(buf, sizeof(buf)-1, "{\"engines\": [%f, %f, %f, %f]}\n", const int len = snprintf(buf, sizeof(buf)-1, "{\"eng\": [%.3f, %.3f, %.3f, %.3f], \"wnd\": [%f, %3.1f, %1.1f, %2.1f]}\n",
m_front, m_right, m_back, m_left); m_front, m_right, m_back, m_left,
buf[sizeof(buf)-1] = 0; input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
buf[len] = 0;
sim_sock->send(buf, strlen(buf)); sim_sock->send(buf, len);
} }
/* /*
@ -409,13 +411,14 @@ 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[200];
snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u]}\n", 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",
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],
input.servos[12], input.servos[13], input.servos[14], input.servos[15]); input.servos[12], input.servos[13], input.servos[14], input.servos[15],
buf[sizeof(buf)-1] = 0; input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
sim_sock->send(buf, strlen(buf)); buf[len ] = 0;
sim_sock->send(buf, len);
} }
@ -554,6 +557,7 @@ void Webots::update(const struct sitl_input &input)
// update magnetic field // update magnetic field
update_mag_field_bf(); update_mag_field_bf();
update_wind (input);
output(input); output(input);
report_FPS(); report_FPS();