From 1c1f8da7d9cc416aaa53d76254fe08c2e9fa65e6 Mon Sep 17 00:00:00 2001
From: Ayham Alharbat <44285656+ayhamalharbat@users.noreply.github.com>
Date: Thu, 27 Jul 2023 11:34:27 +0200
Subject: [PATCH] add a gz-omnicopter model (#21818)

* [not-working] add a gz-omnicopter model

* Fix axis directions on omnicopter model

The omnicopter joint axis directions had to be adapted for sdf 1.9 as it has different conventions for joint axis definitions.

* include model from gz-fuel & remove mesh files

* Fix omnicopter model using fuel

---------

Co-authored-by: Jaeyoung Lim <jalim@ethz.ch>
---
 .../init.d-posix/airframes/8011_gz_omnicopter | 122 ++++++++++++++++++
 .../init.d-posix/airframes/CMakeLists.txt     |   2 +
 .../gz/models/omnicopter/model.config         |  15 +++
 .../simulation/gz/models/omnicopter/model.sdf |   8 ++
 4 files changed, 147 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter
 create mode 100644 Tools/simulation/gz/models/omnicopter/model.config
 create mode 100644 Tools/simulation/gz/models/omnicopter/model.sdf

diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter
new file mode 100644
index 000000000000..707df1bba118
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter
@@ -0,0 +1,122 @@
+#!/bin/sh
+#
+# @name 6DoF Omnicopter SITL
+#
+# @type Quadrotor Wide
+#
+# @maintainer Jaeyoung Lim <jalim@ethz.ch>
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=omnicopter}
+
+param set-default CA_AIRFRAME 0
+
+param set-default CA_ROTOR_COUNT 8
+param set-default CA_R_REV 255
+
+param set-default CA_ROTOR0_PX 0.14435
+param set-default CA_ROTOR0_PY -0.14435
+param set-default CA_ROTOR0_PZ -0.14435
+param set-default CA_ROTOR0_KM 0.05 # CCW
+param set-default CA_ROTOR0_AX -0.788675
+param set-default CA_ROTOR0_AY -0.211325
+param set-default CA_ROTOR0_AZ -0.57735
+
+param set-default CA_ROTOR1_PX -0.14435
+param set-default CA_ROTOR1_PY -0.14435
+param set-default CA_ROTOR1_PZ -0.14435
+param set-default CA_ROTOR1_KM 0.05
+param set-default CA_ROTOR1_AX 0.211325
+param set-default CA_ROTOR1_AY -0.788675
+param set-default CA_ROTOR1_AZ 0.57735
+
+param set-default CA_ROTOR2_PX 0.14435
+param set-default CA_ROTOR2_PY 0.14435
+param set-default CA_ROTOR2_PZ -0.14435
+param set-default CA_ROTOR2_KM 0.05
+param set-default CA_ROTOR2_AX -0.211325
+param set-default CA_ROTOR2_AY 0.788675
+param set-default CA_ROTOR2_AZ 0.57735
+
+param set-default CA_ROTOR3_PX -0.14435
+param set-default CA_ROTOR3_PY 0.14435
+param set-default CA_ROTOR3_PZ -0.14435
+param set-default CA_ROTOR3_KM 0.05
+param set-default CA_ROTOR3_AX 0.788675
+param set-default CA_ROTOR3_AY 0.211325
+param set-default CA_ROTOR3_AZ -0.57735
+
+param set-default CA_ROTOR4_PX 0.14435
+param set-default CA_ROTOR4_PY -0.14435
+param set-default CA_ROTOR4_PZ 0.14435
+param set-default CA_ROTOR4_KM 0.05
+param set-default CA_ROTOR4_AX 0.788675
+param set-default CA_ROTOR4_AY 0.211325
+param set-default CA_ROTOR4_AZ -0.57735
+
+param set-default CA_ROTOR5_PX -0.14435
+param set-default CA_ROTOR5_PY -0.14435
+param set-default CA_ROTOR5_PZ 0.14435
+param set-default CA_ROTOR5_KM 0.05
+param set-default CA_ROTOR5_AX -0.211325
+param set-default CA_ROTOR5_AY 0.788675
+param set-default CA_ROTOR5_AZ 0.57735
+
+param set-default CA_ROTOR6_PX 0.14435
+param set-default CA_ROTOR6_PY 0.14435
+param set-default CA_ROTOR6_PZ 0.14435
+param set-default CA_ROTOR6_KM 0.05
+param set-default CA_ROTOR6_AX 0.211325
+param set-default CA_ROTOR6_AY -0.788675
+param set-default CA_ROTOR6_AZ 0.57735
+
+param set-default CA_ROTOR7_PX -0.14435
+param set-default CA_ROTOR7_PY 0.14435
+param set-default CA_ROTOR7_PZ 0.14435
+param set-default CA_ROTOR7_KM 0.05
+param set-default CA_ROTOR7_AX -0.788675
+param set-default CA_ROTOR7_AY -0.211325
+param set-default CA_ROTOR7_AZ -0.57735
+
+param set-default SIM_GZ_EN 1
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+
+param set-default SIM_GZ_EC_FUNC1 101
+param set-default SIM_GZ_EC_FUNC2 102
+param set-default SIM_GZ_EC_FUNC3 103
+param set-default SIM_GZ_EC_FUNC4 104
+param set-default SIM_GZ_EC_FUNC5 105
+param set-default SIM_GZ_EC_FUNC6 106
+param set-default SIM_GZ_EC_FUNC7 107
+param set-default SIM_GZ_EC_FUNC8 108
+
+param set-default SIM_GZ_EC_MIN1 0
+param set-default SIM_GZ_EC_MIN2 0
+param set-default SIM_GZ_EC_MIN3 0
+param set-default SIM_GZ_EC_MIN4 0
+param set-default SIM_GZ_EC_MIN5 0
+param set-default SIM_GZ_EC_MIN6 0
+param set-default SIM_GZ_EC_MIN7 0
+param set-default SIM_GZ_EC_MIN8 0
+
+param set-default SIM_GZ_EC_MAX1 1100
+param set-default SIM_GZ_EC_MAX2 1100
+param set-default SIM_GZ_EC_MAX3 1100
+param set-default SIM_GZ_EC_MAX4 1100
+param set-default SIM_GZ_EC_MAX5 1100
+param set-default SIM_GZ_EC_MAX6 1100
+param set-default SIM_GZ_EC_MAX7 1100
+param set-default SIM_GZ_EC_MAX8 1100
+# disable MC desaturation which improves attitude tracking
+param set-default CA_METHOD 0
+
+# disable attitude failure detection
+param set-default FD_FAIL_P 0
+param set-default FD_FAIL_R 0
+
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
index 743d54fc702e..0ba0c0be9d47 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -81,6 +81,8 @@ px4_add_romfs_files(
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
 
+	8011_gz_omnicopter
+
 	10015_gazebo-classic_iris
 	10016_none_iris
 	10017_jmavsim_iris
diff --git a/Tools/simulation/gz/models/omnicopter/model.config b/Tools/simulation/gz/models/omnicopter/model.config
new file mode 100644
index 000000000000..a090295062ec
--- /dev/null
+++ b/Tools/simulation/gz/models/omnicopter/model.config
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Omnicopter</name>
+  <version>1.0</version>
+  <sdf version='1.9'>model.sdf</sdf>
+
+  <author>
+   <name>Jaeyoung Lim</name>
+   <email>jalim@ethz.ch</email>
+  </author>
+
+  <description>
+    Omnicopter model for over actuated system
+  </description>
+</model>
diff --git a/Tools/simulation/gz/models/omnicopter/model.sdf b/Tools/simulation/gz/models/omnicopter/model.sdf
new file mode 100644
index 000000000000..ad2434fb67e6
--- /dev/null
+++ b/Tools/simulation/gz/models/omnicopter/model.sdf
@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version='1.9'>
+  <include>
+      <name>omnicopter</name>
+      <pose>0 0 0 0 0 0</pose>
+      <uri>https://fuel.gazebosim.org/1.0/PX4/models/Omnicopter</uri>
+    </include>
+</sdf>