diff --git a/libraries/SITL/SIM_Webots.cpp b/libraries/SITL/SIM_Webots.cpp index 845f6aa1ca..8c264f6b2f 100644 --- a/libraries/SITL/SIM_Webots.cpp +++ b/libraries/SITL/SIM_Webots.cpp @@ -107,6 +107,8 @@ Webots::Webots(const char *frame_str) : output_type = OUTPUT_ROVER; } else if (strstr(frame_option, "-quad")) { output_type = OUTPUT_QUAD; + } else if (strstr(frame_option, "-tri")) { + output_type = OUTPUT_TRICOPTER; } else if (strstr(frame_option, "-pwm")) { output_type = OUTPUT_PWM; } else { @@ -370,6 +372,34 @@ void Webots::output_rover(const struct sitl_input &input) sim_sock->send(buf, len); } +/* + output control command assuming a 3 channels motors and 1 channel servo +*/ +void Webots::output_tricopter(const struct sitl_input &input) +{ + const float max_thrust = 1.0; + float motors[3]; + const float servo = ((input.servos[6]-1000)/1000.0f - 0.5f); + motors[0] = constrain_float(((input.servos[0]-1000)/1000.0f) * max_thrust, 0, max_thrust); + motors[1] = constrain_float(((input.servos[1]-1000)/1000.0f) * max_thrust, 0, max_thrust); + motors[2] = constrain_float(((input.servos[3]-1000)/1000.0f) * max_thrust, 0, max_thrust); + + const float &m_right = motors[0]; + const float &m_left = motors[1]; + const float &m_servo = servo ; + const float &m_back = motors[2]; + + // construct a JSON packet for motors + char buf[200]; + const int len = snprintf(buf, sizeof(buf)-1, "{\"eng\": [%.3f, %.3f, %.3f, %.3f], \"wnd\": [%f, %3.1f, %1.1f, %2.1f]}\n", + m_right, m_left, m_servo, m_back, + input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z); + //printf("\"eng\": [%.3f, %.3f, %.3f, %.3f]\n",m_right, m_left, m_servo, m_back); + buf[len] = 0; + + sim_sock->send(buf, len); +} + /* output control command assuming a 4 channel quad */ @@ -430,6 +460,9 @@ void Webots::output (const struct sitl_input &input) case OUTPUT_QUAD: output_quad(input); break; + case OUTPUT_TRICOPTER: + output_tricopter(input); + break; case OUTPUT_PWM: output_pwm(input); break; diff --git a/libraries/SITL/SIM_Webots.h b/libraries/SITL/SIM_Webots.h index c28fe5e164..dd9651d0ba 100644 --- a/libraries/SITL/SIM_Webots.h +++ b/libraries/SITL/SIM_Webots.h @@ -52,7 +52,8 @@ private: enum { OUTPUT_ROVER=1, OUTPUT_QUAD=2, - OUTPUT_PWM=3 + OUTPUT_TRICOPTER=3, + OUTPUT_PWM=4 } output_type; bool connect_sockets(void); @@ -60,6 +61,7 @@ private: bool sensors_receive(void); void output_rover(const struct sitl_input &input); void output_quad(const struct sitl_input &input); + void output_tricopter(const struct sitl_input &input); void output_pwm(const struct sitl_input &input); void report_FPS(); diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/Makefile b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/Makefile new file mode 100644 index 0000000000..19a37475bc --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/Makefile @@ -0,0 +1,76 @@ +# Copyright 1996-2019 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +### Generic Makefile.include for Webots controllers, physics plugins, robot +### window libraries, remote control libraries and other libraries +### to be used with GNU make +### +### Platforms: Windows, macOS, Linux +### Languages: C, C++ +### +### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer +### Edmund Ronald, Sergei Poskriakov +### +###----------------------------------------------------------------------------- +### +### This file is meant to be included from the Makefile files located in the +### Webots projects subdirectories. It is possible to set a number of variables +### to customize the build process, i.e., add source files, compilation flags, +### include paths, libraries, etc. These variables should be set in your local +### Makefile just before including this Makefile.include. This Makefile.include +### should never be modified. +### +### Here is a description of the variables you may set in your local Makefile: +### +### ---- C Sources ---- +### if your program uses several C source files: +### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c +### +### ---- C++ Sources ---- +### if your program uses several C++ source files: +### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++ +### +### ---- Compilation options ---- +### if special compilation flags are necessary: +### CFLAGS = -Wno-unused-result +### +### ---- Linked libraries ---- +### if your program needs additional libraries: +### INCLUDE = -I"/my_library_path/include" +### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library +### +### ---- Linking options ---- +### if special linking flags are needed: +### LFLAGS = -s +### +### ---- Webots included libraries ---- +### if you want to use the Webots C API in your C++ controller program: +### USE_C_API = true +### if you want to link with the Qt framework embedded in Webots: +### QT = core gui widgets network +### +### ---- Debug mode ---- +### if you want to display the gcc command line for compilation and link, as +### well as the rm command details used for cleaning: +### VERBOSE = 1 +### +###----------------------------------------------------------------------------- + +C_SOURCES = ardupilot_SITL_TRICOPTER.c sockets.c sensors.c +INCLUDE = -I"./" +### Do not modify: this includes Webots global Makefile.include +space := +space += +WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER new file mode 100755 index 0000000000..3d85c7f577 Binary files /dev/null and b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER differ diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c new file mode 100644 index 0000000000..ab5a11e8c7 --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c @@ -0,0 +1,520 @@ +/* + * File: ardupilot_SITL_TRICOPTER.c + * Date: 18 Aug 2019 + * Description: integration with ardupilot SITL simulation. + * Author: M.S.Hefny (HefnySco) + * Modifications: + */ + + +/* + * You may need to add include files like or + * , etc. + */ +#include +#include +#include +#include +#include +#include +#include +#include "ardupilot_SITL_TRICOPTER.h" +#include "sockets.h" +#include "sensors.h" + + + +#define MOTOR_NUM 3 + +static WbDeviceTag motors[MOTOR_NUM]; +static WbDeviceTag servo; +static WbDeviceTag gyro; +static WbDeviceTag accelerometer; +static WbDeviceTag compass; +static WbDeviceTag gps; +static WbDeviceTag camera; +static WbDeviceTag inertialUnit; +static WbDeviceTag emitter; +static WbNodeRef world_info; + +static const double *northDirection; +static double v[MOTOR_NUM]; +static double servo_value = 0; +static double servo_value_extra = 0; + +int port; + + +static int timestep; + + +#ifdef DEBUG_USE_KB +/* +// Code used tp simulae motors using keys to make sure that sensors directions and motor torques and thrusts are all correct. +// You can start this controller and use telnet instead of SITL to start the simulator. +Then you can use Keyboard to emulate motor input. +*/ +void process_keyboard () +{ + switch (wb_keyboard_get_key()) + { + case 'Q': // Q key -> up & left + v[0] = 0.0; + v[1] = 0.0; + v[2] = 0.0; + servo_value_extra = 0.0; + break; + + case 'Y': + v[0] = v[0] + 0.01; + v[1] = v[1] + 0.01; + v[2] = v[2] - 0.02; + break; + + case 'H': + v[0] = v[0] - 0.01; + v[1] = v[1] - 0.01; + v[2] = v[2] + 0.02; + break; + + case 'G': + v[0] = v[0] + 0.01; + v[1] = v[1] - 0.01; + break; + + case 'J': + v[0] = v[0] - 0.01; + v[1] = v[1] + 0.01; + break; + + case 'W': + for (int i=0; i=600) v[i]=10; + + wb_motor_set_position(motors[i], INFINITY); + wb_motor_set_velocity(motors[i], v[i]); + } + + wb_motor_set_position (servo, servo_value_extra); + wb_motor_set_velocity (servo, 100); + + + printf ("Motors Internal right:%f left:%f back:%f servo:%f\n", v[0],v[1],v[2],servo_value); + +} +#endif + + + + +/* +// apply motor thrust. +*/ +void update_controls() +{ + /* + 1 N = 101.9716213 grams force + Thrust = t1 * |omega| * omega - t2 * |omega| * V + Where t1 and t2 are the constants specified in the thrustConstants field, + omega is the motor angular velocity + and V is the component of the linear velocity of the center of thrust along the shaft axis. + + if Vehicle mass = 1 Kg. and we want omega = 1.0 to hover + then (mass / 0.10197) / (4 motors) = t1 + + LINEAR_THRUST + we also want throttle to be linear with thrust so we use sqrt to calculate omega from input. + */ + static float factor = 1.0f; + static float offset = 0.0f; + static float v[MOTOR_NUM]; + +#ifdef LINEAR_THRUST + v[0] = sqrt(state.motors.w ) * factor + offset; + v[1] = sqrt(state.motors.x ) * factor + offset; + v[2] = sqrt(state.motors.z ) * factor + offset; +#else + v[0] = (state.motors.w ) * factor + offset; + v[1] = (state.motors.x ) * factor + offset; + v[2] = (state.motors.z ) * factor + offset; +#endif + + servo_value = -state.motors.y ; + + for ( int i=0; isection, key->key); + // look for section header + const char *p = strstr(json, key->section); + if (!p) { + // we don't have this sensor + continue; + } + p += strlen(key->section)+1; + + // find key inside section + p = strstr(p, key->key); + if (!p) { + printf("Failed to find key %s/%s\n", key->section, key->key); + return false; + } + + p += strlen(key->key)+3; + + switch (key->type) + { + case DATA_FLOAT: + *((float *)key->ptr) = atof(p); + #ifdef DEBUG_INPUT_DATA + printf("GOT %s/%s\n", key->section, key->key); + #endif + break; + + case DATA_DOUBLE: + *((double *)key->ptr) = atof(p); + #ifdef DEBUG_INPUT_DATA + printf("GOT %s/%s\n", key->section, key->key); + #endif + break; + + case DATA_VECTOR4F: { + VECTOR4F *v = (VECTOR4F *)key->ptr; + if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) { + printf("Failed to parse Vector3f for %s %s/%s\n",p, key->section, key->key); + return false; + } + else + { + #ifdef DEBUG_INPUT_DATA + printf("GOT %s/%s\n[%f, %f, %f, %f]\n ", key->section, key->key,v->w,v->x,v->y,v->z); + #endif + } + + break; + } + } + } + return true; +} + + +void run () +{ + + char send_buf[1000]; //1000 just a safe margin + char command_buffer[200]; + fd_set rfds; + while (wb_robot_step(timestep) != -1) + { + #ifdef DEBUG_USE_KB + process_keyboard(); + #endif + + if (fd == 0) + { + // if no socket wait till you get a socket + fd = socket_accept(sfd); + if (fd > 0) + socket_set_non_blocking(fd); + else if (fd < 0) + break; + } + + getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit); + + #ifdef DEBUG_SENSORS + printf("%s\n",send_buf); + #endif + + if (write(fd,send_buf,strlen(send_buf)) <= 0) + { + printf ("Send Data Error\n"); + } + + if (fd) + { + FD_ZERO(&rfds); + FD_SET(fd, &rfds); + struct timeval tv; + tv.tv_sec = 0.05; + tv.tv_usec = 0; + int number = select(fd + 1, &rfds, NULL, NULL, &tv); + if (number != 0) + { + // there is a valid connection + + int n = recv(fd, (char *)command_buffer, 200, 0); + if (n < 0) { + #ifdef _WIN32 + int e = WSAGetLastError(); + if (e == WSAECONNABORTED) + fprintf(stderr, "Connection aborted.\n"); + else if (e == WSAECONNRESET) + fprintf(stderr, "Connection reset.\n"); + else + fprintf(stderr, "Error reading from socket: %d.\n", e); + #else + if (errno) + fprintf(stderr, "Error reading from socket: %d.\n", errno); + #endif + break; + } + if (n==0) + { + break; + } + if (command_buffer[0] == 'e') + { + break; + } + if (n > 0) + { + + //printf("Received %d bytes:\n", n); + command_buffer[n] = 0; + parse_controls (command_buffer); + update_controls(); + + + } + } + + } + } + + socket_cleanup(); +} + + +void initialize (int argc, char *argv[]) +{ + + fd_set rfds; + + port = 5599; // default port + for (int i = 0; i < argc; ++i) + { + if (strcmp (argv[i],"-p")==0) + { + if (argc > i+1 ) + { + port = atoi (argv[i+1]); + } + } + } + + + sfd = create_socket_server(port); + + /* necessary to initialize webots stuff */ + wb_robot_init(); + + // Get WorldInfo Node. + WbNodeRef root, node; + WbFieldRef children, field; + int n, i; + root = wb_supervisor_node_get_root(); + children = wb_supervisor_node_get_field(root, "children"); + n = wb_supervisor_field_get_count(children); + printf("This world contains %d nodes:\n", n); + for (i = 0; i < n; i++) { + node = wb_supervisor_field_get_mf_node(children, i); + if (wb_supervisor_node_get_type(node) == WB_NODE_WORLD_INFO) + { + world_info = node; + break; + } + } + + printf("\n"); + node = wb_supervisor_field_get_mf_node(children, 0); + field = wb_supervisor_node_get_field(node, "northDirection"); + northDirection = wb_supervisor_field_get_sf_vec3f(field); + + if (northDirection[0] == 1) + { + printf ("Axis Default Directions"); + } + + printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]); + + + // get Self Node + self_node = wb_supervisor_node_get_self(); + + + // keybaard + timestep = (int)wb_robot_get_basic_time_step(); + wb_keyboard_enable(timestep); + + + + // inertialUnit + inertialUnit = wb_robot_get_device("inertial_unit"); + wb_inertial_unit_enable(inertialUnit, timestep); + + // gyro + gyro = wb_robot_get_device("gyro1"); + wb_gyro_enable(gyro, timestep); + + // accelerometer + accelerometer = wb_robot_get_device("accelerometer1"); + wb_accelerometer_enable(accelerometer, timestep); + + // compass + compass = wb_robot_get_device("compass1"); + wb_compass_enable(compass, timestep); + + // gps + gps = wb_robot_get_device("gps1"); + wb_gps_enable(gps, timestep); + + // camera + camera = wb_robot_get_device("camera1"); + wb_camera_enable(camera, timestep); + + #ifdef WIND_SIMULATION + // emitter + emitter = wb_robot_get_device("emitter_plugin"); + #endif + + const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3"}; + + // get motor device tags + for (i = 0; i < MOTOR_NUM; i++) { + motors[i] = wb_robot_get_device(MOTOR_NAMES[i]); + v[i] = 0.0f; + //assert(motors[i]); + } + + servo = wb_robot_get_device("servo_tail"); + + FD_ZERO(&rfds); + FD_SET(sfd, &rfds); +} +/* + * This is the main program. + * The arguments of the main function can be specified by the + * "controllerArgs" field of the Robot node + */ +int main(int argc, char **argv) +{ + + + + initialize( argc, argv); + + /* + * You should declare here WbDeviceTag variables for storing + * robot devices like this: + * WbDeviceTag my_sensor = wb_robot_get_device("my_sensor"); + * WbDeviceTag my_actuator = wb_robot_get_device("my_actuator"); + */ + + /* main loop + * Perform simulation steps of TIME_STEP milliseconds + * and leave the loop when the simulation is over + */ + + + /* + * Read the sensors : + * Enter here functions to read sensor data, like: + * double val = wb_distance_sensor_get_value(my_sensor); + */ + + /* Process sensor data here */ + + /* + * Enter here functions to send actuator commands, like: + * wb_differential_wheels_set_speed(100.0,100.0); + */ + run(); + + + /* Enter your cleanup code here */ + + /* This is necessary to cleanup webots resources */ + wb_robot_cleanup(); + + return 0; +} diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.h b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.h new file mode 100644 index 0000000000..4027e190d8 --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.h @@ -0,0 +1,61 @@ +//#define DEBUG_MOTORS +// #define DEBUG_SENSORS +//#define DEBUG_USE_KB +//#define DEBUG_INPUT_DATA +// #define LINEAR_THRUST +//#define WIND_SIMULATION + + +#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) + + +enum data_type { + DATA_FLOAT, + DATA_DOUBLE, + DATA_VECTOR4F, + }; + +struct vector4f +{ + float w,x,y,z; +}; + + +typedef struct vector4f VECTOR4F; + +struct { + double timestamp; + VECTOR4F motors; + VECTOR4F wind; + /* + struct { + float speed; // m/s + float direction; // degrees 0..360 + float turbulence; + float dir_z; //degrees -90..90 + } wind; + */ + } state, last_state; + + + +// table to aid parsing of JSON sensor data +struct keytable { + const char *section; + const char *key; + void *ptr; + enum data_type type; + +} keytable[2] = { + //{ "", "timestamp", &state.timestamp, DATA_DOUBLE }, + { "", "eng", &state.motors, DATA_VECTOR4F }, /*right,left,servo,back*/ + { "", "wnd", &state.wind, DATA_VECTOR4F } /*speed,x,y,z in Ardupilot AXIS*/ +}; + + +/* + w: wind speed + x , y, z: wind direction. +*/ +VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis; + diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.c new file mode 100644 index 0000000000..742979754c --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.c @@ -0,0 +1,194 @@ +#include +#include +#include +#include "sensors.h" + +#define M_PI 3.14159265358979323846 +#define M_PI2 6.28318530718 + + +/* +https://discuss.ardupilot.org/t/copter-x-y-z-which-is-which/6823/2 + +Copy pasted what’s important: +NED Coordinate System: + +The x axis is aligned with the vector to the north pole (tangent to meridians). +The y axis points to the east side (tangent to parallels) +The z axis points to the center of the earth +There is also Body Fixed Frame: +Body Fixed Frame (Attached to the aircraft) + +The x axis points in forward (defined by geometry and not by movement) direction. (= roll axis) +The y axis points to the right (geometrically) (= pitch axis) +The z axis points downwards (geometrically) (= yaw axis) +In order to convert from Body Frame to NED what you need to call this function: + +copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); + + + + + */ +/* + returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}} +*/ +void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf) +{ + const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit); + if (northDirection[0] == 1) + { + sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]); + } + else + { + sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]); + } + + return ; +} + +/* + returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875] +*/ +void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf) +{ + const double *north3D = wb_compass_get_values(compass); + if (northDirection[0] == 1) + { + sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]); + } + else + { + sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]); + } + + return ; +} + + + +/* + returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426}, +*/ +void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf) +{ + + const double *north3D = wb_gps_get_values(gps); + if (northDirection[0] == 1) + { + sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]); + } + else + { + sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]); + } + + + return ; +} + +/* + returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039] +*/ +void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf) +{ + //SHOULD BE CORRECT + const double *a = wb_accelerometer_get_values(accelerometer); + if (northDirection[0] == 1) + { + sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]); + } + else + { + sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]); + } + + + //sprintf(buf,"[0.0, 0.0, 0.0]"); + + return ; +} + + +/* + returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09] +*/ +void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf) +{ + + const double *g = wb_gyro_get_values(gyro); + if (northDirection[0] == 1) + { + sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]); + } + else + { + sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]); + } + + return ; +} + + +void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf) +{ + linear_velocity = wb_supervisor_node_get_velocity (nodeRef); + if (linear_velocity != NULL) + { + if (northDirection[0] == 1) + { + sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]); + } + else + { + sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]); + } + } + +} + +void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit) +{ + +/* +{"timestamp": 1563544049.2840538, + "vehicle.imu": {"timestamp": 1563544049.2673872, + "angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09], + "linear_acceleration": [0.005077465437352657, 0.22471386194229126, 9.807389259338379], + "magnetic_field": [23088.71484375, 3875.498046875, -53204.48046875]}, + "vehicle.gps": { + "timestamp": 1563544049.2673872, + "x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635}, + "vehicle.velocity": {"timestamp": 1563544049.2673872, + "linear_velocity": [-3.12359499377024e-10, -1.3824124955874595e-08, -6.033386625858839e-07], + "angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09], + "world_linear_velocity": [0.0, 0.0, -6.034970851942489e-07]}, + "vehicle.pose": {"timestamp": 1563544049.2673872, + "x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635, "yaw": 8.899446402210742e-05, "pitch": -0.0005175824626348913, "roll": 0.022908702492713928} + } +*/ + + + static char compass_buf [150]; + static char acc_buf [150]; + static char gyro_buf [150]; + static char gps_buf [150]; + static char inertial_buf [150]; + static char linear_velocity_buf [150]; + + char szTime[21]; + double time = wb_robot_get_time(); // current simulation time in [s] + sprintf(szTime,"%f", time); + + getGyro(gyro, northDirection, gyro_buf); + getAcc(accelerometer, northDirection, acc_buf); + getCompass(compass, northDirection, compass_buf); + getGPS(gps, northDirection, gps_buf); + getInertia (inertial_unit, northDirection, inertial_buf); + getLinearVelocity(self_node, northDirection, linear_velocity_buf); + + sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n" + , szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf ); + +} \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.h b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.h new file mode 100644 index 0000000000..4a4a6575f1 --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.h @@ -0,0 +1,23 @@ + + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +WbNodeRef self_node; +double *linear_velocity; + +void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf); +void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf); +void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf); +void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf); +void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf); +void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf); +void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit); \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.c new file mode 100644 index 0000000000..d81338e20f --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.c @@ -0,0 +1,112 @@ + +#include "sockets.h" + + +bool socket_init() { +#ifdef _WIN32 /* initialize the socket API */ + WSADATA info; + if (WSAStartup(MAKEWORD(1, 1), &info) != 0) { + fprintf(stderr, "Cannot initialize Winsock.\n"); + return false; + } +#endif + return true; +} + +bool socket_set_non_blocking(int fd) { + if (fd < 0) + return false; +#ifdef _WIN32 + unsigned long mode = 1; + return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false; +#else + int flags = fcntl(fd, F_GETFL, 0) | O_NONBLOCK; + return (fcntl(fd, F_SETFL, flags) == 0) ? true : false; +#endif +} + +int socket_accept(int server_fd) { + int cfd; + struct sockaddr_in client; + struct hostent *client_info; +#ifndef _WIN32 + socklen_t asize; +#else + int asize; +#endif + asize = sizeof(struct sockaddr_in); + cfd = accept(server_fd, (struct sockaddr *)&client, &asize); + if (cfd == -1) { +#ifdef _WIN32 + int e = WSAGetLastError(); + if (e == WSAEWOULDBLOCK) + return 0; + fprintf(stderr, "Accept error: %d.\n", e); +#else + if (errno == EWOULDBLOCK) + return 0; + fprintf(stderr, "Accept error: %d.\n", errno); +#endif + return -1; + } + client_info = gethostbyname((char *)inet_ntoa(client.sin_addr)); + printf("Accepted connection from: %s.\n", client_info->h_name); + return cfd; +} + +bool socket_close(int fd) { +#ifdef _WIN32 + return (closesocket(fd) == 0) ? true : false; +#else + return (close(fd) == 0) ? true : false; +#endif +} + +bool socket_cleanup() { +#ifdef _WIN32 + return (WSACleanup() == 0) ? true : false; +#else + return true; +#endif +} + + + + +/* + Creates a socket and bind it to port. + */ +int create_socket_server(int port) { + int sfd, rc; + struct sockaddr_in address; + if (!socket_init()) + { + fprintf (stderr, "socket_init failed"); + return -1; + } + sfd = socket(AF_INET, SOCK_STREAM, 0); + if (sfd == -1) { + fprintf(stderr, "Cannot create socket.\n"); + return -1; + } + int one = 1; + setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + memset(&address, 0, sizeof(struct sockaddr_in)); + address.sin_family = AF_INET; + address.sin_port = htons((unsigned short)port); + address.sin_addr.s_addr = INADDR_ANY; + rc = bind(sfd, (struct sockaddr *)&address, sizeof(struct sockaddr)); + if (rc == -1) { + fprintf(stderr, "Cannot bind port %d.\n", port); + socket_close(sfd); + return -1; + } + if (listen(sfd, 1) == -1) { + fprintf(stderr, "Cannot listen for connections.\n"); + socket_close(sfd); + return -1; + } + + printf ("socket initialized at port %d.\n", port); + return sfd; +} \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.h b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.h new file mode 100644 index 0000000000..8224c73926 --- /dev/null +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.h @@ -0,0 +1,28 @@ + +#include +#include +#include +#include + + +#ifdef _WIN32 +#include +#else +#include /* definition of inet_ntoa */ +#include +#include +#include /* definition of gethostbyname */ +#include /* definition of struct sockaddr_in */ +#include +#include +#include +#include /* definition of close */ +#endif + +int create_socket_server(int port); +bool socket_cleanup(); +int socket_accept(int server_fd); +bool socket_set_non_blocking(int fd); + +int fd; +int sfd; diff --git a/libraries/SITL/examples/Webots/droneTricopter.sh b/libraries/SITL/examples/Webots/droneTricopter.sh new file mode 100755 index 0000000000..ac6f520ac2 --- /dev/null +++ b/libraries/SITL/examples/Webots/droneTricopter.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +# assume we start the script from the root directory +ROOTDIR=$PWD +$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-tri --add-param-file=libraries/SITL/examples/Webots/tricopter.parm diff --git a/libraries/SITL/examples/Webots/tricopter.parm b/libraries/SITL/examples/Webots/tricopter.parm new file mode 100644 index 0000000000..62f6cbf73c --- /dev/null +++ b/libraries/SITL/examples/Webots/tricopter.parm @@ -0,0 +1,13 @@ +FRAME_CLASS 7.000000 +FRAME_TYPE 0.000000 +ATC_ANG_PIT_P 1.0 +ATC_ANG_RLL_P 1.0 +ATC_ANG_YAW_P 0.5 +ATC_RAT_PIT_P 3.5 +ATC_RAT_RLL_P 3.5 +ATC_RAT_YAW_P 1.5 +ATC_RAT_YAW_I 0.03 +ATC_RAT_YAW_IMAX 0.50000 +SIM_WIND_DIR 90.0 +SIM_WIND_SPD 0.0 +SIM_WIND_TURB 0.0 \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt b/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt new file mode 100644 index 0000000000..59bff94deb --- /dev/null +++ b/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt @@ -0,0 +1,286 @@ +#VRML_SIM R2019b utf8 +WorldInfo { + gravity 0 -9.80665 0 + physics "sitl_physics_env" + basicTimeStep 2 + FPS 25 + optimalThreadCount 4 + randomSeed 52 +} +DogHouse { + translation 34.82 0.76 -24.56 + name "dog house(1)" +} +DogHouse { + translation 161.819 0.75 -152.174 + name "dog house(2)" +} +DogHouse { + translation 185.42 0.77 48.97 + name "dog house(5)" +} +Viewpoint { + orientation 0.9997750421775041 -0.014997750480821456 -0.01499775048082146 4.712163997257285 + position 0.17712971811531414 14.83048200793912 -1.4201693307676047 + follow "tricopter" +} +Background { + skyColor [ + 0.15 0.5 1 + ] + cubemap Cubemap { + } +} +Solid { + translation 36.93 0.77 -37.93 + children [ + HouseWithGarage { + } + ] +} +Solid { + translation 192.76999999999998 0 64.98 + rotation 0 1 0 -1.5707963071795863 + children [ + HouseWithGarage { + } + ] + name "solid(1)" +} +DEF DEF_VEHICLE Robot { + translation -8.233889875751989e-05 0.666500515499142 -1.3598750857814472 + rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804 + children [ + Compass { + name "compass1" + } + Camera { + translation 0 0.25 0 + name "camera1" + } + Transform { + translation -0.34 0 0 + rotation 0 1 0 1.5707959999999999 + children [ + HingeJoint { + jointParameters HingeJointParameters { + position -9.388038782122357e-12 + axis 0 0 1 + } + device [ + RotationalMotor { + name "servo_tail" + maxVelocity 50000 + maxTorque 1000 + } + ] + endPoint Solid { + translation -4.884982810326628e-15 -4.1022629410960365e-12 1.8091922030330322e-13 + rotation 2.3470124341273664e-11 1 -3.708275057969111e-10 1.5707963071795863 + children [ + Propeller { + shaftAxis 0 1 0 + thrustConstants 11.44 0 + torqueConstants 1e-05 0 + device RotationalMotor { + name "motor3" + controlPID 10.001 0 0 + maxVelocity 1000 + } + fastHelix Solid { + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 1 0 0.1 + } + } + geometry Cylinder { + height 0.002 + radius 0.02 + } + } + ] + } + slowHelix Solid { + rotation 0 1 0 2.238367478129037 + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 0 1 0.09999999999999999 + } + } + geometry Cylinder { + height 0.002 + radius 0.02 + } + } + ] + } + } + ] + name "solid(1)" + boundingObject Box { + size 0.01 0.01 0.01 + } + physics Physics { + mass 0.001 + } + } + } + ] + } + Transform { + translation 0.17 0 0.3 + children [ + Propeller { + shaftAxis 0 -1 0 + thrustConstants -11.443 0 + torqueConstants 1e-05 0 + device RotationalMotor { + name "motor1" + controlPID 10.001 0 0 + maxVelocity 1000 + } + fastHelix Solid { + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 1 0 0.1 + } + } + geometry Cylinder { + height 0.002 + radius 0.02 + } + } + ] + } + slowHelix Solid { + rotation 0 1 0 0.012993 + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 1 0 0.1 + } + } + geometry Cylinder { + height 0.002 + radius 0.02 + } + } + ] + } + } + ] + } + Transform { + translation 0.16 0 -0.3 + children [ + Propeller { + shaftAxis 0 1 0 + thrustConstants 11.443 0 + torqueConstants 1e-05 0 + device RotationalMotor { + name "motor2" + controlPID 10.001 0 0 + maxVelocity 1000 + } + fastHelix Solid { + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 1 0 0.1 + } + } + geometry Cylinder { + height 0.002 + radius 0.02 + } + } + ] + } + slowHelix Solid { + rotation 0 1 0 0.012993 + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 1 0 0.1 + } + } + geometry Cylinder { + height 0.002 + radius 0.02 + } + } + ] + } + } + ] + } + Emitter { + rotation 0 1 0 -1.5707963071795863 + name "emitter_plugin" + } + InertialUnit { + name "inertial_unit" + } + Gyro { + name "gyro1" + } + Accelerometer { + name "accelerometer1" + } + GPS { + name "gps1" + } + Solid { + children [ + Shape { + appearance Appearance { + material Material { + } + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + boundingObject Box { + size 0.1 0.1 0.1 + } + physics Physics { + mass 0.6 + } + } + ] + name "tricopter" + physics Physics { + mass 0.001 + } + controller "ardupilot_SITL_TRICOPTER" + supervisor TRUE +} +DirectionalLight { + direction 0 -1 0 +} +UnevenTerrain { + size 500 1 500 +} +HouseWithGarage { + translation 174.25 1.88 -157.5 + rotation 0 1 0 -1.5707963071795863 +} +AdvertisingBoard { + translation 0 2.35 -5.71 +} +AdvertisingBoard { + translation 84.03999999999999 2.35 -5.81 + rotation 0 1 0 -1.5707963071795863 + name "advertising board(1)" +}