From 03e7435fbe97a4ddc34c716e00ebad225b9ade44 Mon Sep 17 00:00:00 2001
From: Azarakhsh Keipour <keipour@gmail.com>
Date: Mon, 13 Jul 2020 13:48:44 -0400
Subject: [PATCH]  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
---
 .../mc_att_control/mc_att_control_params.c    | 62 ++++++++++++++++++-
 .../mc_pos_control/mc_pos_control_main.cpp    |  7 ++-
 2 files changed, 66 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index a7dcd63a46..3beb1bbe64 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -172,14 +172,72 @@ PARAM_DEFINE_FLOAT(OMNI_DFC_MAX_THR, 0.15f);
  * 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" mode enforces a given tilt angle and tilt direction for the vehicle.
+ * 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 3
+ * @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
+ * @value 6 estimate roll/pitch
  * @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, 15.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);
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index b5640eabc1..6d06d8f666 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -174,8 +174,13 @@ private:
 		(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,
+
 		(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_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
 	);
 
 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */