BlueROV2 Height control Altitude Mode (#26364)

* removed commented out parts * changed the height controller to work in Altitude mode and moved the controller to the uuv_pos_control.hpp instead of uuv_att_control.hpp * Updated format changes etc. Removed one parameter, that is not used anymore(UUV_HGT_MODE) added my correct email * added a rotation to the thrust, that with different roll and pitch values, x y z thrust is still working as if roll/pitch is zero. * fixed constant roll/pitch to be 0.0 again * added parameter for maximum distance between controlled des height and current height. Added state observation to reset the desired height to current height when altitude mode is turned on. * added first short descriptions of manual modes. * update descriptions * removed vector dependency * feat: updated gz submodule * fix: newline * fix: gz submodule --------- Co-authored-by: Pedro Roque <roque@caltech.edu>
2026-03-24 02:24:09 +08:00 · 2026-02-21 07:46:12 +01:00
parent 9048a40277
commit 8f870a1346
6 changed files with 199 additions and 24 deletions
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/60002_gz_uuv_bluerov2_heavy
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/60002_gz_uuv_bluerov2_heavy
@@ -42,23 +42,21 @@ param set-default FD_FAIL_R 0
 param set-default CA_ROTOR_COUNT 8
 param set-default CA_R_REV 255

-param set-default CA_ROTOR0_AX -1
-param set-default CA_ROTOR0_AY 1
+param set-default CA_ROTOR0_AX 1
+param set-default CA_ROTOR0_AY -1
 param set-default CA_ROTOR0_AZ 0
 param set-default CA_ROTOR0_KM 0
 param set-default CA_ROTOR0_PX 0.14
 param set-default CA_ROTOR0_PY 0.10
 param set-default CA_ROTOR0_PZ 0.06
-#param set-default CA_ROTOR0_PZ 0.0

-param set-default CA_ROTOR1_AX -1
-param set-default CA_ROTOR1_AY -1
+param set-default CA_ROTOR1_AX 1
+param set-default CA_ROTOR1_AY 1
 param set-default CA_ROTOR1_AZ 0
 param set-default CA_ROTOR1_KM 0
 param set-default CA_ROTOR1_PX 0.14
 param set-default CA_ROTOR1_PY -0.10
 param set-default CA_ROTOR1_PZ 0.06
-#param set-default CA_ROTOR1_PZ 0.0

 param set-default CA_ROTOR2_AX 1
 param set-default CA_ROTOR2_AY 1
@@ -67,7 +65,6 @@ param set-default CA_ROTOR2_KM 0
 param set-default CA_ROTOR2_PX -0.14
 param set-default CA_ROTOR2_PY 0.10
 param set-default CA_ROTOR2_PZ 0.06
-#param set-default CA_ROTOR2_PZ 0.0

 param set-default CA_ROTOR3_AX 1
 param set-default CA_ROTOR3_AY -1
@@ -79,7 +76,7 @@ param set-default CA_ROTOR3_PZ 0.06

 param set-default CA_ROTOR4_AX 0
 param set-default CA_ROTOR4_AY 0
-param set-default CA_ROTOR4_AZ 1
+param set-default CA_ROTOR4_AZ -1
 param set-default CA_ROTOR4_KM 0
 param set-default CA_ROTOR4_PX 0.12
 param set-default CA_ROTOR4_PY 0.22
@@ -103,7 +100,7 @@ param set-default CA_ROTOR6_PZ 0

 param set-default CA_ROTOR7_AX 0
 param set-default CA_ROTOR7_AY 0
-param set-default CA_ROTOR7_AZ 1
+param set-default CA_ROTOR7_AZ -1
 param set-default CA_ROTOR7_KM 0
 param set-default CA_ROTOR7_PX -0.12
 param set-default CA_ROTOR7_PY -0.22
--- a/Tools/simulation/gz
+++ b/Tools/simulation/gz
--- a/docs/en/frames_sub/bluerov2.md
+++ b/docs/en/frames_sub/bluerov2.md
@@ -2,9 +2,11 @@

 <Badge type="tip" text="PX4 v1.12" />

-The [BlueROV2](https://bluerobotics.com/store/rov/bluerov2-upgrade-kits/brov2-heavy-retrofit-r1-rp/BlueROV2) is an affordable high-performance underwater vehicle that is perfect for inspections, research, and adventuring.
+The [BlueROV2](https://bluerobotics.com/store/rov/bluerov2-upgrade-kits/brov2-heavy-retrofit-r1-rp/BlueROV2) is an
+affordable high-performance underwater vehicle that is perfect for inspections, research, and adventuring.

-PX4 provides [experimental support](index.md) for an 8-thrust vectored configuration, known as the _BlueROV2 Heavy Configuration_.
+PX4 provides [experimental support](index.md) for an 8-thrust vectored configuration, known as the _BlueROV2 Heavy
+Configuration_.

 ![Hero](../../assets/airframes/sub/bluerov/bluerov_hero.jpg)

@@ -14,9 +16,11 @@ PX4 provides [experimental support](index.md) for an 8-thrust vectored configura

 ### Motor Mapping/Wiring

-The motors must be wired to the flight controller following the standard instructions supplied by BlueRobotics for this vehicle .
+The motors must be wired to the flight controller following the standard instructions supplied by BlueRobotics for this
+vehicle .

-The vehicle will then match the configuration documented in the [Airframe Reference](../airframes/airframe_reference.md#vectored-6-dof-uuv):
+The vehicle will then match the configuration documented in
+the [Airframe Reference](../airframes/airframe_reference.md#vectored-6-dof-uuv):

 <img src="../../assets/airframes/types/Vectored6DofUUV.svg" width="29%" style="max-height: 180px;"/>

@@ -29,6 +33,15 @@ The vehicle will then match the configuration documented in the [Airframe Refere
 - **MAIN7:** motor 7 CCW, stern starboard vertical, propeller CW
 - **MAIN8:** motor 8 CCW, stern port vertical, propeller CCW

+## Manual Modes
+
+| Mode     | Description                                                                                                            |
+|----------|------------------------------------------------------------------------------------------------------------------------|
+| Manual   | Direct manual control of yaw and thrust.                                                                               |
+| Acro     | Manual control of yaw/thrust, but keeps roll/pitch zero                                                                |
+| Altitude | Manual control of x/y thrust and yaw. Control of height with PID, manually controlled by user.   Keeps roll/pitch zero |
+| Position | Controlls x/y/z and yaw. Manually controlled by user.  Keeps roll/pitch zero                                           |
+
 ## Airframe Configuration

 BlueROV2 does not come with PX4 installed.
--- a/src/modules/uuv_pos_control/uuv_pos_control.cpp
+++ b/src/modules/uuv_pos_control/uuv_pos_control.cpp
@@ -38,7 +38,7 @@
 *
 * All the acknowledgments and credits for the fw wing app are reported in those files.
 *
- * @author Tim Hansen <t.hansen@jacobs-university.de>
+ * @author Tim Hansen <timhansen93@googlemail.com>
 * @author Daniel Duecker <daniel.duecker@tuhh.de>
 */

@@ -76,6 +76,9 @@ bool UUVPOSControl::init()
 		return false;
 	}

+	hgtData[0] = 0.0f;
+	hgtData[1] = 0.0f;
+	altitudeStateFlag = false;
 	return true;
 }

@@ -268,6 +271,7 @@ void UUVPOSControl::Run()
 		return;
 	}

+
 	perf_begin(_loop_perf);

 	/* check vehicle control mode for changes to publication state */
@@ -279,6 +283,11 @@ void UUVPOSControl::Run()
 	//vehicle_attitude_s attitude;
 	vehicle_local_position_s vlocal_pos;

+	//state observer for hgt controller
+	if (!_vcontrol_mode.flag_control_altitude_enabled && altitudeStateFlag && ! _vcontrol_mode.flag_control_position_enabled) {
+		altitudeStateFlag = false;
+	}
+
 	/* only run controller if local_pos changed */
 	if (_vehicle_local_position_sub.update(&vlocal_pos)) {
 		const float dt = math::constrain(((vlocal_pos.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f);
@@ -287,6 +296,7 @@ void UUVPOSControl::Run()
 		// Update vehicle attitude
 		_vehicle_attitude_sub.update(&_vehicle_attitude);

+
 		/* Run position or altitude mode from manual setpoints*/
 		if (_vcontrol_mode.flag_control_manual_enabled
 		    && (_vcontrol_mode.flag_control_altitude_enabled
@@ -294,19 +304,93 @@ void UUVPOSControl::Run()
 		    && _vcontrol_mode.flag_armed) {
 			/* Update manual setpoints */

-			const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled
-							&& ! _vcontrol_mode.flag_control_position_enabled;

 			_manual_control_setpoint_sub.update(&_manual_control_setpoint);

 			// Ensure no nan and sufficiently recent setpoint
 			check_setpoint_validity(vlocal_pos);

-			// Generate _trajectory_setpoint -> creates _trajectory_setpoint
-			generate_trajectory_setpoint(vlocal_pos, _vehicle_attitude, dt);
+			const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled && ! _vcontrol_mode.flag_control_position_enabled;

-			pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude,
-					     vlocal_pos, altitude_only_flag);
+			if (altitude_only_flag) {
+				// reset hgt desired position if altitude mode is turned on
+				if (!altitudeStateFlag) {
+					hgtData[0] = vlocal_pos.z ;
+					altitudeStateFlag = true;
+				}
+
+				// Avoid accumulating absolute yaw error with arming stick gesture
+				// roll and pitch for future implementation
+				// float roll = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).phi();
+				// float pitch = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).theta();
+				float yaw = Eulerf(matrix::Quatf(_attitude_setpoint.q_d)).psi();
+
+				float roll_setpoint = 0.0f;
+				float pitch_setpoint = 0.0f;
+				float yaw_setpoint = yaw + _manual_control_setpoint.roll * dt * _param_sgm_yaw.get();
+
+				// Generate target quaternion
+				Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint);
+				Quatf q_sp = euler_sp;
+
+				// Normalize the quaternion to avoid numerical issues
+				q_sp.normalize();
+
+				q_sp.copyTo(_attitude_setpoint.q_d);
+
+
+				float maximumDistanceAllowed = _param_hgt_max_diff.get();
+
+				//change the desired hgt
+				if (_manual_control_setpoint.buttons & (1 << _param_hgt_b_up.get())) { //up
+					if (abs(hgtData[0] - 0.001f * _param_hgt_strength.get() - vlocal_pos.z) < maximumDistanceAllowed) {
+						hgtData[0] = hgtData[0] - 0.001f * _param_hgt_strength.get();
+					}
+				}
+
+				if (_manual_control_setpoint.buttons & (1 << _param_hgt_b_down.get())) { //down
+					if (abs(hgtData[0] + 0.001f * _param_hgt_strength.get() - vlocal_pos.z) < maximumDistanceAllowed) {
+						hgtData[0] = hgtData[0] + 0.001f * _param_hgt_strength.get();
+					}
+
+				}
+
+				float errorInZ = hgtData[0] - vlocal_pos.z;
+
+				//make sure the integrational part is not to high
+				if (std::abs(hgtData[1] + 0.005f * errorInZ * _param_hgt_i_speed.get()) < 1.0f) {
+					hgtData[1] = hgtData[1] + 0.005f * errorInZ * _param_hgt_i_speed.get();
+				}
+
+				matrix::Vector3f thrustxyz;
+				thrustxyz(2) = _param_hgt_p.get() * errorInZ - _param_hgt_d.get() * vlocal_pos.vz + _param_hgt_i.get() *
+					       hgtData[1];//PID values
+
+				const float throttle_manual_attitude_gain = _param_sgm_thrtl.get();
+
+				thrustxyz(0) = _manual_control_setpoint.throttle * throttle_manual_attitude_gain; // surge +x
+				thrustxyz(1) = _manual_control_setpoint.yaw * throttle_manual_attitude_gain; // sway +y
+
+				//Rotate thrust to compensate roll/pitch
+				float roll_att = Eulerf(matrix::Quatf(_vehicle_attitude.q)).phi();
+				float pitch_att = Eulerf(matrix::Quatf(_vehicle_attitude.q)).theta();
+				// attitude without yaw
+				Eulerf euler_att(roll_att, pitch_att, 0);
+				Quatf q_att = matrix::Quatf(euler_att);
+				thrustxyz = q_att.rotateVectorInverse(thrustxyz);
+
+				_attitude_setpoint.thrust_body[0] = thrustxyz(0);
+				_attitude_setpoint.thrust_body[1] = thrustxyz(1);
+				_attitude_setpoint.thrust_body[2] = thrustxyz(2);
+
+			} else {
+
+				// Generate _trajectory_setpoint -> creates _trajectory_setpoint
+				generate_trajectory_setpoint(vlocal_pos, _vehicle_attitude, dt);
+
+				pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude,
+						     vlocal_pos, altitude_only_flag);
+			}

 		} else if (!_vcontrol_mode.flag_control_manual_enabled
 			   && (_vcontrol_mode.flag_control_altitude_enabled
--- a/src/modules/uuv_pos_control/uuv_pos_control.hpp
+++ b/src/modules/uuv_pos_control/uuv_pos_control.hpp
@@ -38,7 +38,7 @@
 *
 * All the acknowledgments and credits for the fw wing app are reported in those files.
 *
- * @author Tim Hansen <t.hansen@jacobs-university.de>
+ * @author Tim Hansen <timhansen93@googlemail.com>
 * @author Daniel Duecker <daniel.duecker@tuhh.de>
 */

@@ -119,6 +119,9 @@ private:
 	perf_counter_t	_loop_perf;
 	hrt_abstime _last_run{0};

+	//control variables
+	float hgtData[2];//des_hgt,integrated hgt error
+	bool altitudeStateFlag;
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::UUV_GAIN_X_P>) _param_pose_gain_x,
 		(ParamFloat<px4::params::UUV_GAIN_Y_P>) _param_pose_gain_y,
@@ -133,7 +136,17 @@ private:
 		(ParamFloat<px4::params::UUV_SGM_PITCH>) _param_sgm_pitch,
 		(ParamFloat<px4::params::UUV_SGM_YAW>) _param_sgm_yaw,
 		(ParamFloat<px4::params::UUV_SP_MAX_AGE>) _param_setpoint_max_age,
-		(ParamInt<px4::params::UUV_POS_MODE>) _param_pos_mode
+		(ParamInt<px4::params::UUV_POS_MODE>) _param_pos_mode,
+
+		(ParamFloat<px4::params::UUV_SGM_THRTL>) _param_sgm_thrtl,
+		(ParamFloat<px4::params::UUV_HGT_P>) _param_hgt_p,
+		(ParamFloat<px4::params::UUV_HGT_D>) _param_hgt_d,
+		(ParamFloat<px4::params::UUV_HGT_I>) _param_hgt_i,
+		(ParamFloat<px4::params::UUV_HGT_I_SPD>) _param_hgt_i_speed,
+		(ParamFloat<px4::params::UUV_HGT_STR>) _param_hgt_strength,
+		(ParamInt<px4::params::UUV_HGT_B_UP>) _param_hgt_b_up,
+		(ParamInt<px4::params::UUV_HGT_B_DOWN>) _param_hgt_b_down,
+		(ParamFloat<px4::params::UUV_HGT_MAX_DIFF>) _param_hgt_max_diff
 	)

 	void Run() override;
--- a/src/modules/uuv_pos_control/uuv_pos_control_params.c
+++ b/src/modules/uuv_pos_control/uuv_pos_control_params.c
@@ -41,7 +41,7 @@
 *
 * All the ackowledgments and credits for the fw wing/rover app are reported in those files.
 *
- * @author Tim Hansen <t.hansen@jacobs-university.de>
+ * @author Tim Hansen <timhansen93@googlemail.com>
 * @author Daniel Duecker <daniel.duecker@tuhh.de>
 */

@@ -118,3 +118,71 @@ PARAM_DEFINE_FLOAT(UUV_PGM_VEL, 0.5f);
 * @group UUV Position Control
 */
 PARAM_DEFINE_INT32(UUV_POS_MODE, 1);
+
+
+/**
+ * Height proportional gain
+ *
+ * @group UUV Attitude Control
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_HGT_P, 1.0f);
+
+/**
+ * Height differential gain
+ *
+ * @group UUV Attitude Control
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_HGT_D, 1.0f);
+
+/**
+ * Height integrational gain
+ *
+ * @group UUV Attitude Control
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_HGT_I, 0.2f);
+
+/**
+ * sum speed of error for integrational gain
+ *
+ * @group UUV Attitude Control
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_HGT_I_SPD, 1.0f);
+
+/**
+ * Height change strength from manual input
+ *
+ * @group UUV Attitude Control
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_HGT_STR, 1.0f);
+
+/**
+ * maximum Height distance controlled by manual input. Diff between actual and desired Height cant be higher than that
+ *
+ *
+ * @group UUV Attitude Control
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(UUV_HGT_MAX_DIFF, 0.3f);
+
+/**
+ * Height rc-button up
+ *
+ * @group UUV Attitude Control
+ * @min 0
+ * @max 16
+ */
+PARAM_DEFINE_INT32(UUV_HGT_B_UP, 11);
+
+/**
+ * Height rc-button down
+ *
+ * @group UUV Attitude Control
+ * @min 0
+ * @max 16
+ */
+PARAM_DEFINE_INT32(UUV_HGT_B_DOWN, 12);