mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
docs(frames_sub): update BlueROV2 docs and align UUV surge/heave stick mapping (#26822)
* Swap joystick surge/heave mapping in manual, stabilized and acro modes to make it similar to position modes * docs: update UUV/BlueROV2 modes and joystick mapping * Document basic control axes and joystick mapping Added basic control axes and stick mapping for BlueROV2. * Fixed formatting issue * Enhance clarity of control axes and stick mapping Clarified descriptions of motion axes and joystick controls for BlueROV2.
This commit is contained in:
parent
1e769a76d6
commit
62b94fa73e
@ -33,14 +33,67 @@ the [Airframe Reference](../airframes/airframe_reference.md#vectored-6-dof-uuv):
|
||||
- **MAIN7:** motor 7 CCW, stern starboard vertical, propeller CW
|
||||
- **MAIN8:** motor 8 CCW, stern port vertical, propeller CCW
|
||||
|
||||
## Basic Control Axes
|
||||
|
||||
For underwater vehicles, motion is defined in terms of body axes:
|
||||
|
||||
- **Surge:** forward/back motion - translation along the body X axis.
|
||||
- **Sway:** left/right motion - translation along the body Y axis.
|
||||
- **Heave:** up/down motion - translation along the body Z axis.
|
||||
- **Yaw:** rotation about the (vertical) body Z axis.
|
||||
|
||||
### Stick Mapping (Mode 2)
|
||||
|
||||
The mapping below illustrates the default joystick behavior:
|
||||
|
||||
- **Pitch stick (forward/back):** surge
|
||||
- **Roll stick (left/right):** sway
|
||||
- **Throttle stick (up/down):** heave
|
||||
- **Yaw stick (left/right):** yaw
|
||||
|
||||

|
||||
|
||||
## 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 | Controls x/y/z and yaw. Manually controlled by user. Keeps roll/pitch zero |
|
||||
The following manual and assisted modes are currently supported on BlueROV2 Heavy:
|
||||
|
||||
| Mode | Description |
|
||||
| ---------- | --------------------------------------------------------------------------------------------------------------------------- |
|
||||
| Manual | Direct manual control of thrust and yaw. |
|
||||
| Stabilized | Manual control of thurst and yaw with roll/pitch stabilization. |
|
||||
| Acro | Manual control of yaw-rate and direct thrust commands with roll/pitch stabilization. |
|
||||
| Altitude | Manual control of x/y thrust and yaw. Control of height with PID, manually controlled by user. Keeps roll/pitch stabilized. |
|
||||
| Position | Controls x, y, z and yaw with position hold when sticks are released. Keeps roll/pitch stabilized. |
|
||||
|
||||
## Joystick Stick Mode
|
||||
|
||||
BlueROV2 supports two joystick mappings for manual control, selected using the
|
||||
[UUV_STICK_MODE](../advanced_config/parameter_reference.md#uuv_stick_mode) parameter.
|
||||
|
||||
By default, `UUV_STICK_MODE` is set to `0`, which enables the UUV stick mapping intended for vectored underwater vehicles.
|
||||
|
||||
### UUV_STICK_MODE = 0 (default)
|
||||
|
||||
This mode is intended for normal BlueROV2 operation.
|
||||
In `Manual`, `Stabilized`, and `Acro` modes, the sticks command:
|
||||
|
||||
- **Pitch stick:** surge - moving stick up -> moving forward, +X translation in body frame.
|
||||
- **Roll stick:** sway - moving stick right -> moving sideways right, +Y translation in body frame.
|
||||
- **Throttle stick:** heave - moving stick up -> moving upwards, -Z translation in body frame (note the Z axis points Down of the vehicle in PX4).
|
||||
- **Yaw stick:** yaw - moving stick right -> yawing to the right, +Z rotation in body frame.
|
||||
|
||||
In this mode, roll and pitch are kept level rather than commanded directly.
|
||||
|
||||
### UUV_STICK_MODE = 1
|
||||
|
||||
This mode enables the legacy multicopter-style stick mapping for `Manual`, `Stabilized`, and `Acro` modes:
|
||||
|
||||
- **Throttle stick:** surge - moving stick up -> moving forward, +X translation in body frame.
|
||||
- **Roll stick:** roll - moving stick right -> rolling to the right side, +X rotation in body frame.
|
||||
- **Pitch stick:** pitch - moving stick up -> pitching down, -X translation in body frame (note signs are switched to follow PX4 standard).
|
||||
- **Yaw stick:** yaw - moving stick right -> yawing to the right, +Z rotation in body frame.
|
||||
|
||||
This mode is mainly provided for compatibility with older setups and user preference.
|
||||
|
||||
## Airframe Configuration
|
||||
|
||||
|
||||
@ -6,15 +6,15 @@
|
||||
Support for UUVs is [experimental](../airframes/index.md#experimental-vehicles).
|
||||
Maintainer volunteers, [contribution](../contribute/index.md) of new features, new frame configurations, or other improvements would all be very welcome!
|
||||
|
||||
At time of writing it has only been tested using ROS in offboard mode.
|
||||
At time of writing manual and assisted manual modes are available for supported UUV frames, as well as ROS in offboard mode.
|
||||
The following features have not been implemented:
|
||||
|
||||
- Modes like missions, depth hold, stabilised manual control, etc.
|
||||
- Autonomous mission-style underwater workflows are still limited compared to aerial vehicles.
|
||||
- BlueRobotics gripper support.
|
||||
|
||||
:::
|
||||
|
||||
PX4 has basic support for UUVs.
|
||||
PX4 has basic support for UUVs. For BlueROV2 Heavy, PX4 currently supports Manual, Stabilized, Acro, Altitude and Position modes.
|
||||
|
||||
## Supported Frames
|
||||
|
||||
|
||||
@ -270,9 +270,9 @@ void UUVAttitudeControl::generate_attitude_setpoint(float dt)
|
||||
|
||||
if (js_heave_sway_mode) {
|
||||
// XYZ thrust
|
||||
_attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * throttle_manual_attitude_gain; // surge +x
|
||||
_attitude_setpoint.thrust_body[0] = _manual_control_setpoint.pitch * throttle_manual_attitude_gain; // heave +z down
|
||||
_attitude_setpoint.thrust_body[1] = _manual_control_setpoint.roll * throttle_manual_attitude_gain; // sway +y
|
||||
_attitude_setpoint.thrust_body[2] = -_manual_control_setpoint.pitch * throttle_manual_attitude_gain; // heave +z down
|
||||
_attitude_setpoint.thrust_body[2] = -_manual_control_setpoint.throttle * throttle_manual_attitude_gain; // surge +x
|
||||
|
||||
} else {
|
||||
// Throttle only on +x (surge)
|
||||
@ -295,9 +295,9 @@ void UUVAttitudeControl::generate_rates_setpoint(float dt)
|
||||
_rates_setpoint.pitch = 0.0f;
|
||||
_rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get();
|
||||
|
||||
_rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * throttle_manual_rate_gain; // surge +x
|
||||
_rates_setpoint.thrust_body[0] = _manual_control_setpoint.pitch * throttle_manual_rate_gain; // heave +z down
|
||||
_rates_setpoint.thrust_body[1] = _manual_control_setpoint.roll * throttle_manual_rate_gain; // sway +y
|
||||
_rates_setpoint.thrust_body[2] = -_manual_control_setpoint.pitch * throttle_manual_rate_gain; // heave +z down
|
||||
_rates_setpoint.thrust_body[2] = -_manual_control_setpoint.throttle * throttle_manual_rate_gain; // surge +x
|
||||
|
||||
} else {
|
||||
// Roll/pitch/yaw are rate commands; thrust only surge
|
||||
@ -404,9 +404,9 @@ void UUVAttitudeControl::Run()
|
||||
const float pitch_u = 0.0f;
|
||||
const float yaw_u = _manual_control_setpoint.yaw * _param_mgm_yaw.get();
|
||||
|
||||
const float thrust_x = _manual_control_setpoint.throttle * throttle_manual_gain; // surge
|
||||
const float thrust_x = _manual_control_setpoint.pitch * throttle_manual_gain; // heave
|
||||
const float thrust_y = _manual_control_setpoint.roll * throttle_manual_gain; // sway
|
||||
const float thrust_z = -_manual_control_setpoint.pitch * throttle_manual_gain; // heave
|
||||
const float thrust_z = -_manual_control_setpoint.throttle * throttle_manual_gain; // surge
|
||||
|
||||
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user