diff --git a/CMakeLists.txt b/CMakeLists.txt
index 409c657..8d69ea0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -37,6 +37,7 @@ add_library(ArduPilotPlugin
SHARED
src/ArduPilotPlugin.cc
src/Socket.cpp
+ src/Util.cc
)
target_include_directories(ArduPilotPlugin PUBLIC
diff --git a/include/Util.hh b/include/Util.hh
new file mode 100644
index 0000000..cd4a26a
--- /dev/null
+++ b/include/Util.hh
@@ -0,0 +1,60 @@
+/*
+ Copyright (C) 2022 ardupilot.org
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+
+#include
+#include
+#include
+
+namespace gz
+{
+namespace sim
+{
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+
+/// \brief Helper function to get an entity given its unscoped name.
+///
+/// \param[in] _name Entity's unscoped name.
+/// \param[in] _ecm Immutable reference to ECM.
+/// \param[in] _relativeTo Entity that the unscoped name is relative to.
+/// If not provided, the unscoped name could be relative to any entity.
+/// \return All entities that match the unscoped name and relative to
+/// requirements, or an empty set otherwise.
+std::unordered_set EntitiesFromUnscopedName(
+ const std::string &_name, const EntityComponentManager &_ecm,
+ Entity _relativeTo = kNullEntity);
+
+/// \brief Get the ID of a joint entity which is a descendent of this model.
+///
+/// A replacement for gz::sim::Model::JointByName which does not resolve
+/// joints for nested models.
+/// \param[in] _ecm Entity-component manager.
+/// \param[in] _entity Model entity.
+/// \param[in] _name Scoped joint name.
+/// \return Joint entity.
+Entity JointByName(EntityComponentManager &_ecm,
+ Entity _modelEntity,
+ const std::string &_name);
+
+}
+} // namespace sim
+} // namespace gz
diff --git a/models/iris_with_ardupilot/model.config b/models/iris_with_ardupilot/model.config
index 0a4e0fd..673f5a4 100755
--- a/models/iris_with_ardupilot/model.config
+++ b/models/iris_with_ardupilot/model.config
@@ -1,7 +1,7 @@
- Iris with Standoffs and Camera LiftDrag ArduCopter Plugins
+ Iris with ArduCopter
1.0
model.sdf
@@ -24,9 +24,8 @@
john hsu
-
- starting with iris_with_standoffs
+ Starting with iris_with_standoffs
add LiftDragPlugin
add ArduCopterPlugin
attach gimbal_small_2d model with GimbalSmall2dPlugin
diff --git a/models/iris_with_ardupilot/model.sdf b/models/iris_with_ardupilot/model.sdf
index 23c62b9..4765ac3 100755
--- a/models/iris_with_ardupilot/model.sdf
+++ b/models/iris_with_ardupilot/model.sdf
@@ -1,10 +1,11 @@
-
+
model://iris_with_standoffs
+
+
-
+
+
+
0.3
1.4
4.2500
@@ -395,9 +402,10 @@
0.084 0 0
0 1 0
0 0 1
- iris::rotor_0
+ iris_with_standoffs::rotor_0
-
+
0.3
1.4
4.2500
@@ -411,10 +419,11 @@
-0.084 0 0
0 -1 0
0 0 1
- iris::rotor_0
+ iris_with_standoffs::rotor_0
-
+
0.3
1.4
4.2500
@@ -428,9 +437,10 @@
0.084 0 0
0 1 0
0 0 1
- iris::rotor_1
+ iris_with_standoffs::rotor_1
-
+
0.3
1.4
4.2500
@@ -444,10 +454,11 @@
-0.084 0 0
0 -1 0
0 0 1
- iris::rotor_1
+ iris_with_standoffs::rotor_1
-
+
0.3
1.4
4.2500
@@ -461,9 +472,10 @@
0.084 0 0
0 -1 0
0 0 1
- iris::rotor_2
+ iris_with_standoffs::rotor_2
-
+
0.3
1.4
4.2500
@@ -477,10 +489,11 @@
-0.084 0 0
0 1 0
0 0 1
- iris::rotor_2
+ iris_with_standoffs::rotor_2
-
+
0.3
1.4
4.2500
@@ -494,9 +507,10 @@
0.084 0 0
0 -1 0
0 0 1
- iris::rotor_3
+ iris_with_standoffs::rotor_3
-
+
0.3
1.4
4.2500
@@ -510,21 +524,42 @@
-0.084 0 0
0 1 0
0 0 1
- iris::rotor_3
+ iris_with_standoffs::rotor_3
+
+
+
+ iris_with_standoffs::rotor_0_joint
-
+
+ iris_with_standoffs::rotor_1_joint
+
+
+ iris_with_standoffs::rotor_2_joint
+
+
+ iris_with_standoffs::rotor_3_joint
+
+
+
+
127.0.0.1
9002
- 9003
-
0 0 0 3.141593 0 0
0 0 0 3.141593 0 0
- iris_demo::iris::iris/imu_link::imu_sensor
- 5
-
+
+
+ iris_with_standoffs::imu_link::imu_sensor
+
- VELOCITY
+
+ iris_with_standoffs::rotor_0_joint
+ 1
+ 838
0
+ 1100
+ 1900
+ VELOCITY
0.20
0
0
@@ -541,13 +582,17 @@
0
2.5
-2.5
- iris::rotor_0_joint
- 838
1
+
- VELOCITY
+ iris_with_standoffs::rotor_1_joint
+ 1
+ 838
0
+ 1100
+ 1900
+ VELOCITY
0.20
0
0
@@ -555,13 +600,17 @@
0
2.5
-2.5
- iris::rotor_1_joint
- 838
1
+
- VELOCITY
+ iris_with_standoffs::rotor_2_joint
+ 1
+ -838
0
+ 1100
+ 1900
+ VELOCITY
0.20
0
0
@@ -569,13 +618,17 @@
0
2.5
-2.5
- iris::rotor_2_joint
- -838
1
+
- VELOCITY
+ iris_with_standoffs::rotor_3_joint
+ 1
+ -838
0
+ 1100
+ 1900
+ VELOCITY
0.20
0
0
@@ -583,8 +636,6 @@
0
2.5
-2.5
- iris::rotor_3_joint
- -838
1
diff --git a/models/iris_with_standoffs/model.config b/models/iris_with_standoffs/model.config
index e33118a..d6b7228 100755
--- a/models/iris_with_standoffs/model.config
+++ b/models/iris_with_standoffs/model.config
@@ -1,5 +1,4 @@
-
Iris with Standoffs
1.0
@@ -24,7 +23,6 @@
john hsu
-
A copy of the 3DR Iris model taken from
https://github.com/PX4/sitl_gazebo/tree/master/models
diff --git a/models/iris_with_standoffs/model.sdf b/models/iris_with_standoffs/model.sdf
index 9e6ed0f..93747b9 100755
--- a/models/iris_with_standoffs/model.sdf
+++ b/models/iris_with_standoffs/model.sdf
@@ -1,6 +1,6 @@
-
-
+
+
0 0 0.194923 0 0 0
@@ -19,7 +19,7 @@
0.017
-
+
0 0 -0.08 0 0 0
@@ -41,7 +41,7 @@
-
+
model://iris_with_standoffs/meshes/iris.dae
@@ -130,43 +130,6 @@
-
- 0 0 0 0 -0 0
-
- 0 0 0 0 -0 0
- 0.15
-
- 0.0001
- 0
- 0
- 0.0002
- 0
- 0.0002
-
-
-
-
- iris/ground_truth/odometry_sensorgt_link
- base_link
-
- 0 0 1
-
- 0
- 0
- 0
- 0
-
-
- 1.0
-
- 1
-
-
-
- 1
-
-
-
0 0 0 0 0 0
@@ -186,7 +149,7 @@
1000.0
-
+
imu_link
base_link
@@ -200,49 +163,8 @@
1.0
- 1
-
-
-
- 1
-
-
-
-
0.13 -0.22 0.023 0 -0 0
@@ -535,264 +457,5 @@
- 0
-
-
-
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- 0.084 0 0
- 0 1 0
- 0 0 1
- rotor_0
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- -0.084 0 0
- 0 -1 0
- 0 0 1
- rotor_0
-
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- 0.084 0 0
- 0 1 0
- 0 0 1
- rotor_1
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- -0.084 0 0
- 0 -1 0
- 0 0 1
- rotor_1
-
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- 0.084 0 0
- 0 -1 0
- 0 0 1
- rotor_2
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- -0.084 0 0
- 0 1 0
- 0 0 1
- rotor_2
-
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- 0.084 0 0
- 0 -1 0
- 0 0 1
- rotor_3
-
-
- 0.3
- 1.4
- 4.2500
- 0.10
- 0.00
- -0.025
- 0.0
- 0.0
- 0.002
- 1.2041
- -0.084 0 0
- 0 1 0
- 0 0 1
- rotor_3
-
-
-
- rotor_0_joint
-
-
- rotor_1_joint
-
-
- rotor_2_joint
-
-
- rotor_3_joint
-
-
-
-
- 127.0.0.1
- 9002
- 5
- 1
-
-
- 0 0 0 3.141593 0 0
- 0 0 0 3.141593 0 0
-
-
- imu_sensor
-
-
-
- rotor_0_joint
- 1
- 838
- 0
- 1100
- 1900
- VELOCITY
- 0.20
- 0
- 0
- 0
- 0
- 2.5
- -2.5
- 1
-
-
-
- rotor_1_joint
- 1
- 838
- 0
- 1100
- 1900
- VELOCITY
- 0.20
- 0
- 0
- 0
- 0
- 2.5
- -2.5
- 1
-
-
-
- rotor_2_joint
- 1
- -838
- 0
- 1100
- 1900
- VELOCITY
- 0.20
- 0
- 0
- 0
- 0
- 2.5
- -2.5
- 1
-
-
-
- rotor_3_joint
- 1
- -838
- 0
- 1100
- 1900
- VELOCITY
- 0.20
- 0
- 0
- 0
- 0
- 2.5
- -2.5
- 1
-
-
-
diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc
index e4ba4f5..c6956c9 100755
--- a/src/ArduPilotPlugin.cc
+++ b/src/ArduPilotPlugin.cc
@@ -28,13 +28,16 @@
#include
#include
+#include
#include
#include
#include
#include
#include
+#include
#include
#include
+#include
#include
#include
#include
@@ -50,6 +53,7 @@
#include
#include "Socket.h"
+#include "Util.hh"
#define DEBUG_JSON_IO 0
@@ -157,8 +161,8 @@ class gz::sim::systems::ArduPilotPluginPrivate
/// \brief The model
public: gz::sim::Model model{gz::sim::kNullEntity};
- /// \brief The entity representing the base or root link of the model
- public: gz::sim::Entity modelLink{gz::sim::kNullEntity};
+ /// \brief The entity representing the link containing the imu sensor.
+ public: gz::sim::Entity imuLink{gz::sim::kNullEntity};
/// \brief The model name;
public: std::string modelName;
@@ -293,16 +297,16 @@ gz::sim::systems::ArduPilotPlugin::~ArduPilotPlugin()
void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
- if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink,
+ if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink,
components::WorldPose::typeId))
{
- _ecm.CreateComponent(this->dataPtr->modelLink,
+ _ecm.CreateComponent(this->dataPtr->imuLink,
gz::sim::components::WorldPose());
}
- if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink,
+ if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink,
components::WorldLinearVelocity::typeId))
{
- _ecm.CreateComponent(this->dataPtr->modelLink,
+ _ecm.CreateComponent(this->dataPtr->imuLink,
gz::sim::components::WorldLinearVelocity());
}
@@ -503,7 +507,8 @@ void gz::sim::systems::ArduPilotPlugin::LoadControlChannels(
}
// Get the pointer to the joint.
- control.joint = this->dataPtr->model.JointByName(_ecm, control.jointName);
+ control.joint = JointByName(_ecm,
+ this->dataPtr->model.Entity(), control.jointName);
if (control.joint == gz::sim::kNullEntity)
{
gzerr << "[" << this->dataPtr->modelName << "] "
@@ -865,43 +870,78 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate(
// Set unconditionally because we're only going to try this once.
this->dataPtr->imuInitialized = true;
std::string imuTopicName;
- _ecm.Each(
- [&](const gz::sim::Entity &_imu_entity,
- const gz::sim::components::Imu * /*_imu*/,
- const gz::sim::components::Name *_name)->bool
+
+ // The model must contain an imu sensor element:
+ //
+ //
+ // Extract the following:
+ // - Sensor topic name: to subscribe to the imu data
+ // - Link containing the sensor: to get the pose to transform to
+ // the correct frame for ArduPilot
+
+ // try scoped names first
+ auto entities = entitiesFromScopedName(
+ this->dataPtr->imuName, _ecm, this->dataPtr->model.Entity());
+
+ // fall-back to unscoped name
+ if (entities.empty())
+ {
+ entities = EntitiesFromUnscopedName(
+ this->dataPtr->imuName, _ecm, this->dataPtr->model.Entity());
+ }
+
+ if (!entities.empty())
{
- if (_name->Data() == this->dataPtr->imuName)
+ if (entities.size() > 1)
+ {
+ gzwarn << "Multiple IMU sensors with name ["
+ << this->dataPtr->imuName << "] found. "
+ << "Using the first one.\n";
+ }
+
+ // select first entity
+ gz::sim::Entity imuEntity = *entities.begin();
+
+ // validate
+ if (!_ecm.EntityHasComponentType(imuEntity,
+ gz::sim::components::Imu::typeId))
+ {
+ gzerr << "Entity with name ["
+ << this->dataPtr->imuName
+ << "] is not an IMU sensor\n";
+ }
+ else
+ {
+ gzmsg << "Found IMU sensor with name ["
+ << this->dataPtr->imuName
+ << "]\n";
+
+ // verify the parent of the imu sensor is a link.
+ gz::sim::Entity parent = _ecm.ParentEntity(imuEntity);
+ if (_ecm.EntityHasComponentType(parent,
+ gz::sim::components::Link::typeId))
{
- // The parent of the imu is imu_link
- gz::sim::Entity parent = _ecm.ParentEntity(_imu_entity);
- if (parent != gz::sim::kNullEntity)
- {
- // The grandparent of the imu is the quad itself,
- // which is where this plugin is attached
- gz::sim::Entity gparent = _ecm.ParentEntity(parent);
- if (gparent != gz::sim::kNullEntity)
- {
- gz::sim::Model gparent_model(gparent);
- if (gparent_model.Name(_ecm) ==
- this->dataPtr->modelName)
- {
- this->dataPtr->modelLink = parent;
- imuTopicName = gz::sim::scopedName(
- _imu_entity, _ecm) + "/imu";
- gzdbg << "Computed IMU topic to be: "
- << imuTopicName << std::endl;
- }
- }
- }
- }
- return true;
- });
+ this->dataPtr->imuLink = parent;
+
+ imuTopicName = gz::sim::scopedName(
+ imuEntity, _ecm) + "/imu";
- if (imuTopicName.empty())
+ gzdbg << "Computed IMU topic to be: "
+ << imuTopicName << std::endl;
+ }
+ else
+ {
+ gzerr << "Parent of IMU sensor ["
+ << this->dataPtr->imuName
+ << "] is not a link\n";
+ }
+ }
+ }
+ else
{
gzerr << "[" << this->dataPtr->modelName << "] "
- << "imu_sensor [" << this->dataPtr->imuName
- << "] not found, abort ArduPilot plugin." << "\n";
+ << "imu_sensor [" << this->dataPtr->imuName
+ << "] not found, abort ArduPilot plugin." << "\n";
return;
}
@@ -909,18 +949,18 @@ void gz::sim::systems::ArduPilotPlugin::PreUpdate(
&gz::sim::systems::ArduPilotPluginPrivate::imuCb,
this->dataPtr.get());
- // Make sure that the "imu_link" entity has WorldPose
+ // Make sure that the 'imuLink' entity has WorldPose
// and WorldLinearVelocity components, which we'll need later.
- if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink,
+ if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink,
components::WorldPose::typeId))
{
- _ecm.CreateComponent(this->dataPtr->modelLink,
+ _ecm.CreateComponent(this->dataPtr->imuLink,
gz::sim::components::WorldPose());
}
- if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink,
+ if (!_ecm.EntityHasComponentType(this->dataPtr->imuLink,
components::WorldLinearVelocity::typeId))
{
- _ecm.CreateComponent(this->dataPtr->modelLink,
+ _ecm.CreateComponent(this->dataPtr->imuLink,
gz::sim::components::WorldLinearVelocity());
}
}
@@ -1082,11 +1122,14 @@ void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces(
gz::sim::components::JointVelocity* vComp =
_ecm.Component(
this->dataPtr->controls[i].joint);
- const double vel = vComp->Data()[0];
- const double error = vel - velTarget;
- const double force = this->dataPtr->controls[i].pid.Update(
- error, std::chrono::duration(_dt));
- jfcComp->Data()[0] = force;
+ if (vComp && !vComp->Data().empty())
+ {
+ const double vel = vComp->Data()[0];
+ const double error = vel - velTarget;
+ const double force = this->dataPtr->controls[i].pid.Update(
+ error, std::chrono::duration(_dt));
+ jfcComp->Data()[0] = force;
+ }
}
else if (this->dataPtr->controls[i].type == "POSITION")
{
@@ -1094,11 +1137,14 @@ void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces(
gz::sim::components::JointPosition* pComp =
_ecm.Component(
this->dataPtr->controls[i].joint);
- const double pos = pComp->Data()[0];
- const double error = pos - posTarget;
- const double force = this->dataPtr->controls[i].pid.Update(
- error, std::chrono::duration(_dt));
- jfcComp->Data()[0] = force;
+ if (pComp && !pComp->Data().empty())
+ {
+ const double pos = pComp->Data()[0];
+ const double error = pos - posTarget;
+ const double force = this->dataPtr->controls[i].pid.Update(
+ error, std::chrono::duration(_dt));
+ jfcComp->Data()[0] = force;
+ }
}
else if (this->dataPtr->controls[i].type == "EFFORT")
{
@@ -1478,11 +1524,11 @@ void gz::sim::systems::ArduPilotPlugin::CreateStateJSON(
// get pose and velocity in Gazebo world frame
const gz::sim::components::WorldPose* worldPose =
_ecm.Component(
- this->dataPtr->modelLink);
+ this->dataPtr->imuLink);
const gz::sim::components::WorldLinearVelocity* worldLinearVel =
_ecm.Component(
- this->dataPtr->modelLink);
+ this->dataPtr->imuLink);
// position and orientation transform (Aircraft world to Aircraft body)
gz::math::Pose3d bdyAToBdyG =
diff --git a/src/Util.cc b/src/Util.cc
new file mode 100644
index 0000000..894e0bd
--- /dev/null
+++ b/src/Util.cc
@@ -0,0 +1,115 @@
+/*
+ Copyright (C) 2022 ardupilot.org
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "Util.hh"
+
+#include
+#include
+#include
+#include
+#include
+
+namespace gz
+{
+namespace sim
+{
+inline namespace GZ_SIM_VERSION_NAMESPACE {
+
+//////////////////////////////////////////////////
+std::unordered_set EntitiesFromUnscopedName(
+ const std::string &_name, const EntityComponentManager &_ecm,
+ Entity _relativeTo)
+{
+ // holds entities that match
+ std::vector entities;
+
+ if (_relativeTo == kNullEntity)
+ {
+ // search everything
+ entities = _ecm.EntitiesByComponents(components::Name(_name));
+ }
+ else
+ {
+ // search all descendents
+ auto descendents = _ecm.Descendants(_relativeTo);
+ for (const auto& descendent : descendents)
+ {
+ if (_ecm.EntityHasComponentType(descendent,
+ gz::sim::components::Name::typeId))
+ {
+ auto nameComp = _ecm.Component(descendent);
+ if (nameComp->Data() == _name)
+ {
+ entities.push_back(descendent);
+ }
+ }
+ }
+
+ }
+ if (entities.empty())
+ return {};
+
+ return std::unordered_set(entities.begin(), entities.end());
+}
+
+//////////////////////////////////////////////////
+Entity JointByName(EntityComponentManager &_ecm,
+ Entity _modelEntity,
+ const std::string &_name)
+{
+ // Retrieve entities from a scoped name.
+ // See for example:
+ // https://github.com/gazebosim/ign-gazebo/pull/955
+ // which applies to the LiftDrag plugin
+ auto entities = entitiesFromScopedName(_name, _ecm, _modelEntity);
+
+ if (entities.empty())
+ {
+ gzerr << "Joint with name [" << _name << "] not found. "
+ << "The joint will not respond to ArduPilot commands\n";
+ return kNullEntity;
+ }
+ else if (entities.size() > 1)
+ {
+ gzwarn << "Multiple joint entities with name[" << _name << "] found. "
+ << "Using the first one.\n";
+ }
+
+ Entity joint = *entities.begin();
+
+ // Validate
+ if (!_ecm.EntityHasComponentType(joint,
+ components::Joint::typeId))
+ {
+ gzerr << "Entity with name[" << _name << "] is not a joint\n";
+ return kNullEntity;
+ }
+
+ // Ensure the joint has a velocity component
+ if (!_ecm.EntityHasComponentType(joint,
+ components::JointVelocity::typeId))
+ {
+ _ecm.CreateComponent(joint,
+ components::JointVelocity());
+ }
+
+ return joint;
+};
+
+}
+} // namespace sim
+} // namespace gz
diff --git a/worlds/iris_arducopter_runway.world b/worlds/iris_arducopter_runway.world
index 3149f17..b621fd0 100755
--- a/worlds/iris_arducopter_runway.world
+++ b/worlds/iris_arducopter_runway.world
@@ -39,7 +39,7 @@
- model://iris_with_standoffs
+ model://iris_with_ardupilot