SITL: adding Tricopter model in Webots

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;

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();
 

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

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 <webots/distance_sensor.h> or
+ * <webots/differential_wheels.h>, etc.
+ */
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/types.h>
+#include <webots/robot.h>
+#include <webots/supervisor.h>
+#include <webots/emitter.h>
+#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<MOTOR_NUM;++i)
+      {
+        v[i] += 0.01;
+      }
+      break;
+
+    case 'S':
+      for (int i=0; i<MOTOR_NUM;++i)
+      {
+        v[i] -= 0.01;
+      }
+      break;
+  
+    case 'A':
+      servo_value_extra = servo_value_extra + 0.01;
+      break;
+
+    case 'D':
+      servo_value_extra = servo_value_extra - 0.01;
+      break;
+
+    
+  }
+
+  for (int i=0; i< MOTOR_NUM; ++i)
+  {
+    if (v[i] <=0) v[i]=0;
+    if (v[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; i<MOTOR_NUM; ++i)
+  {
+    wb_motor_set_position(motors[i], INFINITY);
+    wb_motor_set_velocity(motors[i], v[i]); 
+  }
+
+#ifdef DEBUG_USE_KB
+  wb_motor_set_position(servo, servo_value + servo_value_extra);
+#else
+  wb_motor_set_position(servo, servo_value);
+#endif
+  wb_motor_set_velocity(servo, 1000); 
+
+  #ifdef DEBUG_MOTORS
+  printf ("RAW    R:%f L:%f SRV:%f B:%f\n", state.motors.w, state.motors.x, state.motors.y, state.motors.z);
+  printf ("Motors R:%f L:%f SRV:%f B:%f\n", v[0], v[1], servo_value, v[2]);
+  #endif
+
+
+#ifdef WIND_SIMULATION
+  
+  double linear_speed = sqrt(linear_velocity[0] * linear_velocity[0] + linear_velocity[1] * linear_velocity[1] + linear_velocity[2] * linear_velocity[2]);
+  wind_webots_axis.w =  state.wind.w + 0.01 * linear_speed * linear_speed;
+  
+  if (northDirection[0] == 1)
+  {
+    wind_webots_axis.x =  state.wind.x - linear_velocity[0];
+    wind_webots_axis.z = -state.wind.y - linear_velocity[2];   // "-state.wind.y" as angle 90 wind is from EAST.
+    wind_webots_axis.y =  state.wind.z - linear_velocity[1];
+  }
+  else
+  { // as in pyramids and any open map street world.
+    wind_webots_axis.x =  state.wind.y  - linear_velocity[0]; // always add "linear_velocity" as there is no axis transformation here.
+    wind_webots_axis.z =  -state.wind.x - linear_velocity[2];
+    wind_webots_axis.y =  state.wind.z  - linear_velocity[1];
+  }
+
+  wb_emitter_send(emitter, &wind_webots_axis, sizeof(VECTOR4F));
+
+#endif
+}
+
+
+// data example: [my_controller_SITL] {"engines": [0.000, 0.000, 0.000, 0.000]}
+// the JSON parser is directly inspired by https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_Morse.cpp
+bool parse_controls(const char *json)
+{
+    //state.timestamp = 1.0;
+    #ifdef DEBUG_INPUT_DATA
+    printf("%s\n", json);
+    #endif
+    
+    for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) {
+        struct keytable *key;
+        key = &keytable[i];
+        //printf("search   %s/%s\n", key->section, 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;
+}

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;
+

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.c

@@ -0,0 +1,194 @@
+#include <stdio.h>
+#include <sys/time.h>
+#include <webots/supervisor.h>
+#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 );
+
+}

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.h

@@ -0,0 +1,23 @@
+
+
+#include <webots/robot.h>
+#include <webots/keyboard.h>
+#include <webots/compass.h>
+#include <webots/accelerometer.h>
+#include <webots/inertial_unit.h>
+#include <webots/gps.h>
+#include <webots/gyro.h>
+#include <webots/motor.h>
+#include <webots/camera.h>
+
+
+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);

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;
+}

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.h

@@ -0,0 +1,28 @@
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/types.h>
+
+
+#ifdef _WIN32
+#include <winsock.h>
+#else
+#include <arpa/inet.h> /* definition of inet_ntoa */
+#include <errno.h>
+#include <fcntl.h>
+#include <netdb.h>      /* definition of gethostbyname */
+#include <netinet/in.h> /* definition of struct sockaddr_in */
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <sys/time.h>
+#include <unistd.h> /* 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;

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 

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

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)"
+}