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

src/modules/mc_att_control/mc_att_control.hpp

@@ -161,7 +161,7 @@ private:
 
 		/* Omnidirectional vehicle params */
 		(ParamInt<px4::params::OMNI_ATT_MODE>) _param_omni_att_mode,
-		(ParamFloat<px4::params::OMNI_MAX_HOR_THR>) _param_omni_max_hor_thr
+		(ParamFloat<px4::params::OMNI_DFC_MAX_THR>) _param_omni_dfc_max_thr
 	)
 
 	bool _is_tailsitter{false};

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -241,7 +241,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 			h_thrust /= h_thrust_norm;
 		}
 
-		h_thrust *= _param_omni_max_hor_thr.get();
+		h_thrust *= _param_omni_dfc_max_thr.get();
 
 		// Check if the total thrust has exceeded the maximum
 		Vector3f total_thrust = Vector3f(h_thrust(0), h_thrust(1), z_thrust);

src/modules/mc_att_control/mc_att_control_params.c

@@ -146,16 +146,38 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 200.0f);
 PARAM_DEFINE_FLOAT(MC_RATT_TH, 0.8f);
 
 /**
- * Maximum horizontal thrust ratio for omnidirectional vehicles
+ * 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.
+ * 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_MAX_HOR_THR, 0.15f);
+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.
+ *
+ * @min 0
+ * @max 2
+ * @value 0 tilted attitude
+ * @value 1 min-tilt attitude
+ * @value 2 constant zero tilt
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0);

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -45,7 +45,7 @@ using namespace matrix;
 namespace ControlMath
 {
 void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
-		      vehicle_attitude_setpoint_s &att_sp)
+		      const float omni_dfc_max_thrust, vehicle_attitude_setpoint_s &att_sp)
 {
 	// Print an error if the omni_att_mode parameter is out of range
 	if (omni_att_mode > 2 || omni_att_mode < 0) {
@@ -54,7 +54,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni
 
 	switch (omni_att_mode) {
 	case 1: // Attitude is set to the minimum roll and pitch (used for omnidirectional vehicles)
-		thrustToMinTiltAttitude(thr_sp, yaw_sp, att_sp);
+		thrustToMinTiltAttitude(thr_sp, yaw_sp, omni_dfc_max_thrust, att_sp);
 		break;
 
 	case 2: // Attitude is set to the fixed zero roll and pitch (used for omnidirectional vehicles)
@@ -157,32 +157,41 @@ void thrustToZeroTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicl
 	att_sp.thrust_body[2] = thr_sp(2);
 }
 
-void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
+void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
+			     vehicle_attitude_setpoint_s &att_sp)
 {
-	// TEMP: Define the maximum dfc horizontal thrust
-	const float omni_dfc_max_thrust = 0.15f;
+	Vector3f body_z;
+	float lambda = 0.f;
 
-	// 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) {
-		return thrustToAttitude(thr_sp, yaw_sp, 2, att_sp);
+	// 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) {
+			thrustToAttitude(thr_sp, yaw_sp, 2, omni_dfc_max_thrust, 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;
 	}
 
-	// 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
-	float 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();
-	Vector3f 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);
 

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -50,9 +50,11 @@ namespace ControlMath
  * @param thr_sp desired 3D thrust vector
  * @param yaw_sp the desired yaw
  * @param att_sp attitude setpoint to fill
- * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt
+ * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt
+ * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
  */
 void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
+		      const float omni_dfc_max_thrust,
 		      vehicle_attitude_setpoint_s &att_sp);
 /**
  * Converts a body z vector and yaw set-point to a desired attitude.
@@ -74,9 +76,11 @@ void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp
  * Converts thrust vector and yaw set-point to a minimum-tilt attitude 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_sp attitude setpoint to fill
  */
-void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp);
+void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
+			     vehicle_attitude_setpoint_s &att_sp);
 
 /**
  * Outputs the sum of two vectors but respecting the limits and priority.

src/modules/mc_pos_control/PositionControl/ControlMathTest.cpp

@@ -46,8 +46,9 @@ TEST(ControlMathTest, ThrottleAttitudeMapping)
 	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, omni_att_mode, 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);
@@ -56,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, omni_att_mode, 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);
@@ -66,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, omni_att_mode, 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);

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -361,8 +361,9 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }
 
-void PositionControl::getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const
+void PositionControl::getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust,
+		vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, attitude_setpoint);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -169,10 +169,12 @@ 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 omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-daisy-chain 2-zero-tilt
+	 * @param omni_att_mode attitude mode for omnidirectional vehicles: 0-tilted 1-min-tilt 2-zero-tilt
+	 * @param omni_dfc_max_thrust maximum direct-force (horizontal) scaled thrust for omnidirectional vehicles
 	 * @param attitude_setpoint reference to struct to fill up
 	 */
-	void getAttitudeSetpoint(const int omni_att_mode, vehicle_attitude_setpoint_s &attitude_setpoint) const;
+	void getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust,
+				 vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:
 	/**

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -173,7 +173,8 @@ 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
+		(ParamInt<px4::params::OMNI_ATT_MODE>) _param_omni_att_mode,
+		(ParamFloat<px4::params::OMNI_DFC_MAX_THR>) _param_omni_dfc_max_thr
 	);
 
 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -656,7 +657,7 @@ MulticopterPositionControl::Run()
 
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
-			_control.getAttitudeSetpoint(_param_omni_att_mode.get(), attitude_setpoint);
+			_control.getAttitudeSetpoint(_param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), attitude_setpoint);
 
 			// 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.

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -745,25 +745,3 @@ PARAM_DEFINE_FLOAT(MPC_SPOOLUP_TIME, 1.0f);
  * @group Mission
  */
 PARAM_DEFINE_INT32(MPC_YAW_MODE, 0);
-
-/**
- * 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 daisy-chain 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.
- *
- * @min 0
- * @max 2
- * @value 0 tilted attitude
- * @value 1 daisy-chain tilt/no-tilt attitude
- * @value 2 constant zero tilt
- * @group Multicopter Attitude Control
- */
-PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0);