extend uuv_att_control module by thrust in y/z-direction (#16642)

* extend uuv_att_control
* add feedthrough thrust_y + thrust_z direction

* extend uuv_att_control
* add feedthrough thrust_y + thrust_z direction

* update formatting

* fix submarine sitl: indicate motor channel range 0..1 or -1..1 in simulation_mavlink.cpp (#16637)

change motor_count variable to 'pos_thrust_motor_count'
This is more specific to what is actually happening in the code.
'pos_thrust_motors_count' indicates number of motor channels which are configured with 0..1 range (positive thrust) all other motors are configured for -1..1 range

submarine only have motors with -1..1 range.
Thus, pos_thrust_motor_count = 0

Co-authored-by: Thies Lennart Alff <33184858+lennartalff@users.noreply.github.com>

* extend uuv_att_control
* add feedthrough thrust_y + thrust_z direction

* update formatting

Co-authored-by: Thies Lennart Alff <33184858+lennartalff@users.noreply.github.com>

src/modules/uuv_att_control/uuv_att_control.cpp

@@ -93,7 +93,8 @@ void UUVAttitudeControl::parameters_update(bool force)
 	}
 }
 
-void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u)
+void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u,
+		float thrust_x, float thrust_y, float thrust_z)
 {
 	if (PX4_ISFINITE(roll_u)) {
 		roll_u = math::constrain(roll_u, -1.0f, 1.0f);
@@ -119,13 +120,29 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
 		_actuators.control[actuator_controls_s::INDEX_YAW] = 0.0f;
 	}
 
-	if (PX4_ISFINITE(thrust_u)) {
+	if (PX4_ISFINITE(thrust_x)) {
-		thrust_u = math::constrain(thrust_u, -1.0f, 1.0f);
+		thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
-		_actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_u;
+		_actuators.control[actuator_controls_s::INDEX_THROTTLE] = thrust_x;
 
 	} else {
 		_actuators.control[actuator_controls_s::INDEX_THROTTLE] = 0.0f;
 	}
+
+	if (PX4_ISFINITE(thrust_y)) {
+		thrust_y = math::constrain(thrust_y, -1.0f, 1.0f);
+		_actuators.control[actuator_controls_s::INDEX_FLAPS] = thrust_y;
+
+	} else {
+		_actuators.control[actuator_controls_s::INDEX_FLAPS] = 0.0f;
+	}
+
+	if (PX4_ISFINITE(thrust_z)) {
+		thrust_z = math::constrain(thrust_z, -1.0f, 1.0f);
+		_actuators.control[actuator_controls_s::INDEX_SPOILERS] = thrust_z;
+
+	} else {
+		_actuators.control[actuator_controls_s::INDEX_SPOILERS] = 0.0f;
+	}
 }
 
 void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude,
@@ -143,7 +160,9 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
 	float roll_u;
 	float pitch_u;
 	float yaw_u;
-	float thrust_u;
+	float thrust_x;
+	float thrust_y;
+	float thrust_z;
 
 	float roll_body = attitude_setpoint.roll_body;
 	float pitch_body = attitude_setpoint.pitch_body;
@@ -191,9 +210,12 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
 	yaw_u = torques(2);
 
 	// take thrust as
-	thrust_u = attitude_setpoint.thrust_body[0];
+	thrust_x = attitude_setpoint.thrust_body[0];
+	thrust_y = attitude_setpoint.thrust_body[1];
+	thrust_z = attitude_setpoint.thrust_body[2];
+
 
-	constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_u);
+	constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
 	/* Geometric Controller END*/
 }
 
@@ -235,6 +257,8 @@ void UUVAttitudeControl::Run()
 				_attitude_setpoint.pitch_body = _param_direct_pitch.get();
 				_attitude_setpoint.yaw_body = _param_direct_yaw.get();
 				_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
+				_attitude_setpoint.thrust_body[1] = 0.f;
+				_attitude_setpoint.thrust_body[2] = 0.f;
 			}
 
 			/* Geometric Control*/
@@ -242,7 +266,7 @@ void UUVAttitudeControl::Run()
 
 			if (skip_controller) {
 				constrain_actuator_commands(_rates_setpoint.roll, _rates_setpoint.pitch, _rates_setpoint.yaw,
-							    _rates_setpoint.thrust_body[0]);
+							    _rates_setpoint.thrust_body[0], _rates_setpoint.thrust_body[1], _rates_setpoint.thrust_body[2]);
 
 			} else {
 				control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint);
@@ -257,7 +281,8 @@ void UUVAttitudeControl::Run()
 		if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
 			/* manual/direct control */
 			constrain_actuator_commands(_manual_control_setpoint.y, -_manual_control_setpoint.x,
-						    _manual_control_setpoint.r, _manual_control_setpoint.z);
+						    _manual_control_setpoint.r,
+						    _manual_control_setpoint.z, 0.f, 0.f);
 		}
 
 	}

src/modules/uuv_att_control/uuv_att_control.hpp

@@ -146,5 +146,6 @@ private:
 	 */
 	void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint,
 				  const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint);
-	void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u, float thrust_u);
+	void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u,
+					 float thrust_x, float thrust_y, float thrust_z);
 };