Hi, I am using gazebo harmonic along with ardupilot SITL to simulate a Hexacopter with X configuration. I have used this model https://app.gazebosim.org/OpenRobotics/fuel/models/X4%20UAV%20Config%201 and modified it for use with the ardupilot plugin by looking at the iris example.
The problem that I have been experiencing is that the drone yaws uncontrollably, then crashes. When running it in stabilized and looking at the log I see that servos 1,3, and 6 are all almost constantly running at the minimum PWM, while the rest are at max. Which would explain the uncontrolled yawing, but I have no Idea why it is happening
I have tried reinstalling gazbo and chaning the release of ardupilot,as well as countless changes to the .sdf model, but I cant figure out what is wrong.
Here is the flight log: log_38_2025-3-6-21-25-53.bin - Google Drive
And here is my .sdf file:
<model name="arus_test_platform">
<include>
<uri>model://X4</uri>
</include>
<!-- plugins -->
<!-- CW -->
<plugin filename="gz-sim-joint-state-publisher-system"
name="gz::sim::systems::JointStatePublisher">
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_0</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_0</link_name>
</plugin>
<!-- CCW -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_1</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_1</link_name>
</plugin>
<!-- CW -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_2</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_2</link_name>
</plugin>
<!-- CW -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_3</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_3</link_name>
</plugin>
<!-- CW -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_4</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_4</link_name>
</plugin>
<!-- CCW -->
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_5</link_name>
</plugin>
<plugin filename="gz-sim-lift-drag-system"
name="gz::sim::systems::LiftDrag">
<a0>0.3</a0>
<alpha_stall>1.4</alpha_stall>
<cla>4.2500</cla>
<cda>0.10</cda>
<cma>0.0</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>X4::rotor_5</link_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>X4::rotor_0_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>X4::rotor_1_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>X4::rotor_2_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>X4::rotor_3_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>X4::rotor_4_joint</joint_name>
</plugin>
<plugin filename="gz-sim-apply-joint-force-system"
name="gz::sim::systems::ApplyJointForce">
<joint_name>X4::rotor_5_joint</joint_name>
</plugin>
<plugin name="ArduPilotPlugin"
filename="ArduPilotPlugin">
<!-- Port settings -->
<fdm_addr>127.0.0.1</fdm_addr>
<fdm_port_in>9002</fdm_port_in>
<connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
<lock_step>1</lock_step>
<!-- Frame conventions
Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
-->
<!-- <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0
0</modelXYZToAirplaneXForwardZDown>
<gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED> -->
<gazeboXYZToNED>0 0 0 3.141593 0 1.57079632</gazeboXYZToNED>
<modelXYZToAirplaneXForwardZDown>0 0 0 3.141593 0 0</modelXYZToAirplaneXForwardZDown>
<!-- Sensors -->
<imuName>X4::imu_link::imu_sensor</imuName>
<!--
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
-->
<control channel="2">
<jointName>X4::rotor_0_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier> <!-- NEG -->
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="1">
<jointName>X4::rotor_1_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier> <!-- Counter Clockwise POS-->
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="5">
<jointName>X4::rotor_2_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier> <!-- Clockwise NEG -->
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="3">
<jointName>X4::rotor_3_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier> <!-- Counter Clockwise POS -->
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<control channel="0">
<jointName>X4::rotor_4_joint</jointName>
<useForce>1</useForce>
<multiplier>-838</multiplier> <!-- Clockwise NEG-->
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
<!-- Counter Clockwise-->
<control channel="4">
<jointName>X4::rotor_5_joint</jointName>
<useForce>1</useForce>
<multiplier>838</multiplier> <!-- POS -->
<offset>0</offset>
<servo_min>1100</servo_min>
<servo_max>1900</servo_max>
<type>VELOCITY</type>
<p_gain>0.20</p_gain>
<i_gain>0</i_gain>
<d_gain>0</d_gain>
<i_max>1</i_max>
<i_min>-1</i_min>
<cmd_max>2.5</cmd_max>
<cmd_min>-2.5</cmd_min>
<controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
</control>
</plugin>
</model>