From 8310058c8c613015038fa84755cc34788b7d89ea Mon Sep 17 00:00:00 2001 From: mhefny Date: Sun, 15 Sep 2019 21:53:56 +0200 Subject: [PATCH] SITL: adding Tricopter model in Webots --- libraries/SITL/SIM_Webots.cpp | 33 ++ libraries/SITL/SIM_Webots.h | 4 +- .../ardupilot_SITL_TRICOPTER/Makefile | 76 +++ .../ardupilot_SITL_TRICOPTER | Bin 0 -> 18496 bytes .../ardupilot_SITL_TRICOPTER.c | 520 ++++++++++++++++++ .../ardupilot_SITL_TRICOPTER.h | 61 ++ .../ardupilot_SITL_TRICOPTER/sensors.c | 194 +++++++ .../ardupilot_SITL_TRICOPTER/sensors.h | 23 + .../ardupilot_SITL_TRICOPTER/sockets.c | 112 ++++ .../ardupilot_SITL_TRICOPTER/sockets.h | 28 + .../SITL/examples/Webots/droneTricopter.sh | 5 + libraries/SITL/examples/Webots/tricopter.parm | 13 + .../Webots/worlds/webots_tricopter.wbt | 286 ++++++++++ 13 files changed, 1354 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/Makefile create mode 100755 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.h create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.c create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.h create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.c create mode 100644 libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sockets.h create mode 100755 libraries/SITL/examples/Webots/droneTricopter.sh create mode 100644 libraries/SITL/examples/Webots/tricopter.parm create mode 100644 libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt 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 0000000000000000000000000000000000000000..3d85c7f57743d30425bc2eb0bac170e5c719e2b7 GIT binary patch literal 18496 zcmeHPdwf*YoxgbuhDT-wD-RLn8iNKjF~dthBN+&ALxW8v2#PovCNpF-WTrDWAv~(l z1j=+6OTbZUMbrh~_X27J#At=t@9*4mW{yK<+dul* zzqmU2-QV~8Ugvkt`Q3Bx+~Jww1DGzprki7*8(!>Xf7wHh>)m_xAjN5CR(VVilQ=r^e9&4 z2F8M_)(NNe6bugMK#K}J3Xk-hT+hkn1yy+bf{Ol;Ir`M`_DouZHaY|)SvqRp*Bj{k zNEu_#p}QI@z+Z0?jWzw}x4W&)N=9ae7 zwy9G}r%Wn~hRP-|QL<06b8lKm+6s-gNEMm7h-1{AchxiB7DHxZNm;VN6D`oOxTV1xbfNk?t^P*&HnG>2fwp;^wbma(a$D6wI6%+6FZu+ z9xeoNVLz>*Eci|Y(x3cg2&_LmmPP(oS#TO=wV!wv0qf8H_AL0-S?njX#A(YSUz-K* z%aWh}$YTF{S>)YW>>tn4uajBiAIXA0p2hEDS>$JAiT~$W;-N87@yR^=S?vFNmUt>) zK+QA)zB-GY9^gg&$p2Rs`%PKw{7aVp-jPM#lSRGSa{w(p2#6`fVj8vSnRYaUVYz!Zv)K{u$5NAWUd?x;L_#Fy60vFfc*O}kJ$~L;t zNXI^wH;3~3t0j<5aXp(AYSts6rY3&`*D=LYT{qv;;E(v12cx<_Qa68AQ>fWr=Uvw1 zS3I8OD?-hlsP2vE9*-s3ui(mMp5@`F$KTA+E#>gs6-J_I1~-)!BBICMj+^KYOu@&Jgt7;#H=P;{Y@cXP+vs`g3bPj z9`rVOTAG7a=P1{na8UO(dRBQ?W;jWO90`SVYV-Phehf`$g&(7lk+5{gvPUb8LAcAX zo$kPj0Lu-vThb!S=^>!xQ#*}g)&!?5`a&(u%%9J@!XNQUqr?U}Jrlm3k2K!j&@l7q zAbm}KZ*vQq4e2-={H;MBre**gCeg3wi`&O6_t7S8iV_X^?(*xH$>B(_Sr2%8jdy{; zZ1GK-=7~mq-sS-KK!d+YXKe^q_cZGvuM%$2qiDk9_uWNJo`5&lM5wRP8^Jn43Y!&7 zG`~+*qIyIRHCUvHNE5Z`5&RDm{z#-b_OK!& z3dW%^6xEllYDSWkz{*Ha_ba}pP}Hwbw;|~9_}aXb3vW|!HAH;PdJ_dL+4guS8+;M3 z@CKU|kH5jIdm*+AE0is6K!|B>P4&!Ko(W~H$*I#xsk7@+XA{fP=!CM#yuhH~VE0RZ z=^&bqzbIRr%Vn6{PEy2+PnwtSe8_?&XIY~2Tz0RLqtJi3>~9Y4mOg|bvETpU4<5#C z!T{w`R!^}H4+abAj;e(7BN(5rOyInJaS(SzdCCo(Zy7~=uA+fw2frT@bvJym1s7!C z~@EWA7ef3iwt4V4+Vu%l(*4{$rR8TdAC=im&>9^F%2 zz${yTj!5`7B;1g2-c{DxE8(Lh`92B1Ov1%=j7EBfgqPf?GLB-&k??W}7juZ3r%Jdr z*DR$<2^T|1Et-VOv#eIa<@?LU5-!G)?9@wm>Q)+fql6D4L4JfKe6RpP+a!F5gttpL zCT{B3AmQ>2V3&kDQqqdDNy5e2pytgIK2(x_Qo`kZ?-mJ{w;J0de3)dXTf&D+_+AMg zA>lm|&L7FJ)`Jp0N|HY!;g?9bA>o%wc&~(yk?=kVS0!BG^Ook}X;YAYel<+Gg ze58aIOSmfGVBw}F0NqI{G^0W zk>s~X_;nJ#O~R*2c(;U4lkmL~K3&3lB>Z{_*E)_DXmQ)Ht5ro?*RAI!d$f*S1v{1G z)Kc zcq(aTH{+ioo-S?X7RLXScq(n?X2w5CJe4%Fi}4QOkXZ-hxrxIp{8Gj$~RJzQ1 z#;+!xE|F#}<0HgV2{S7he<$%&y3BIMFC(5VouKVZENA>X#8ZheRmLA8 zo=S`9Wc&f*sic?+nYbQwcGTFn&AnR65Ka#{ZUhDj8-sjE^t{4Gmq`Z5s%=915qM0z0`A zCea@fUup&*VetXSC*AJ4lMZL$D3r2=HHoo@D2zmD5@f+i$Kt)NH%>Z+7VZK|Qhnoz zF5Zie?G!s7TKG%WHq`c{C1X1X+_A{^Z6x-j2}fal0)qZi_*zqc6wN8AhAqzITw-j=o&|yITCxRy5VbJ|ss^ zTSnkUUqFtQNRAe9N8b~Ub~!qI+|fkKQ9c}13P)ElN3UCs9G#nRvJH?zb9o86?q0f6 ziw`r4X}~78qQ+vH#|~?WdHFSoc|&RvGYgE-UnY|seff^gTQKBkP7HgB#CIIAL1hHW z@C(dfPLCElR1>enHLUjb+zar*--`v+kEddq$f>^n`nWWO{AEmk;xL9Q^XW?LoS zTBmWTeQT=y6|4uojwc5Ym=-h9QPyX4a}0uVb({ajggTK}YkklhTA(HVnaziLk1w_< zN`n^9A4^44i_Jdh>Q1lkc*QeUU~c}bkGhw1bl#3DdA#Czti5%~7ByZ`jOuGFA3#$@ ziP@e2?s%XcGLCf<=D>SI6BV&ZT|xa`W>CU zV6;sLttL@gTcj$xJ8jgW%yzwJoWcpkhPpivO;oz3!LU`jh87TCn&KDR#PR@ zGDB@|G3vAx0!JxZkNu4(Wq(A|EL1H&t4wRrVl2Ywb7zmGT2IbIP1{`Eu`9>ea~egY zf?mwH0Z%$)ps$-|iwA&Ku#LK1lV~VLQ)xZaTyGdl$y2w{%KBrQf%*Wh z4NgxuuIY*1FB=F9@iF^BMnaYVs*~d&C6U<5rVphT*s{moL+5nw0n2aZ0A>CYafNt`sP4B zS~TBIkf!a4OW9gi%<G|dAUYBeezrc0ic^d85UyU~) z)&6op?C|qg_DbwU7~cyBW%Hr#B|E!LX6!d<-&3w7>WYVG3BB04r>b}e)o*D%Ruz}P zh7KF3EU}#}rA8Yv5ijWYVsOhm*Be^Arnm&19l|bmv4br)W4BShMC>EuYl|;3uA6@4%$Q+DW$f@jNvpa)^(Zo>!0`X4zW%8*Xm1V zeo$>(2NuN6FoQ3koX}OX{G9H!Apfj;j?UkbOroZE2vc!yqcmLo4%H!JJ#~cg zr2%4((YBGqc1m6BFeVT(IR^_AoDtZuw}6lfjULMh%7)&?jAN&?5tiT>n4_}`^2p7n z1yDOG>dMpTYi5oe>wZ9@lxOVf*obQJLiZ&q)GKB~eLLOXG8e{OaC+?d)P5IfzL?1k zn?!C*CWP_b^$E)6;i}lX#-lhpmJs(7uxpNDcBhlwF)6#>fL%)=2nF+2YysH#rl%$w zuZ0U7TQT;ZX|Y#nyJ8$icj#)?d4WP@Y__sGSDyu-0H4Z&fMZeNQTPFag4VGM>-8}1 zwUQ8CvQ*piBAg^y#H_ZN9wd+7uP_b#(a4Z;3_0mUsMw!n_#c8`Az{n!&%cxW-^u+K zr2StDs6YRQ(HY4c0eceEg-s_k0W^>$-6jJAKl0WH`5!*qQtCGDycBTjAQK0<|$~?GEPePJkvXpLSt@+?y`#&M zSrtcwaNB3Nj(W2t@&cn*lhF;CMz52MIxVAhBhNLu1>JLOJ!2SFpp)Azn-j_AXvrok1Mi|2a7_D} z!XyL{X#I$DZQf2Pi&wNDLxuvo*7W)bSiKJF6{Dd_W?-}IdYlj=&n|0L(YlYoA$-kZ zq@r&4xxRYIe!})TnzY2F6F_Pbh2KQ>phoY>%#Zc)l~fC{n-MLWAhipKeHSdzHAqaY zhRb3(s_sw7BFeaiL8OalKD&svECe>y+Ag^7aC=b9tW+R&tG<~Ziv0oW8{w?(m}|1& zcG|P|9K!^~{I8Dvh2}q=-TcalXE@j$5?#PH+NW#Ay-$T&+i@n(m}^o(Khjm#8)E&` z#J;SFovw;~;!a-lChf4bX>YY0wN@G3P_THr((CW!PGptrVIAU>eb2jOY~UI-{aLEa z!58xW22~HK_yW8^#Tz5)*yy!mql1(M_&Qsy!k66Y>_{XOQA)##uQAxv5W&~-&7p|i zSQW%q?(ExmcUv&3R`~Fk1OQCFf_j*=q%n5O1E81y{5HSySX~HXSTJakwK z+6B6aaM0g_*5h%QK@?BpYEd@n`TQnOI@qQdG-vhAN=}kCi0iWZPZ{$IlX?G@aXWB<@ z%&W4i>+{|Ak`7yyy*&5LLG}{2U3J?>LISGp!3z1=g-ZFjEEomDtsb|Pe1? zSE}sJTd4y$=nkX?Z3nS6#x{i=Bwr8g&%o%56J@U5*^#^4UNCc~qJpKl(}%XFs10#< z1SH;Cm9o{+oQ)FXxEFQ4Wh?)2ISVEKbyN8Wt98XjaH`ArtiQixNo%OP8g z`TY0D(lge!zeAStv;nd&L-u+eLq~2M%Ph6gyV|||w3VZc*3U&)N8jUm>#5HjdGqXQ zu9gbEesGn2Bszw)HbCwabi&Y4_#AkE=Gy&vv+Zgkf2O@;Bh9t-1Kjqh9T&{Ahx6v= z+S}&a+jHFZ4fu5BX!cF`Y|g2*hwBE}r@HOsn4mN5DvVguvI@DPHE}!a7hui$DK<2n z<3}Su8u^)TuZ;9(ijDQ-uu~2@2>bkTJ_F}7a6SX)GjKiw=QD6V1OIOs5btY?_p@n> zNk=B7cgX2r58*+@?`r5pG&;oZY3Ri+cHE#ca*4$$)^1wCFNUo{ydO^YB6NuM&(0=8 z#BbvM@brrv1^5z!4zVvj$lJvYVqLz~ZtZ@pJv|}xSW!M`YeV7L|Fa89q}bPA_)IK73_0H>{-uIKb&PM_fP8BTX| z`WmMnaC(~40eoT%=k!WWr*Jxl(_1(VaN5f0dQKna^a)O%;dD2ruW|YTr>8j`KsQi0 zhI4u)r&Bn!TL1pP;&&LC8shg8;&&I~cNSvZh<5RN3i101(LQ(9tm$gW!e#hTREv6D z*`%`aQdbM(Tz9*smX%K`bB(ibMJbCm((i@5%ak(wB*`BMD|jfU`^(%jt4nq7a?UMp zZYf*Vf@hYc!3L#_xJGZZQ7LO!)f`>5!lL+f5o>AnN22umHJS0CE#hzTk^(;qH|gXO zzY{CNY1wl8;H*sdx8X1QF;!V4#C{i4=5O=_B6xo3X>5QI!C9&vZzSSfWod|Wc<`$>`_z>=1Qd&>J|Ox(;w&kQ;GAIS~2>`H7(A z{DMJ*`Y!6VSSE9T(KSKni|dS_;<^G6c4X%7CZJTag}$iMf_8E}5xu1K#VdYMoP)>!uxGo7Qu1msyX8&m-lKxD5=n&UALB(~E^r`e^>fa5Fa>CP` z+82oZ0WVjOu#U{}UjsSHj3c^{|bzdQ}GGojC?Q>s;TN2bDe SN(d+MN{jng25~O*1pL21)=4b@ literal 0 HcmV?d00001 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)" +}