Hello, I’m trying to implement gimbal control through arducopter sitl in gazebo simulator with the “iris” drone.
I can fly the drone using sitl and QGC, but when i try to add a control for tilt joint I cant get a mavlink connction…
This is the code (the control i added is channel 5 )
Can someone help me make this work?
thank you
this is the code
<sdf version="1.6" xmlns:xacro="http://ros.org/wiki/xacro"> <model name="iris_demo"> <include> <uri>model://iris_with_standoffs</uri> </include>
<include>
<uri>model://gimbal_small_2d</uri>
<pose>0 -0.01 0.070 1.57 0 1.57</pose>
</include>
!-->
<joint name="iris_gimbal_mount" type="revolute">
<parent>iris::base_link</parent>
<child>gimbal_small_2d::base_link</child>
<axis>
<xyz>0 0 1</xyz>
<limit>
<upper>0</upper>
<lower>0</lower>
</limit>
<use_parent_model_frame>true</use_parent_model_frame>
</axis>
</joint>
<!-- visual markers for debugging
<link name="rotor_0_blade_1_cp">
<gravity>0</gravity>
<pose>0.13 -0.22 0.216 0 -0 0</pose>
<visual name='rotor_0_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_tip'>
<pose>0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_cp'>
<pose>0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_cp_forward'>
<pose>0.084 0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_cp_upward'>
<pose>0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_0_blade_2_cp">
<gravity>0</gravity>
<pose>0.13 -0.22 0.216 0 -0 0</pose>
<visual name='rotor_0_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_tip'>
<pose>-0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_cp'>
<pose>-0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_cp_forward'>
<pose>-0.084 -0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_0_visual_cp_upward'>
<pose>-0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_1_blade_1_cp">
<gravity>0</gravity>
<pose>-0.13 0.2 0.216 0 -0 0</pose>
<visual name='rotor_1_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_tip'>
<pose>0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_cp'>
<pose>0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_cp_forward'>
<pose>0.084 0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_cp_upward'>
<pose>0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_1_blade_2_cp">
<gravity>0</gravity>
<pose>-0.13 0.2 0.216 0 -0 0</pose>
<visual name='rotor_1_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_tip'>
<pose>-0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_cp'>
<pose>-0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_cp_forward'>
<pose>-0.084 -0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_1_visual_cp_upward'>
<pose>-0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_2_blade_1_cp">
<gravity>0</gravity>
<pose>0.13 0.22 0.216 0 -0 0</pose>
<visual name='rotor_2_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_tip'>
<pose>0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_cp'>
<pose>0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_cp_forward'>
<pose>0.084 -0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_cp_upward'>
<pose>0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_2_blade_2_cp">
<gravity>0</gravity>
<pose>0.13 0.22 0.216 0 -0 0</pose>
<visual name='rotor_2_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_tip'>
<pose>-0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_cp'>
<pose>-0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_cp_forward'>
<pose>-0.084 0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_2_visual_cp_upward'>
<pose>-0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_3_blade_1_cp">
<gravity>0</gravity>
<pose>-0.13 -0.2 0.216 0 -0 0</pose>
<visual name='rotor_3_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_tip'>
<pose>0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_cp'>
<pose>0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_cp_forward'>
<pose>0.084 -0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_cp_upward'>
<pose>0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
<link name="rotor_3_blade_2_cp">
<gravity>0</gravity>
<pose>-0.13 -0.2 0.216 0 -0 0</pose>
<visual name='rotor_3_visual_root'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_tip'>
<pose>-0.12 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.01</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_cp'>
<pose>-0.084 0 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_cp_forward'>
<pose>-0.084 0.02 0 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
<visual name='rotor_3_visual_cp_upward'>
<pose>-0.084 0 0.02 0 -0 0</pose>
<geometry>
<sphere>
<radius>0.003</radius>
</sphere>
</geometry>
</visual>
</link>
-->
<!-- plugins -->
<plugin name="rotor_0_blade_1" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_0</link_name>
</plugin>
<plugin name="rotor_0_blade_2" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_0</link_name>
</plugin>
<plugin name="rotor_1_blade_1" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_1</link_name>
</plugin>
<plugin name="rotor_1_blade_2" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_1</link_name>
</plugin>
<plugin name="rotor_2_blade_1" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_2</link_name>
</plugin>
<plugin name="rotor_2_blade_2" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_2</link_name>
</plugin>
<plugin name="rotor_3_blade_1" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>0.084 0 0</cp>
<forward>0 -1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_3</link_name>
</plugin>
<plugin name="rotor_3_blade_2" filename="libLiftDragPlugin.so">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.00</cma>
<cla_stall>-0.025</cla_stall>
<cda_stall>0.0</cda_stall>
<cma_stall>0.0</cma_stall>
<area>0.002</area>
<air_density>1.2041</air_density>
<cp>-0.084 0 0</cp>
<forward>0 1 0</forward>
<upward>0 0 1</upward>
<link_name>iris::rotor_3</link_name>
</plugin>
<plugin name="arducopter_plugin" filename="libArduPilotPlugin.so">
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<fdm_port_out>9003</fdm_port_out>
<!--
Require by APM :
Only change model and gazebo from XYZ to XY-Z coordinates
-->
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED>0 0 0 3.141593 0 0</gazeboXYZToNED>
<imuName>iris_demo::iris::iris/imu_link::imu_sensor</imuName>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<control channel="0">
<!--
incoming control command [0, 1]
so offset it by 0 to get [0, 1]
and divide max target by 1.
offset = 0
multiplier = 838 max rpm / 1 = 838
-->
<type>VELOCITY</type>
<offset>0</offset>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<jointName>iris::rotor_0_joint</jointName>
<multiplier>838</multiplier>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="1">
<type>VELOCITY</type>
<offset>0</offset>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<jointName>iris::rotor_1_joint</jointName>
<multiplier>838</multiplier>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="2">
<type>VELOCITY</type>
<offset>0</offset>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<jointName>iris::rotor_2_joint</jointName>
<multiplier>-838</multiplier>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="3">
<type>VELOCITY</type>
<offset>0</offset>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<jointName>iris::rotor_3_joint</jointName>
<multiplier>-838</multiplier>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="5">
<type>VELOCITY</type>
<offset>0</offset>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>0</i_max>
<i_min>0</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<jointName>iris::gimbal_small_2d::tilt_joint</jointName>
<multiplier>-838</multiplier>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
</plugin>
</model> </sdf>