Omni Pos-Ctrl: Added a parameter for the attitude change rate (OMNI_ATT_MODE=6)

src/modules/mc_pos_control/PositionControl/ControlMath.cpp

@@ -48,7 +48,8 @@ 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, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
-		      float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp)
+		      float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, int omni_proj_axes,
+		      vehicle_attitude_setpoint_s &att_sp)
 {
 
 	// Print an error if the omni_att_mode parameter is out of range
@@ -76,7 +77,7 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
 		}
 
 	case 6: { // Attitude is changed very slowly given the rate
-			float tilt_rate = 0.01;
+			float tilt_rate = math::radians(omni_att_rate);
 			thrustToSlowAttitude(thr_sp, yaw_sp, att, tilt_rate, omni_proj_axes, att_sp);
 			break;
 		}
@@ -88,17 +89,23 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix::
 
 	// Estimate the optimal tilt angle and direction to counteract the wind
 	if (omni_att_mode == 5 || omni_att_mode == 6) {
-		// Calculate the tilt angle
-		omni_att_tilt_angle = asinf(Vector2f(thr_sp(0), thr_sp(1)).norm() / thr_sp.norm());
 
-		// Calculate the tilt direction
+		// Calculate the current z axis
-		omni_att_tilt_dir = wrap_2pi(atan2f(thr_sp(1), thr_sp(0)));
+		Vector3f curr_z;
+		matrix::Dcmf R_body = att;
 
-		// Set the roll angle
+		for (int i = 0; i < 3; i++) {
-		omni_att_roll = att_sp.roll_body;
+			curr_z(i) = R_body(i, 2);
+		}
 
-		// Set the pitch angle
+		// Calculate the tilt angle and direction
-		omni_att_pitch = att_sp.pitch_body;
+		omni_att_tilt_angle = asinf(Vector2f(curr_z(0), curr_z(1)).norm() / curr_z.norm());
+		omni_att_tilt_dir = wrap_2pi(atan2f(-curr_z(1), -curr_z(0)));
+
+		// Calculate the roll and pitch
+		Eulerf euler = R_body;
+		omni_att_roll = euler(0);
+		omni_att_pitch = euler(1);
 	}
 }
 

src/modules/mc_pos_control/PositionControl/ControlMath.hpp

@@ -50,17 +50,21 @@ namespace ControlMath
  * @param thr_sp desired 3D thrust vector
  * @param yaw_sp the desired yaw
  * @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_att_mode attitude mode for omnidirectional vehicles
  * @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_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians)
- * @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_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians)
- * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees)
+ * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians)
- * @param omni_att_pitch the desired pitch 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 radians)
+ * @param omni_att_rate the attitude change rate for mode=6
+ * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
  * @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 float omni_dfc_max_thrust, float &omni_att_tilt_angle, float &omni_att_tilt_dir,
-		      float &omni_att_roll, float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &att_sp);
+		      float &omni_att_roll, float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
+		      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
@@ -122,7 +126,7 @@ void thrustToFixedRollPitch(const matrix::Vector3f &thr_sp, const float yaw_sp,
  * @param thr_sp a 3D vector
  * @param yaw_sp the desired yaw
  * @param att current attitude of the robot
- * @param tilt_rate rate for the tilt change (0-1)
+ * @param tilt_rate rate for the tilt change (non-negative in radians per loop)
  * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
  * @param att_sp attitude setpoint to fill
  */

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -363,9 +363,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 &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll,
-		float &omni_att_pitch, int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const
+		float &omni_att_pitch, const float omni_att_rate, const int omni_proj_axes,
+		vehicle_attitude_setpoint_s &attitude_setpoint) const
 {
 	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, omni_proj_axes, attitude_setpoint);
+				      omni_att_tilt_dir, omni_att_roll, omni_att_pitch, omni_att_rate, omni_proj_axes, attitude_setpoint);
 	attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
 }

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -170,17 +170,19 @@ public:
 	 * 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_att_mode attitude mode for omnidirectional vehicles
 	 * @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_angle the desired tilt for the vehicle in mode=3, is output for mode > 5 (in radians)
-	 * @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_tilt_dir the direction of the desired tilt with respec to North in mode=3, is output for mode > 5 (in radians)
-	 * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode=6 (in degrees)
+	 * @param omni_att_roll the desired roll for the vehicle in mode=4, is output for mode > 5 (in radians)
-	 * @param omni_att_pitch the desired pitch 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 radians)
+	 * @param omni_att_rate the attitude change rate for mode=6
+	 * @param omni_proj_axes the axes used for thrust projection (0=calculated, 1=current)
 	 * @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 &omni_att_tilt_angle, float &omni_att_tilt_dir, float &omni_att_roll, float &omni_att_pitch,
-				 int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const;
+				 const float omni_att_rate, const int omni_proj_axes, vehicle_attitude_setpoint_s &attitude_setpoint) const;
 
 private:
 	/**

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -187,7 +187,8 @@ private:
 		(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,
-		(ParamInt<px4::params::OMNI_PROJ_AXES>) _param_omni_proj_axes
+		(ParamInt<px4::params::OMNI_PROJ_AXES>) _param_omni_proj_axes,
+		(ParamFloat<px4::params::OMNI_ATT_RATE>) _param_omni_att_rate
 	);
 
 	control::BlockDerivative _vel_x_deriv; /**< velocity derivative in x */
@@ -686,7 +687,8 @@ MulticopterPositionControl::Run()
 			vehicle_attitude_setpoint_s attitude_setpoint{};
 			attitude_setpoint.timestamp = time_stamp_now;
 			_control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(),
-						     _tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_proj_axes.get(), attitude_setpoint);
+						     _tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_att_rate.get(), _param_omni_proj_axes.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

@@ -856,3 +856,17 @@ PARAM_DEFINE_FLOAT(OMNI_ATT_PITCH, 0.0f);
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_INT32(OMNI_PROJ_AXES, 1);
+
+
+/**
+ * Rate for change of attitude
+ *
+ * Specifies the rate in which the attitude can change in slow attitude mode
+ * (OMNI_ATT_MODE = 6), rotating the Z axis from the current attitude to the
+ * optimal attitude. The value is in degrees per position control loop.
+ *
+ * @min 0
+ * @decimal 3
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(OMNI_ATT_RATE, 0.5f);