Gazebo model for the fully-actuated hexarotor added

Updated README with some documentation
Added information on fully-actuated research
2026-05-28 07:40:06 +08:00 · 2020-12-06 20:51:48 -05:00 · 2020-11-21 16:35:26 -05:00 · 2020-11-11 20:06:28 -05:00 · 2020-09-29 14:01:24 -04:00 · 2020-09-22 11:53:14 -04:00
45 changed files with 3070 additions and 198 deletions
@@ -129,6 +129,46 @@
                }
            ]
        },
+        {
+            "label": "gazebo hexa_x_tilt",
+            "type": "shell",
+            "dependsOn": "gazebo build",
+            "options": {
+                "cwd": "${workspaceRoot}",
+                "env": {
+                    "GAZEBO_PLUGIN_PATH": "${workspaceRoot}/build/px4_sitl_default/build_gazebo",
+                    "GAZEBO_MODEL_PATH": "${workspaceRoot}/Tools/sitl_gazebo/models",
+                    "PX4_SIM_SPEED_FACTOR": "1"
+                }
+            },
+            "command": "gzserver --verbose ${workspaceRoot}/Tools/sitl_gazebo/worlds/hexa_x_tilt.world",
+            "isBackground": true,
+            "presentation": {
+                "echo": true,
+                "reveal": "never",
+                "focus": false,
+                "panel": "shared",
+                "showReuseMessage": false,
+                "clear": true
+            },
+            "problemMatcher": [
+                {
+                    "pattern": [
+                        {
+                            "regexp": ".",
+                            "file": 1,
+                            "location": 2,
+                            "message": 3
+                        }
+                    ],
+                    "background": {
+                        "activeOnStart": true,
+                        "beginsPattern": ".",
+                        "endsPattern": ".",
+                    }
+                }
+            ]
+        },
        {
            "label": "gazebo plane",
            "type": "shell",
@@ -1,103 +1,49 @@
-# PX4 Drone Autopilot
+# PX4 Autopilot Extended to Fully-Actuated Multirotors

-[![Releases](https://img.shields.io/github/release/PX4/Firmware.svg)](https://github.com/PX4/Firmware/releases) [![DOI](https://zenodo.org/badge/22634/PX4/Firmware.svg)](https://zenodo.org/badge/latestdoi/22634/PX4/Firmware)
+This is the repository for the PX4 autopilot with the enhanced ability to work with fully-actuated robots. It is based on the original PX4 firmware (current version branched from master on Jan 21, 2020) and supports all the functionality and airframes of the original PX4 in the same way. However, it additionally provides implementations of attitude strategies for fully-actuated multorotors that allow the 6-DoF flight. It includes the definitions for a hexarotor with all rotors tilted sideways and has been tested on several UAVs.

-[![Build Status](http://ci.px4.io:8080/buildStatus/icon?job=PX4/Firmware/master)](http://ci.px4.io:8080/blue/organizations/jenkins/PX4%2FFirmware/activity)
+For more information and to access the publications, please visit [the AirLab's website](http://theairlab.org/fully-actuated).

-[![Slack](https://px4-slack.herokuapp.com/badge.svg)](http://slack.px4.io)
+To access the original PX4 autopilot repository and website, please visit [here](https://github.com/PX4/PX4-Autopilot) and [here](https://px4.io/).

-This repository holds the [PX4](http://px4.io) flight control solution for drones, with the main applications located in the [src/modules](https://github.com/PX4/Firmware/tree/master/src/modules) directory. It also contains the PX4 Drone Middleware Platform, which provides drivers and middleware to run drones.
+### How to use the firmware

-* Official Website: http://px4.io (License: BSD 3-clause, [LICENSE](https://github.com/PX4/Firmware/blob/master/LICENSE))
-* [Supported airframes](https://docs.px4.io/en/airframes/airframe_reference.html) ([portfolio](http://px4.io/#airframes)):
-  * [Multicopters](https://docs.px4.io/en/airframes/airframe_reference.html#copter)
-  * [Fixed wing](https://docs.px4.io/en/airframes/airframe_reference.html#plane)
-  * [VTOL](https://docs.px4.io/en/airframes/airframe_reference.html#vtol)
-  * many more experimental types (Rovers, Blimps, Boats, Submarines, etc)
-* Releases: [Downloads](https://github.com/PX4/Firmware/releases)
+For general PX4 functionality, please visit [PX4 User Guide](https://docs.px4.io/master/en/) and [PX4 Developer Guide](https://dev.px4.io/master/en/).

+To be able to compile the firmware, you need to first [setup your system](https://dev.px4.io/master/en/setup/config_initial.html) and [install the toolchain](https://dev.px4.io/master/en/setup/dev_env.html). Then clone this repository and all its submodules:

-## PX4 Users
+```
+git clone https://github.com/castacks/PX4-fully-actuated.git --recursive
+```

-The [PX4 User Guide](https://docs.px4.io/en/) explains how to assemble [supported vehicles](https://docs.px4.io/en/airframes/airframe_reference.html) and fly drones with PX4.
-See the [forum and chat](https://docs.px4.io/en/#support) if you need help!
+Test that everything works fine using the guide on [this page](https://dev.px4.io/master/en/setup/building_px4.html#first-build-using-the-jmavsim-simulator). 

+The current tilted-hex airframe is defined to have 30 degrees of side tilt, similar to what is described in our papers. To change the geometry for your airframe, you can modify the definition of the tilted hex or add your own definition. To add the new definition file, please consult the PX4 guides above. To modify our airframe, you can find it [here](https://github.com/castacks/PX4-fully-actuated/blob/v1.10-master/src/lib/mixer/MultirotorMixer/geometries/hex_tilt_x.toml).

-## PX4 Developers
+After compiling the firmware and uploading it on your hardware (see [here](https://dev.px4.io/master/en/setup/building_px4.html)), choose `Hexarotor x with tilted arms` airframe. This firmware also adds a set of parameters all starting with `OMNI_`. To see the description of the parameters, please refer [here](https://github.com/castacks/PX4-fully-actuated/blob/v1.10-master/src/modules/mc_pos_control/mc_pos_control_params.c). The most important parameter is `OMNI_ATT_MODE` which supports the attitude strategies. 

-This [Developer Guide](https://dev.px4.io/) is for software developers who want to modify the flight stack and middleware (e.g. to add new flight modes), hardware integrators who want to support new flight controller boards and peripherals, and anyone who wants to get PX4 working on a new (unsupported) airframe/vehicle.
+We have tested the firmware only with Pixracer. If you have flash memory issues when compiling for your hardware, one solution is to comment the geometries that you don't need in [this CMakeLists file](https://github.com/castacks/PX4-fully-actuated/blob/v1.10-master/src/lib/mixer/MultirotorMixer/CMakeLists.txt).

-Developers should read the [Guide for Contributions](https://dev.px4.io/en/contribute/).
-See the [forum and chat](https://dev.px4.io/en/#support) if you need help!
+### Simulation and more

+More detailed guide will be incrementally added here. Please reach us directly or submit an issue on this repository if you have specific questions, concerns, feedback or bug reports.

-### Weekly Dev Call
+To simulate using gazebo, you need to add the model and the world of your fully-actuated UAV to the firmware. Optionally you can modify the model files for our hexarotor (with all arms tilted for 30 degrees). 

-The PX4 Dev Team syncs up on a [weekly dev call](https://dev.px4.io/en/contribute/#dev_call).
+To enable simulation for our multirotor, copy the contents of the `world` and `model` folders from `sitl_gazebo_fully_actuated` to the `world` and `model` folders in `Tools/sitl_gazebo`. Now you can run the SITL simulation of our UAV using the following command:

-> **Note** The dev call is open to all interested developers (not just the core dev team). This is a great opportunity to meet the team and contribute to the ongoing development of the platform. It includes a QA session for newcomers. All regular calls are listed in the [Dronecode calendar](https://www.dronecode.org/calendar/).
+```
+make px4_sitl gazebo_hexa_x_tilt
+```

+If you get `gazebo_opticalflow_plugin.h:43:18: error: ‘TRUE’ was not declared in this scope`, simply replace `TRUE` with `true` in `Tools/sitl_gazebo/include/gazebo_opticalflow_plugin.h` line 43.

-## Maintenance Team
+### Disclaimer

-  * Project: Founder - [Lorenz Meier](https://github.com/LorenzMeier), Architecture: [Daniel Agar](https://github.com/dagar)
-    * [Dev Call](https://github.com/PX4/Firmware/labels/devcall) - [Ramon Roche](https://github.com/mrpollo)
-  * Communication Architecture
-    * [Beat Kueng](https://github.com/bkueng)
-    * [Julian Oes](https://github.com/JulianOes)
-  * UI in QGroundControl
-    * [Gus Grubba](https://github.com/dogmaphobic)
-  * [Multicopter Flight Control](https://github.com/PX4/Firmware/labels/multicopter)
-    * [Mathieu Bresciani](https://github.com/bresch)
-  * [Multicopter Software Architecture](https://github.com/PX4/Firmware/labels/multicopter)
-    * [Matthias Grob](https://github.com/MaEtUgR)
-  * [VTOL Flight Control](https://github.com/PX4/Firmware/labels/vtol)
-    * [Roman Bapst](https://github.com/RomanBapst)
-  * [Fixed Wing Flight Control](https://github.com/PX4/Firmware/labels/fixedwing)
-    * [Roman Bapst](https://github.com/RomanBapst)
-  * OS / NuttX [David Sidrane](https://github.com/davids5)
-  * Driver Architecture [Daniel Agar](https://github.com/dagar)
-  * Commander Architecture [Julian Oes](https://github.com/julianoes)
-  * [UAVCAN](https://github.com/PX4/Firmware/labels/uavcan) [Daniel Agar](https://github.com/dagar)
-  * [State Estimation](https://github.com/PX4/Firmware/issues?q=is%3Aopen+is%3Aissue+label%3A%22state+estimation%22) - [Paul Riseborough](https://github.com/priseborough)
-  * Vision based navigation
-    * [Julian Kent](https://github.com/jkflying)
-  * Obstacle Avoidance - [Martina Rivizzigno](https://github.com/mrivi)
-  * RTPS/ROS2 Interface - [Nuno Marques](https://github.com/TSC21)
+Note that changing any of the parameters (e.g., the attitude modes) will affect the flight behavior of the UAV mid-flight. 

-See also [About Us](http://px4.io/about-us/#development_team) (px4.io) and the [contributors list](https://github.com/PX4/Firmware/graphs/contributors) (Github).
+Finally, we are still in the testing/development phase and the code is in no way fully tested or complete. Use the code at your own risk and please report any bugs or issues to help us make the firmware more useful for the users.

-## Supported Hardware
+### Acknowledgment

-This repository contains code supporting these boards:
-  * [Snapdragon Flight](https://docs.px4.io/master/en/flight_controller/snapdragon_flight.html)
-  * [Intel Aero](https://docs.px4.io/master/en/flight_controller/intel_aero.html)
-  * [Raspberry PI with Navio 2](https://docs.px4.io/master/en/flight_controller/raspberry_pi_navio2.html)
-  * FMUv2
-    * [Pixhawk](https://docs.px4.io/master/en/flight_controller/pixhawk.html)
-    * [Pixfalcon](https://docs.px4.io/master/en/flight_controller/pixfalcon.html)
-  * FMUv3
-    * [Pixhawk 2](https://docs.px4.io/master/en/flight_controller/pixhawk-2.html)
-    * [Pixhawk Mini](https://docs.px4.io/master/en/flight_controller/pixhawk_mini.html)
-    * [CUAV Pixhack v3](https://docs.px4.io/master/en/flight_controller/pixhack_v3.html)
-  * FMUv4
-    * [Pixracer](https://docs.px4.io/master/en/flight_controller/pixracer.html)
-    * [Pixhawk 3 Pro](https://docs.px4.io/master/en/flight_controller/pixhawk3_pro.html)
-  * FMUv5 (ARM Cortex M7)
-    * [Pixhawk 4](https://docs.px4.io/master/en/flight_controller/pixhawk4.html)
-    * [Pixhawk 4 mini](https://docs.px4.io/master/en/flight_controller/pixhawk4_mini.html)
-    * [CUAV V5+](https://docs.px4.io/master/en/flight_controller/cuav_v5_plus.html)
-    * [CUAV V5 nano](https://docs.px4.io/master/en/flight_controller/cuav_v5_nano.html)
-  * [Airmind MindPX V2.8](http://www.mindpx.net/assets/accessories/UserGuide_MindPX.pdf)
-  * [Airmind MindRacer V1.2](http://mindpx.net/assets/accessories/mindracer_user_guide_v1.2.pdf)
-  * [Bitcraze Crazyflie 2.0](https://docs.px4.io/master/en/flight_controller/crazyflie2.html)
-  * [Omnibus F4 SD](https://docs.px4.io/master/en/flight_controller/omnibus_f4_sd.html)
-  * [BeagleBone Blue](https://docs.px4.io/master/en/flight_controller/beaglebone_blue.html)
-  * [Holybro Durandal](https://docs.px4.io/master/en/flight_controller/durandal.html)
-  * [Holybro Kakute F7](https://docs.px4.io/master/en/flight_controller/kakutef7.html)
-
-Additional information about supported hardware can be found in [PX4 user Guide > Autopilot Hardware](https://docs.px4.io/master/en/flight_controller/).
-
-## Project Roadmap
-
-A high level project roadmap is available [here](https://www.dronecode.org/roadmap/).
+In the initial commits, the idea of how to extend PX4 to support 3-D thrust and fully-actuated vehicles have been taken from [here](https://github.com/jlecoeur/Firmware/pull/1).
@@ -0,0 +1,48 @@
+#!/bin/sh
+#
+# @name Hexarotor x with tilted arms
+#
+# @url http://theairlab.org/
+#
+# @type Tilt-Hexarotor
+# @class Copter
+#
+# @output AUX1 feed-through of RC AUX1 channel
+# @output AUX2 feed-through of RC AUX2 channel
+# @output AUX3 feed-through of RC AUX3 channel
+#
+# @maintainer Azarakhsh Keipour <keipour@cmu.edu>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $AUTOCNF = yes ]
+then
+	param set MC_PITCHRATE_P 0.1
+	param set MC_ROLLRATE_P 0.1
+	param set MC_YAWRATE_P 0.08
+	param set MPC_Z_VEL_I 0.15
+	param set MPC_Z_VEL_P 0.3
+	param set MC_PITCHRATE_MAX 20.0
+	param set MC_ROLLRATE_MAX 20.0
+	param set MC_YAWRATE_MAX 60.0
+	param set MPC_ACC_DOWN_MAX 2
+	param set MPC_ACC_HOR 0.5
+	param set MPC_ACC_HOR_MAX 2
+	param set MPC_ACC_UP_MAX 2
+	# param set MPC_XY_VEL_P 0.13
+	# param set MPC_XY_VEL_MAX 4
+
+	param set RTL_DESCEND_ALT 5.0
+	param set RTL_LAND_DELAY 5
+
+	param set SDLOG_PROFILE 179
+
+	param set OMNI_ATT_MODE 2
+	param set OMNI_DFC_MAX_THR 0.20
+fi
+
+set MAV_TYPE 13
+
+# Set mixer
+set MIXER hexa_tilt_x
@@ -0,0 +1,10 @@
+
+mixer append /dev/pwm_output0 etc/mixers/mount_legs.aux.mix
+
+mavlink start -x -u 14558 -r 4000 -f -m onboard -o 14530
+
+# shellcheck disable=SC2154
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_gcs_port_local
+# shellcheck disable=SC2154
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u $udp_offboard_port_local
+mavlink stream -r 10 -s MOUNT_ORIENTATION -u $udp_offboard_port_local
@@ -0,0 +1,58 @@
+#!/bin/sh
+#
+# @name Hexarotor x with tilted arms
+#
+# @url http://theairlab.org/
+#
+# @type Tilt-Hexarotor
+# @class Copter
+#
+# @output MAIN1 motor 1
+# @output MAIN2 motor 2
+# @output MAIN3 motor 3
+# @output MAIN4 motor 4
+# @output MAIN5 motor 5
+# @output MAIN6 motor 6
+
+#
+# @maintainer Azarakhsh Keipour <keipour@cmu.edu>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+if [ $AUTOCNF == yes ]
+then
+	param set MC_PITCH_P 4.0
+	param set MC_PITCHRATE_P 0.24
+	param set MC_PITCHRATE_I 0.09
+	param set MC_PITCHRATE_D 0.013
+	param set MC_PITCHRATE_MAX 180.0
+
+	param set MC_ROLL_P 4.0
+	param set MC_ROLLRATE_P 0.16
+	param set MC_ROLLRATE_I 0.07
+	param set MC_ROLLRATE_D 0.009
+	param set MC_ROLLRATE_MAX 180.0
+
+	param set MC_YAW_P 3.0
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
+	param set MC_YAWRATE_MAX 60.0
+
+	param set MPC_HOLD_MAX_XY 0.25
+	param set MPC_THR_MIN 0.15
+	param set MPC_Z_VEL_MAX_DN 2.0
+
+	param set BAT_N_CELLS 4
+
+	param set OMNI_ATT_MODE 2
+	param set OMNI_DFC_MAX_THR 0.20
+fi
+
+# Set mixer
+set MIXER hexa_tilt_x
+
+# Need to set all 8 channels
+set PWM_OUT 12345678
@@ -96,6 +96,7 @@ px4_add_romfs_files(
 	# [6000, 6999] Hexarotor x"
 	6001_hexa_x
 	6002_draco_r
+        6100_hexa_tilt_x

 	# [7000, 7999] Hexarotor +"
 	7001_hexa_+
@@ -18,6 +18,8 @@ then
 	param set PWM_MAX 1950
 	param set PWM_MIN 1075
 	param set PWM_RATE 400
+
+	param set OMNI_ATT_MODE 0
 fi

 #
@@ -53,7 +53,8 @@ px4_add_romfs_files(
 	generic_diff_rover.main.mix
 	hexa_cox.main.mix
 	hexa_+.main.mix
-	hexa_x.main.mix
+        hexa_tilt_x.main.mix
+        hexa_x.main.mix
 	IO_pass.main.mix
 	mount.aux.mix
 	mount_legs.aux.mix
@@ -0,0 +1,4 @@
+# Hexarotor with tilted arms
+
+# R: 6xt 10000 10000 10000 0
+S: 6xt 10000 10000 10000 10000 10000 10000 0
@@ -92,6 +92,8 @@ class ParameterGroup(object):
            return "Rover"
        elif (self.name == "Boat"):
            return "Boat"
+        elif (self.name == "Tilt-Hexarotor"):
+            return "HexaRotorX"
        return "AirframeUnknown"

    def GetParams(self):
@@ -82,6 +82,7 @@ set(msg_files
 	mount_orientation.msg
 	multirotor_motor_limits.msg
 	obstacle_distance.msg
+	omni_attitude_status.msg
 	offboard_control_mode.msg
 	onboard_computer_status.msg
 	optical_flow.msg
@@ -1,5 +1,5 @@
 uint64 timestamp			# time since system start (microseconds)
-uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROLS = 11
 uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
 uint8 INDEX_ROLL = 0
 uint8 INDEX_PITCH = 1
@@ -9,10 +9,13 @@ uint8 INDEX_FLAPS = 4
 uint8 INDEX_SPOILERS = 5
 uint8 INDEX_AIRBRAKES = 6
 uint8 INDEX_LANDING_GEAR = 7
+uint8 INDEX_X_THRUST = 8
+uint8 INDEX_Y_THRUST = 9
+uint8 INDEX_Z_THRUST = 10
 uint8 GROUP_INDEX_ATTITUDE = 0
 uint8 GROUP_INDEX_ATTITUDE_ALTERNATE = 1
 uint64 timestamp_sample	    # the timestamp the data this control response is based on was sampled
-float32[8] control
+float32[11] control

 # TOPICS actuator_controls actuator_controls_0 actuator_controls_1 actuator_controls_2 actuator_controls_3
 # TOPICS actuator_controls_virtual_fw actuator_controls_virtual_mc
@@ -1,14 +1,18 @@
 uint64 timestamp		# time since system start (microseconds)
 uint16 saturation_status	# Integer bit mask indicating which axes in the control mixer are saturated

-# 0 - True if the saturation status is valid
-# 1 - True if any motor is saturated at the upper limit
-# 2 - True if any motor is saturated at the lower limit
-# 3 - True if a positive roll increment will increase motor saturation
-# 4 - True if negative roll increment will increase motor saturation
-# 5 - True if positive pitch increment will increase motor saturation
-# 6 - True if negative pitch increment will increase motor saturation
-# 7 - True if positive yaw increment will increase motor saturation
-# 8 - True if negative yaw increment will increase motor saturation
-# 9 - True if positive thrust increment will increase motor saturation
-# 10 - True if negative thrust increment will increase motor saturation
+uint8 INDEX_VALID = 0           # True if the saturation status is valid
+uint8 INDEX_MOTOR_POS = 1       # True if any motor is saturated at the upper limit
+uint8 INDEX_MOTOR_NEG = 2       # True if any motor is saturated at the lower limit
+uint8 INDEX_ROLL_POS = 3        # True if a positive roll increment will increase motor saturation
+uint8 INDEX_ROLL_NEG = 4        # True if negative roll increment will increase motor saturation
+uint8 INDEX_PITCH_POS = 5       # True if positive pitch increment will increase motor saturation
+uint8 INDEX_PITCH_NEG = 6       # True if negative pitch increment will increase motor saturation
+uint8 INDEX_YAW_POS = 7         # True if positive yaw increment will increase motor saturation
+uint8 INDEX_YAW_NEG = 8         # True if negative yaw increment will increase motor saturation
+uint8 INDEX_X_THRUST_POS = 9    # True if positive X thrust increment will increase motor saturation
+uint8 INDEX_X_THRUST_NEG = 10   # True if negative X thrust increment will increase motor saturation
+uint8 INDEX_Y_THRUST_POS = 11   # True if positive Y thrust increment will increase motor saturation
+uint8 INDEX_Y_THRUST_NEG = 12   # True if negative Y thrust increment will increase motor saturation
+uint8 INDEX_Z_THRUST_POS = 13   # True if positive Z thrust increment will increase motor saturation
+uint8 INDEX_Z_THRUST_NEG = 14   # True if negative Z thrust increment will increase motor saturation
@@ -0,0 +1,14 @@
+uint64 timestamp		# time since system start (microseconds)
+
+float32 att_mode		# attitude modes as defined in OMNI_ATT_MODE parameter
+
+float32 tilt_angle_est		# estimated optimal tilt angle of the robot in rad
+float32 tilt_angle_meas		# measured tilt angle of the robot in rad
+
+float32 tilt_direction_est	# estimated optimal tilt direction in local NED frame
+float32 tilt_direction_meas	# tilt direction in local NED frame
+
+float32 tilt_roll_est		# estimated optimal roll angle of the robot in rad
+float32 tilt_pitch_est		# estimated optimal pitch angle of the robot in rad
+
+# TOPICS omni_attitude_status
@@ -50,6 +50,55 @@
                ]
            }
        },
+        {
+            "name": "SITL (gazebo tilthex)",
+            "type": "cppdbg",
+            "request": "launch",
+            "program": "${command:cmake.launchTargetPath}",
+            "args": [
+                "${workspaceFolder}/ROMFS/px4fmu_common",
+                "-s",
+                "etc/init.d-posix/rcS",
+                "-t",
+                "${workspaceFolder}/test_data"
+            ],
+            "stopAtEntry": false,
+            "cwd": "${command:cmake.buildDirectory}/tmp",
+            "environment": [
+                {
+                    "name": "PX4_SIM_MODEL",
+                    "value": "hexa_x_tilt"
+                }
+            ],
+            "externalConsole": false,
+            "preLaunchTask": "gazebo hexa_x_tilt",
+            "postDebugTask": "gazebo kill",
+            "linux": {
+                "MIMode": "gdb",
+                "externalConsole": false,
+                "setupCommands": [
+                    {
+                        "description": "Enable pretty-printing for gdb",
+                        "text": "-enable-pretty-printing",
+                        "ignoreFailures": true
+                    },
+                    {
+                        "description": "PX4 ignore wq signals",
+                        "text": "handle SIGCONT nostop noprint nopass",
+                        "ignoreFailures": true
+                    }
+                ]
+            },
+            "osx": {
+                "MIMode": "lldb",
+                "externalConsole": true,
+                "setupCommands": [
+                    {
+                        "text": "pro hand -p true -s false -n false SIGCONT",
+                    }
+                ]
+            }
+        },
        {
            "name": "SITL (gazebo plane)",
            "type": "cppdbg",
@@ -63,7 +63,7 @@ set(debuggers none ide gdb lldb ddd valgrind callgrind)
 set(models none shell
 	if750a iris iris_opt_flow iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps solo typhoon_h480
 	plane
-	standard_vtol tailsitter tiltrotor
+	standard_vtol tailsitter tiltrotor hexa_x_tilt
 	hippocampus rover)
 set(all_posix_vmd_make_targets)
 foreach(viewer ${viewers})
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<model>
+  <name>Tilt-Hex</name>
+  <version>1.0</version>
+  <sdf version='1.6'>hexa_x_tilt.sdf</sdf>
+
+  <author>
+   <name>Azarakhsh Keipour</name>
+   <email>keipour@cmu.edu</email>
+  </author>
+
+  <description>
+    This is a model of omnidirectional hexarotor with tilted rotors.
+  </description>
+</model>
+
@@ -0,0 +1,52 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <!-- A ground plane -->
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://hexa_x_tilt</uri>
+    </include>
+    <include>
+      <uri>model://asphalt_plane</uri>
+    </include>
+    <physics name='default_physics' default='0' type='ode'>
+      <gravity>0 0 -9.8066</gravity>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>50</iters>
+          <sor>1.0</sor>
+          <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>100</contact_max_correcting_vel>
+          <contact_surface_layer>0.001</contact_surface_layer>
+        </constraints>
+      </ode>
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>250</real_time_update_rate>
+      <magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
+    </physics>
+    <!-- <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose frame=''>5.4634 -5.46339 2.17586 0 0.275643 2.35619</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+        <track_visual>
+          <name>iris</name>
+          <use_model_frame>1</use_model_frame>
+        </track_visual>
+      </camera>
+    </gui>
+-->
+  </world>
+</sdf>
@@ -200,6 +200,10 @@ MixerGroup::load_from_buf(Mixer::ControlCallback control_cb, uintptr_t cb_handle
 			m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
 			break;

+		case 'S':
+			m = MultirotorMixer::from_text(control_cb, cb_handle, p, resid);
+			break;
+
 		case 'H':
 			m = HelicopterMixer::from_text(control_cb, cb_handle, p, resid);
 			break;
@@ -38,6 +38,7 @@ set(geometry_files
 	hex_plus.toml
 	hex_t.toml
 	hex_x.toml
+        hex_tilt_x.toml
 	octa_cox.toml
 	octa_cox_wide.toml
 	octa_plus.toml
@@ -75,25 +76,16 @@ add_custom_command(
 	)
 add_custom_target(mixer_gen DEPENDS mixer_multirotor.generated.h ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h)

-# 6DOF mixers
-add_custom_command(
-	OUTPUT ${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
-	COMMAND ${PYTHON_EXECUTABLE} ${MIXER_TOOLS}/px_generate_mixers.py --sixdof -f ${geometries_list} -o mixer_multirotor_6dof.generated.h
-	DEPENDS ${MIXER_TOOLS}/px_generate_mixers.py ${geometries_list}
-	)
-add_custom_target(mixer_gen_6dof DEPENDS mixer_multirotor_6dof.generated.h)
-
 add_library(MultirotorMixer
 	MultirotorMixer.cpp
 	MultirotorMixer.hpp

 	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor.generated.h
 	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_normalized.generated.h
-	${CMAKE_CURRENT_BINARY_DIR}/mixer_multirotor_6dof.generated.h
 )
 target_link_libraries(MultirotorMixer PRIVATE Mixer)
 target_include_directories(MultirotorMixer PRIVATE ${CMAKE_CURRENT_BINARY_DIR})
-add_dependencies(MultirotorMixer mixer_gen mixer_gen_6dof prebuild_targets)
+add_dependencies(MultirotorMixer mixer_gen prebuild_targets)

 if(BUILD_TESTING)

@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2019 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
@@ -87,6 +87,22 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle
 	_pitch_scale = pitch_scale;
 	_yaw_scale = yaw_scale;
 	_idle_speed = -1.0f + idle_speed * 2.0f;	/* shift to output range here to avoid runtime calculation */
+	_is_6dof = false;
+}
+
+MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
+				 float roll_scale, float pitch_scale, float yaw_scale,
+				 float x_scale, float y_scale, float z_scale, float idle_speed) :
+	MultirotorMixer(control_cb, cb_handle, _config_index_6dof[(int)geometry], _config_rotor_count[(int)geometry])
+{
+	_roll_scale = roll_scale;
+	_pitch_scale = pitch_scale;
+	_yaw_scale = yaw_scale;
+	_x_scale = x_scale;
+	_y_scale = y_scale;
+	_z_scale = z_scale;
+	_idle_speed = -1.0f + idle_speed * 2.0f;	/* shift to output range here to avoid runtime calculation */
+	_is_6dof = true;
 }

 MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors,
@@ -102,6 +118,19 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle
 	}
 }

+MultirotorMixer::MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor6Dof *rotors,
+				 unsigned rotor_count) :
+	Mixer(control_cb, cb_handle),
+	_rotor_count(rotor_count),
+	_rotors_6dof(rotors),
+	_outputs_prev(new float[_rotor_count]),
+	_tmp_array(new float[_rotor_count])
+{
+	for (unsigned i = 0; i < _rotor_count; ++i) {
+		_outputs_prev[i] = _idle_speed;
+	}
+}
+
 MultirotorMixer::~MultirotorMixer()
 {
 	delete[] _outputs_prev;
@@ -113,7 +142,7 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 {
 	MultirotorGeometry geometry = MultirotorGeometry::MAX_GEOMETRY;
 	char geomname[8];
-	int s[4];
+	int s[7];
 	int used;

 	/* enforce that the mixer ends with a new line */
@@ -121,9 +150,20 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 		return nullptr;
 	}

-	if (sscanf(buf, "R: %7s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
-		debug("multirotor parse failed on '%s'", buf);
-		return nullptr;
+	bool is_6dof = (buf[0] == 'S');
+
+	if (is_6dof) {
+		if (sscanf(buf, "S: %7s %d %d %d %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &s[4], &s[5], &s[6],
+			   &used) != 8) {
+			debug("multirotor parse failed on '%s'", buf);
+			return nullptr;
+		}
+
+	} else {
+		if (sscanf(buf, "R: %7s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
+			debug("multirotor parse failed on '%s'", buf);
+			return nullptr;
+		}
 	}

 	if (used > (int)buflen) {
@@ -155,14 +195,29 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl

 	debug("adding multirotor mixer '%s'", geomname);

-	return new MultirotorMixer(
-		       control_cb,
-		       cb_handle,
-		       geometry,
-		       s[0] / 10000.0f,
-		       s[1] / 10000.0f,
-		       s[2] / 10000.0f,
-		       s[3] / 10000.0f);
+	if (is_6dof) {
+		return new MultirotorMixer(
+			       control_cb,
+			       cb_handle,
+			       geometry,
+			       s[0] / 10000.0f,
+			       s[1] / 10000.0f,
+			       s[2] / 10000.0f,
+			       s[3] / 10000.0f,
+			       s[4] / 10000.0f,
+			       s[5] / 10000.0f,
+			       s[6] / 10000.0f);
+
+	} else {
+		return new MultirotorMixer(
+			       control_cb,
+			       cb_handle,
+			       geometry,
+			       s[0] / 10000.0f,
+			       s[1] / 10000.0f,
+			       s[2] / 10000.0f,
+			       s[3] / 10000.0f);
+	}
 }

 float
@@ -248,6 +303,30 @@ MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust
 	mix_yaw(yaw, outputs);
 }

+void
+MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
+				float *outputs)
+{
+	// Airmode for roll and pitch, but not yaw in 6-DoF vehicles
+
+	// Mix without yaw
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors_6dof[i].roll_scale +
+			     pitch * _rotors_6dof[i].pitch_scale +
+			     x_thrust * _rotors_6dof[i].x_scale +
+			     y_thrust * _rotors_6dof[i].y_scale +
+			     z_thrust * _rotors_6dof[i].z_scale;
+
+		// Z thrust will be used to unsaturate if needed
+		_tmp_array[i] = math::abs_t<float>(_rotors_6dof[i].z_scale);
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Mix yaw independently
+	mix_yaw(yaw, outputs);
+}
+
 void
 MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs)
 {
@@ -310,14 +389,52 @@ MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float
 	mix_yaw(yaw, outputs);
 }

+void
+MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust,
+				      float z_thrust, float *outputs)
+{
+	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
+
+	// Mix without yaw
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors_6dof[i].roll_scale +
+			     pitch * _rotors_6dof[i].pitch_scale +
+			     x_thrust * _rotors_6dof[i].x_scale +
+			     y_thrust * _rotors_6dof[i].y_scale +
+			     z_thrust * _rotors_6dof[i].z_scale;
+
+		// Z thrust will be used to unsaturate if needed
+		_tmp_array[i] = math::abs_t<float>(_rotors_6dof[i].z_scale);
+	}
+
+	// only reduce thrust
+	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
+
+	// Reduce roll/pitch acceleration if needed to unsaturate
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors_6dof[i].roll_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors_6dof[i].pitch_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Mix yaw independently
+	mix_yaw(yaw, outputs);
+}
+
 void MultirotorMixer::mix_yaw(float yaw, float *outputs)
 {
 	// Add yaw to outputs
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		outputs[i] += yaw * _rotors[i].yaw_scale;
+		outputs[i] += yaw * (_is_6dof ? _rotors_6dof[i].yaw_scale : _rotors[i].yaw_scale);

 		// Yaw will be used to unsaturate if needed
-		_tmp_array[i] = _rotors[i].yaw_scale;
+		_tmp_array[i] = (_is_6dof ? _rotors_6dof[i].yaw_scale : _rotors[i].yaw_scale);
 	}

 	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
@@ -325,7 +442,7 @@ void MultirotorMixer::mix_yaw(float yaw, float *outputs)
 	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);

 	for (unsigned i = 0; i < _rotor_count; i++) {
-		_tmp_array[i] = _rotors[i].thrust_scale;
+		_tmp_array[i] = (_is_6dof ? _rotors_6dof[i].z_scale : _rotors[i].thrust_scale);
 	}

 	// reduce thrust only
@@ -342,25 +459,44 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float roll    = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
 	float pitch   = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
 	float yaw     = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
-	float thrust  = math::constrain(get_control(0, 3), 0.0f, 1.0f);

 	// clean out class variable used to capture saturation
 	_saturation_status.value = 0;

-	// Do the mixing using the strategy given by the current Airmode configuration
-	switch (_airmode) {
-	case Airmode::roll_pitch:
-		mix_airmode_rp(roll, pitch, yaw, thrust, outputs);
-		break;
+	if (!_is_6dof) {
+		// Do the mixing using the strategy given by the current Airmode configuration
+		switch (_airmode) {
+		case Airmode::roll_pitch:
+			mix_airmode_rp(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs);
+			break;

-	case Airmode::roll_pitch_yaw:
-		mix_airmode_rpy(roll, pitch, yaw, thrust, outputs);
-		break;
+		case Airmode::roll_pitch_yaw:
+			mix_airmode_rpy(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs);
+			break;

-	case Airmode::disabled:
-	default: // just in case: default to disabled
-		mix_airmode_disabled(roll, pitch, yaw, thrust, outputs);
-		break;
+		case Airmode::disabled:
+		default: // just in case: default to disabled
+			mix_airmode_disabled(roll, pitch, yaw, math::constrain(get_control(0, 3), 0.0f, 1.0f), outputs);
+			break;
+
+		}
+
+	} else { // if is 6-DoF vehicle
+		// Do the mixing using the strategy given by the current Airmode configuration
+		float x_thrust  = math::constrain(get_control(0, 8), -1.0f, 1.0f);
+		float y_thrust  = math::constrain(get_control(0, 9), -1.0f, 1.0f);
+		float z_thrust  = math::constrain(get_control(0, 10), -1.0f, 1.0f);
+
+		switch (_airmode) {
+		case Airmode::roll_pitch:
+			mix_airmode_rp(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
+			break;
+
+		case Airmode::disabled:
+		default: // just in case: default to disabled
+			mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
+			break;
+		}
 	}

 	// Apply thrust model and scale outputs to range [idle_speed, 1].
@@ -438,80 +574,161 @@ void
 MultirotorMixer::update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch,
 		bool clipping_low_yaw)
 {
+	float roll_scale = (_is_6dof) ? _rotors_6dof[index].roll_scale : _rotors[index].roll_scale;
+	float pitch_scale = (_is_6dof) ? _rotors_6dof[index].pitch_scale : _rotors[index].pitch_scale;
+	float yaw_scale = (_is_6dof) ? _rotors_6dof[index].yaw_scale : _rotors[index].yaw_scale;
+	float x_scale = (_is_6dof) ? _rotors_6dof[index].x_scale : 0.f;
+	float y_scale = (_is_6dof) ? _rotors_6dof[index].y_scale : 0.f;
+	float z_scale = (_is_6dof) ? _rotors_6dof[index].z_scale : 0.f;
+
 	// The motor is saturated at the upper limit
 	// check which control axes and which directions are contributing
 	if (clipping_high) {
-		if (_rotors[index].roll_scale > 0.0f) {
+		if (roll_scale > 0.0f) {
 			// A positive change in roll will increase saturation
 			_saturation_status.flags.roll_pos = true;

-		} else if (_rotors[index].roll_scale < 0.0f) {
+		} else if (roll_scale < 0.0f) {
 			// A negative change in roll will increase saturation
 			_saturation_status.flags.roll_neg = true;
 		}

 		// check if the pitch input is saturating
-		if (_rotors[index].pitch_scale > 0.0f) {
+		if (pitch_scale > 0.0f) {
 			// A positive change in pitch will increase saturation
 			_saturation_status.flags.pitch_pos = true;

-		} else if (_rotors[index].pitch_scale < 0.0f) {
+		} else if (pitch_scale < 0.0f) {
 			// A negative change in pitch will increase saturation
 			_saturation_status.flags.pitch_neg = true;
 		}

 		// check if the yaw input is saturating
-		if (_rotors[index].yaw_scale > 0.0f) {
+		if (yaw_scale > 0.0f) {
 			// A positive change in yaw will increase saturation
 			_saturation_status.flags.yaw_pos = true;

-		} else if (_rotors[index].yaw_scale < 0.0f) {
+		} else if (yaw_scale < 0.0f) {
 			// A negative change in yaw will increase saturation
 			_saturation_status.flags.yaw_neg = true;
 		}

-		// A positive change in thrust will increase saturation
-		_saturation_status.flags.thrust_pos = true;
+		if (_is_6dof) {
+			// check if the x input is saturating
+			if (x_scale > 0.0f) {
+				// A positive change in x will increase saturation
+				_saturation_status.flags.x_thrust_pos = true;
+
+			} else if (x_scale < 0.0f) {
+				// A negative change in x will increase saturation
+				_saturation_status.flags.x_thrust_neg = true;
+			}
+
+			// check if the y input is saturating
+			if (y_scale > 0.0f) {
+				// A positive change in y will increase saturation
+				_saturation_status.flags.y_thrust_pos = true;
+
+			} else if (y_scale < 0.0f) {
+				// A negative change in y will increase saturation
+				_saturation_status.flags.y_thrust_neg = true;
+			}
+
+			// check if the z input is saturating
+			if (z_scale > 0.0f) {
+				// A positive change in z will increase saturation
+				_saturation_status.flags.z_thrust_pos = true;
+
+			} else if (z_scale < 0.0f) {
+				// A negative change in z will increase saturation
+				_saturation_status.flags.z_thrust_neg = true;
+			}
+
+		} else {
+			// A negative change in Z thrust (positive change in total thrust) will increase saturation
+			_saturation_status.flags.z_thrust_neg = true;
+		}
 	}

 	// The motor is saturated at the lower limit
 	// check which control axes and which directions are contributing
 	if (clipping_low_roll_pitch) {
 		// check if the roll input is saturating
-		if (_rotors[index].roll_scale > 0.0f) {
+		if (roll_scale > 0.0f) {
 			// A negative change in roll will increase saturation
 			_saturation_status.flags.roll_neg = true;

-		} else if (_rotors[index].roll_scale < 0.0f) {
+		} else if (roll_scale < 0.0f) {
 			// A positive change in roll will increase saturation
 			_saturation_status.flags.roll_pos = true;
 		}

 		// check if the pitch input is saturating
-		if (_rotors[index].pitch_scale > 0.0f) {
+		if (pitch_scale > 0.0f) {
 			// A negative change in pitch will increase saturation
 			_saturation_status.flags.pitch_neg = true;

-		} else if (_rotors[index].pitch_scale < 0.0f) {
+		} else if (pitch_scale < 0.0f) {
 			// A positive change in pitch will increase saturation
 			_saturation_status.flags.pitch_pos = true;
 		}

-		// A negative change in thrust will increase saturation
-		_saturation_status.flags.thrust_neg = true;
+		if (_is_6dof) {
+			// check if the x input is saturating
+			if (x_scale > 0.0f) {
+				// A negative change in x will increase saturation
+				_saturation_status.flags.x_thrust_neg = true;
+
+			} else if (x_scale < 0.0f) {
+				// A positive change in x will increase saturation
+				_saturation_status.flags.x_thrust_pos = true;
+			}
+
+			// check if the y input is saturating
+			if (y_scale > 0.0f) {
+				// A negative change in y will increase saturation
+				_saturation_status.flags.y_thrust_neg = true;
+
+			} else if (y_scale < 0.0f) {
+				// A positive change in y will increase saturation
+				_saturation_status.flags.y_thrust_pos = true;
+			}
+
+			// check if the z input is saturating
+			if (z_scale > 0.0f) {
+				// A negative change in z will increase saturation
+				_saturation_status.flags.z_thrust_neg = true;
+
+			} else if (z_scale < 0.0f) {
+				// A positive change in z will increase saturation
+				_saturation_status.flags.z_thrust_pos = true;
+			}
+
+		} else {
+			// A positive change in Z thrust (negative change in total thrust) will increase saturation
+			_saturation_status.flags.z_thrust_pos = true;
+		}
 	}

 	if (clipping_low_yaw) {
 		// check if the yaw input is saturating
-		if (_rotors[index].yaw_scale > 0.0f) {
+		if (yaw_scale > 0.0f) {
 			// A negative change in yaw will increase saturation
 			_saturation_status.flags.yaw_neg = true;

-		} else if (_rotors[index].yaw_scale < 0.0f) {
+		} else if (yaw_scale < 0.0f) {
 			// A positive change in yaw will increase saturation
 			_saturation_status.flags.yaw_pos = true;
 		}
 	}

+	if (!_is_6dof) {
+		// X and Y thrusts are not controlled
+		_saturation_status.flags.x_thrust_pos = true;
+		_saturation_status.flags.x_thrust_neg = true;
+		_saturation_status.flags.y_thrust_pos = true;
+		_saturation_status.flags.y_thrust_neg = true;
+	}
+
 	_saturation_status.flags.valid = true;
 }
@@ -46,22 +46,30 @@ enum class MultirotorGeometry : MultirotorGeometryUnderlyingType;
 /**
 * Multi-rotor mixer for pre-defined vehicle geometries.
 *
- * Collects four inputs (roll, pitch, yaw, thrust) and mixes them to
+ * Collects either four inputs (roll, pitch, yaw, thrust) or six inputs
+ * (roll, pitch, yaw, x thrust, y thrust and z thrust) and mixes them to
 * a set of outputs based on the configured geometry.
 */
 class MultirotorMixer : public Mixer
 {
 public:
 	/**
-
 	 * Precalculated rotor mix.
 	 */
 	struct Rotor {
-		float	roll_scale;		/**< scales roll for this rotor */
+		float	roll_scale;	/**< scales roll for this rotor */
 		float	pitch_scale;	/**< scales pitch for this rotor */
-		float	yaw_scale;		/**< scales yaw for this rotor */
+		float	yaw_scale;	/**< scales yaw for this rotor */
 		float	thrust_scale;	/**< scales thrust for this rotor */
 	};
+	struct Rotor6Dof {
+		float	roll_scale;	/**< scales roll for this rotor */
+		float	pitch_scale;	/**< scales pitch for this rotor */
+		float	yaw_scale;	/**< scales yaw for this rotor */
+		float	x_scale;	/**< scales x thrust for this rotor */
+		float	y_scale;	/**< scales y thrust for this rotor */
+		float	z_scale;	/**< scales z thrust for this rotor */
+	};

 	/**
 	 * Constructor.
@@ -73,7 +81,7 @@ public:
 	 *				compared to thrust.
 	 * @param pitch_scale		Scaling factor applied to pitch inputs
 	 *				compared to thrust.
-	 * @param yaw_wcale		Scaling factor applied to yaw inputs compared
+	 * @param yaw_scale		Scaling factor applied to yaw inputs compared
 	 *				to thrust.
 	 * @param idle_speed		Minimum rotor control output value; usually
 	 *				tuned to ensure that rotors never stall at the
@@ -82,6 +90,32 @@ public:
 	MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
 			float roll_scale, float pitch_scale, float yaw_scale, float idle_speed);

+	/**
+	 * Constructor for 6-DoF.
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param geometry		The selected geometry.
+	 * @param roll_scale		Scaling factor applied to roll inputs
+	 *				compared to thrust.
+	 * @param pitch_scale		Scaling factor applied to pitch inputs
+	 *				compared to thrust.
+	 * @param yaw_scale		Scaling factor applied to yaw inputs compared
+	 *				to thrust.
+	 * @param x_scale		Scaling factor applied to x thrust inputs
+	 *				compared to thrust.
+	 * @param y_scale		Scaling factor applied to y thrust inputs
+	 *				compared to thrust.
+	 * @param z_scale		Scaling factor applied to z thrust inputs
+	 *				compared to thrust.
+	 * @param idle_speed		Minimum rotor control output value; usually
+	 *				tuned to ensure that rotors never stall at the
+	 * 				low end of their control range.
+	 */
+	MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, MultirotorGeometry geometry,
+			float roll_scale, float pitch_scale, float yaw_scale,
+			float x_scale, float y_scale, float z_scale, float idle_speed);
+
 	/**
 	 * Constructor (for testing).
 	 *
@@ -91,6 +125,17 @@ public:
 	 * @param rotor_count		length of rotors array (= number of motors)
 	 */
 	MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor *rotors, unsigned rotor_count);
+
+	/**
+	 * Constructor (for testing 6-DoF).
+	 *
+	 * @param control_cb		Callback invoked to read inputs.
+	 * @param cb_handle		Passed to control_cb.
+	 * @param rotors		control allocation matrix
+	 * @param rotor_count		length of rotors array (= number of motors)
+	 */
+	MultirotorMixer(ControlCallback control_cb, uintptr_t cb_handle, const Rotor6Dof *rotors, unsigned rotor_count);
+
 	virtual ~MultirotorMixer();

 	// no copy, assignment, move, move assignment
@@ -161,8 +206,12 @@ public:
 			uint16_t pitch_neg	: 1; // 6 - true when a negative pitch demand change will increase saturation
 			uint16_t yaw_pos	: 1; // 7 - true when a positive yaw demand change will increase saturation
 			uint16_t yaw_neg	: 1; // 8 - true when a negative yaw demand change will increase saturation
-			uint16_t thrust_pos	: 1; // 9 - true when a positive thrust demand change will increase saturation
-			uint16_t thrust_neg	: 1; //10 - true when a negative thrust demand change will increase saturation
+			uint16_t x_thrust_pos	: 1; // 9 - true when a positive x thrust demand change will increase saturation
+			uint16_t x_thrust_neg	: 1; //10 - true when a negative x thrust demand change will increase saturation
+			uint16_t y_thrust_pos	: 1; //11 - true when a positive y thrust demand change will increase saturation
+			uint16_t y_thrust_neg	: 1; //12 - true when a negative y thrust demand change will increase saturation
+			uint16_t z_thrust_pos	: 1; //13 - true when a positive z thrust demand change will increase saturation
+			uint16_t z_thrust_neg	: 1; //14 - true when a negative z thrust demand change will increase saturation
 		} flags;
 		uint16_t value;
 	};
@@ -182,14 +231,14 @@ private:
 	 * Minimize the saturation of the actuators by adding or substracting a fraction of desaturation_vector.
 	 * desaturation_vector is the vector that added to the output outputs, modifies the thrust or angular
 	 * acceleration on a specific axis.
-	 * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale), the
+	 * For example, if desaturation_vector is given to slide along the vertical thrust axis (thrust_scale/z_scale), the
 	 * saturation will be minimized by shifting the vertical thrust setpoint, without changing the
 	 * roll/pitch/yaw accelerations.
 	 *
 	 * Note that as we only slide along the given axis, in extreme cases outputs can still contain values
 	 * outside of [min_output, max_output].
 	 *
-	 * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale
+	 * @param desaturation_vector vector that is added to the outputs, e.g. thrust_scale/z_scale
 	 * @param outputs output vector that is modified
 	 * @param sat_status saturation status output
 	 * @param min_output minimum desired value in outputs
@@ -208,6 +257,16 @@ private:
 	 */
 	inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);

+	/**
+	 * Mix roll, pitch, yaw, x_thrust, y_thrust, z_thrust and set the outputs vector for 6-DoF vehicles.
+	 *
+	 * Desaturation behavior: airmode for roll/pitch:
+	 * thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+	 * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+	 */
+	inline void mix_airmode_rp(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
+				   float *outputs);
+
 	/**
 	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
 	 *
@@ -227,6 +286,17 @@ private:
 	 */
 	inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);

+	/**
+	 * Mix roll, pitch, yaw, x_thrust, y_thrust, z_thrust and set the outputs vector for 6-DoF vehicles.
+	 *
+	 * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+	 * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+	 * Thrust can be reduced to unsaturate the upper side.
+	 * @see mix_yaw() for the exact yaw behavior.
+	 */
+	inline void mix_airmode_disabled(float roll, float pitch, float yaw, float x_thrust, float y_thrust, float z_thrust,
+					 float *outputs);
+
 	/**
 	 * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
 	 *
@@ -241,9 +311,13 @@ private:

 	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low_roll_pitch, bool clipping_low_yaw);

+	bool				_is_6dof{false};
 	float				_roll_scale{1.0f};
 	float				_pitch_scale{1.0f};
 	float				_yaw_scale{1.0f};
+	float				_x_scale{1.0f};
+	float				_y_scale{1.0f};
+	float				_z_scale{1.0f};
 	float				_idle_speed{0.0f};
 	float 				_delta_out_max{0.0f};
 	float 				_thrust_factor{0.0f};
@@ -254,6 +328,7 @@ private:

 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
+	const Rotor6Dof			*_rotors_6dof;

 	float 				*_outputs_prev{nullptr};
 	float 				*_tmp_array{nullptr};
@@ -0,0 +1,46 @@
+# Hexacopter in with tilted arms
+
+[info]
+key = "6xt"
+description = "Hexacopter with tilted arms"
+
+[rotor_default]
+# No default axis, each rotor has different axis
+Ct        = 1.0
+Cm        = 0.05
+
+[[rotors]]
+name      = "mid_right"
+axis      = [0.5, 0.0, -0.866025]
+position  = [0.0, 1.0, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "mid_left"
+axis      = [0.5, 0.0, -0.866025]
+position  = [0.0, -1.0, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_left"
+axis      = [-0.25, -0.4330125, -0.866025]
+position  = [0.866025, -0.5, 0.0]
+direction = "CW"
+
+[[rotors]]
+name      = "rear_right"
+axis      = [-0.25, -0.4330125, -0.866025]
+position  = [-0.866025, 0.5, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "front_right"
+axis      = [-0.25, 0.4330125, -0.866025]
+position  = [0.866025, 0.5, 0.0]
+direction = "CCW"
+
+[[rotors]]
+name      = "rear_left"
+axis      = [-0.25, 0.4330125, -0.866025]
+position  = [-0.866025, -0.5, 0.0]
+direction = "CW"
@@ -217,7 +217,7 @@ def normalize_mix_px4(B):

    return B_px

-def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False, use_6dof=False):
+def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False):
    '''
    Generate C header file with same format as multi_tables.py
    TODO: rewrite using templates (see generation of uORB headers)
@@ -255,25 +255,35 @@ def generate_mixer_multirotor_header(geometries_list, use_normalized_mix=False,
        buf.write(u"static constexpr MultirotorMixer::Rotor _config_{}[] {{\n".format(geometry['info']['name']))

        for row in mix:
-            if use_6dof:
-            # 6dof mixer
-                buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
-                    row[0], row[1], row[2],
-                    row[3], row[4], row[5]))
-            else:
            # 4dof mixer
-                buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
-                    row[0], row[1], row[2],
-                    -row[5]))  # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly
+            buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
+                row[0], row[1], row[2],
+                -row[5]))  # Upward thrust is positive TODO: to remove this, adapt PX4 to use NED correctly

        buf.write(u"};\n\n")

-    # Print geometry indeces
+        buf.write(u"static constexpr MultirotorMixer::Rotor6Dof _config_6dof_{}[] {{\n".format(geometry['info']['name']))
+
+        for row in mix:
+            # 6dof mixer
+            buf.write(u"\t{{ {:9f}, {:9f}, {:9f}, {:9f}, {:9f}, {:9f} }},\n".format(
+                row[0], row[1], row[2],
+                row[3], row[4], row[5]))
+
+        buf.write(u"};\n\n")
+
+    # Print geometry indices
    buf.write(u"static constexpr const MultirotorMixer::Rotor *_config_index[] {\n")
    for geometry in geometries_list:
        buf.write(u"\t&_config_{}[0],\n".format(geometry['info']['name']))
    buf.write(u"};\n\n")

+    # Print geometry indices for 6Dof
+    buf.write(u"static constexpr const MultirotorMixer::Rotor6Dof *_config_index_6dof[] {\n")
+    for geometry in geometries_list:
+        buf.write(u"\t&_config_6dof_{}[0],\n".format(geometry['info']['name']))
+    buf.write(u"};\n\n")
+
    # Print geometry rotor counts
    buf.write(u"static constexpr unsigned _config_rotor_count[] {\n")
    for geometry in geometries_list:
@@ -312,8 +322,6 @@ if __name__ == '__main__':
                        action='store_true')
    parser.add_argument('--normalize', help='Use normalized mixers (compatibility mode)',
                        action='store_true')
-    parser.add_argument('--sixdof', help='Use 6dof mixers',
-                        action='store_true')
    args = parser.parse_args()

    # Find toml files
@@ -380,8 +388,7 @@ if __name__ == '__main__':

    # Generate header file
    header = generate_mixer_multirotor_header(geometries_list,
-                                              use_normalized_mix=args.normalize,
-                                              use_6dof=args.sixdof)
+                                              use_normalized_mix=args.normalize)

    if args.outputfile is not None:
        # Write header file
@@ -93,6 +93,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("vehicle_status", 200);
 	add_topic("vehicle_status_flags");
 	add_topic("vtol_vehicle_status", 200);
+	add_topic("omni_attitude_status", 100);

 	// multi topics
 	add_topic_multi("actuator_outputs", 100);
@@ -42,12 +42,93 @@

 using namespace matrix;

+//#pragma GCC optimize("O0")
+
 namespace ControlMath
 {
-void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, const int omni_att_mode,
+		      const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
+		      float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, int omni_proj_axes,
+		      vehicle_attitude_setpoint_s &att_sp, omni_attitude_status_s &omni_status)
 {
-	bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
-	att_sp.thrust_body[2] = -thr_sp.length();
+
+	// Print an error if the omni_att_mode parameter is out of range
+	if (omni_att_mode > 6 || omni_att_mode < 0) {
+		PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!");
+	}
+
+	switch (omni_att_mode) {
+	case 1: // Attitude is set to the minimum roll and pitch (used for omnidirectional vehicles)
+		thrustToMinTiltAttitude(thr_sp, yaw_sp, omni_dfc_max_thrust, att, omni_proj_axes, att_sp);
+		break;
+
+	case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles)
+		thrustToZeroTiltAttitude(thr_sp, yaw_sp, att, omni_proj_axes, att_sp);
+		break;
+
+	case 3: { // Attitude is set to a fixed tilt at a fixed global direction (used for omnidirectional vehicles)
+			thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, omni_att_tilt_angle, omni_att_tilt_dir, omni_proj_axes, att_sp);
+			break;
+		}
+
+	case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles)
+			thrustToFixedRollPitch(thr_sp, yaw_sp, att, omni_att_roll, omni_att_pitch, omni_proj_axes, att_sp);
+			break;
+		}
+
+	case 6: { // Attitude is changed very slowly given the rate
+			float tilt_rate = math::radians(omni_att_rate);
+			thrustToSlowAttitude(thr_sp, yaw_sp, att, tilt_rate, omni_proj_axes, att_sp);
+			break;
+		}
+
+	default: // Attitude is calculated from the desired thrust direction
+		bodyzToAttitude(-thr_sp, yaw_sp, att_sp);
+		att_sp.thrust_body[2] = -thr_sp.length();
+	}
+
+	// Estimate the optimal tilt angle and direction to counteract the wind
+
+	// Calculate the setpoint z axis
+	Vector3f cmd_z;
+	matrix::Dcmf R_cmd = matrix::Quatf(att_sp.q_d);
+
+	for (int i = 0; i < 3; i++) {
+		cmd_z(i) = R_cmd(i, 2);
+	}
+
+	omni_status.tilt_angle_est = asinf(Vector2f(cmd_z(0), cmd_z(1)).norm() / cmd_z.norm());;
+	omni_status.tilt_direction_est = wrap_2pi(atan2f(-cmd_z(1), -cmd_z(0)));
+	omni_status.tilt_roll_est = att_sp.roll_body;
+	omni_status.tilt_pitch_est = att_sp.pitch_body;
+
+	// Calculate the current z axis
+	Vector3f curr_z;
+	matrix::Dcmf R_body = att;
+
+	for (int i = 0; i < 3; i++) {
+		curr_z(i) = R_body(i, 2);
+	}
+
+	// Calculate the tilt angle and direction
+	float current_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm());
+	float current_tilt_dir = wrap_2pi(atan2f(-curr_z(1), -curr_z(0)));
+
+	omni_status.tilt_angle_meas = current_tilt_angle;
+	omni_status.tilt_direction_meas = current_tilt_dir;
+
+	// Save the optimal tilt angle and direction to counteract the wind
+	if (omni_att_mode == 5 || omni_att_mode == 6) {
+
+		// Calculate the tilt angle and direction
+		omni_att_tilt_angle = current_tilt_angle;
+		omni_att_tilt_dir = current_tilt_dir;
+
+		// Calculate the roll and pitch
+		Eulerf euler = R_body;
+		omni_att_roll = euler(0);
+		omni_att_pitch = euler(1);
+	}
 }

 void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
@@ -103,6 +184,315 @@ void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpo
 	att_sp.yaw_body = euler(2);
 }

+void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att, int omni_proj_axes,
+			      vehicle_attitude_setpoint_s &att_sp)
+{
+	// set Z axis to upward direction
+	Vector3f body_z = Vector3f(0.f, 0.f, 1.f);
+
+	// desired body_x and body_y axis
+	Vector3f body_x = Vector3f(cos(yaw_sp), sin(yaw_sp), 0.0f);
+	Vector3f body_y = Vector3f(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
+
+	Dcmf R_sp;
+
+	// fill rotation matrix
+	for (int i = 0; i < 3; i++) {
+		R_sp(i, 0) = body_x(i);
+		R_sp(i, 1) = body_y(i);
+		R_sp(i, 2) = body_z(i);
+	}
+
+	// copy quaternion setpoint to attitude setpoint topic
+	Quatf q_sp = R_sp;
+	q_sp.copyTo(att_sp.q_d);
+	att_sp.q_d_valid = true;
+
+	// set the euler angles, for logging only, must not be used for control
+	att_sp.roll_body = 0;
+	att_sp.pitch_body = 0;
+	att_sp.yaw_body = yaw_sp;
+
+
+	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
+		matrix::Dcmf R_body = att;
+
+		for (int i = 0; i < 3; i++) {
+			body_x(i) = R_body(i, 0);
+			body_y(i) = R_body(i, 1);
+			body_z(i) = R_body(i, 2);
+		}
+	}
+
+	att_sp.thrust_body[0] = thr_sp.dot(body_x);
+	att_sp.thrust_body[1] = thr_sp.dot(body_y);
+	att_sp.thrust_body[2] = thr_sp.dot(body_z);
+}
+
+void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
+			     const matrix::Quatf &att, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
+{
+	Vector3f body_z;
+	float lambda = 0.f; // the minimum tilt angle
+
+	// zero vector, no direction, set safe level value
+	if (thr_sp.norm_squared() < FLT_EPSILON) {
+		body_z(2) = 1.f;
+
+	} else {
+		// Check if the horizontal force is less than the maximum possible
+		Vector2f thr_sp_h(thr_sp(0), thr_sp(1));
+
+		if (thr_sp_h.norm() <= omni_dfc_max_thrust) {
+			thrustToZeroTiltAttitude(thr_sp, yaw_sp, att, omni_proj_axes, att_sp);
+			return;
+		}
+
+		// Calculate the tilt angle
+		float thr_sp_norm = thr_sp.norm();
+		float xi = asinf(Vector2f(thr_sp(0),
+					  thr_sp(1)).norm() / thr_sp_norm); // angle between upward direction and the desired thrust
+		float mu = asinf(omni_dfc_max_thrust / thr_sp_norm); // angle between the Z thrust and the desired thrust
+		lambda = xi - mu; // the desired tilt angle
+
+		// Calculate the direction of the body Z axis
+		Vector3f v_hat(0.f, 0.f, -1.f); // upward direction
+		Vector3f p_hat = v_hat % thr_sp; // the axis of rotation for lambda
+		p_hat.normalize();
+		body_z = -(1 - cosf(lambda)) * p_hat * (p_hat.dot(v_hat)) + cosf(lambda) * v_hat - sinf(lambda) *
+			 (v_hat % p_hat); // Rodrigues' rotation formula
+		body_z = -body_z;
+	}
+
+	// vector of desired yaw direction in XY plane, rotated by PI/2
+	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
+
+	// desired body_x axis, orthogonal to body_z
+	Vector3f body_x = y_C % body_z;
+
+	// keep nose to front while inverted upside down
+	if (body_z(2) < 0.0f) {
+		body_x = -body_x;
+	}
+
+	if (fabsf(body_z(2)) < 0.000001f) {
+		// desired thrust is in XY plane, set X downside to construct correct matrix,
+		// but yaw component will not be used actually
+		body_x.zero();
+		body_x(2) = 1.0f;
+	}
+
+	body_x.normalize();
+
+	// desired body_y axis
+	Vector3f body_y = body_z % body_x;
+
+	Dcmf R_sp;
+
+	// fill rotation matrix
+	for (int i = 0; i < 3; i++) {
+		R_sp(i, 0) = body_x(i);
+		R_sp(i, 1) = body_y(i);
+		R_sp(i, 2) = body_z(i);
+	}
+
+	// copy quaternion setpoint to attitude setpoint topic
+	Quatf q_sp = R_sp;
+	q_sp.copyTo(att_sp.q_d);
+	att_sp.q_d_valid = true;
+
+	// calculate euler angles, for logging only, must not be used for control
+	Eulerf euler = R_sp;
+	att_sp.roll_body = euler(0);
+	att_sp.pitch_body = euler(1);
+	att_sp.yaw_body = euler(2);
+
+
+	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
+		matrix::Dcmf R_body = att;
+
+		for (int i = 0; i < 3; i++) {
+			body_x(i) = R_body(i, 0);
+			body_y(i) = R_body(i, 1);
+			body_z(i) = R_body(i, 2);
+		}
+	}
+
+	// Calculate the direct force vector
+	float f_eff_z = -(omni_dfc_max_thrust * tanf(lambda) + thr_sp(2) / cosf(lambda));
+	Vector2f f_eff_h(thr_sp.dot(body_x), thr_sp.dot(body_y));
+
+	// Prevent the division by zero
+	float f_norm = f_eff_h.norm();
+
+	if (f_norm > 0.0001f) {
+		f_eff_h = f_eff_h / f_eff_h.norm() * omni_dfc_max_thrust;
+
+	} else {
+		f_eff_h.zero();
+	}
+
+	att_sp.thrust_body[0] = f_eff_h(0);
+	att_sp.thrust_body[1] = f_eff_h(1);
+	att_sp.thrust_body[2] = -f_eff_z;
+}
+
+void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			       const float tilt_angle, const float tilt_dir,
+			       int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
+{
+	Vector3f body_z;
+
+	// zero vector, no direction, set safe level value
+	if (thr_sp.norm_squared() < FLT_EPSILON) {
+		body_z(2) = 1.f;
+
+	} else {
+		// Calculate the direction of the body Z axis
+		Vector3f v_hat(0.f, 0.f, -1.f); // upward direction
+		// vector of desired yaw direction in XY plane, rotated by PI/2
+		Vector3f kappa_C(cosf(tilt_dir), sinf(tilt_dir), 0.0f);
+		Vector3f p_hat = v_hat % kappa_C; // the axis of rotation for tilt_angle
+		p_hat.normalize();
+		body_z = -(1 - cosf(tilt_angle)) * p_hat * (p_hat.dot(v_hat)) + cosf(tilt_angle) * v_hat - sinf(tilt_angle) *
+			 (v_hat % p_hat); // Rodrigues' rotation formula
+		body_z = -body_z;
+	}
+
+	// vector of desired yaw direction in XY plane, rotated by PI/2
+	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);
+
+	// desired body_x axis, orthogonal to body_z
+	Vector3f body_x = y_C % body_z;
+
+	// keep nose to front while inverted upside down
+	if (body_z(2) < 0.0f) {
+		body_x = -body_x;
+	}
+
+	if (fabsf(body_z(2)) < 0.000001f) {
+		// desired thrust is in XY plane, set X downside to construct correct matrix,
+		// but yaw component will not be used actually
+		body_x.zero();
+		body_x(2) = 1.0f;
+	}
+
+	body_x.normalize();
+
+	// desired body_y axis
+	Vector3f body_y = body_z % body_x;
+
+	Dcmf R_sp;
+
+	// fill rotation matrix
+	for (int i = 0; i < 3; i++) {
+		R_sp(i, 0) = body_x(i);
+		R_sp(i, 1) = body_y(i);
+		R_sp(i, 2) = body_z(i);
+	}
+
+	// copy quaternion setpoint to attitude setpoint topic
+	Quatf q_sp = R_sp;
+	q_sp.copyTo(att_sp.q_d);
+	att_sp.q_d_valid = true;
+
+	// calculate euler angles, for logging only, must not be used for control
+	Eulerf euler = R_sp;
+	att_sp.roll_body = euler(0);
+	att_sp.pitch_body = euler(1);
+	att_sp.yaw_body = euler(2);
+
+	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
+		matrix::Dcmf R_body = att;
+
+		for (int i = 0; i < 3; i++) {
+			body_x(i) = R_body(i, 0);
+			body_y(i) = R_body(i, 1);
+			body_z(i) = R_body(i, 2);
+		}
+	}
+
+	att_sp.thrust_body[0] = thr_sp.dot(body_x);
+	att_sp.thrust_body[1] = thr_sp.dot(body_y);
+	att_sp.thrust_body[2] = thr_sp.dot(body_z);
+}
+
+void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			    const float roll_angle, const float pitch_angle, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
+{
+	Eulerf euler_cmd(roll_angle, pitch_angle, yaw_sp);
+
+	Quatf q_sp = euler_cmd;
+	q_sp.copyTo(att_sp.q_d);
+
+	// calculate euler angles, for logging only, must not be used for control
+	att_sp.roll_body = roll_angle;
+	att_sp.pitch_body = pitch_angle;
+	att_sp.yaw_body = yaw_sp;
+
+	matrix::Dcmf R_body;
+
+	if (omni_proj_axes == 0) { // if thrust is projected onm the commanded attitude
+		R_body = q_sp;
+
+	} else if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
+		R_body = att;
+	}
+
+	Vector3f body_x, body_y, body_z;
+
+	for (int i = 0; i < 3; i++) {
+		body_x(i) = R_body(i, 0);
+		body_y(i) = R_body(i, 1);
+		body_z(i) = R_body(i, 2);
+	}
+
+	att_sp.thrust_body[0] = thr_sp.dot(body_x);
+	att_sp.thrust_body[1] = thr_sp.dot(body_y);
+	att_sp.thrust_body[2] = thr_sp.dot(body_z);
+}
+
+void thrustToSlowAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			  const float tilt_rate, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
+{
+	// Calculate the desired z axis
+	Vector3f des_z = -thr_sp;
+
+	// Calculate the current z axis
+	Vector3f curr_z;
+	matrix::Dcmf R_body = att;
+
+	for (int i = 0; i < 3; i++) {
+		curr_z(i) = R_body(i, 2);
+	}
+
+	// Calculate the commanded z axis
+	Vector3f p_hat = curr_z % des_z; // the axis of rotation between current and desired z
+	p_hat.normalize();
+	Vector3f cmd_z = -(1 - cosf(tilt_rate)) * p_hat * (p_hat.dot(curr_z)) + cosf(tilt_rate) * curr_z - sinf(tilt_rate) *
+			 (curr_z % p_hat); // Rodrigues' rotation formula
+
+	// Calculate the attitude from the z axis
+	bodyzToAttitude(cmd_z, yaw_sp, att_sp);
+
+	// Project the thrust on the axes
+	att_sp.thrust_body[2] = -thr_sp.length();
+
+	if (omni_proj_axes == 1) { // if thrust is projected on the current attitude
+		Vector3f body_x, body_y, body_z;
+
+		for (int i = 0; i < 3; i++) {
+			body_x(i) = R_body(i, 0);
+			body_y(i) = R_body(i, 1);
+			body_z(i) = R_body(i, 2);
+		}
+
+		att_sp.thrust_body[0] = thr_sp.dot(body_x);
+		att_sp.thrust_body[1] = thr_sp.dot(body_y);
+		att_sp.thrust_body[2] = thr_sp.dot(body_z);
+	}
+}
+
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)
 {
 	if (Vector2f(v0 + v1).norm() <= max) {
@@ -42,6 +42,7 @@

 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/omni_attitude_status.h>

 namespace ControlMath
 {
@@ -49,9 +50,22 @@ namespace ControlMath
 * Converts thrust vector and yaw set-point to a desired attitude.
 * @param thr_sp desired 3D thrust vector
 * @param yaw_sp the desired yaw
+ * @param att current attitude of the robot
+ * @param omni_att_mode attitude mode for omnidirectional vehicles
+ * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
+ * @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians)
+ * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians)
+ * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians)
+ * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in radians)
+ * @param omni_att_rate the attitude change rate for mode=6
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
 * @param att_sp attitude setpoint to fill
 */
-void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+		      const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
+		      float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
+		      vehicle_attitude_setpoint_s &att_sp, omni_attitude_status_s &omni_status);
+
 /**
 * Converts a body z vector and yaw set-point to a desired attitude.
 * @param body_z a world frame 3D vector in direction of the desired body z axis
@@ -60,6 +74,66 @@ void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicl
 */
 void bodyzToAttitude(matrix::Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);

+/**
+ * Converts inertial thrust vector and yaw set-point to a zero-tilt attitude and body thrust vector for an omni-directional multirotor.
+ * @param thr_sp a 3D vector
+ * @param yaw_sp the desired yaw
+ * @param att current attitude of the robot
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
+ * @param att_sp attitude setpoint to fill
+ */
+void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			      int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
+
+/**
+ * Converts inertial thrust vector and yaw set-point to a minimum-tilt attitude and body thrust vector for an omni-directional multirotor.
+ * @param thr_sp a 3D vector
+ * @param yaw_sp the desired yaw
+ * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
+ * @param att current attitude of the robot
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
+ * @param att_sp attitude setpoint to fill
+ */
+void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
+			     const matrix::Quatf &att, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
+/**
+ * Converts inertial thrust vector and yaw set-point to a desired-tilt attitude and body thrust vector for an omni-directional multirotor.
+ * @param thr_sp a 3D vector
+ * @param yaw_sp the desired yaw
+ * @param att current attitude of the robot
+ * @param tilt_angle the desired tilt angle
+ * @param tilt_dir the desired tilt direction
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
+ * @param att_sp attitude setpoint to fill
+ */
+void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			       const float tilt_angle, const float tilt_dir, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
+
+/**
+ * Converts inertial thrust vector and yaw set-point to a desired given attitude and body thrust vector for an omni-directional multirotor.
+ * @param thr_sp a 3D vector
+ * @param yaw_sp the desired yaw
+ * @param att current attitude of the robot
+ * @param roll_angle the desired roll angle
+ * @param pitch_angle the desired pitch angle
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
+ * @param att_sp attitude setpoint to fill
+ */
+void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			    const float roll_angle, const float pitch_angle, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
+
+/**
+ * Converts inertial thrust vector and yaw set-point to a slow-changing attitude and body thrust vector for an omni-directional multirotor.
+ * @param thr_sp a 3D vector
+ * @param yaw_sp the desired yaw
+ * @param att current attitude of the robot
+ * @param tilt_rate rate for the tilt change (non-negative in radians per loop)
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
+ * @param att_sp attitude setpoint to fill
+ */
+void thrustToSlowAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			  const float tilt_angle_rate, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
+
 /**
 * Outputs the sum of two vectors but respecting the limits and priority.
 * The sum of two vectors are constraint such that v0 has priority over v1.
@@ -45,8 +45,10 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * reason: thrust pointing full upward */
 	Vector3f thr{0.f, 0.f, -1.f};
 	float yaw = 0.f;
+	int omni_att_mode = 0;
+	float omni_dfc_max_thrust = 0.0f;
 	vehicle_attitude_setpoint_s att{};
-	thrustToAttitude(thr, yaw, att);
+	thrustToAttitude(thr, yaw, omni_att_mode, omni_dfc_max_thrust, att);
 	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
 	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
 	EXPECT_FLOAT_EQ(att.yaw_body, 0.f);
@@ -55,7 +57,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	/* expected: same as before but with 90 yaw
 	 * reason: only yaw changed */
 	yaw = M_PI_2_F;
-	thrustToAttitude(thr, yaw, att);
+	thrustToAttitude(thr, yaw, omni_att_mode, omni_dfc_max_thrust, att);
 	EXPECT_FLOAT_EQ(att.roll_body, 0.f);
 	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
 	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
@@ -65,7 +67,7 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	 * reason: thrust points straight down and order Euler
 	 * order is: 1. roll, 2. pitch, 3. yaw */
 	thr = Vector3f(0.f, 0.f, 1.f);
-	thrustToAttitude(thr, yaw, att);
+	thrustToAttitude(thr, yaw, omni_att_mode, omni_dfc_max_thrust, att);
 	EXPECT_FLOAT_EQ(att.roll_body, -M_PI_F);
 	EXPECT_FLOAT_EQ(att.pitch_body, 0.f);
 	EXPECT_FLOAT_EQ(att.yaw_body, M_PI_2_F);
@@ -89,7 +89,7 @@ bool PositionControl::setInputSetpoint(const vehicle_local_position_setpoint_s &
 	return mapping_succeeded;
 }

-void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
+void PositionControl::setConstraints(const vehicle_constraints_s &constraints, const float lim_thr_hor_max)
 {
 	_constraints = constraints;

@@ -107,6 +107,8 @@ void PositionControl::setConstraints(const vehicle_constraints_s &constraints)
 		_constraints.speed_down = _lim_vel_down;
 	}

+	_lim_thr_hor_max = lim_thr_hor_max;
+
 	// ignore _constraints.speed_xy TODO: remove it completely as soon as no task uses it anymore to avoid confusion
 }

@@ -321,6 +323,7 @@ void PositionControl::_velocityControl(const float dt)
 		float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
 		float thrust_max_NE = sqrtf(_lim_thr_max * _lim_thr_max - _thr_sp(2) * _thr_sp(2));
 		thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);
+		thrust_max_NE = math::min(_lim_thr_hor_max, thrust_max_NE);

 		// Saturate thrust in NE-direction.
 		_thr_sp(0) = thrust_desired_NE(0);
@@ -361,8 +364,12 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }

-void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
+void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode,
+		const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll,
+		float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
+		vehicle_attitude_setpoint_s &attitude_setpoint, omni_attitude_status_s &omni_status) const
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, omni_att_tilt_angle,
+				      omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_att_rate, omni_proj_axes, attitude_setpoint, omni_status);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }
@@ -43,6 +43,7 @@
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/omni_attitude_status.h>

 struct PositionControlStates {
 	matrix::Vector3f position;
@@ -137,8 +138,9 @@ public:
 	 * Pass constraints that are stricter than the global limits
 	 * Note: NAN value means no constraint, take maximum limit of controller.
 	 * @param constraints a PositionControl structure with supported constraints
+	 * @param lim_thr_hor_max maximum horizontal thrust (read from parameter for OMNI_ATT_MODE = 2, otherwise = 1.0F)
 	 */
-	void setConstraints(const vehicle_constraints_s &constraints);
+	void setConstraints(const vehicle_constraints_s &constraints, const float lim_thr_hor_max);

 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
@@ -169,9 +171,21 @@ public:
 	 * Get the controllers output attitude setpoint
 	 * This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
 	 * It needs to be executed by the attitude controller to achieve velocity and position tracking.
+	 * @param att current attitude of the robot
+	 * @param omni_att_mode attitude mode for omnidirectional vehicles
+	 * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
+	 * @param omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians)
+	 * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians)
+	 * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians)
+	 * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in radians)
+	 * @param omni_att_rate the attitude change rate for mode=6
+	 * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
 	 * @param attitude_setpoint reference to struct to fill up
 	 */
-	void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const;
+	void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust,
+				 float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch,
+				 const float omni_att_rate, const int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint,
+				 omni_attitude_status_s &omni_status) const;

 private:
 	/**
@@ -196,6 +210,7 @@ private:
 	float _lim_thr_min{}; ///< Minimum collective thrust allowed as output [-1,0] e.g. -0.9
 	float _lim_thr_max{}; ///< Maximum collective thrust allowed as output [-1,0] e.g. -0.1
 	float _lim_tilt{}; ///< Maximum tilt from level the output attitude is allowed to have
+	float _lim_thr_hor_max; ///< Maximum direct-force horizontal thrust allowed as output [0, 1]

 	float _hover_thrust{}; ///< Thrust [0,1] with which the vehicle hovers not aacelerating down or up with level orientation

@@ -66,6 +66,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/omni_attitude_status.h>

 #include "PositionControl/PositionControl.hpp"
 #include "Takeoff/Takeoff.hpp"
@@ -83,6 +84,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 	public ModuleParams, public px4::WorkItem
 {
 public:
+	vehicle_attitude_s att;
 	MulticopterPositionControl(bool vtol = false);
 	~MulticopterPositionControl() override;

@@ -113,6 +115,8 @@ private:
 	uORB::Publication<vehicle_local_position_setpoint_s>	_local_pos_sp_pub{ORB_ID(vehicle_local_position_setpoint)};	/**< vehicle local position setpoint publication */
 	uORB::Publication<vehicle_local_position_setpoint_s>	_traj_sp_pub{ORB_ID(trajectory_setpoint)};			/**< trajectory setpoints publication */

+	uORB::Publication<omni_attitude_status_s>	_omni_attitude_status_pub{ORB_ID(omni_attitude_status)};
+
 	uORB::SubscriptionCallbackWorkItem _local_pos_sub{this, ORB_ID(vehicle_local_position)};	/**< vehicle local position */

 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
@@ -143,6 +147,11 @@ private:
 	landing_gear_s _landing_gear{};
 	int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};

+	float _param_tilt_angle = 0, _param_tilt_dir = 0; // Keeping the last parameter values in degrees
+	float _tilt_angle = 0, _tilt_dir = 0; // Keeping the currently used values in radians
+	float _param_roll_angle = 0, _param_pitch_angle = 0; // Keeping the last parameter values in degrees
+	float _tilt_roll = 0, _tilt_pitch = 0; // Keeping the currently used values in radians
+
 	DEFINE_PARAMETERS(
 		// Position Control
 		(ParamFloat<px4::params::MPC_XY_P>) _param_mpc_xy_p,
@@ -172,7 +181,17 @@ private:
 		(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
 		(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd, /**< maximum tilt for landing and smooth takeoff */
 		(ParamFloat<px4::params::MPC_THR_MIN>) _param_mpc_thr_min,
-		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max
+		(ParamFloat<px4::params::MPC_THR_MAX>) _param_mpc_thr_max,
+
+		// Omni-directional vehicle parameters
+		(ParamInt<px4::params::OMNI_ATT_MODE>) _param_omni_att_mode,
+		(ParamFloat<px4::params::OMNI_DFC_MAX_THR>) _param_omni_dfc_max_thr,
+		(ParamFloat<px4::params::OMNI_ATT_TLT_ANG>) _param_omni_att_tilt_angle,
+		(ParamFloat<px4::params::OMNI_ATT_TLT_DIR>) _param_omni_att_tilt_dir,
+		(ParamFloat<px4::params::OMNI_ATT_ROLL>) _param_omni_att_roll,
+		(ParamFloat<px4::params::OMNI_ATT_PITCH>) _param_omni_att_pitch,
+		(ParamInt<px4::params::OMNI_PROJ_AXES>) _param_omni_proj_axes,
+		(ParamFloat<px4::params::OMNI_ATT_RATE>) _param_omni_att_rate
 	);

 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -384,6 +403,22 @@ MulticopterPositionControl::parameters_update(bool force)
 		if (_wv_controller != nullptr) {
 			_wv_controller->update_parameters();
 		}
+
+		if (abs(_param_omni_att_tilt_angle.get() - _param_tilt_angle) > FLT_EPSILON
+		    || abs(_param_omni_att_tilt_dir.get() - _param_tilt_dir) > FLT_EPSILON) {
+			_param_tilt_angle = _param_omni_att_tilt_angle.get();
+			_param_tilt_dir = _param_omni_att_tilt_dir.get();
+			_tilt_angle = math::radians(_param_tilt_angle);
+			_tilt_dir = math::radians(_param_tilt_dir);
+		}
+
+		if (abs(_param_omni_att_roll.get() - _param_roll_angle) > FLT_EPSILON
+		    || abs(_param_omni_att_pitch.get() - _param_pitch_angle) > FLT_EPSILON) {
+			_param_roll_angle = _param_omni_att_roll.get();
+			_param_pitch_angle = _param_omni_att_pitch.get();
+			_tilt_roll = math::radians(_param_roll_angle);
+			_tilt_pitch = math::radians(_param_pitch_angle);
+		}
 	}

 	return OK;
@@ -398,7 +433,6 @@ MulticopterPositionControl::poll_subscriptions()
 	_home_pos_sub.update(&_home_pos);

 	if (_att_sub.updated()) {
-		vehicle_attitude_s att;

 		if (_att_sub.copy(&att) && PX4_ISFINITE(att.q[0])) {
 			_states.yaw = Eulerf(Quatf(att.q)).psi();
@@ -616,7 +650,9 @@ MulticopterPositionControl::Run()

 			// Run position control
 			_control.setState(_states);
-			_control.setConstraints(constraints);
+
+			float max_hor_thrust = (_param_omni_att_mode.get() == 2) ? _param_omni_dfc_max_thr.get() : 1.0F;
+			_control.setConstraints(constraints, max_hor_thrust);

 			if (!_control.setInputSetpoint(setpoint)) {
 				warn_rate_limited("PositionControl: invalid setpoints");
@@ -655,7 +691,15 @@ MulticopterPositionControl::Run()

 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
-			_control.getAttitudeSetpoint(attitude_setpoint);
+			omni_attitude_status_s omni_status{};
+			omni_status.timestamp = time_stamp_now;
+			_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
+						     _tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_att_rate.get(), _param_omni_proj_axes.get(),
+						     attitude_setpoint, omni_status);
+
+			omni_status.att_mode = _param_omni_att_mode.get();
+
+			_omni_attitude_status_pub.publish(omni_status);

 			// Part of landing logic: if ground-contact/maybe landed was detected, turn off
 			// controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.
@@ -910,6 +954,8 @@ MulticopterPositionControl::limit_thrust_during_landing(vehicle_attitude_setpoin
 	if (_vehicle_land_detected.ground_contact
 	    || _vehicle_land_detected.maybe_landed) {
 		// we set the collective thrust to zero, this will help to decide if we are actually landed or not
+		setpoint.thrust_body[0] = 0.f;
+		setpoint.thrust_body[1] = 0.f;
 		setpoint.thrust_body[2] = 0.f;
 		// go level to avoid corrections but keep the heading we have
 		Quatf(AxisAngle<float>(0, 0, _states.yaw)).copyTo(setpoint.q_d);
@@ -745,3 +745,128 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
 * @group Mission
 */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
+
+/**
+ * Maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
+ *
+ * Specifies the maximum horizontal thrust compared to the maximum possible
+ * thrust generated by the vehicle for an omnidirectional multirotor. The
+ * value of this parameter does not affect the behavior if the attitude mode
+ * is not set to one of omni-directional modes (if OMNI_ATT_MODE is 0).
+ *
+ * @min 0
+ * @max 1
+ * @decimal 2
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_DFC_MAX_THR, 0.15f);
+
+/**
+ * Omni-directional attitude setpoint mode
+ *
+ * Specifies the type of attitude setpoint sent to the attitude controller.
+ * The parameter can be set to a normal attitude setpoint, where the tilt
+ * of the vehicle (roll and pitch) are calculated from the desired thrust
+ * vector. This should be the behavior for the non-omnidirectional vehicles,
+ * such as quadrotors and other multirotors with coplanar rotors.
+ * The "constant zero tilt" mode enforces zero roll and pitch and can only be
+ * used for omnidirectional vehicles. The min-tilt mode enforces zero tilt
+ * up to a maximum set acceleration (thrust) and tilts the vehicle as small
+ * as possible if the thrust vector is larger than the maximum. The "constant
+ * tilt" and "constant roll/pitch" modes enforce a given tilt or roll/pitch
+ * for the vehicle. The estimation modes estimate the optimal tilt or roll/pitch
+ * to counteract with the external force (e.g., wind).
+ *
+ * @min 0
+ * @max 6
+ * @value 0 tilted attitude
+ * @value 1 min-tilt attitude
+ * @value 2 constant zero tilt
+ * @value 3 constant tilt
+ * @value 4 constant roll/pitch
+ * @value 5 estimate tilt/roll/pitch
+ * @value 6 slow attitude change
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0);
+
+/**
+ * Omni-directional attitude setpoint tilt angle
+ *
+ * Specifies the tilt angle in degrees for the constant-tilt attitude
+ * setpoint generation mode (OMNI_ATT_MODE=3).
+ *
+ * @min 0
+ * @max 90
+ * @decimal 2
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_ATT_TLT_ANG, 0.0f);
+
+/**
+ * Omni-directional attitude setpoint tilt direction angle
+ *
+ * Specifies the direction of the tilt in degrees for the constant-tilt
+ * attitude setpoint generation mode (OMNI_ATT_MODE=3). The direction is
+ * measured from North.
+ *
+ * @min -360
+ * @max 360
+ * @decimal 2
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_ATT_TLT_DIR, 0.0f);
+
+/**
+ * Omni-directional attitude setpoint roll angle
+ *
+ * Specifies the roll angle in degrees for the constant-roll/pitch attitude
+ * setpoint generation mode (OMNI_ATT_MODE=4).
+ *
+ * @min -90
+ * @max 90
+ * @decimal 2
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_ATT_ROLL, 0.0f);
+
+/**
+ * Omni-directional attitude setpoint pitch angle
+ *
+ * Specifies the pitch angle in degrees for the constant-roll/pitch attitude
+ * setpoint generation mode (OMNI_ATT_MODE=4).
+ *
+ * @min -90
+ * @max 90
+ * @decimal 2
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_ATT_PITCH, 0.0f);
+
+/**
+ * Axes for thrust projection in omni-directional attitude modes
+ *
+ * Specifies the axis used for projecting inertial thrust setpoint
+ * to the body-fixed frame in omni-directional modes (OMNI_ATT_MODE > 0).
+ *
+ * @min 0
+ * @max 1
+ * @value 0 commanded attitude
+ * @value 1 current attitude
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_INT32(OMNI_PROJ_AXES, 1);
+
+
+/**
+ * Rate for change of attitude
+ *
+ * Specifies the rate in which the attitude can change in slow attitude mode
+ * (OMNI_ATT_MODE = 6), rotating the Z axis from the current attitude to the
+ * optimal attitude. The value is in degrees per position control loop.
+ *
+ * @min 0
+ * @decimal 3
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_ATT_RATE, 0.5f);
@@ -213,16 +213,18 @@ MulticopterRateControl::Run()
 					math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};

 				_rates_sp = man_rate_sp.emult(_acro_rate_max);
-				_thrust_sp = _manual_control_sp.z;
+				_thrust_sp(0) = _manual_control_sp.x;
+				_thrust_sp(1) = _manual_control_sp.y;
+				_thrust_sp(2) = -_manual_control_sp.z;

 				// publish rate setpoint
 				vehicle_rates_setpoint_s v_rates_sp{};
 				v_rates_sp.roll = _rates_sp(0);
 				v_rates_sp.pitch = _rates_sp(1);
 				v_rates_sp.yaw = _rates_sp(2);
-				v_rates_sp.thrust_body[0] = 0.0f;
-				v_rates_sp.thrust_body[1] = 0.0f;
-				v_rates_sp.thrust_body[2] = -_thrust_sp;
+				v_rates_sp.thrust_body[0] = _thrust_sp(0);
+				v_rates_sp.thrust_body[1] = _thrust_sp(1);
+				v_rates_sp.thrust_body[2] = _thrust_sp(2);
 				v_rates_sp.timestamp = hrt_absolute_time();

 				_v_rates_sp_pub.publish(v_rates_sp);
@@ -236,7 +238,9 @@ MulticopterRateControl::Run()
 				_rates_sp(0) = v_rates_sp.roll;
 				_rates_sp(1) = v_rates_sp.pitch;
 				_rates_sp(2) = v_rates_sp.yaw;
-				_thrust_sp = -v_rates_sp.thrust_body[2];
+				_thrust_sp(0) = v_rates_sp.thrust_body[0];
+				_thrust_sp(1) = v_rates_sp.thrust_body[1];
+				_thrust_sp(2) = v_rates_sp.thrust_body[2];
 			}
 		}

@@ -288,8 +292,11 @@ MulticopterRateControl::Run()
 			actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp) ? _thrust_sp : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_sp(2)) ? -_thrust_sp(2) : 0.0f;
 			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = (float)_landing_gear.landing_gear;
+			actuators.control[actuator_controls_s::INDEX_X_THRUST] = (PX4_ISFINITE(_thrust_sp(0))) ? _thrust_sp(0) : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_Y_THRUST] = (PX4_ISFINITE(_thrust_sp(1))) ? _thrust_sp(1) : 0.0f;
+			actuators.control[actuator_controls_s::INDEX_Z_THRUST] = (PX4_ISFINITE(_thrust_sp(2))) ? _thrust_sp(2) : 0.0f;
 			actuators.timestamp_sample = angular_velocity.timestamp_sample;

 			// scale effort by battery status if enabled
@@ -129,7 +129,7 @@ private:

 	matrix::Vector3f _rates_sp;			/**< angular rates setpoint */

-	float		_thrust_sp{0.0f};		/**< thrust setpoint */
+	matrix::Vector3f _thrust_sp;			/**< 3-D thrust setpoint */

 	bool _gear_state_initialized{false};		/**< true if the gear state has been initialized */
Author	SHA1	Message	Date
Azarakhsh Keipour	5c09254a83	Gazebo model for the fully-actuated hexarotor added	2020-12-06 20:51:48 -05:00
Azarakhsh Keipour	f33b118dfb	Updated README with some documentation	2020-11-21 16:35:26 -05:00
Azarakhsh Keipour	a99bce1026	Added information on fully-actuated research	2020-11-11 20:06:28 -05:00
Azarakhsh Keipour	f3cf2d309e	Omni Att Ctrl: Manual mode no longer uses the attitude mode and acts like an underactuated UAV	2020-09-29 14:01:24 -04:00
Azarakhsh Keipour	96a496b07d	Pos Ctrl: Logging more tilt information for debugging	2020-09-22 11:53:14 -04:00
Azarakhsh Keipour	b825e2f140	Logger: Logging omni_attitude_status topic by default	2020-08-05 22:05:01 -04:00
Azarakhsh Keipour	8a09314ef9	Omni Pos-Ctrl: omni_attitude_status topic is filled and published	2020-08-05 22:01:41 -04:00
Azarakhsh Keipour	61ef6b2255	Omni: Added a new message type to record omni-directional attitude generation status	2020-08-05 21:59:18 -04:00
Azarakhsh Keipour	f3e21ddc7a	Omni Pos-Ctrl: Reduced jerking when a large command is given in offboard - OMNI_ATT_MODE = 2 uses OMNI_DFC_MAX_THR parameter to limit the horizontal commanded thrust now - Other modes are not affected	2020-08-03 22:22:03 -04:00
Azarakhsh Keipour	431a0cb11e	Omni Pos-Ctrl: Added a parameter for the attitude change rate (OMNI_ATT_MODE=6)	2020-07-28 21:30:23 -04:00
Azarakhsh Keipour	948e75c230	Omni Pos-Ctrl: Slow attitude change works now.	2020-07-28 20:46:23 -04:00
Azarakhsh Keipour	2d101c5998	Omni Pos-Ctrl: Fixed a compilation issue for pixracer)	2020-07-28 19:38:56 -04:00
Azarakhsh Keipour	1ccd452de6	(WIP) Omni Pos-Ctrl: Added slow attitude change OMNI_ATT_MODE = 6 (disabled for now)	2020-07-28 18:59:07 -04:00
Azarakhsh Keipour	a7920aa09f	Omni Pos-Ctrl: Wind estimation is only OMNI_ATT_MODE = 5 now for both tilt and roll/pitch estimation	2020-07-26 01:48:17 -04:00
Azarakhsh Keipour	45af173da1	Omni Params: Attitude setpoint params moved from attitude controller to position controller	2020-07-26 01:46:20 -04:00
Azarakhsh Keipour	3124e76aaa	Omni Pos-Ctrl: Added (unfiltered) roll and pitch angle estimation mode	2020-07-26 01:23:53 -04:00
Azarakhsh Keipour	0630abd01a	Omni Pos-Ctrl: Added (unfiltered) tilt angle and direction estimation mode	2020-07-26 00:55:45 -04:00
Azarakhsh Keipour	0e65bfffa4	Omni Pos-Ctrl: OMNI_PROJ_AXES works for OMNI_ATT_MODE = 1	2020-07-23 06:29:57 -04:00
Azarakhsh Keipour	d77f74bb53	Omni Pos-Ctrl: Roll/Pitch airmode implemented for Omni vehicles - For MC_AIRMODE = 1	2020-07-21 08:15:17 -04:00
Azarakhsh Keipour	7a07f41b65	Omni Pos-Ctrl: Added the OMNI_PROJ_AXES parameter	2020-07-13 19:35:08 -04:00
Azarakhsh Keipour	68005486a1	Omni Pos-Ctrl: Thrust projected on current/commanded axes based on input	2020-07-13 19:06:10 -04:00
Azarakhsh Keipour	2ee4eaf4a4	Omni Att-Ctrl: Desired tilt/roll/pitch params passed to attitude controller - OMNI_ATT_TLT_ANG default set to 0 degrees	2020-07-13 17:45:08 -04:00
Azarakhsh Keipour	651c75558e	Omni Pos-Ctrl: Added desired roll/pitch attitude setpoint generation	2020-07-13 14:06:28 -04:00
Azarakhsh Keipour	03e7435fbe	Omni Att-Ctrl: New parameters for fixed tilt and fixed roll/pitch - OMNI_ATT_TLT_ANG - OMNI_ATT_TLT_DIR - OMNI_ATT_ROLL - OMNI_ATT_PITCH	2020-07-13 13:48:44 -04:00
Azarakhsh Keipour	725fff3052	Omni Pos-Ctrl: Current attitude passed to attitude generation methods	2020-07-13 13:37:21 -04:00
Azarakhsh Keipour	f18f92a505	Omni Pos-Ctrl: Fixed-tilt attitude generation strategy implemented - Currently receiving hardcoded tilt angle and direction	2020-07-13 12:47:45 -04:00
Azarakhsh Keipour	4d3c915183	MultiRotor Mixer: Fix a styling issue for readability	2020-07-13 12:21:04 -04:00
Azarakhsh Keipour	69fe37e6be	Omni Mixer: The mixer for 6-DoF vehicles is refactored to the MultirotorMixer now	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	d5ff0d8ea6	VSCode: Tasks formatted as the one on master now - The task for tilted hex has the same presentation and does not start gzclient now - The Gazebo Kill task does not kill the client anymore	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	145ada8046	Bug Fix: Fixed the parameter name in the tilted hex definition files	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	81d554ef71	Omni Pos-Ctrl: The maximum dfc thrust is defined as a parameter - The parameter is shared with the manual mode's maximum horizontal thrust (renamed from OMNI_MAX_HOR_THR to OMNI_DFC_MAX_THR) defined in the mc_att_control module - The definition for the OMNI_ATT_MODE moved from mc_pos_control module to mc_att_control - The thrustToAttitude function now has additional omni_dfc_max_thrust parameter - Test modules are fixed to call the new thrustToAttitude function appropriately - The code is tested in Gazebo for both manual and (semi-)autonomous modes	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	a655425849	Omni: Set the default OMNI_MAX_HOR_THR to 0.15	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	0cdaf4a801	Omni Pos-Ctrl: Minimum-tilt attitude setpoint for omni-directional vehicles implemented - The goal is to use all the possible (set by the user) horizontal thrust first and then tilt if necessary, thus achieving minimum possible tilt. - This is an implementation of the following paper for OMNI_ATT_MODE = 1: "A Daisy-Chain Control Design for a Multirotor UAV with Direct Force Capabilities", M. Hamza and E.N. Johnson, 2017 AIAA GNC Conference - Still need to define a parameter for the maximum direct force	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	494595111b	Omni Att-Ctrl: Added a new parameter for controlling the maximum horizontal thrust - OMNI_MAX_HOR_THR parameter specifies the maximum horizontal thrust compared to the maximum possible thrust generated by the vehicle for an omnidirectional multirotor	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	3c2087a69d	Omni MC: Changed the parameter name MC_OMNI_MODE to OMNI_ATT_MODE - To be more descriptive and similar to other (future) OMNI_ parameters	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	3ae19f254c	Omni Att-Ctrl: Manual mode works smoother now and based on the MC_OMNI_MODE parameter - Some observations: - It's still not as smooth as expected (or maybe I'm not a good pilot) - Acceleration on the body X axis feels lower than the body Y axis (no idea why) - Flight tested in Gazebo sim in both Manual and Autonomous modes	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	8fd875f8c7	Omni Pos-Ctrl: Refactoring of the attitude set point generation for omni-directional modes	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	db6b09adf0	Omni Pos-Ctrl: The attitude setpoint is generated based on the MC_OMNI_MODE parameter now	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	1d8894c930	Tilted-Hex: Added the default values for the MC_OMNI_MODE parameter to multirotor airframes	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	c386938839	VSCode: Autogenerating launch.json entry for the tilted_hex gazebo simulation - Couldn't make the recent changes in tasks from master branch work, so: - Added gzclient to the command for gazebo tiltedhex and gazebo kill tasks - Also used the desired presentation for the gazebo tiltedhex task	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	4afd11d09c	VSCode: Added tilted hex gazebo simulation task to VSCode	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	252284d89b	Omni Att-Ctrl: Added attitude controller for omni vehicles 'Manual' mode - Still the control feels weird and need more refining	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	d6c27c6f37	Omni Pos-Ctrl: Defined MC_OMNI_MODE PX4 parameter in the position controller	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	1882ff6b4b	Omni Pos-Ctrl: Attitude setpoint generation for omni vehicles completed	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	bc489492d4	Omni Pos-Ctrl: (WIP) Continue code for attitude setpoint generation for omni vehicles	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	62d06357df	Tilt-Hex: Added dynamic limits to the hexa tilt model	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	8bfdf29055	Tilt-Hex Pos-Ctrl: (WIP) Started code for attitude setpoint generation for omni vehicles	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	a0a0c2280b	Bug Fix: Fixed the number of controls in the actuators_controls message to account for the 3D-thrust controls	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	400f8ea4be	Bug Fix(es): 3D-thrust ranges fixed to -1..1 and manual z maps to negative z-thrust now - The range for 3D-thrust in the 6-DOF multirotor mixer is changed to -1 to 1 now (fixed from 0 to 1) - The Z thrust in the 6-DOF multirotor mixer is mapped to the Z-thrust command now (fixed from thrust command) - The manual Z command in the rate controller maps to the negative Z-thrust (fixed from positive Z) - The variable _thrust_body_sp in the rate controller renamed to _thrust_sp to be compatible with the older variable removed in the last commit - The code tested in TakeOff, Manual, Hold, Position and Land modes on both tilted hex and iris Accepting and working with 3D thrust commands now	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	3a321302f0	(WIP) MC-Att-Ctrl: Added 3d thrust set point to the 6-Dof attitude and rate controllers	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	de9942b204	Tilt-Hex SITL: P gains for rpy changed to make the simulated flight smoother - Also enabled logging for the simulated tilt-hex drone	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	0f40eb953d	Bug Fix: The absolute value of Z thrust is used in the 6-Dof mixer now - Tilt-Hex flies like a normal hex but with the new 6-dof mixer now (with some jitters) - Some minor parameter changes in the hexa_x_tilt definition to make the simulated flight smoother	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	b2bc14f706	Bug Fix: Fixed the name of the tilt hex airframe in the CMakeLists - Also added some commands to 6101_hexa_x_tilt.post	2020-01-21 18:23:07 -05:00
Azarakhsh Keipour	a878d6823d	Mixer: Added 3D thrust capability to the multirotor and 6-dof mixers - Added 3D thrust definitions to actuator controls and multirotor motor limits messages	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	d7f61e41d8	Mixer 6-DoF: Airmode and saturation enabled - More info at https://github.com/Auterion/Flight_Control_Prototyping_Scripts/tree/master/control_allocation - The drone is still not flying stably, has vibration and flies away.	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	0d7d567c4b	Mixer 6DoF (WIP): Added a special S mixer for 6-DoF multirotors	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	1af1b40733	Mixer Generation: Generate normalized 6-DoF mixer matrices	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	c53923d1d3	Mixer Generation: Generate mixers for a new MultirotorMixer6dof class for 6-DoF vehicles	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	4bcf796a89	Added mixer and gazebo model for the tilted-arm hexarotor	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	6b8fbc3c45	Hex TOML: Changed to reflect the omni-8 for now	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	e49fb5bf50	Tilt-Hex: Completed mixer definitions (for now)	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	ceb21657c0	Tilt-Hex: Added tilt hex definition files to CMakeLists	2020-01-21 18:23:06 -05:00
Azarakhsh Keipour	82d52d96c5	Defined the new tilted hex airframe	2020-01-21 18:23:06 -05:00