Omni Att-Ctrl: Desired tilt/roll/pitch params passed to attitude controller

- OMNI_ATT_TLT_ANG default set to 0 degrees

src/modules/mc_att_control/mc_att_control_params.c

@@ -200,7 +200,7 @@ PARAM_DEFINE_INT32(OMNI_ATT_MODE, 0);
  * @decimal 2
  * @group Multicopter Attitude Control
  */
-PARAM_DEFINE_FLOAT(OMNI_ATT_TLT_ANG, 15.0f);
+PARAM_DEFINE_FLOAT(OMNI_ATT_TLT_ANG, 0.0f);
 
 /**
  * Omni-directional attitude setpoint tilt direction angle

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -45,7 +45,8 @@ using namespace matrix;
 namespace ControlMath
 {
 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)
+		      const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
+		      float &omni_att_roll, float &omni_att_pitch, vehicle_attitude_setpoint_s &att_sp)
 {
 	// Print an error if the omni_att_mode parameter is out of range
 	if (omni_att_mode > 6 || omni_att_mode < 0) {
@@ -62,15 +63,15 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
 		break;
 
 	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_angle = math::radians(omni_att_tilt_angle);
-			float tilt_dir = math::radians(270.F);
+			float tilt_dir = math::radians(omni_att_tilt_dir);
 			thrustToFixedTiltAttitude(thr_sp, yaw_sp, att, tilt_angle, tilt_dir, att_sp);
 			break;
 		}
 
 	case 4: { // Attitude is set to a fixed roll and pitch (used for omnidirectional vehicles)
-			float roll_angle = math::radians(5.F);
+			float roll_angle = math::radians(omni_att_roll);
-			float pitch_angle = math::radians(5.F);
+			float pitch_angle = math::radians(omni_att_pitch);
 			thrustToFixedRollPitch(thr_sp, yaw_sp, att, roll_angle, pitch_angle, att_sp);
 			break;
 		}
@@ -182,7 +183,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, matrix::Quatf(), 2, omni_dfc_max_thrust, att_sp);
+			thrustToZeroTiltAttitude(thr_sp, yaw_sp, att_sp);
 			return;
 		}
 

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -49,14 +49,18 @@ namespace ControlMath
  * Converts thrust vector and yaw set-point to a desired attitude.
  * @param thr_sp desired 3D thrust vector
  * @param yaw_sp the desired yaw
- * @param att_sp attitude setpoint to fill
+ * @param att current attitude of the robot
  * @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 omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode=5 (in degrees)
+ * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode=5 (in degrees)
+ * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees)
+ * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in degrees)
+ * @param att_sp attitude setpoint to fill
  */
 void thrustToAttitude(const matrix::Vector3f &thr_sp, const float yaw_sp, const matrix::Quatf &att,
-		      const int omni_att_mode,
+		      const int omni_att_mode, const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
-		      const float omni_dfc_max_thrust,
+		      float &omni_att_roll, float &omni_att_pitch, vehicle_attitude_setpoint_s &att_sp);
-		      vehicle_attitude_setpoint_s &att_sp);
 /**
  * Converts a body z vector and yaw set-point to a desired attitude.
  * @param body_z a world frame 3D vector in direction of the desired body z axis

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -362,10 +362,10 @@ 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, float &omnni_att_tilt_angle, float &omnni_att_tilt_dir, float &omnni_att_roll,
+		const float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll,
-		float &omnni_att_pitch,
+		float &omni_att_pitch, vehicle_attitude_setpoint_s &attitude_setpoint) const
-		vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
-	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, attitude_setpoint);
+	ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, att, omni_att_mode, omni_dfc_max_thrust, omni_att_tilt_angle,
+				      omni_att_tilt_dir, omni_att_roll, omni_att_pitch, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -169,12 +169,17 @@ 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 att current attitude of the robot
 	 * @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 omni_att_tilt_angle the desired tilt for the vehicle in mode=3, is output for mode=5 (in degrees)
+	 * @param omni_att_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode=5 (in degrees)
+	 * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees)
+	 * @param omni_att_pitch the desired pitch for the vehicle in mode=4, is output for mode=6 (in degrees)
 	 * @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,
+				 float &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch,
 				 vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -662,9 +662,24 @@ 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;
+			float omni_att_tilt_angle = _param_omni_att_tilt_angle.get();
+			float omni_att_tilt_dir = _param_omni_att_tilt_dir.get();
+			float omni_att_roll = _param_omni_att_roll.get();
+			float omni_att_pitch = _param_omni_att_pitch.get();
 			_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
-						     omnni_att_tilt_angle, omnni_att_tilt_dir, omnni_att_roll, omnni_att_pitch, attitude_setpoint);
+						     omni_att_tilt_angle, omni_att_tilt_dir, omni_att_roll, omni_att_pitch, attitude_setpoint);
+
+			// Update the tilt angle and direciton parameters if we are in tilt estimation mode
+			if (_param_omni_att_mode.get() == 5) {
+				_param_omni_att_tilt_angle.set(omni_att_tilt_angle);
+				_param_omni_att_tilt_dir.set(omni_att_tilt_dir);
+			}
+
+			// Update the roll and pitch angle parameters if we are in roll/pitch estimation mode
+			if (_param_omni_att_mode.get() == 6) {
+				_param_omni_att_roll.set(omni_att_roll);
+				_param_omni_att_pitch.set(omni_att_pitch);
+			}
 
 			// 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.