From d77f74bb53a5f096f9a645851928c192b7b5451b Mon Sep 17 00:00:00 2001
From: Azarakhsh Keipour <keipour@gmail.com>
Date: Tue, 21 Jul 2020 08:15:17 -0400
Subject: [PATCH] Omni Pos-Ctrl: Roll/Pitch airmode implemented for Omni
 vehicles

- For MC_AIRMODE = 1
---
 .../mixer/MultirotorMixer/MultirotorMixer.cpp | 36 ++++++++++++++++++-
 .../mixer/MultirotorMixer/MultirotorMixer.hpp | 11 +++++-
 2 files changed, 45 insertions(+), 2 deletions(-)

diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp
index 260b6ea9f8..b00ed48993 100644
--- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp
+++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp
@@ -303,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)
 {
@@ -458,11 +482,21 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		}
 
 	} 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);
 
-		mix_airmode_disabled(roll, pitch, yaw, x_thrust, y_thrust, z_thrust, outputs);
+		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].
diff --git a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
index 7af3cd2de1..0a58c1a9b5 100644
--- a/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
+++ b/src/lib/mixer/MultirotorMixer/MultirotorMixer.hpp
@@ -257,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.
 	 *
@@ -282,7 +292,6 @@ private:
 	 * 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.
-	 * This is the only airmode available for 6-DoF vehicles.
 	 * @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,