Omni Pos-Ctrl: Current attitude passed to attitude generation methods

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -44,7 +44,7 @@ using namespace matrix;
 
 namespace ControlMath
 {
-void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni_att_mode,
+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, vehicle_attitude_setpoint_s &att_sp)
 {
 	// Print an error if the omni_att_mode parameter is out of range
@@ -64,7 +64,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const int omni
 	case 3: { // Attitude is set to a fixed tilt at a fixed global direction (used for omnidirectional vehicles)
 			float tilt_angle = math::radians(10.F);
 			float tilt_dir = math::radians(270.F);
-			thrustToFixedTiltAttitude(thr_sp, yaw_sp, tilt_angle, tilt_dir, att_sp);
+			thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, tilt_angle, tilt_dir, att_sp);
 			break;
 		}
 
@@ -175,7 +175,7 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f
 		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);
+			thrustToAttitude(thr_sp, yaw_sp, matrix::Quatf(), 2, omni_dfc_max_thrust, att_sp);
 			return;
 		}
 
@@ -257,7 +257,8 @@ void thrustToMinTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const f
 	att_sp.thrust_body[2] = -f_eff_z;
 }
 
-void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const float tilt_angle, const float tilt_dir,
+void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			       const float tilt_angle, const float tilt_dir,
 			       vehicle_attitude_setpoint_s &att_sp)
 {
 	Vector3f body_z;
@@ -321,6 +322,16 @@ void thrustToFixedTiltAttitude(const Vector3f &thr_sp, const float yaw_sp, const
 	att_sp.pitch_body = euler(1);
 	att_sp.yaw_body = euler(2);
 
+	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(body_x);
 	att_sp.thrust_body[1] = thr_sp.dot(body_y);
 	att_sp.thrust_body[2] = thr_sp.dot(body_z);

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -53,7 +53,8 @@ namespace ControlMath
  * @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,
+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,
 		      vehicle_attitude_setpoint_s &att_sp);
 /**
@@ -89,7 +90,8 @@ void thrustToMinTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp,
  * @param tilt_dir the desired tilt direction
  * @param att_sp attitude setpoint to fill
  */
-void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const float tilt_angle,
+void thrustToFixedTiltAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
+			       const float tilt_angle,
 			       const float tilt_dir, vehicle_attitude_setpoint_s &att_sp);
 
 /**

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -361,9 +361,10 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
 	_thr_sp.copyTo(local_position_setpoint.thrust);
 }
 
-void PositionControl::getAttitudeSetpoint(const int omni_att_mode, const float omni_dfc_max_thrust,
+void PositionControl::getAttitudeSetpoint(const matrix::Quatf &att, 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, omni_dfc_max_thrust, attitude_setpoint);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, 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

@@ -173,7 +173,7 @@ public:
 	 * @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, const float omni_dfc_max_thrust,
+	void getAttitudeSetpoint(const matrix::Quatf &att, 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

@@ -83,6 +83,7 @@ class MulticopterPositionControl : public ModuleBase<MulticopterPositionControl>
 	public ModuleParams, public px4::WorkItem
 {
 public:
+	vehicle_attitude_s att;
 	MulticopterPositionControl(bool vtol = false);
 	~MulticopterPositionControl() override;
 
@@ -400,7 +401,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();
@@ -657,7 +657,8 @@ MulticopterPositionControl::Run()
 
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
-			_control.getAttitudeSetpoint(_param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), attitude_setpoint);
+			_control.getAttitudeSetpoint(matrix::Quatf(att.q), _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.