fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 08:30:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

BlueROV2 Height control Altitude Mode (#26364)

Browse Source 
* 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>
This commit is contained in:
Tim
2026-02-21 07:46:12 +01:00
committed by GitHub
parent 9048a40277
commit 8f870a1346
6 changed files with 199 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/uuv_pos_control/uuv_pos_control.cpp
+91 -7
View File
@@ -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
src/modules/uuv_pos_control/uuv_pos_control.hpp
+15 -2
View File
@@ -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;
src/modules/uuv_pos_control/uuv_pos_control_params.c
+69 -1
View File
@@ -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);