diff --git a/libraries/SITL/SIM_Webots.cpp b/libraries/SITL/SIM_Webots.cpp
index a5b44bb7ae..ac6c4655e9 100644
--- a/libraries/SITL/SIM_Webots.cpp
+++ b/libraries/SITL/SIM_Webots.cpp
@@ -396,37 +396,7 @@ void Webots::output_tricopter(const struct sitl_input &input)
     sim_sock->send(buf, len);
 }
 
-/*
-  output control command assuming a 4 channel quad
-*/
-void Webots::output_quad(const struct sitl_input &input)
-{
-    const float max_thrust = 1.0;
-    float motors[4];
-    for (uint8_t i=0; i<4; i++) {
-        //return a filtered servo input as a value from 0 to 1
-        //servo is assumed to be 1000 to 2000
-        motors[i] = constrain_float(((input.servos[i]-1000)/1000.0f) * max_thrust, 0, max_thrust); 
-    }
-    const float &m_right = motors[0]; 
-    const float &m_left  = motors[1]; 
-    const float &m_front = motors[2]; 
-    const float &m_back  = motors[3]; 
 
-    // quad format in Webots is:
-    // m1: front
-    // m2: right
-    // m3: back
-    // m4: left
-
-    // 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_front, m_right, m_back, m_left,
-             input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
-    buf[len] = 0;
-    sim_sock->send(buf, len);
-}
 
 /*
   output all 16 channels as PWM values. This allows for general
@@ -454,7 +424,7 @@ void Webots::output (const struct sitl_input &input)
             output_rover(input);
             break;
         case OUTPUT_QUAD:
-            output_quad(input);
+            output_pwm(input);
             break;
         case OUTPUT_TRICOPTER:
             output_tricopter(input);
@@ -481,9 +451,6 @@ void Webots::update(const struct sitl_input &input)
         return ;
     }
 
-    //printf("%lf     %lf\n", state.timestamp, state.timestamp * 1.0e6f);
-    // printf("state.timestamp %lf\n", state.timestamp);
-    
     
     //time frame from simulator
     frame_time_us = ((state.timestamp - last_state.timestamp) * 1.0e6f); //HERE
@@ -495,7 +462,6 @@ void Webots::update(const struct sitl_input &input)
         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;
     
 
diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c
index 63c8fa5504..e33a3fab89 100644
--- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c
+++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c
@@ -67,99 +67,7 @@ static int timestep;
 FILE *fptr;
 #endif
 
-#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.
-*/
-static float factor = 1.0f;
-static float offset = 0.0f;
-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;
-      v[3] = 0.0;
-      break;
-
-    case 'Y':
-      v[0] = v[0] + 0.01;
-      v[2] = v[2] - 0.01;
-      break;
-
-    case 'H':
-      v[0] = v[0] - 0.01;
-      v[2] = v[2] + 0.01;
-      break;
-
-    case 'G':
-      v[1] = v[1] + 0.01;
-      v[3] = v[3] - 0.01;
-      break;
-
-    case 'J':
-      v[1] = v[1] - 0.01;
-      v[3] = v[3] + 0.01;
-      break;
-
-    case 'E':
-      for (int i=0; i<MOTOR_NUM;++i)
-      {
-        v[i] += 0.00002;
-      }
-      break;
-
-    case 'W':
-      for (int i=0; i<MOTOR_NUM;++i)
-      {
-        v[i] += 0.0002;
-      }
-      break;
-
-    case 'S':
-      for (int i=0; i<MOTOR_NUM;++i)
-      {
-        v[i] -= 0.0002;
-      }
-      break;
-  
-    case 'A':
-      v[1] = v[1] + 0.001;
-      v[3] = v[3] + 0.001;
-      v[0] = v[0] - 0.001;
-      v[2] = v[2] - 0.001;
-      break;
-
-    case 'D':
-      v[1] = v[1] - 0.001;
-      v[3] = v[3] - 0.001;
-      v[0] = v[0] + 0.001;
-      v[2] = v[2] + 0.001;
-      break;
-
-    
-  }
-
-  for (int i=0; i< MOTOR_NUM; ++i)
-  {
-    
-    wb_motor_set_position(motors[i], INFINITY);
-    wb_motor_set_velocity(motors[i], factor* v[i] + offset); 
-
-    
-  }
-  
-  printf ("Motors Internal %f %f %f %f\n", v[0],v[1],v[2],v[3]);
-  
-}
-#endif
-
-
-/*
+/**
 // apply motor thrust.
 */
 void update_controls()
@@ -178,15 +86,9 @@ void update_controls()
       we also want throttle to be linear with thrust so we use sqrt to calculate omega from input.
       Check this doc: https://docs.google.com/spreadsheets/d/1eR4Fb6cgaTb-BHUKJbhAXPzyX0ZLtUcEE3EY-wQYvM8/edit?usp=sharing
    */
-#ifndef DEBUG_USE_KB
-  //static float factor = 2.1f;
-  //static float offset = 0.0f;
-  
-  
-  //static float factor = 1.0f;
   static float offset = 0.0f;
   
-  static float v[4];
+  static float motor_value[4];
   // pls check https://docs.google.com/spreadsheets/d/1eR4Fb6cgaTb-BHUKJbhAXPzyX0ZLtUcEE3EY-wQYvM8/edit?usp=sharing
   static float factorDyn[11] = {
             3.6f, // 0.0
@@ -204,33 +106,24 @@ void update_controls()
   //#define LINEAR_THRUST
 
 
+// SCALE SERVO SIGNALS from 1000-2000
+for (int i=0;i<4;++i) {
+  state.motors.v[i] = (state.motors.v[i] - 1000.0f) * 0.001f;
+}
 
-#ifdef LINEAR_THRUST
-  v[0] = sqrt(state.motors.w) * factor + offset;
-  v[1] = sqrt(state.motors.x) * factor + offset;
-  v[2] = sqrt(state.motors.y) * factor + offset;
-  v[3] = sqrt(state.motors.z) * factor + offset;
-#else  
-  v[0] = (state.motors.w ) * factorDyn[(int)(10 * state.motors.w)] + offset;
-  v[1] = (state.motors.x ) * factorDyn[(int)(10 * state.motors.x)]  + offset;
-  v[2] = (state.motors.y ) * factorDyn[(int)(10 * state.motors.y)]  + offset;
-  v[3] = (state.motors.z ) * factorDyn[(int)(10 * state.motors.z)]  + offset;
-#endif //LINEAR_THRUST
 
-  for ( int i=0; i<4; ++i)
-  {
-    wb_motor_set_position(motors[i], INFINITY);
-    wb_motor_set_velocity(motors[i], v[i]); 
-  }
+motor_value[0] = (state.motors.v[2]) * factorDyn[10 * (int)(state.motors.v[2])]  + offset;
+motor_value[1] = (state.motors.v[0]) * factorDyn[10 * (int)(state.motors.v[0])]  + offset;
+motor_value[2] = (state.motors.v[3]) * factorDyn[10 * (int)(state.motors.v[3])]  + offset;
+motor_value[3] = (state.motors.v[1]) * factorDyn[10 * (int)(state.motors.v[1])]  + offset;
+
+for (int i=0; i<4; ++i)
+{
+  wb_motor_set_position(motors[i], INFINITY);
+  wb_motor_set_velocity(motors[i], motor_value[i]); 
+}
 
-  #ifdef DEBUG_MOTORS
-  printf ("RAW    F:%f  R:%f  B:%f  L:%f\n", state.motors.w, state.motors.x, state.motors.y, state.motors.z);
-  printf ("Factors    F:%f  \n", factorDyn[(int)(10 * state.motors.w)]);
-  printf ("Motors F:%f  R:%f  B:%f  L:%f\n", v[0], v[1], v[2], v[3]);
   
-  #endif //DEBUG_MOTORS
-
-#endif //DEBUG_USE_KB
 
   #ifdef WIND_SIMULATION
   /*
@@ -277,7 +170,7 @@ bool parse_controls(const char *json)
         // look for section header 
         const char *p = strstr(json, key->section);
         if (!p) {
-            // we don't have this sensor
+            // we don't have this section
             continue;
         }
         p += strlen(key->section)+1;
@@ -308,20 +201,36 @@ bool parse_controls(const char *json)
               break;
 
           case DATA_VECTOR4F: {
-              VECTOR4F *v = (VECTOR4F *)key->ptr;
+             VECTOR4F *v = (VECTOR4F *)key->ptr;
               if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) {
                   fprintf(stderr,"Failed to parse Vector3f for %s %s/%s\n",p,  key->section, key->key);
                   return false;
               }
-              else
-              {
+              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;
+            break;
+            }
+
+          case DATA_VECTOR16F: {
+              VECTOR16F *v = (VECTOR16F *)key->ptr;
+              if (sscanf(p, "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", &(v->v[0]), &(v->v[1]), &(v->v[2]), &(v->v[3])
+                  , &(v->v[4]), &(v->v[5]), &(v->v[6]), &(v->v[7])
+                  , &(v->v[8]), &(v->v[9]), &(v->v[10]), &(v->v[11])
+                  , &(v->v[12]), &(v->v[13]), &(v->v[14]), &(v->v[15])
+                ) != 16) {
+                  printf("Failed to parse DATA_VECTOR16F 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, (float)v->v[0], (float)v->v[1], (float)v->v[2],  (float)v->v[3]);
+                  #endif
+              }
+            break;
+            }
         }
     }
     return true;
@@ -329,9 +238,8 @@ bool parse_controls(const char *json)
 
 void run ()
 {
-
     char send_buf[1000]; 
-    char command_buffer[1000];
+    char command_buffer[2020];
     fd_set rfds;
     
     // calculate initial sensor values.
@@ -339,10 +247,6 @@ void run ()
     
     while (true) 
     {
-        #ifdef DEBUG_USE_KB
-        process_keyboard();
-        #endif
-
         if (fd == 0) 
         {
           // if no socket wait till you get a socket
@@ -418,14 +322,12 @@ void run ()
           
         }
     }
-    
     socket_cleanup();
 }
 
 
 bool initialize (int argc, char *argv[])
 {
-  
   fd_set rfds;
   #ifdef DEBUG_SENSORS
   fptr = fopen ("/tmp/log.txt","w");
@@ -468,12 +370,6 @@ bool initialize (int argc, char *argv[])
   printf("timestep_scale: %f \n", timestep_scale);
   
   
-  // keybaard
-  #ifdef DEBUG_USE_KB
-  wb_keyboard_enable(timestep);
-  #endif
-
-
   // inertialUnit
   inertialUnit = wb_robot_get_device("inertial_unit");
   wb_inertial_unit_enable(inertialUnit, timestep);
diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h
index 5365556539..d1e6002cee 100644
--- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h
+++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h
@@ -1,7 +1,7 @@
-//#define DEBUG_MOTORS 
+// #define DEBUG_MOTORS 
 // #define DEBUG_WIND    
 // #define DEBUG_SENSORS   
-//#define DEBUG_USE_KB 
+// #define DEBUG_USE_KB 
 // #define DEBUG_INPUT_DATA
 // #define LINEAR_THRUST
 // #define DEBUG_SOCKETS
@@ -21,6 +21,7 @@ enum data_type {
         DATA_FLOAT,
         DATA_DOUBLE,
         DATA_VECTOR4F,
+        DATA_VECTOR16F,
     };
 
 struct vector4f 
@@ -30,9 +31,16 @@ struct vector4f
 
 typedef struct vector4f VECTOR4F;
 
+struct vector16f 
+{
+    float v[16];
+};
+
+typedef struct vector16f VECTOR16F;
+
 struct {
         double timestamp;
-        VECTOR4F motors;
+        VECTOR16F motors;
         VECTOR4F wind; 
         /*
          struct {
@@ -54,9 +62,9 @@ struct keytable {
         enum data_type type;
 
 } keytable[2] = {
-        //{ "", "timestamp", &state.timestamp, DATA_DOUBLE },
-        { "", "eng",    &state.motors, DATA_VECTOR4F },
+        { "", "pwm",    &state.motors, DATA_VECTOR16F },
         { "", "wnd",    &state.wind, DATA_VECTOR4F }
+        
 };
 
 /*
diff --git a/libraries/SITL/examples/Webots/quadPlus.parm b/libraries/SITL/examples/Webots/quadPlus.parm
index d39faa6e64..c1aed755ac 100644
--- a/libraries/SITL/examples/Webots/quadPlus.parm
+++ b/libraries/SITL/examples/Webots/quadPlus.parm
@@ -3,12 +3,12 @@ FRAME_CLASS      1.000000
 FRAME_TYPE       0.000000
 ATC_ANG_PIT_P    0.5
 ATC_ANG_RLL_P    0.5
-ATC_ANG_YAW_P    3.50000
+ATC_ANG_YAW_P    0.50000
 ATC_RAT_PIT_P    3.5
 ATC_RAT_RLL_P    3.5
 ATC_RAT_PIT_D    0.03
 ATC_RAT_RLL_D    0.03  
-ATC_RAT_YAW_P    2.5 
+ATC_RAT_YAW_P    1.5 
 ATC_RAT_PIT_I    0.030000
 ATC_RAT_RLL_I    0.0300000
 ATC_RAT_PIT_D    0.003
diff --git a/libraries/SITL/examples/Webots/quadPlus2.parm b/libraries/SITL/examples/Webots/quadPlus2.parm
index 1238349668..c99a1faa90 100644
--- a/libraries/SITL/examples/Webots/quadPlus2.parm
+++ b/libraries/SITL/examples/Webots/quadPlus2.parm
@@ -3,12 +3,12 @@ FRAME_CLASS      1.000000
 FRAME_TYPE       0.000000
 ATC_ANG_PIT_P    0.5
 ATC_ANG_RLL_P    0.5
-ATC_ANG_YAW_P    3.50000
+ATC_ANG_YAW_P    0.50000
 ATC_RAT_PIT_P    3.5
 ATC_RAT_RLL_P    3.5
 ATC_RAT_PIT_D    0.03
 ATC_RAT_RLL_D    0.03  
-ATC_RAT_YAW_P    2.5 
+ATC_RAT_YAW_P    1.5 
 ATC_RAT_PIT_I    0.030000
 ATC_RAT_RLL_I    0.0300000
 ATC_RAT_PIT_D    0.003
diff --git a/libraries/SITL/examples/Webots/quadX.parm b/libraries/SITL/examples/Webots/quadX.parm
index c9bc9d9e97..5b9932d49b 100644
--- a/libraries/SITL/examples/Webots/quadX.parm
+++ b/libraries/SITL/examples/Webots/quadX.parm
@@ -3,12 +3,12 @@ FRAME_CLASS      1.000000
 FRAME_TYPE       1.000000
 ATC_ANG_PIT_P    0.5
 ATC_ANG_RLL_P    0.5
-ATC_ANG_YAW_P    3.50000
+ATC_ANG_YAW_P    0.50000
 ATC_RAT_PIT_P    3.5
 ATC_RAT_RLL_P    3.5
 ATC_RAT_PIT_D    0.03
 ATC_RAT_RLL_D    0.03  
-ATC_RAT_YAW_P    2.5 
+ATC_RAT_YAW_P    1.5 
 ATC_RAT_PIT_I    0.030000
 ATC_RAT_RLL_I    0.0300000
 ATC_RAT_PIT_D    0.003
diff --git a/libraries/SITL/examples/Webots/quadX2.parm b/libraries/SITL/examples/Webots/quadX2.parm
index aae6a9662e..b9805cf3db 100644
--- a/libraries/SITL/examples/Webots/quadX2.parm
+++ b/libraries/SITL/examples/Webots/quadX2.parm
@@ -3,12 +3,12 @@ FRAME_CLASS      1.000000
 FRAME_TYPE       1.000000
 ATC_ANG_PIT_P    0.5
 ATC_ANG_RLL_P    0.5
-ATC_ANG_YAW_P    3.50000
+ATC_ANG_YAW_P    0.50000
 ATC_RAT_PIT_P    3.5
 ATC_RAT_RLL_P    3.5
 ATC_RAT_PIT_D    0.03
 ATC_RAT_RLL_D    0.03  
-ATC_RAT_YAW_P    2.5 
+ATC_RAT_YAW_P    1.5 
 ATC_RAT_PIT_I    0.030000
 ATC_RAT_RLL_I    0.0300000
 ATC_RAT_PIT_D    0.003
diff --git a/libraries/SITL/examples/Webots/worlds/pyramidMap.wbt b/libraries/SITL/examples/Webots/worlds/pyramidMap.wbt
index dfb4c99e76..40cb17a874 100644
--- a/libraries/SITL/examples/Webots/worlds/pyramidMap.wbt
+++ b/libraries/SITL/examples/Webots/worlds/pyramidMap.wbt
@@ -14,8 +14,8 @@ WorldInfo {
   randomSeed 52
 }
 Viewpoint {
-  orientation 0.9105337223064622 0.3972882470335872 0.11441323923269492 5.670531893452289
-  position -81.01261461993163 254.55784447899657 285.7682724220764
+  orientation 0.8382771255106711 0.49551794864617704 0.2274937876405669 5.281073186765893
+  position -3.115785721141444 8.10790622845529 5.55883340805579
   near 3
   follow "quad_x_sitl"
   followType "Mounted Shot"
@@ -65,7 +65,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.1 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -133,7 +133,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0.01 0.1
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -200,7 +200,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -267,7 +267,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {
diff --git a/libraries/SITL/examples/Webots/worlds/pyramidMap_two_quads.wbt b/libraries/SITL/examples/Webots/worlds/pyramidMap_two_quads.wbt
index f6d086a180..88ebca354e 100644
--- a/libraries/SITL/examples/Webots/worlds/pyramidMap_two_quads.wbt
+++ b/libraries/SITL/examples/Webots/worlds/pyramidMap_two_quads.wbt
@@ -64,7 +64,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.1 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -132,7 +132,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0.01 0.1
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -199,7 +199,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -266,7 +266,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {
@@ -399,7 +399,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.1 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -467,7 +467,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0.01 0.1
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -534,7 +534,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -601,7 +601,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {
diff --git a/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt b/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt
index dc0fbf4c9a..598b9f8097 100644
--- a/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt
+++ b/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt
@@ -10,8 +10,8 @@ WorldInfo {
   randomSeed 52
 }
 Viewpoint {
-  orientation -0.5970464555560485 -0.7963994022827472 -0.0963510350315054 0.3998584260381905
-  position -5.109140421817085 5.5744493754094595 15.933129307498696
+  orientation -0.8111585917362343 -0.5680085466888879 -0.13924090613345938 0.5869628064711815
+  position -3.6530619136063534 7.264658421908483 10.832677706150493
   follow "quad_plus_sitl"
   followType "Mounted Shot"
 }
@@ -49,7 +49,8 @@ Solid {
   name "solid(1)"
 }
 DEF DEF_VEHICLE Robot {
-  translation -0.027601 0.6742971933499999 0.005031
+  translation -0.027601000000000007 0.6742873870366666 0.005030999999999996
+  rotation 0.9925492643529304 -0.0818450865332256 -0.09026261486809202 1.0340410161155028e-17
   children [
     Emitter {
       name "emitter_plugin"
@@ -88,7 +89,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.09999999999999999 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -153,7 +154,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 0.09999999999999999
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -216,7 +217,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -280,7 +281,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {
diff --git a/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt b/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt
index a866fdf170..3032e54e6a 100644
--- a/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt
+++ b/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt
@@ -9,8 +9,8 @@ WorldInfo {
   randomSeed 52
 }
 Viewpoint {
-  orientation -0.876127950041513 -0.46933997493480994 -0.11008997721976312 0.5231847446439049
-  position -3.507998466973923 7.686764746094164 13.66147356160692
+  orientation -0.943398879972064 -0.32399608276223696 -0.0708878806438778 0.45578096925392375
+  position -2.49488296075554 5.810716177630025 10.814322133903834
   follow "quad_x_sitl"
   followType "Mounted Shot"
 }
@@ -92,7 +92,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.1 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -160,7 +160,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0.01 0.1
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -227,7 +227,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -294,7 +294,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {
diff --git a/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt b/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt
index 21a736cd99..b5859ad859 100644
--- a/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt
+++ b/libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt
@@ -9,8 +9,8 @@ WorldInfo {
   randomSeed 52
 }
 Viewpoint {
-  orientation 0.9817595579497901 0.18074833080110114 0.05897636210252833 5.641450773408579
-  position -0.25435571209748176 3.4363486643124848 2.2561209069971624
+  orientation 0.6321101793339116 0.7585904605856989 0.15804187511804393 5.646432505192663
+  position -5.214083564314484 4.660863205046186 6.918610542541648
   follow "tricopter"
 }
 DogHouse {
@@ -80,8 +80,8 @@ DEF DEF_VEHICLE Robot {
             }
           ]
           endPoint Solid {
-            translation -4.884985467668676e-15 -4.1022629410928594e-12 1.8091922030330322e-13
-            rotation 3.80215598061924e-10 1 -7.275729865152367e-10 1.5707963071795863
+            translation -4.884985544693083e-15 -4.102262941092767e-12 1.8091922030330322e-13
+            rotation 3.896036368440465e-10 1 -7.369610254815084e-10 1.5707963071795863
             children [
               Propeller {
                 shaftAxis 0 1 0
diff --git a/libraries/SITL/examples/Webots/worlds/webots_two_quadX.wbt b/libraries/SITL/examples/Webots/worlds/webots_two_quadX.wbt
index 7a2446bd03..7d59599088 100644
--- a/libraries/SITL/examples/Webots/worlds/webots_two_quadX.wbt
+++ b/libraries/SITL/examples/Webots/worlds/webots_two_quadX.wbt
@@ -92,7 +92,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.1 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -160,7 +160,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0.01 0.1
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -227,7 +227,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -294,7 +294,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {
@@ -383,7 +383,7 @@ DEF DEF_VEHICLE Robot {
   customData "1"
 }
 DEF DEF_VEHICLE Robot {
-  translation -2.21265 0.694307 2.19001
+  translation -2.21265 0.904307 2.19001
   rotation 0 1 0 0.785388
   children [
     Emitter {
@@ -427,7 +427,7 @@ DEF DEF_VEHICLE Robot {
       name "inertial_unit"
     }
     Transform {
-      translation -0.1 0 0
+      translation -0.2 0 0
       rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
       children [
         Solid {
@@ -495,7 +495,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0.01 0.1
+      translation 0 0 0.2
       rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
       children [
         Solid {
@@ -562,7 +562,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0.09999999999999999 0 0
+      translation 0.2 0 0
       rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
       children [
         Solid {
@@ -629,7 +629,7 @@ DEF DEF_VEHICLE Robot {
       ]
     }
     Transform {
-      translation 0 0 -0.09999999999999999
+      translation 0 0 -0.2
       rotation 1 0 0 -1.5707963071795863
       children [
         Solid {