Omni Pos-Ctrl: Added desired roll/pitch attitude setpoint generation

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -48,7 +48,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
 		      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 > 3 || omni_att_mode < 0) {
+	if (omni_att_mode > 6 || omni_att_mode < 0) {
 		PX4_ERR("OMNI_ATT_MODE parameter set to unknown value!");
 	}
 
@@ -68,6 +68,13 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
 			break;
 		}
 
+	case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles)
+			float roll_angle = math::radians(5.F);
+			float pitch_angle = math::radians(5.F);
+			thrustToFixedRollPitch(thr_sp, yaw_sp, att, roll_angle, pitch_angle, 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();
@@ -332,9 +339,47 @@ void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const
 		curr_z(i) = R_curr(i, 2);
 	}
 
-	att_sp.thrust_body[0] = thr_sp.dot(body_x);
+	att_sp.thrust_body[0] = thr_sp.dot(curr_x);
-	att_sp.thrust_body[1] = thr_sp.dot(body_y);
+	att_sp.thrust_body[1] = thr_sp.dot(curr_y);
-	att_sp.thrust_body[2] = thr_sp.dot(body_z);
+	att_sp.thrust_body[2] = thr_sp.dot(curr_z);
+}
+
+void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			    const float roll_angle,
+			    const float pitch_angle, 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);
+
+	matrix::Dcmf R_body = q_sp;
+	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);
+	}
+
+	// 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_curr = att;
+
+	Vector3f curr_x, curr_y, curr_z;
+
+	for (int i = 0; i < 3; i++) {
+		curr_x(i) = R_curr(i, 0);
+		curr_y(i) = R_curr(i, 1);
+		curr_z(i) = R_curr(i, 2);
+	}
+
+	att_sp.thrust_body[0] = thr_sp.dot(curr_x);
+	att_sp.thrust_body[1] = thr_sp.dot(curr_y);
+	att_sp.thrust_body[2] = thr_sp.dot(curr_z);
 }
 
 Vector2f constrainXY(const Vector2f &v0, const Vector2f &v1, const float &max)

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -83,7 +83,7 @@ void thrustToZeroTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp
 void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float omni_dfc_max_thrust,
 			     vehicle_attitude_setpoint_s &att_sp);
 /**
- * Converts thrust vector and yaw set-point to a zero-tilt attitude for an omni-directional multirotor.
+ * Converts thrust vector and yaw set-point to a desired-tilt attitude for an omni-directional multirotor.
  * @param thr_sp a 3D vector
  * @param yaw_sp the desired yaw
  * @param tilt_angle the desired tilt angle
@@ -94,6 +94,18 @@ void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_s
 			       const float tilt_angle,
 			       const float tilt_dir, vehicle_attitude_setpoint_s &att_sp);
 
+/**
+ * Converts thrust vector and yaw set-point to a desired given attitude for an omni-directional multirotor.
+ * @param thr_sp a 3D vector
+ * @param yaw_sp the desired yaw
+ * @param roll_angle the desired roll angle
+ * @param pitch_angle the desired pitch angle
+ * @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, 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.

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -362,7 +362,8 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 }
 
 void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode,
-		const float omni_dfc_max_thrust,
+		const float omni_dfc_max_thrust, float &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll,
+		float &omnni_att_pitch,
 		vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
 	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint);

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -174,6 +174,7 @@ public:
 	 * @param attitude_setpoint reference to struct to fill up
 	 */
 	void getAttitudeSetpoint(const matrix::Quatf &att, const int omni_att_mode, const float omni_dfc_max_thrust,
+				 float &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll, float &omnni_att_pitch,
 				 vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -662,8 +662,9 @@ MulticopterPositionControl::Run()
 
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
+			float omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch;
 			_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
-						     attitude_setpoint);
+						     omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch, 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.