diff --git 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 index b80bf079b7..ef620441ac 100644 --- 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 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index fe3fe236e3..3eb05f716a 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit fe3fe236e36a3ed5bce01a7501347d20a466c407 +Subproject commit 3eb05f716a81bca316ab5771f53e509a07dce3a6 diff --git a/docs/en/frames_sub/bluerov2.md b/docs/en/frames_sub/bluerov2.md index e0ab0129bc..1f2b92be8c 100644 --- a/docs/en/frames_sub/bluerov2.md +++ b/docs/en/frames_sub/bluerov2.md @@ -2,9 +2,11 @@ -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): @@ -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. diff --git a/src/modules/uuv_pos_control/uuv_pos_control.cpp b/src/modules/uuv_pos_control/uuv_pos_control.cpp index 53ba388f99..d394ec54f0 100644 --- 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 + * @author Tim Hansen * @author Daniel Duecker */ @@ -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 diff --git a/src/modules/uuv_pos_control/uuv_pos_control.hpp b/src/modules/uuv_pos_control/uuv_pos_control.hpp index 1d07cd71ea..fd7e5605f2 100644 --- 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 + * @author Tim Hansen * @author Daniel Duecker */ @@ -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) _param_pose_gain_x, (ParamFloat) _param_pose_gain_y, @@ -133,7 +136,17 @@ private: (ParamFloat) _param_sgm_pitch, (ParamFloat) _param_sgm_yaw, (ParamFloat) _param_setpoint_max_age, - (ParamInt) _param_pos_mode + (ParamInt) _param_pos_mode, + + (ParamFloat) _param_sgm_thrtl, + (ParamFloat) _param_hgt_p, + (ParamFloat) _param_hgt_d, + (ParamFloat) _param_hgt_i, + (ParamFloat) _param_hgt_i_speed, + (ParamFloat) _param_hgt_strength, + (ParamInt) _param_hgt_b_up, + (ParamInt) _param_hgt_b_down, + (ParamFloat) _param_hgt_max_diff ) void Run() override; diff --git a/src/modules/uuv_pos_control/uuv_pos_control_params.c b/src/modules/uuv_pos_control/uuv_pos_control_params.c index c5b0705d74..082949fb4d 100644 --- 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 + * @author Tim Hansen * @author Daniel Duecker */ @@ -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);