mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
WIP
This commit is contained in:
Daniel Agar
parent
93d00528ac
commit
91064202f6
63
Tools/simulation/gazebo/models/x3/meshes/propeller_ccw.dae
Normal file
63
Tools/simulation/gazebo/models/x3/meshes/propeller_ccw.dae
Normal file
File diff suppressed because one or more lines are too long
63
Tools/simulation/gazebo/models/x3/meshes/propeller_cw.dae
Normal file
63
Tools/simulation/gazebo/models/x3/meshes/propeller_cw.dae
Normal file
File diff suppressed because one or more lines are too long
10172
Tools/simulation/gazebo/models/x3/meshes/x3.dae
Normal file
10172
Tools/simulation/gazebo/models/x3/meshes/x3.dae
Normal file
File diff suppressed because one or more lines are too long
BIN
Tools/simulation/gazebo/models/x3/meshes/x3.jpg
Normal file
BIN
Tools/simulation/gazebo/models/x3/meshes/x3.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 206 KiB |
20
Tools/simulation/gazebo/models/x3/model.config
Normal file
20
Tools/simulation/gazebo/models/x3/model.config
Normal file
@ -0,0 +1,20 @@
|
||||
<?xml version="1.0"?>
|
||||
|
||||
<model>
|
||||
<name>x3</name>
|
||||
<version>1.0</version>
|
||||
<sdf version="1.6">model.sdf</sdf>
|
||||
|
||||
<author>
|
||||
<name>Carlos Aguero</name>
|
||||
<email>caguero@openrobotics.org</email>
|
||||
</author>
|
||||
|
||||
<author>
|
||||
<name>Cole Biesemeyer</name>
|
||||
<email>cole@openrobotics.org</email>
|
||||
</author>
|
||||
|
||||
<description>
|
||||
</description>
|
||||
</model>
|
||||
318
Tools/simulation/gazebo/models/x3/model.sdf
Normal file
318
Tools/simulation/gazebo/models/x3/model.sdf
Normal file
@ -0,0 +1,318 @@
|
||||
<sdf version='1.6'>
|
||||
<model name='x3'>
|
||||
<pose>0 0 0.053302 0 0 0</pose>
|
||||
<link name='X3/base_link'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<mass>1.5</mass>
|
||||
<inertia>
|
||||
<ixx>0.0347563</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.07</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.0977</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='X3/base_link_inertia_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.47 0.47 0.11</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name='X3/base_link_inertia_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>1 1 1</scale>
|
||||
<uri>model://x3/meshes/x3.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
</visual>
|
||||
<sensor name="imu_sensor" type="imu">
|
||||
<always_on>1</always_on>
|
||||
<update_rate>250</update_rate>
|
||||
<visualize>true</visualize>
|
||||
<topic>imu</topic>
|
||||
</sensor>
|
||||
</link>
|
||||
<link name='X3/rotor_0'>
|
||||
<pose frame=''>0.13 -0.22 0.023 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>4.17041e-05</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>4.26041e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='X3/rotor_0_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='X3/rotor_0_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://x3/meshes/propeller_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Blue</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
</link>
|
||||
<joint name='X3/rotor_0_joint' type='revolute'>
|
||||
<child>X3/rotor_0</child>
|
||||
<parent>X3/base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='X3/rotor_1'>
|
||||
<pose frame=''>-0.13 0.2 0.023 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>4.17041e-05</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>4.26041e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='X3/rotor_1_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='X3/rotor_1_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://x3/meshes/propeller_ccw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Red</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
</link>
|
||||
<joint name='X3/rotor_1_joint' type='revolute'>
|
||||
<child>X3/rotor_1</child>
|
||||
<parent>X3/base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='X3/rotor_2'>
|
||||
<pose frame=''>0.13 0.22 0.023 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>4.17041e-05</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>4.26041e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='X3/rotor_2_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='X3/rotor_2_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://x3/meshes/propeller_cw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Blue</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
</link>
|
||||
<joint name='X3/rotor_2_joint' type='revolute'>
|
||||
<child>X3/rotor_2</child>
|
||||
<parent>X3/base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<link name='X3/rotor_3'>
|
||||
<pose frame=''>-0.13 -0.2 0.023 0 -0 0</pose>
|
||||
<inertial>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<mass>0.005</mass>
|
||||
<inertia>
|
||||
<ixx>9.75e-07</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>4.17041e-05</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>4.26041e-05</izz>
|
||||
</inertia>
|
||||
</inertial>
|
||||
<collision name='X3/rotor_3_collision'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<cylinder>
|
||||
<length>0.005</length>
|
||||
<radius>0.1</radius>
|
||||
</cylinder>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode/>
|
||||
</friction>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name='X3/rotor_3_visual'>
|
||||
<pose frame=''>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<scale>0.1 0.1 0.1</scale>
|
||||
<uri>model://x3/meshes/propeller_cw.dae</uri>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<name>Gazebo/Red</name>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<gravity>1</gravity>
|
||||
<velocity_decay/>
|
||||
</link>
|
||||
<joint name='X3/rotor_3_joint' type='revolute'>
|
||||
<child>X3/rotor_3</child>
|
||||
<parent>X3/base_link</parent>
|
||||
<axis>
|
||||
<xyz>0 0 1</xyz>
|
||||
<limit>
|
||||
<lower>-1e+16</lower>
|
||||
<upper>1e+16</upper>
|
||||
</limit>
|
||||
<dynamics>
|
||||
<spring_reference>0</spring_reference>
|
||||
<spring_stiffness>0</spring_stiffness>
|
||||
</dynamics>
|
||||
<use_parent_model_frame>1</use_parent_model_frame>
|
||||
</axis>
|
||||
</joint>
|
||||
<plugin
|
||||
filename="libgazebo_barometer_plugin.so"
|
||||
name="barometer_plugin::BarometerPlugin">
|
||||
<link_name>X3/base_link</link_name>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="libgazebo_magnetometer_plugin.so"
|
||||
name="magnetometer_plugin::MagnetometerPlugin">
|
||||
<link_name>X3/base_link</link_name>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="libgazebo_gps_plugin.so"
|
||||
name="gps_plugin::GpsPlugin">
|
||||
<link_name>X3/base_link</link_name>
|
||||
</plugin>
|
||||
</model>
|
||||
</sdf>
|
||||
4
Tools/simulation/gazebo/setup.bash
Normal file
4
Tools/simulation/gazebo/setup.bash
Normal file
@ -0,0 +1,4 @@
|
||||
# !/bin/bash
|
||||
|
||||
export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=$1/Tools/sitl_ign_gazebo/build
|
||||
export IGN_GAZEBO_RESOURCE_PATH=$IGN_GAZEBO_RESOURCE_PATH:$1/Tools/sitl_ign_gazebo/models
|
||||
341
Tools/simulation/gazebo/worlds/iris.world
Normal file
341
Tools/simulation/gazebo/worlds/iris.world
Normal file
@ -0,0 +1,341 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<world name="quadcopter">
|
||||
<physics name="4ms" type="ode">
|
||||
<max_step_size>0.004</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
</physics>
|
||||
<plugin
|
||||
filename="libignition-gazebo-physics-system.so"
|
||||
name="ignition::gazebo::systems::Physics">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="libignition-gazebo-scene-broadcaster-system.so"
|
||||
name="ignition::gazebo::systems::SceneBroadcaster">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="libignition-gazebo-user-commands-system.so"
|
||||
name="ignition::gazebo::systems::UserCommands">
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="libignition-gazebo-sensors-system.so"
|
||||
name="ignition::gazebo::systems::Sensors">
|
||||
<render_engine>ogre2</render_engine>
|
||||
</plugin>
|
||||
<plugin filename="libignition-gazebo-imu-system.so"
|
||||
name="ignition::gazebo::systems::Imu">
|
||||
</plugin>
|
||||
<gui fullscreen="0">
|
||||
<!-- 3D scene -->
|
||||
<plugin filename="GzScene3D" name="3D View">
|
||||
<ignition-gui>
|
||||
<title>3D View</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="string" key="state">docked</property>
|
||||
</ignition-gui>
|
||||
|
||||
<engine>ogre2</engine>
|
||||
<scene>scene</scene>
|
||||
<ambient_light>0.4 0.4 0.4</ambient_light>
|
||||
<background_color>0.8 0.8 0.8</background_color>
|
||||
<camera_pose>-6 0 6 0 0.5 0</camera_pose>
|
||||
</plugin>
|
||||
|
||||
<!-- World control -->
|
||||
<plugin filename="WorldControl" name="World control">
|
||||
<ignition-gui>
|
||||
<title>World control</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="bool" key="resizable">false</property>
|
||||
<property type="double" key="height">72</property>
|
||||
<property type="double" key="width">121</property>
|
||||
<property type="double" key="z">1</property>
|
||||
|
||||
<property type="string" key="state">floating</property>
|
||||
<anchors target="3D View">
|
||||
<line own="left" target="left"/>
|
||||
<line own="bottom" target="bottom"/>
|
||||
</anchors>
|
||||
</ignition-gui>
|
||||
|
||||
<play_pause>true</play_pause>
|
||||
<step>true</step>
|
||||
<start_paused>true</start_paused>
|
||||
<service>/world/quadcopter/control</service>
|
||||
<stats_topic>/world/quadcopter/stats</stats_topic>
|
||||
|
||||
</plugin>
|
||||
|
||||
<!-- World statistics -->
|
||||
<plugin filename="WorldStats" name="World stats">
|
||||
<ignition-gui>
|
||||
<title>World stats</title>
|
||||
<property type="bool" key="showTitleBar">false</property>
|
||||
<property type="bool" key="resizable">false</property>
|
||||
<property type="double" key="height">110</property>
|
||||
<property type="double" key="width">290</property>
|
||||
<property type="double" key="z">1</property>
|
||||
|
||||
<property type="string" key="state">floating</property>
|
||||
<anchors target="3D View">
|
||||
<line own="right" target="right"/>
|
||||
<line own="bottom" target="bottom"/>
|
||||
</anchors>
|
||||
</ignition-gui>
|
||||
|
||||
<sim_time>true</sim_time>
|
||||
<real_time>true</real_time>
|
||||
<real_time_factor>true</real_time_factor>
|
||||
<iterations>true</iterations>
|
||||
<topic>/world/quadcopter/stats</topic>
|
||||
|
||||
</plugin>
|
||||
|
||||
</gui>
|
||||
|
||||
<light type="directional" name="sun">
|
||||
<cast_shadows>true</cast_shadows>
|
||||
<pose>0 0 10 0 0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
</light>
|
||||
|
||||
<model name="ground_plane">
|
||||
<static>true</static>
|
||||
<link name="link">
|
||||
<collision name="collision">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
</plane>
|
||||
</geometry>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<ambient>0.8 0.8 0.8 1</ambient>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.8 0.8 0.8 1</specular>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
</model>
|
||||
<include>
|
||||
<uri>model://x3</uri>
|
||||
<plugin
|
||||
filename="ignition-gazebo-multicopter-motor-model-system"
|
||||
name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<robotNamespace>X3</robotNamespace>
|
||||
<jointName>X3/rotor_0_joint</jointName>
|
||||
<linkName>X3/rotor_0</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>800.0</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>0</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/0</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-multicopter-motor-model-system"
|
||||
name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<robotNamespace>X3</robotNamespace>
|
||||
<jointName>X3/rotor_1_joint</jointName>
|
||||
<linkName>X3/rotor_1</linkName>
|
||||
<turningDirection>ccw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>800.0</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>1</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/1</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-multicopter-motor-model-system"
|
||||
name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<robotNamespace>X3</robotNamespace>
|
||||
<jointName>X3/rotor_2_joint</jointName>
|
||||
<linkName>X3/rotor_2</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>800.0</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>2</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/2</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="ignition-gazebo-multicopter-motor-model-system"
|
||||
name="ignition::gazebo::systems::MulticopterMotorModel">
|
||||
<robotNamespace>X3</robotNamespace>
|
||||
<jointName>X3/rotor_3_joint</jointName>
|
||||
<linkName>X3/rotor_3</linkName>
|
||||
<turningDirection>cw</turningDirection>
|
||||
<timeConstantUp>0.0125</timeConstantUp>
|
||||
<timeConstantDown>0.025</timeConstantDown>
|
||||
<maxRotVelocity>800.0</maxRotVelocity>
|
||||
<motorConstant>8.54858e-06</motorConstant>
|
||||
<momentConstant>0.016</momentConstant>
|
||||
<commandSubTopic>gazebo/command/motor_speed</commandSubTopic>
|
||||
<motorNumber>3</motorNumber>
|
||||
<rotorDragCoefficient>8.06428e-05</rotorDragCoefficient>
|
||||
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
|
||||
<motorSpeedPubTopic>motor_speed/3</motorSpeedPubTopic>
|
||||
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
|
||||
<motorType>velocity</motorType>
|
||||
</plugin>
|
||||
<plugin
|
||||
filename="libmavlink_sitl_ign_gazebo.so"
|
||||
name="mavlink_interface::GazeboMavlinkInterface">
|
||||
<robotNamespace/>
|
||||
<imuSubTopic>/imu</imuSubTopic>
|
||||
<magSubTopic>/mag</magSubTopic>
|
||||
<baroSubTopic>/baro</baroSubTopic>
|
||||
<mavlink_addr>INADDR_ANY</mavlink_addr>
|
||||
<mavlink_udp_port>14560</mavlink_udp_port>
|
||||
<mavlink_tcp_port>4560</mavlink_tcp_port>
|
||||
<serialEnabled>0</serialEnabled>
|
||||
<serialDevice>/dev/ttyACM0</serialDevice>
|
||||
<baudRate>921600</baudRate>
|
||||
<qgc_addr>INADDR_ANY</qgc_addr>
|
||||
<qgc_udp_port>14550</qgc_udp_port>
|
||||
<sdk_addr>INADDR_ANY</sdk_addr>
|
||||
<sdk_udp_port>14540</sdk_udp_port>
|
||||
<hil_mode>0</hil_mode>
|
||||
<hil_state_level>0</hil_state_level>
|
||||
<vehicle_is_tailsitter>0</vehicle_is_tailsitter>
|
||||
<send_vision_estimation>0</send_vision_estimation>
|
||||
<send_odometry>1</send_odometry>
|
||||
<enable_lockstep>1</enable_lockstep>
|
||||
<use_tcp>1</use_tcp>
|
||||
<motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
|
||||
<control_channels>
|
||||
<channel name='rotor1'>
|
||||
<input_index>0</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>1000</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>100</zero_position_armed>
|
||||
<joint_control_type>velocity</joint_control_type>
|
||||
</channel>
|
||||
<channel name='rotor2'>
|
||||
<input_index>1</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>1000</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>100</zero_position_armed>
|
||||
<joint_control_type>velocity</joint_control_type>
|
||||
</channel>
|
||||
<channel name='rotor3'>
|
||||
<input_index>2</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>1000</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>100</zero_position_armed>
|
||||
<joint_control_type>velocity</joint_control_type>
|
||||
</channel>
|
||||
<channel name='rotor4'>
|
||||
<input_index>3</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>1000</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>100</zero_position_armed>
|
||||
<joint_control_type>velocity</joint_control_type>
|
||||
</channel>
|
||||
<channel name='rotor5'>
|
||||
<input_index>4</input_index>
|
||||
<input_offset>1</input_offset>
|
||||
<input_scaling>324.6</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>0</zero_position_armed>
|
||||
<joint_control_type>velocity</joint_control_type>
|
||||
<joint_control_pid>
|
||||
<p>0.1</p>
|
||||
<i>0</i>
|
||||
<d>0</d>
|
||||
<iMax>0.0</iMax>
|
||||
<iMin>0.0</iMin>
|
||||
<cmdMax>2</cmdMax>
|
||||
<cmdMin>-2</cmdMin>
|
||||
</joint_control_pid>
|
||||
<joint_name>zephyr_delta_wing::propeller_joint</joint_name>
|
||||
</channel>
|
||||
<channel name='rotor6'>
|
||||
<input_index>5</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>0.524</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>0</zero_position_armed>
|
||||
<joint_control_type>position</joint_control_type>
|
||||
<joint_name>zephyr_delta_wing::flap_left_joint</joint_name>
|
||||
<joint_control_pid>
|
||||
<p>10.0</p>
|
||||
<i>0</i>
|
||||
<d>0</d>
|
||||
<iMax>0</iMax>
|
||||
<iMin>0</iMin>
|
||||
<cmdMax>20</cmdMax>
|
||||
<cmdMin>-20</cmdMin>
|
||||
</joint_control_pid>
|
||||
</channel>
|
||||
<channel name='rotor7'>
|
||||
<input_index>6</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>0.524</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>0</zero_position_armed>
|
||||
<joint_control_type>position</joint_control_type>
|
||||
<joint_name>zephyr_delta_wing::flap_right_joint</joint_name>
|
||||
<joint_control_pid>
|
||||
<p>10.0</p>
|
||||
<i>0</i>
|
||||
<d>0</d>
|
||||
<iMax>0</iMax>
|
||||
<iMin>0</iMin>
|
||||
<cmdMax>20</cmdMax>
|
||||
<cmdMin>-20</cmdMin>
|
||||
</joint_control_pid>
|
||||
</channel>
|
||||
<channel name='rotor8'>
|
||||
<input_index>7</input_index>
|
||||
<input_offset>0</input_offset>
|
||||
<input_scaling>0.524</input_scaling>
|
||||
<zero_position_disarmed>0</zero_position_disarmed>
|
||||
<zero_position_armed>0</zero_position_armed>
|
||||
<joint_control_type>position</joint_control_type>
|
||||
</channel>
|
||||
</control_channels>
|
||||
</plugin>
|
||||
</include>
|
||||
</world>
|
||||
</sdf>
|
||||
@ -1,3 +1,2 @@
|
||||
CONFIG_MODULES_SIMULATOR=y
|
||||
CONFIG_BOARD_NOLOCKSTEP=y
|
||||
CONFIG_MODULES_IGNITION_SIMULATOR=y
|
||||
|
||||
@ -87,6 +87,33 @@ void IgnitionSimulator::PoseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
// position.x(), position.y(), position.z()
|
||||
// orientation. wxyz
|
||||
|
||||
static const auto q_FLU_to_FRD = ignition::math::Quaterniond(0, 1, 0, 0);
|
||||
|
||||
/**
|
||||
* @brief Quaternion for rotation between ENU and NED frames
|
||||
*
|
||||
* NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
|
||||
* ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
|
||||
* This rotation is symmetric, so q_ENU_to_NED == q_NED_to_ENU.
|
||||
*/
|
||||
static const auto q_ENU_to_NED = ignition::math::Quaterniond(0, 0.70711, 0.70711, 0);
|
||||
|
||||
|
||||
// ground truth
|
||||
ignition::math::Quaterniond q_gr = ignition::math::Quaterniond(
|
||||
last_imu_message_.orientation().w(),
|
||||
last_imu_message_.orientation().x(),
|
||||
last_imu_message_.orientation().y(),
|
||||
last_imu_message_.orientation().z());
|
||||
|
||||
ignition::math::Quaterniond q_gb = q_gr * q_FLU_to_FRD.Inverse();
|
||||
ignition::math::Quaterniond q_nb = q_ENU_to_NED * q_gb;
|
||||
|
||||
hil_state_quat.attitude_quaternion[0] = q_nb.W();
|
||||
hil_state_quat.attitude_quaternion[1] = q_nb.X();
|
||||
hil_state_quat.attitude_quaternion[2] = q_nb.Y();
|
||||
hil_state_quat.attitude_quaternion[3] = q_nb.Z();
|
||||
|
||||
//PX4_INFO("matched, position [%.6f, %.6f, %.6f]", (double)position.x(), (double)position.y(), (double)position.z());
|
||||
|
||||
vehicle_local_position_s local_position_groundtruth{};
|
||||
@ -96,6 +123,11 @@ void IgnitionSimulator::PoseInfoCallback(const ignition::msgs::Pose_V &pose)
|
||||
local_position_groundtruth.timestamp = hrt_absolute_time();
|
||||
|
||||
_lpos_ground_truth_pub.publish(local_position_groundtruth);
|
||||
|
||||
|
||||
// _param_sim_home_lat.get()
|
||||
// reproject
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -182,7 +214,7 @@ void IgnitionSimulator::run()
|
||||
|
||||
int IgnitionSimulator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
_task_id = px4_task_spawn_cmd("module",
|
||||
_task_id = px4_task_spawn_cmd("ignition_simulator",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_DEFAULT,
|
||||
1024,
|
||||
|
||||
@ -93,8 +93,9 @@ private:
|
||||
void PoseInfoCallback(const ignition::msgs::Pose_V &pose);
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
|
||||
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig /**< another parameter */
|
||||
(ParamFloat<px4::params::SIM_HOME_LAT>) _param_sim_home_lat,
|
||||
(ParamFloat<px4::params::SIM_HOME_LON>) _param_sim_home_lon,
|
||||
(ParamFloat<px4::params::SIM_HOME_ALT>) _param_sim_home_alt
|
||||
)
|
||||
|
||||
// Subscriptions
|
||||
@ -107,8 +108,4 @@ private:
|
||||
|
||||
PX4Accelerometer _px4_accel;
|
||||
PX4Gyroscope _px4_gyro;
|
||||
|
||||
hrt_abstime _time_start_us{0};
|
||||
|
||||
uint32_t _sensor_interval_us{1250};
|
||||
};
|
||||
|
||||
56
src/modules/ignition_simulator/parameters.c
Normal file
56
src/modules/ignition_simulator/parameters.c
Normal file
@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* simulator origin latitude
|
||||
*
|
||||
* @unit deg
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_LAT, 47.397742f);
|
||||
|
||||
/**
|
||||
* simulator origin longitude
|
||||
*
|
||||
* @unit deg
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_LON, 8.545594);
|
||||
|
||||
/**
|
||||
* simulator origin altitude
|
||||
*
|
||||
* @unit m
|
||||
* @group Simulator
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(SIM_HOME_ALT, 488.0);
|
||||
Loading…
x
Reference in New Issue
Block a user