mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-23 06:36:45 +08:00
feat: BlueROV2 Heavy updated control (attitude and position) and model (#25052)
* rft: clean merge to PX4 * fix: formatting * fix: extra line * fix: moved submarine out of "is_ground_vehicle", added proper check for center-throttle * feat: updated gazebo models to include bluerov update * fix: use 'is_uuv_vehicle', remove FW_MM/LLC from uuv build * fix: added saturation to thrust and torque messages via param * doc: updated parameters documentation for uuv * fix: formatting * feat: matching hardware reference * fix: thrusters kg * rft: removed commented lines * fix: update gz reference given hw setup * fix: hardware references * fix: recommendations * fix: updated settings to match hardware * rft: check only for fixed and rotary wing for high throttle Co-authored-by: Daniel Agar <daniel@agar.ca> * fix: commit oupsie * fix: format * rft: remove is_uuv * fix: hw parameters, uuv build target for v6x * feat: added support for D-pad attitude changes in stabilized position control * fix: position setpoint update and parametrized trajectory age and att change * fix: format * fix: removed duplicated call to check_validity_setpoint * fix: setpoint update on arming logic * fix: setpoint initialization for stabilized mode --------- Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Pedro Roque
GitHub
@@ -0,0 +1,138 @@
|
||||
#!/bin/sh
|
||||
#
|
||||
# @name BlueROV2 Heavy
|
||||
#
|
||||
# @type UUV
|
||||
#
|
||||
# @maintainer Pedro Roque <padr@kth.se>
|
||||
# @maintainer Nicola De Carli <ndc@kth.se>
|
||||
#
|
||||
|
||||
# Source PX4 parameters and configuration
|
||||
. ${R}/etc/init.d/rc.uuv_defaults
|
||||
|
||||
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
|
||||
PX4_GZ_WORLD=${PX4_GZ_WORLD:=underwater}
|
||||
PX4_SIM_MODEL=${PX4_SIM_MODEL:=uuv_bluerov2_heavy}
|
||||
|
||||
param set-default SIM_GZ_EN 1
|
||||
|
||||
# Set parameters using `param` utility for SITL
|
||||
param set-default CA_AIRFRAME 7
|
||||
param set-default CA_METHOD 0
|
||||
param set-default MAV_TYPE 12
|
||||
|
||||
param set-default SENS_EN_GPSSIM 1
|
||||
param set-default SENS_EN_BAROSIM 1
|
||||
param set-default SENS_EN_MAGSIM 1
|
||||
param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs
|
||||
param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later?
|
||||
|
||||
# Set proper failsafes
|
||||
param set-default COM_ACT_FAIL_ACT 0
|
||||
param set-default COM_LOW_BAT_ACT 0
|
||||
param set-default NAV_DLL_ACT 0
|
||||
param set-default GF_ACTION 1
|
||||
param set-default NAV_RCL_ACT 1
|
||||
param set-default COM_POSCTL_NAVL 2
|
||||
|
||||
# disable attitude failure detection
|
||||
param set-default FD_FAIL_P 0
|
||||
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_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_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
|
||||
param set-default CA_ROTOR2_AZ 0
|
||||
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
|
||||
param set-default CA_ROTOR3_AZ 0
|
||||
param set-default CA_ROTOR3_KM 0
|
||||
param set-default CA_ROTOR3_PX -0.14
|
||||
param set-default CA_ROTOR3_PY -0.10
|
||||
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_KM 0
|
||||
param set-default CA_ROTOR4_PX 0.12
|
||||
param set-default CA_ROTOR4_PY 0.22
|
||||
param set-default CA_ROTOR4_PZ 0
|
||||
|
||||
param set-default CA_ROTOR5_AX 0
|
||||
param set-default CA_ROTOR5_AY 0
|
||||
param set-default CA_ROTOR5_AZ 1
|
||||
param set-default CA_ROTOR5_KM 0
|
||||
param set-default CA_ROTOR5_PX 0.12
|
||||
param set-default CA_ROTOR5_PY -0.22
|
||||
param set-default CA_ROTOR5_PZ 0
|
||||
|
||||
param set-default CA_ROTOR6_AX 0
|
||||
param set-default CA_ROTOR6_AY 0
|
||||
param set-default CA_ROTOR6_AZ 1
|
||||
param set-default CA_ROTOR6_KM 0
|
||||
param set-default CA_ROTOR6_PX -0.12
|
||||
param set-default CA_ROTOR6_PY 0.22
|
||||
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_KM 0
|
||||
param set-default CA_ROTOR7_PX -0.12
|
||||
param set-default CA_ROTOR7_PY -0.22
|
||||
param set-default CA_ROTOR7_PZ 0
|
||||
|
||||
param set-default SIM_GZ_EC_FUNC1 101
|
||||
param set-default SIM_GZ_EC_FUNC2 102
|
||||
param set-default SIM_GZ_EC_FUNC3 103
|
||||
param set-default SIM_GZ_EC_FUNC4 104
|
||||
param set-default SIM_GZ_EC_FUNC5 105
|
||||
param set-default SIM_GZ_EC_FUNC6 106
|
||||
param set-default SIM_GZ_EC_FUNC7 107
|
||||
param set-default SIM_GZ_EC_FUNC8 108
|
||||
|
||||
param set-default SIM_GZ_EC_MIN1 1100
|
||||
param set-default SIM_GZ_EC_MIN2 1100
|
||||
param set-default SIM_GZ_EC_MIN3 1100
|
||||
param set-default SIM_GZ_EC_MIN4 1100
|
||||
param set-default SIM_GZ_EC_MIN5 1100
|
||||
param set-default SIM_GZ_EC_MIN6 1100
|
||||
param set-default SIM_GZ_EC_MIN7 1100
|
||||
param set-default SIM_GZ_EC_MIN8 1100
|
||||
|
||||
param set-default SIM_GZ_EC_MAX1 1900
|
||||
param set-default SIM_GZ_EC_MAX2 1900
|
||||
param set-default SIM_GZ_EC_MAX3 1900
|
||||
param set-default SIM_GZ_EC_MAX4 1900
|
||||
param set-default SIM_GZ_EC_MAX5 1900
|
||||
param set-default SIM_GZ_EC_MAX6 1900
|
||||
param set-default SIM_GZ_EC_MAX7 1900
|
||||
param set-default SIM_GZ_EC_MAX8 1900
|
||||
@@ -115,6 +115,8 @@ px4_add_romfs_files(
|
||||
|
||||
50000_gz_rover_differential
|
||||
|
||||
60002_gz_uuv_bluerov2_heavy
|
||||
|
||||
71001_gz_atmos
|
||||
71002_gz_spacecraft_2d
|
||||
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
|
||||
. ${R}etc/init.d/rc.uuv_defaults
|
||||
|
||||
param set-default MAV_1_CONFIG 102
|
||||
# param set-default MAV_1_CONFIG 102
|
||||
|
||||
param set-default BAT1_A_PER_V 37.8798
|
||||
param set-default BAT1_CAPACITY 18000
|
||||
@@ -30,53 +30,93 @@ param set-default BAT1_V_DIV 11
|
||||
param set-default BAT1_N_CELLS 4
|
||||
param set-default BAT_V_OFFS_CURR 0.33
|
||||
|
||||
# Set parameters using `param` utility for SITL
|
||||
param set-default CA_AIRFRAME 7
|
||||
param set-default CA_METHOD 0
|
||||
param set-default MAV_TYPE 12
|
||||
|
||||
# Board rotation: set to direct replacement of RPi with PX4
|
||||
# with forward facing vehicle forward
|
||||
param set-default SENS_BOARD_ROT 16
|
||||
|
||||
# Set proper failsafes
|
||||
param set-default COM_ACT_FAIL_ACT 0
|
||||
param set-default COM_LOW_BAT_ACT 0
|
||||
param set-default NAV_DLL_ACT 0
|
||||
param set-default GF_ACTION 1
|
||||
param set-default NAV_RCL_ACT 1
|
||||
param set-default COM_POSCTL_NAVL 2
|
||||
|
||||
# disable attitude failure detection
|
||||
param set-default FD_FAIL_P 0
|
||||
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_AZ 0
|
||||
param set-default CA_ROTOR0_KM 0
|
||||
param set-default CA_ROTOR0_PX 0.5
|
||||
param set-default CA_ROTOR0_PY 0.3
|
||||
param set-default CA_ROTOR0_PZ 0.2
|
||||
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_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.5
|
||||
param set-default CA_ROTOR1_PY -0.3
|
||||
param set-default CA_ROTOR1_PZ 0.2
|
||||
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_ROTOR2_AX 1
|
||||
param set-default CA_ROTOR2_AY 1
|
||||
param set-default CA_ROTOR2_AZ 0
|
||||
param set-default CA_ROTOR2_KM 0
|
||||
param set-default CA_ROTOR2_PX -0.5
|
||||
param set-default CA_ROTOR2_PY 0.3
|
||||
param set-default CA_ROTOR2_PZ 0.2
|
||||
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_ROTOR3_AX 1
|
||||
param set-default CA_ROTOR3_AY -1
|
||||
param set-default CA_ROTOR3_AZ 0
|
||||
param set-default CA_ROTOR3_KM 0
|
||||
param set-default CA_ROTOR3_PX -0.5
|
||||
param set-default CA_ROTOR3_PY -0.3
|
||||
param set-default CA_ROTOR3_PZ 0.2
|
||||
param set-default CA_ROTOR3_PX -0.14
|
||||
param set-default CA_ROTOR3_PY -0.10
|
||||
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_KM 0
|
||||
param set-default CA_ROTOR4_PX 0.5
|
||||
param set-default CA_ROTOR4_PY 0.5
|
||||
param set-default CA_ROTOR4_PX 0.12
|
||||
param set-default CA_ROTOR4_PY 0.22
|
||||
param set-default CA_ROTOR4_PZ 0
|
||||
|
||||
param set-default CA_ROTOR5_AX 0
|
||||
param set-default CA_ROTOR5_AY 0
|
||||
param set-default CA_ROTOR5_AZ 1
|
||||
param set-default CA_ROTOR5_KM 0
|
||||
param set-default CA_ROTOR5_PX 0.5
|
||||
param set-default CA_ROTOR5_PY -0.5
|
||||
param set-default CA_ROTOR5_PX 0.12
|
||||
param set-default CA_ROTOR5_PY -0.22
|
||||
param set-default CA_ROTOR5_PZ 0
|
||||
|
||||
param set-default CA_ROTOR6_AX 0
|
||||
param set-default CA_ROTOR6_AY 0
|
||||
param set-default CA_ROTOR6_AZ 1
|
||||
param set-default CA_ROTOR6_KM 0
|
||||
param set-default CA_ROTOR6_PX -0.5
|
||||
param set-default CA_ROTOR6_PY 0.5
|
||||
param set-default CA_ROTOR6_PX -0.12
|
||||
param set-default CA_ROTOR6_PY 0.22
|
||||
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_KM 0
|
||||
param set-default CA_ROTOR7_PX -0.5
|
||||
param set-default CA_ROTOR7_PY -0.5
|
||||
param set-default CA_ROTOR7_PX -0.12
|
||||
param set-default CA_ROTOR7_PY -0.22
|
||||
param set-default CA_ROTOR7_PZ 0
|
||||
|
||||
param set-default PWM_MAIN_FUNC1 101
|
||||
param set-default PWM_MAIN_FUNC2 102
|
||||
@@ -86,3 +126,30 @@ param set-default PWM_MAIN_FUNC5 105
|
||||
param set-default PWM_MAIN_FUNC6 106
|
||||
param set-default PWM_MAIN_FUNC7 107
|
||||
param set-default PWM_MAIN_FUNC8 108
|
||||
|
||||
param set-default PWM_MAIN_MIN1 1100
|
||||
param set-default PWM_MAIN_MIN2 1100
|
||||
param set-default PWM_MAIN_MIN3 1100
|
||||
param set-default PWM_MAIN_MIN4 1100
|
||||
param set-default PWM_MAIN_MIN5 1100
|
||||
param set-default PWM_MAIN_MIN6 1100
|
||||
param set-default PWM_MAIN_MIN7 1100
|
||||
param set-default PWM_MAIN_MIN8 1100
|
||||
|
||||
param set-default PWM_MAIN_MAX1 1900
|
||||
param set-default PWM_MAIN_MAX2 1900
|
||||
param set-default PWM_MAIN_MAX3 1900
|
||||
param set-default PWM_MAIN_MAX4 1900
|
||||
param set-default PWM_MAIN_MAX5 1900
|
||||
param set-default PWM_MAIN_MAX6 1900
|
||||
param set-default PWM_MAIN_MAX7 1900
|
||||
param set-default PWM_MAIN_MAX8 1900
|
||||
|
||||
param set-default PWM_MAIN_DIS1 1500
|
||||
param set-default PWM_MAIN_DIS2 1500
|
||||
param set-default PWM_MAIN_DIS3 1500
|
||||
param set-default PWM_MAIN_DIS4 1500
|
||||
param set-default PWM_MAIN_DIS5 1500
|
||||
param set-default PWM_MAIN_DIS6 1500
|
||||
param set-default PWM_MAIN_DIS7 1500
|
||||
param set-default PWM_MAIN_DIS8 1500
|
||||
|
||||
@@ -16,6 +16,6 @@ control_allocator start
|
||||
uuv_att_control start
|
||||
|
||||
#
|
||||
# Start UUV Land Detector.
|
||||
# Start UUV Position Controller.
|
||||
#
|
||||
land_detector start rover
|
||||
uuv_pos_control start
|
||||
|
||||
@@ -9,3 +9,21 @@ set VEHICLE_TYPE uuv
|
||||
|
||||
# MAV_TYPE_SUBMARINE 12
|
||||
param set-default MAV_TYPE 12
|
||||
|
||||
# Set micro-dds-client to use ethernet and IP-address 192.168.0.1
|
||||
param set-default UXRCE_DDS_AG_IP -1062731775
|
||||
|
||||
# Disable preflight disarm to not interfere with external launching
|
||||
param set-default COM_DISARM_PRFLT -1
|
||||
param set-default CBRK_SUPPLY_CHK 894281
|
||||
param set-default COM_ARM_HFLT_CHK 0
|
||||
|
||||
#Missing params
|
||||
param set-default CP_DIST -1.0
|
||||
|
||||
# Default to MoCap fusion
|
||||
param set-default ATT_EXT_HDG_M 2
|
||||
param set-default EKF2_EV_CTRL 15
|
||||
param set-default EKF2_EV_DELAY 5
|
||||
param set-default EKF2_GPS_CTRL 0
|
||||
param set-default EKF2_HGT_REF 3
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_FW_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=n
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=n
|
||||
CONFIG_MODULES_ROVER_MECANUM=n
|
||||
CONFIG_MODULES_SPACECRAFT=n
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
CONFIG_MODULES_UUV_ATT_CONTROL=y
|
||||
CONFIG_MODULES_UUV_POS_CONTROL=y
|
||||
@@ -0,0 +1,23 @@
|
||||
CONFIG_MODULES_AIRSPEED_SELECTOR=n
|
||||
CONFIG_MODULES_FLIGHT_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_ATT_CONTROL=n
|
||||
CONFIG_MODULES_FW_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_FW_POS_CONTROL=n
|
||||
CONFIG_MODULES_FW_RATE_CONTROL=n
|
||||
CONFIG_MODULES_FW_MODE_MANAGER=n
|
||||
CONFIG_MODULES_FW_LATERAL_LONGITUDINAL_CONTROL=n
|
||||
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=y
|
||||
CONFIG_MODULES_MC_ATT_CONTROL=n
|
||||
CONFIG_MODULES_MC_AUTOTUNE_ATTITUDE_CONTROL=n
|
||||
CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=n
|
||||
CONFIG_MODULES_MC_POS_CONTROL=n
|
||||
CONFIG_MODULES_MC_RATE_CONTROL=n
|
||||
CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_MODULES_ROVER_ACKERMANN=n
|
||||
CONFIG_MODULES_ROVER_DIFFERENTIAL=n
|
||||
CONFIG_MODULES_ROVER_MECANUM=n
|
||||
CONFIG_MODULES_SPACECRAFT=n
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=y
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
CONFIG_MODULES_UUV_ATT_CONTROL=y
|
||||
CONFIG_MODULES_UUV_POS_CONTROL=y
|
||||
@@ -574,7 +574,9 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
|
||||
|
||||
if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
|
||||
!_failsafe_flags.manual_control_signal_lost && !_is_throttle_low
|
||||
&& !is_ground_vehicle(_vehicle_status)) {
|
||||
&& ((_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING)
|
||||
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING))
|
||||
) {
|
||||
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Arming denied: high throttle\t");
|
||||
events::send(events::ID("commander_arm_denied_throttle_high"), {events::Log::Critical, events::LogInternal::Info},
|
||||
|
||||
@@ -119,7 +119,8 @@ bool is_fixed_wing(const vehicle_status_s ¤t_status)
|
||||
|
||||
bool is_ground_vehicle(const vehicle_status_s ¤t_status)
|
||||
{
|
||||
return (current_status.system_type == VEHICLE_TYPE_BOAT || current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
|
||||
return (current_status.system_type == VEHICLE_TYPE_BOAT ||
|
||||
current_status.system_type == VEHICLE_TYPE_GROUND_ROVER);
|
||||
}
|
||||
|
||||
// End time for currently blinking LED message, 0 if no blink message
|
||||
|
||||
@@ -15,6 +15,8 @@
|
||||
</plugin>
|
||||
<plugin entity_name="*" entity_type="world" filename="libOpticalFlowSystem.so" name="custom::OpticalFlowSystem"/>
|
||||
<plugin entity_name="*" entity_type="world" filename="libGstCameraSystem.so" name="custom::GstCameraSystem"/>
|
||||
<plugin entity_name="*" entity_type="world" filename="libGenericMotorModelPlugin.so" name="gz::sim::systems::GenericMotorModel"/>
|
||||
<plugin entity_name="*" entity_type="world" filename="libBuoyancySystemPlugin.so" name="gz::sim::systems::BuoyancySystem"/>
|
||||
<!-- <plugin entity_name="*" entity_type="world" filename="libTemplatePlugin.so" name="custom::TemplateSystem"/> -->
|
||||
</plugins>
|
||||
</server_config>
|
||||
|
||||
@@ -56,12 +56,14 @@ if (gz-transport_FOUND)
|
||||
add_subdirectory(template_plugin)
|
||||
add_subdirectory(gstreamer)
|
||||
add_subdirectory(moving_platform_controller)
|
||||
add_subdirectory(generic_motor)
|
||||
add_subdirectory(buoyancy)
|
||||
add_subdirectory(spacecraft_thruster)
|
||||
|
||||
# Add an alias target for each plugin
|
||||
if (TARGET GstCameraSystem)
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin GstCameraSystem)
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin)
|
||||
else()
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin)
|
||||
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,158 @@
|
||||
/*
|
||||
* Copyright (C) 2020 Open Source Robotics Foundation
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
*/
|
||||
#ifndef GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_
|
||||
#define GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_
|
||||
|
||||
#include <gz/sim/System.hh>
|
||||
#include <memory>
|
||||
|
||||
namespace gz
|
||||
{
|
||||
namespace sim
|
||||
{
|
||||
// Inline bracket to help doxygen filtering.
|
||||
inline namespace GZ_SIM_VERSION_NAMESPACE
|
||||
{
|
||||
namespace systems
|
||||
{
|
||||
// Forward declaration
|
||||
class BuoyancyPrivate;
|
||||
|
||||
/// \brief A system that simulates buoyancy of objects immersed in fluid.
|
||||
/// All SDF parameters are optional. This system must be attached to the
|
||||
/// world and this system will apply buoyancy to all links that have collision
|
||||
/// shapes.
|
||||
///
|
||||
/// The volume and center of volume will be computed for each link, and
|
||||
/// stored as components. During each iteration, Archimedes' principle is
|
||||
/// applied to each link with a volume and center of volume component.
|
||||
///
|
||||
/// Plane shapes are not handled by this plugin, and will not be affected
|
||||
/// by buoyancy.
|
||||
///
|
||||
/// ## System Parameters
|
||||
///
|
||||
/// * `<uniform_fluid_density>` sets the density of the fluid that surrounds
|
||||
/// the buoyant object. [Units: kgm^-3]
|
||||
/// * `<graded_buoyancy>` allows you to define a world where the buoyancy
|
||||
/// changes along the Z axis. An example of such a world could be if we are
|
||||
/// simulating an open ocean with its surface and under water behaviour. This
|
||||
/// mode slices the volume of each collision mesh according to where the water
|
||||
/// line is set. When defining a `<graded_buoyancy>` tag, one must also define
|
||||
/// `<default_density>` and `<density_change>` tags.
|
||||
/// * `<default_density>` is the default fluid which the world should be
|
||||
/// filled with. [Units: kgm^-3]
|
||||
/// * `<density_change>` allows you to define a new layer.
|
||||
/// * `<above_depth>` a child property of `<density_change>`. This determines
|
||||
/// the height at which the next fluid layer should start. [Units: m]
|
||||
/// * `<density>` the density of the fluid in this layer. [Units: kgm^-3]
|
||||
/// * `<enable>` used to indicate which models will have buoyancy.
|
||||
/// Add one enable element per model or link. This element accepts names
|
||||
/// scoped from the top level model (i.e. `<model>::<nested_model>::<link>`).
|
||||
/// If there are no enabled entities, all models in simulation will be
|
||||
/// affected by buoyancy.
|
||||
///
|
||||
/// ## Examples
|
||||
///
|
||||
/// ### uniform_fluid_density world
|
||||
///
|
||||
/// The `buoyancy.sdf` SDF file contains three submarines. The first
|
||||
/// submarine is neutrally buoyant, the second sinks, and the third
|
||||
/// floats. To run:
|
||||
///
|
||||
/// ```
|
||||
/// gz sim -v 4 buoyancy.sdf
|
||||
/// ```
|
||||
///
|
||||
/// ### graded_buoyancy world
|
||||
///
|
||||
/// Often when simulating a maritime environment one may need to simulate both
|
||||
/// surface and underwater vessels. This means the buoyancy plugin needs to
|
||||
/// take into account two different fluids. One being water with a density of
|
||||
/// 1000kgm^-3 and another being air with a very light density of say 1kgm^-3.
|
||||
/// An example for such a configuration may be found in the
|
||||
/// `graded_buoyancy.sdf` world.
|
||||
///
|
||||
/// ```
|
||||
/// gz sim -v 4 graded_buoyancy.sdf
|
||||
/// ```
|
||||
///
|
||||
/// You should be able to see a sphere bobbing up and down undergoing simple
|
||||
/// harmonic motion on the surface of the fluid (this is expected behaviour
|
||||
/// as the SHM is usually damped by the hydrodynamic forces. See the hydro-
|
||||
/// dynamics plugin for an example of how to use it). The key part of this is
|
||||
///
|
||||
/// ```
|
||||
/// <graded_buoyancy>
|
||||
/// <default_density>1000</default_density>
|
||||
/// <density_change>
|
||||
/// <above_depth>0</above_depth>
|
||||
/// <density>1</density>
|
||||
/// </density_change>
|
||||
/// </graded_buoyancy>
|
||||
/// ```
|
||||
/// The default density tag says that by default the world has a fluid density
|
||||
/// of 1000kgm^-3. This essentially states that by default the world is filled
|
||||
/// with dihydrogen monoxide (aka water). The `<density_change>` tag
|
||||
/// essentially establishes the fact that there is a nother fluid. The
|
||||
/// `<above_depth>` tag says that above z=0 there is another fluid with a
|
||||
/// different density. The density of that fluid is defined by the `<density>`
|
||||
/// tag. We will be simulating air with a fluid density of 1kgm^-3.
|
||||
class Buoyancy
|
||||
: public System,
|
||||
public ISystemConfigure,
|
||||
public ISystemPreUpdate,
|
||||
public ISystemPostUpdate
|
||||
{
|
||||
/// \brief Constructor
|
||||
public: Buoyancy();
|
||||
|
||||
/// \brief Destructor
|
||||
public: ~Buoyancy() override = default;
|
||||
|
||||
// Documentation inherited
|
||||
public: void Configure(const Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
EntityComponentManager &_ecm,
|
||||
EventManager &_eventMgr) override;
|
||||
|
||||
// Documentation inherited
|
||||
public: void PreUpdate(
|
||||
const UpdateInfo &_info,
|
||||
EntityComponentManager &_ecm) override;
|
||||
|
||||
// Documentation inherited
|
||||
public: void PostUpdate(
|
||||
const UpdateInfo &_info,
|
||||
const EntityComponentManager &_ecm) override;
|
||||
|
||||
/// \brief Check if an entity is enabled or not.
|
||||
/// \param[in] _entity Target entity
|
||||
/// \param[in] _ecm Entity component manager
|
||||
/// \return True if buoyancy should be applied.
|
||||
public: bool IsEnabled(Entity _entity,
|
||||
const EntityComponentManager &_ecm) const;
|
||||
|
||||
/// \brief Private data pointer
|
||||
private: std::unique_ptr<BuoyancyPrivate> dataPtr;
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,68 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
# Template for a new plugin project
|
||||
# Replace TemplatePlugin with your plugin name
|
||||
project(BuoyancySystemPlugin)
|
||||
|
||||
# Add external dependencies if needed
|
||||
# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
|
||||
|
||||
# Find required packages
|
||||
# find_package(PackageName REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
# Add your source files here
|
||||
BuoyancySystem.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
PUBLIC px4_gz_msgs
|
||||
PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
|
||||
PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
|
||||
PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
|
||||
PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
|
||||
# Add other dependencies as needed
|
||||
# PUBLIC ${OtherLib_LIBS}
|
||||
)
|
||||
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
|
||||
PUBLIC px4_gz_msgs
|
||||
# Add other include directories as needed
|
||||
# PUBLIC ${OtherLib_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# Add dependencies if needed
|
||||
# add_dependencies(${PROJECT_NAME} ExternalDependency)
|
||||
@@ -0,0 +1,68 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
# Template for a new plugin project
|
||||
# Replace TemplatePlugin with your plugin name
|
||||
project(GenericMotorModelPlugin)
|
||||
|
||||
# Add external dependencies if needed
|
||||
# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
|
||||
|
||||
# Find required packages
|
||||
# find_package(PackageName REQUIRED)
|
||||
|
||||
add_library(${PROJECT_NAME} SHARED
|
||||
# Add your source files here
|
||||
GenericMotorModel.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
PUBLIC px4_gz_msgs
|
||||
PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
|
||||
PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
|
||||
PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
|
||||
PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
|
||||
# Add other dependencies as needed
|
||||
# PUBLIC ${OtherLib_LIBS}
|
||||
)
|
||||
|
||||
target_include_directories(${PROJECT_NAME}
|
||||
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
|
||||
PUBLIC px4_gz_msgs
|
||||
# Add other include directories as needed
|
||||
# PUBLIC ${OtherLib_INCLUDE_DIRS}
|
||||
)
|
||||
|
||||
# Add dependencies if needed
|
||||
# add_dependencies(${PROJECT_NAME} ExternalDependency)
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,94 @@
|
||||
/*
|
||||
* Copyright (C) 2019 Open Source Robotics Foundation
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
*/
|
||||
#ifndef GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_
|
||||
#define GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_
|
||||
|
||||
#include <gz/sim/System.hh>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace gz
|
||||
{
|
||||
namespace sim
|
||||
{
|
||||
// Inline bracket to help doxygen filtering.
|
||||
inline namespace GZ_SIM_VERSION_NAMESPACE
|
||||
{
|
||||
namespace systems
|
||||
{
|
||||
// Forward declaration
|
||||
class GenericMotorModelPrivate;
|
||||
|
||||
/// \brief This system applies a thrust force to models with spinning
|
||||
/// propellers. See examples/worlds/quadcopter.sdf for a demonstration.
|
||||
class GenericMotorModel
|
||||
: public System,
|
||||
public ISystemConfigure,
|
||||
public ISystemPreUpdate
|
||||
{
|
||||
/// \brief Constructor
|
||||
public: GenericMotorModel();
|
||||
|
||||
/// \brief Destructor
|
||||
public: ~GenericMotorModel() override = default;
|
||||
|
||||
// Documentation inherited
|
||||
public: void Configure(const Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
EntityComponentManager &_ecm,
|
||||
EventManager &_eventMgr) override;
|
||||
|
||||
// Documentation inherited
|
||||
public: void PreUpdate(
|
||||
const gz::sim::UpdateInfo &_info,
|
||||
gz::sim::EntityComponentManager &_ecm) override;
|
||||
|
||||
/// \brief Private data pointer
|
||||
private: std::unique_ptr<GenericMotorModelPrivate> dataPtr;
|
||||
private:
|
||||
std::vector<double> ParsePolynomial(const std::string &input)
|
||||
{
|
||||
std::vector<double> result;
|
||||
std::string trimmed = input;
|
||||
|
||||
// Optional: remove brackets
|
||||
trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), '['), trimmed.end());
|
||||
trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), ']'), trimmed.end());
|
||||
|
||||
std::stringstream ss(trimmed);
|
||||
std::string token;
|
||||
|
||||
while (std::getline(ss, token, ',')) {
|
||||
try {
|
||||
result.push_back(std::stod(token));
|
||||
|
||||
} catch (const std::invalid_argument &e) {
|
||||
gzerr << "[YourPlugin] Invalid number: " << token << std::endl;
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2025 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -38,9 +38,12 @@
|
||||
*
|
||||
* All the acknowledgments and credits for the fw wing/rover app are reported in those files.
|
||||
*
|
||||
* 2025: refactoring of the mode settings: attitude, rate and manual control now working.
|
||||
*
|
||||
* @author Daniel Duecker <daniel.duecker@tuhh.de>
|
||||
* @author Philipp Hastedt <philipp.hastedt@tuhh.de>
|
||||
* @author Tim Hansen <t.hansen@tuhh.de>
|
||||
* @author Pedro Roque <padr@kth.se>
|
||||
*/
|
||||
|
||||
#include "uuv_att_control.hpp"
|
||||
@@ -94,7 +97,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_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);
|
||||
roll_u = math::constrain(roll_u, -_param_torque_sat.get(), _param_torque_sat.get());
|
||||
_vehicle_torque_setpoint.xyz[0] = roll_u;
|
||||
|
||||
} else {
|
||||
@@ -102,7 +105,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(pitch_u)) {
|
||||
pitch_u = math::constrain(pitch_u, -1.0f, 1.0f);
|
||||
pitch_u = math::constrain(pitch_u, -_param_torque_sat.get(), _param_torque_sat.get());
|
||||
_vehicle_torque_setpoint.xyz[1] = pitch_u;
|
||||
|
||||
} else {
|
||||
@@ -110,7 +113,7 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(yaw_u)) {
|
||||
yaw_u = math::constrain(yaw_u, -1.0f, 1.0f);
|
||||
yaw_u = math::constrain(yaw_u, -_param_torque_sat.get(), _param_torque_sat.get());
|
||||
_vehicle_torque_setpoint.xyz[2] = yaw_u;
|
||||
|
||||
} else {
|
||||
@@ -118,17 +121,33 @@ void UUVAttitudeControl::constrain_actuator_commands(float roll_u, float pitch_u
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(thrust_x)) {
|
||||
thrust_x = math::constrain(thrust_x, -1.0f, 1.0f);
|
||||
thrust_x = math::constrain(thrust_x, -_param_thrust_sat.get(), _param_thrust_sat.get());
|
||||
_vehicle_thrust_setpoint.xyz[0] = thrust_x;
|
||||
|
||||
} else {
|
||||
_vehicle_thrust_setpoint.xyz[0] = 0.0f;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(thrust_y)) {
|
||||
thrust_y = math::constrain(thrust_y, -_param_thrust_sat.get(), _param_thrust_sat.get());
|
||||
_vehicle_thrust_setpoint.xyz[1] = thrust_y;
|
||||
|
||||
} else {
|
||||
_vehicle_thrust_setpoint.xyz[1] = 0.0f;
|
||||
}
|
||||
|
||||
if (PX4_ISFINITE(thrust_z)) {
|
||||
thrust_z = math::constrain(thrust_z, -_param_thrust_sat.get(), _param_thrust_sat.get());
|
||||
_vehicle_thrust_setpoint.xyz[2] = thrust_z;
|
||||
|
||||
} else {
|
||||
_vehicle_thrust_setpoint.xyz[2] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void UUVAttitudeControl::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)
|
||||
const vehicle_rates_setpoint_s &rates_setpoint, bool attitude_control_enabled)
|
||||
{
|
||||
/** Geometric Controller
|
||||
*
|
||||
@@ -156,6 +175,7 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
|
||||
|
||||
Vector3f e_R_vec;
|
||||
Vector3f torques;
|
||||
torques.setZero();
|
||||
|
||||
/* Compute matrix: attitude error */
|
||||
Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5;
|
||||
@@ -171,29 +191,109 @@ void UUVAttitudeControl::control_attitude_geo(const vehicle_attitude_s &attitude
|
||||
omega(2) -= yaw_rate_desired;
|
||||
|
||||
/**< P-Control */
|
||||
torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */
|
||||
torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /**< Pitch */
|
||||
torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /**< Yaw */
|
||||
if (attitude_control_enabled) {
|
||||
torques(0) = - e_R_vec(0) * _param_roll_p.get(); /**< Roll */
|
||||
torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /**< Pitch */
|
||||
torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /**< Yaw */
|
||||
|
||||
/**< PD-Control */
|
||||
torques(0) = torques(0) - omega(0) * _param_roll_d.get(); /**< Roll */
|
||||
torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */
|
||||
torques(2) = torques(2) - omega(2) * _param_yaw_d.get(); /**< Yaw */
|
||||
// take thrust from attitude message
|
||||
float thrust_x = attitude_setpoint.thrust_body[0];
|
||||
float thrust_y = attitude_setpoint.thrust_body[1];
|
||||
float thrust_z = attitude_setpoint.thrust_body[2];
|
||||
|
||||
float roll_u = torques(0);
|
||||
float pitch_u = torques(1);
|
||||
float yaw_u = torques(2);
|
||||
/**< PD-Control */
|
||||
torques(0) -= omega(0) * _param_roll_d.get(); /**< Roll */
|
||||
torques(1) -= omega(1) * _param_pitch_d.get(); /**< Pitch */
|
||||
torques(2) -= omega(2) * _param_yaw_d.get(); /**< Yaw */
|
||||
|
||||
// take thrust as
|
||||
float thrust_x = attitude_setpoint.thrust_body[0];
|
||||
float thrust_y = attitude_setpoint.thrust_body[1];
|
||||
float thrust_z = attitude_setpoint.thrust_body[2];
|
||||
float roll_u = torques(0);
|
||||
float pitch_u = torques(1);
|
||||
float yaw_u = torques(2);
|
||||
|
||||
|
||||
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
|
||||
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
|
||||
|
||||
} else {
|
||||
// take thrust from rates message
|
||||
float thrust_x = _rates_setpoint.thrust_body[0];
|
||||
float thrust_y = _rates_setpoint.thrust_body[1];
|
||||
float thrust_z = _rates_setpoint.thrust_body[2];
|
||||
|
||||
/**< PD-Control */
|
||||
torques(0) -= omega(0) * _param_roll_d.get(); /**< Roll */
|
||||
torques(1) -= omega(1) * _param_pitch_d.get(); /**< Pitch */
|
||||
torques(2) -= omega(2) * _param_yaw_d.get(); /**< Yaw */
|
||||
|
||||
float roll_u = torques(0);
|
||||
float pitch_u = torques(1);
|
||||
float yaw_u = torques(2);
|
||||
|
||||
|
||||
constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
|
||||
}
|
||||
|
||||
/* Geometric Controller END*/
|
||||
}
|
||||
|
||||
void UUVAttitudeControl::generate_attitude_setpoint(float dt)
|
||||
{
|
||||
// Avoid accumulating absolute yaw error with arming stick gesture
|
||||
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();
|
||||
|
||||
// Integrate manual control inputs
|
||||
float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get();
|
||||
float roll_setpoint = roll + _manual_control_setpoint.roll * dt * _param_sgm_roll.get();
|
||||
float pitch_setpoint = pitch + -_manual_control_setpoint.pitch * dt * _param_sgm_pitch.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);
|
||||
|
||||
_attitude_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_sgm_thrtl.get();
|
||||
|
||||
_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
}
|
||||
|
||||
void UUVAttitudeControl::generate_rates_setpoint(float dt)
|
||||
{
|
||||
// Integrate manual control inputs
|
||||
_rates_setpoint.roll = _manual_control_setpoint.roll * dt * _param_rgm_roll.get();
|
||||
_rates_setpoint.pitch = -_manual_control_setpoint.pitch * dt * _param_rgm_pitch.get();
|
||||
_rates_setpoint.yaw = _manual_control_setpoint.yaw * dt * _param_rgm_yaw.get();
|
||||
|
||||
_rates_setpoint.thrust_body[0] = _manual_control_setpoint.throttle * _param_rgm_thrtl.get();
|
||||
_rates_setpoint.timestamp = hrt_absolute_time();
|
||||
|
||||
}
|
||||
|
||||
void UUVAttitudeControl::check_setpoint_validity(vehicle_attitude_s &v_att)
|
||||
{
|
||||
const float _setpoint_age = (hrt_absolute_time() - _attitude_setpoint.timestamp) * 1e-6f;
|
||||
|
||||
if (_setpoint_age < 0.0f || _setpoint_age > _param_setpoint_max_age.get()) {
|
||||
reset_attitude_setpoint(v_att);
|
||||
}
|
||||
}
|
||||
|
||||
void UUVAttitudeControl::reset_attitude_setpoint(vehicle_attitude_s &v_att)
|
||||
{
|
||||
_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
_attitude_setpoint.q_d[0] = v_att.q[0];
|
||||
_attitude_setpoint.q_d[1] = v_att.q[1];
|
||||
_attitude_setpoint.q_d[2] = v_att.q[2];
|
||||
_attitude_setpoint.q_d[3] = v_att.q[3];
|
||||
_attitude_setpoint.thrust_body[0] = 0.f;
|
||||
_attitude_setpoint.thrust_body[1] = 0.f;
|
||||
_attitude_setpoint.thrust_body[2] = 0.f;
|
||||
}
|
||||
|
||||
void UUVAttitudeControl::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
@@ -214,65 +314,82 @@ void UUVAttitudeControl::Run()
|
||||
|
||||
/* only run controller if attitude changed */
|
||||
if (_vehicle_attitude_sub.update(&attitude)) {
|
||||
const float dt = math::constrain(((attitude.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f);
|
||||
_last_run = attitude.timestamp_sample;
|
||||
|
||||
vehicle_angular_velocity_s angular_velocity {};
|
||||
_angular_velocity_sub.copy(&angular_velocity);
|
||||
|
||||
/* Run geometric attitude controllers if NOT manual mode*/
|
||||
if (!_vcontrol_mode.flag_control_manual_enabled
|
||||
&& _vcontrol_mode.flag_control_attitude_enabled
|
||||
&& _vcontrol_mode.flag_control_rates_enabled) {
|
||||
/* Check that we are not in position / velocity / altitude modes
|
||||
and that we are using manual inputs */
|
||||
if (_vcontrol_mode.flag_control_manual_enabled
|
||||
&& !_vcontrol_mode.flag_control_position_enabled
|
||||
&& !_vcontrol_mode.flag_control_velocity_enabled
|
||||
&& !_vcontrol_mode.flag_control_altitude_enabled) {
|
||||
|
||||
int input_mode = _param_input_mode.get();
|
||||
/* Update manual setpoints */
|
||||
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
|
||||
|
||||
_vehicle_attitude_setpoint_sub.update(&_attitude_setpoint);
|
||||
_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled
|
||||
&& _vcontrol_mode.flag_control_rates_enabled) {
|
||||
/* Run stabilized mode */
|
||||
_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
|
||||
|
||||
if (input_mode == 1) { // process manual data
|
||||
Quatf attitude_setpoint(Eulerf(_param_direct_roll.get(), _param_direct_pitch.get(), _param_direct_yaw.get()));
|
||||
attitude_setpoint.copyTo(_attitude_setpoint.q_d);
|
||||
_attitude_setpoint.thrust_body[0] = _param_direct_thrust.get();
|
||||
_attitude_setpoint.thrust_body[1] = 0.f;
|
||||
_attitude_setpoint.thrust_body[2] = 0.f;
|
||||
// Check setpoint validty
|
||||
check_setpoint_validity(attitude);
|
||||
|
||||
/* Generate atttiude setpoint from sticks */
|
||||
generate_attitude_setpoint(dt);
|
||||
|
||||
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, true);
|
||||
|
||||
} else if (!_vcontrol_mode.flag_control_attitude_enabled
|
||||
&& _vcontrol_mode.flag_control_rates_enabled) {
|
||||
/* Run Rate mode */
|
||||
generate_rates_setpoint(dt);
|
||||
|
||||
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, false);
|
||||
|
||||
} else if (!_vcontrol_mode.flag_control_attitude_enabled
|
||||
&& !_vcontrol_mode.flag_control_rates_enabled) {
|
||||
/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
|
||||
constrain_actuator_commands(_manual_control_setpoint.roll * _param_mgm_roll.get(),
|
||||
-_manual_control_setpoint.pitch * _param_mgm_pitch.get(),
|
||||
_manual_control_setpoint.yaw * _param_mgm_yaw.get(),
|
||||
_manual_control_setpoint.throttle * _param_mgm_thrtl.get(),
|
||||
0.f,
|
||||
0.f);
|
||||
}
|
||||
|
||||
/* Geometric Control*/
|
||||
int skip_controller = _param_skip_ctrl.get();
|
||||
|
||||
if (skip_controller) {
|
||||
constrain_actuator_commands(_rates_setpoint.roll, _rates_setpoint.pitch, _rates_setpoint.yaw,
|
||||
_rates_setpoint.thrust_body[0], _rates_setpoint.thrust_body[1], _rates_setpoint.thrust_body[2]);
|
||||
} else {
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
/* Get attitude and rate setpoints and control system */
|
||||
_vehicle_attitude_setpoint_sub.update(&_attitude_setpoint);
|
||||
_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
|
||||
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, true);
|
||||
|
||||
} else {
|
||||
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint);
|
||||
/* Get rate setpoints and control system */
|
||||
_vehicle_rates_setpoint_sub.update(&_rates_setpoint);
|
||||
control_attitude_geo(attitude, _attitude_setpoint, angular_velocity, _rates_setpoint, false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* Manual Control mode (e.g. gamepad,...) - raw feedthrough no assistance */
|
||||
if (_manual_control_setpoint_sub.update(&_manual_control_setpoint)) {
|
||||
// This should be copied even if not in manual mode. Otherwise, the poll(...) call will keep
|
||||
// returning immediately and this loop will eat up resources.
|
||||
if (_vcontrol_mode.flag_control_manual_enabled && !_vcontrol_mode.flag_control_rates_enabled) {
|
||||
/* manual/direct control */
|
||||
constrain_actuator_commands(_manual_control_setpoint.roll, -_manual_control_setpoint.pitch,
|
||||
_manual_control_setpoint.yaw,
|
||||
_manual_control_setpoint.throttle, 0.f, 0.f);
|
||||
/* Only publish if any of the proper modes are enabled */
|
||||
if (_vcontrol_mode.flag_control_manual_enabled ||
|
||||
_vcontrol_mode.flag_control_rates_enabled ||
|
||||
_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
|
||||
_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_thrust_setpoint.timestamp_sample = 0.f;
|
||||
_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
|
||||
|
||||
_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_torque_setpoint.timestamp_sample = 0.f;
|
||||
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
|
||||
}
|
||||
}
|
||||
|
||||
/* Only publish if any of the proper modes are enabled */
|
||||
if (_vcontrol_mode.flag_control_manual_enabled ||
|
||||
_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
|
||||
_vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_thrust_setpoint.timestamp_sample = 0.f;
|
||||
_vehicle_thrust_setpoint_pub.publish(_vehicle_thrust_setpoint);
|
||||
|
||||
_vehicle_torque_setpoint.timestamp = hrt_absolute_time();
|
||||
_vehicle_torque_setpoint.timestamp_sample = 0.f;
|
||||
_vehicle_torque_setpoint_pub.publish(_vehicle_torque_setpoint);
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
|
||||
@@ -75,7 +75,10 @@ using matrix::Eulerf;
|
||||
using matrix::Quatf;
|
||||
using matrix::Matrix3f;
|
||||
using matrix::Vector3f;
|
||||
using matrix::Vector2f;
|
||||
using matrix::Dcmf;
|
||||
using matrix::AxisAnglef;
|
||||
using matrix::AxisAngle;
|
||||
|
||||
using uORB::SubscriptionData;
|
||||
|
||||
@@ -122,6 +125,7 @@ private:
|
||||
vehicle_control_mode_s _vcontrol_mode{};
|
||||
|
||||
perf_counter_t _loop_perf;
|
||||
hrt_abstime _last_run{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::UUV_ROLL_P>) _param_roll_p,
|
||||
@@ -130,14 +134,23 @@ private:
|
||||
(ParamFloat<px4::params::UUV_PITCH_D>) _param_pitch_d,
|
||||
(ParamFloat<px4::params::UUV_YAW_P>) _param_yaw_p,
|
||||
(ParamFloat<px4::params::UUV_YAW_D>) _param_yaw_d,
|
||||
// control/input modes
|
||||
(ParamInt<px4::params::UUV_INPUT_MODE>) _param_input_mode,
|
||||
(ParamInt<px4::params::UUV_SKIP_CTRL>) _param_skip_ctrl,
|
||||
// direct access to inputs
|
||||
(ParamFloat<px4::params::UUV_DIRCT_ROLL>) _param_direct_roll,
|
||||
(ParamFloat<px4::params::UUV_DIRCT_PITCH>) _param_direct_pitch,
|
||||
(ParamFloat<px4::params::UUV_DIRCT_YAW>) _param_direct_yaw,
|
||||
(ParamFloat<px4::params::UUV_DIRCT_THRUST>) _param_direct_thrust
|
||||
// gains for the different modes
|
||||
(ParamFloat<px4::params::UUV_SGM_ROLL>) _param_sgm_roll,
|
||||
(ParamFloat<px4::params::UUV_SGM_PITCH>) _param_sgm_pitch,
|
||||
(ParamFloat<px4::params::UUV_SGM_YAW>) _param_sgm_yaw,
|
||||
(ParamFloat<px4::params::UUV_SGM_THRTL>) _param_sgm_thrtl,
|
||||
(ParamFloat<px4::params::UUV_RGM_ROLL>) _param_rgm_roll,
|
||||
(ParamFloat<px4::params::UUV_RGM_PITCH>) _param_rgm_pitch,
|
||||
(ParamFloat<px4::params::UUV_RGM_YAW>) _param_rgm_yaw,
|
||||
(ParamFloat<px4::params::UUV_RGM_THRTL>) _param_rgm_thrtl,
|
||||
(ParamFloat<px4::params::UUV_MGM_ROLL>) _param_mgm_roll,
|
||||
(ParamFloat<px4::params::UUV_MGM_PITCH>) _param_mgm_pitch,
|
||||
(ParamFloat<px4::params::UUV_MGM_YAW>) _param_mgm_yaw,
|
||||
(ParamFloat<px4::params::UUV_MGM_THRTL>) _param_mgm_thrtl,
|
||||
(ParamFloat<px4::params::UUV_TORQUE_SAT>) _param_torque_sat,
|
||||
(ParamFloat<px4::params::UUV_THRUST_SAT>) _param_thrust_sat,
|
||||
(ParamFloat<px4::params::UUV_SP_MAX_AGE>) _param_setpoint_max_age
|
||||
|
||||
)
|
||||
|
||||
void Run() override;
|
||||
@@ -150,7 +163,12 @@ private:
|
||||
* Control Attitude
|
||||
*/
|
||||
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);
|
||||
const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_s &rates_setpoint,
|
||||
bool attitude_control_enabled);
|
||||
void constrain_actuator_commands(float roll_u, float pitch_u, float yaw_u,
|
||||
float thrust_x, float thrust_y, float thrust_z);
|
||||
void generate_attitude_setpoint(float dt);
|
||||
void generate_rates_setpoint(float dt);
|
||||
void reset_attitude_setpoint(vehicle_attitude_s &v_att);
|
||||
void check_setpoint_validity(vehicle_attitude_s &v_att);
|
||||
};
|
||||
|
||||
@@ -42,6 +42,7 @@
|
||||
* All the ackowledgments and credits for the fw wing/rover app are reported in those files.
|
||||
*
|
||||
* @author Daniel Duecker <daniel.duecker@tuhh.de>
|
||||
* @author Pedro Roque <padr@kth.se>
|
||||
*/
|
||||
|
||||
/*
|
||||
@@ -103,48 +104,138 @@ PARAM_DEFINE_FLOAT(UUV_YAW_P, 4.0f);
|
||||
PARAM_DEFINE_FLOAT(UUV_YAW_D, 2.0f);
|
||||
|
||||
|
||||
// Input Modes
|
||||
// Gains for Manual Inputs in different Modes
|
||||
/**
|
||||
* Select Input Mode
|
||||
* Roll gain for manual inputs in attitude control mode
|
||||
*
|
||||
* @value 0 use Attitude Setpoints
|
||||
* @value 1 Direct Feedthrough
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UUV_INPUT_MODE, 0);
|
||||
PARAM_DEFINE_FLOAT(UUV_SGM_ROLL, 0.5f);
|
||||
|
||||
/**
|
||||
* Skip the controller
|
||||
* Pitch gain for manual inputs in attitude control mode
|
||||
*
|
||||
* @value 0 use the module's controller
|
||||
* @value 1 skip the controller and feedthrough the setpoints
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UUV_SKIP_CTRL, 0);
|
||||
PARAM_DEFINE_FLOAT(UUV_SGM_PITCH, 0.5f);
|
||||
|
||||
/**
|
||||
* Direct roll input
|
||||
* Yaw gain for manual inputs in attitude control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_SGM_YAW, 0.5f);
|
||||
|
||||
/**
|
||||
* Throttle gain for manual inputs in attitude control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_SGM_THRTL, 0.1f);
|
||||
|
||||
/**
|
||||
* Roll gain for manual inputs in rate control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_RGM_ROLL, 100.0f);
|
||||
|
||||
/**
|
||||
* Pitch gain for manual inputs in rate control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_RGM_PITCH, 100.0f);
|
||||
|
||||
/**
|
||||
* Yaw gain for manual inputs in rate control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_RGM_YAW, 100.0f);
|
||||
|
||||
/**
|
||||
* Throttle gain for manual inputs in rate control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_RGM_THRTL, 10.0f);
|
||||
|
||||
/**
|
||||
* Roll gain for manual inputs in manual control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_MGM_ROLL, 0.05f);
|
||||
|
||||
/**
|
||||
* Pitch gain for manual inputs in manual control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_MGM_PITCH, 0.05f);
|
||||
|
||||
/**
|
||||
* Yaw gain for manual inputs in manual control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_MGM_YAW, 0.05f);
|
||||
|
||||
/**
|
||||
* Throttle gain for manual inputs in manual control mode
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_MGM_THRTL, 0.1f);
|
||||
|
||||
/**
|
||||
* UUV Torque setpoint Saturation
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_TORQUE_SAT, 0.3f);
|
||||
|
||||
/**
|
||||
* UUV Thrust setpoint Saturation
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @decimal 2
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_THRUST_SAT, 0.1f);
|
||||
|
||||
/**
|
||||
* Maximum time (in seconds) before resetting setpoint
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_DIRCT_ROLL, 0.0f);
|
||||
|
||||
/**
|
||||
* Direct pitch input
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_DIRCT_PITCH, 0.0f);
|
||||
|
||||
/**
|
||||
* Direct yaw input
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_DIRCT_YAW, 0.0f);
|
||||
|
||||
/**
|
||||
* Direct thrust input
|
||||
*
|
||||
* @group UUV Attitude Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_DIRCT_THRUST, 0.0f);
|
||||
PARAM_DEFINE_FLOAT(UUV_SP_MAX_AGE, 2.0f);
|
||||
|
||||
@@ -90,61 +90,172 @@ void UUVPOSControl::parameters_update(bool force)
|
||||
}
|
||||
}
|
||||
|
||||
void UUVPOSControl::publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z,
|
||||
const float roll_des, const float pitch_des, const float yaw_des)
|
||||
{
|
||||
//watch if inputs are not to high
|
||||
vehicle_attitude_setpoint_s vehicle_attitude_setpoint = {};
|
||||
vehicle_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_des, pitch_des, yaw_des));
|
||||
attitude_setpoint.copyTo(vehicle_attitude_setpoint.q_d);
|
||||
|
||||
vehicle_attitude_setpoint.thrust_body[0] = thrust_x;
|
||||
vehicle_attitude_setpoint.thrust_body[1] = thrust_y;
|
||||
vehicle_attitude_setpoint.thrust_body[2] = thrust_z;
|
||||
|
||||
|
||||
_att_sp_pub.publish(vehicle_attitude_setpoint);
|
||||
}
|
||||
|
||||
void UUVPOSControl::pose_controller_6dof(const Vector3f &pos_des,
|
||||
const float roll_des, const float pitch_des, const float yaw_des,
|
||||
vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos)
|
||||
void UUVPOSControl::pose_controller_6dof(const Vector3f &pos_des, vehicle_attitude_s &vehicle_attitude,
|
||||
vehicle_local_position_s &vlocal_pos, bool altitude_mode)
|
||||
{
|
||||
//get current rotation of vehicle
|
||||
Quatf q_att(vehicle_attitude.q);
|
||||
|
||||
// Assumes target 0 velocity
|
||||
Vector3f p_control_output = Vector3f(_param_pose_gain_x.get() * (pos_des(0) - vlocal_pos.x) - _param_pose_gain_d_x.get()
|
||||
* vlocal_pos.vx,
|
||||
_param_pose_gain_y.get() * (pos_des(1) - vlocal_pos.y) - _param_pose_gain_d_y.get() * vlocal_pos.vy,
|
||||
_param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z) - _param_pose_gain_d_z.get() * vlocal_pos.vz);
|
||||
|
||||
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
|
||||
if (altitude_mode) {
|
||||
// In altitude mode, we only control the z-axis
|
||||
p_control_output(0) = 0.0f;
|
||||
p_control_output(1) = 0.0f;
|
||||
}
|
||||
|
||||
publish_attitude_setpoint(rotated_input(0),
|
||||
rotated_input(1),
|
||||
rotated_input(2),
|
||||
roll_des, pitch_des, yaw_des);
|
||||
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output); //rotate the coord.sys (from global to body)
|
||||
|
||||
_attitude_setpoint.timestamp = hrt_absolute_time();
|
||||
_attitude_setpoint.q_d[0] = _trajectory_setpoint.quaternion[0];
|
||||
_attitude_setpoint.q_d[1] = _trajectory_setpoint.quaternion[1];
|
||||
_attitude_setpoint.q_d[2] = _trajectory_setpoint.quaternion[2];
|
||||
_attitude_setpoint.q_d[3] = _trajectory_setpoint.quaternion[3];
|
||||
_attitude_setpoint.thrust_body[0] = rotated_input(0);
|
||||
_attitude_setpoint.thrust_body[1] = rotated_input(1);
|
||||
_attitude_setpoint.thrust_body[2] = rotated_input(2);
|
||||
}
|
||||
|
||||
void UUVPOSControl::stabilization_controller_6dof(const Vector3f &pos_des,
|
||||
const float roll_des, const float pitch_des, const float yaw_des,
|
||||
vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos)
|
||||
void UUVPOSControl::check_setpoint_validity(vehicle_local_position_s &vlocal_pos)
|
||||
{
|
||||
//get current rotation of vehicle
|
||||
const float _setpoint_age = (hrt_absolute_time() - _trajectory_setpoint.timestamp) * 1e-6f;
|
||||
|
||||
if (_setpoint_age < 0.0f || _setpoint_age > _param_setpoint_max_age.get()) {
|
||||
reset_trajectory_setpoint(vlocal_pos);
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(_trajectory_setpoint.position[0]) ||
|
||||
!PX4_ISFINITE(_trajectory_setpoint.position[1]) ||
|
||||
!PX4_ISFINITE(_trajectory_setpoint.position[2]) ||
|
||||
!PX4_ISFINITE(_trajectory_setpoint.quaternion[0]) ||
|
||||
!PX4_ISFINITE(_trajectory_setpoint.quaternion[1]) ||
|
||||
!PX4_ISFINITE(_trajectory_setpoint.quaternion[2]) ||
|
||||
!PX4_ISFINITE(_trajectory_setpoint.quaternion[3])) {
|
||||
reset_trajectory_setpoint(vlocal_pos);
|
||||
}
|
||||
}
|
||||
|
||||
void UUVPOSControl::generate_trajectory_setpoint(vehicle_local_position_s &vlocal_pos,
|
||||
vehicle_attitude_s &vehicle_attitude,
|
||||
float dt)
|
||||
{
|
||||
float roll = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).phi();
|
||||
float pitch = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).theta();
|
||||
float yaw = Eulerf(matrix::Quatf(_trajectory_setpoint.quaternion)).psi();
|
||||
|
||||
// Integrate manual control inputs
|
||||
// Info:
|
||||
// - throttle is Z, roll is Y, pitch is X
|
||||
// - if param_stab_mode == 1:
|
||||
// - roll = 0
|
||||
// - pitch = 0
|
||||
// - if param_stab_mode == 0:
|
||||
// - roll can be updated with D-pad (joystick)
|
||||
// - pitch can be updated with D-pad (joystick)
|
||||
float roll_setpoint = roll;
|
||||
float pitch_setpoint = pitch;
|
||||
|
||||
if (_param_stab_mode.get()) {
|
||||
roll_setpoint = 0.0;
|
||||
pitch_setpoint = 0.0;
|
||||
|
||||
} else {
|
||||
// Update target roll and pitch setpoint with D-pad
|
||||
switch (_manual_control_setpoint.buttons) {
|
||||
case 2048:
|
||||
pitch_setpoint -= dt * _param_sgm_pitch.get();
|
||||
break;
|
||||
|
||||
case 4096:
|
||||
pitch_setpoint += dt * _param_sgm_pitch.get();
|
||||
break;
|
||||
|
||||
case 8192:
|
||||
roll_setpoint -= dt * _param_sgm_roll.get();
|
||||
break;
|
||||
|
||||
case 16384:
|
||||
roll_setpoint += dt * _param_sgm_roll.get();
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float yaw_setpoint = yaw + _manual_control_setpoint.yaw * dt * _param_sgm_yaw.get();
|
||||
|
||||
// Update position setpoints based on manual control inputs
|
||||
float vx_sp = 0.0;
|
||||
|
||||
if (_manual_control_setpoint.pitch > _param_pos_stick_db.get()
|
||||
|| _manual_control_setpoint.pitch < -_param_pos_stick_db.get()) {
|
||||
// If pitch is not zero, we use it to set the roll setpoint
|
||||
vx_sp = _manual_control_setpoint.pitch * _param_pgm_vel.get();
|
||||
}
|
||||
|
||||
float vy_sp = 0.0;
|
||||
|
||||
if (_manual_control_setpoint.roll > _param_pos_stick_db.get()
|
||||
|| _manual_control_setpoint.roll < -_param_pos_stick_db.get()) {
|
||||
// If roll is not zero, we use it to set the pitch setpoint
|
||||
vy_sp = _manual_control_setpoint.roll * _param_pgm_vel.get();
|
||||
}
|
||||
|
||||
float vz_sp = 0.0;
|
||||
|
||||
if (_manual_control_setpoint.throttle > _param_pos_stick_db.get()
|
||||
|| _manual_control_setpoint.throttle < -_param_pos_stick_db.get()) {
|
||||
// If throttle is not zero, we use it to set the vertical velocity
|
||||
vz_sp = -_manual_control_setpoint.throttle * _param_pgm_vel.get();
|
||||
}
|
||||
|
||||
// rotate velocity setpoint in body frame to global frame
|
||||
Vector3f velocity_setpoint(vx_sp, vy_sp, vz_sp);
|
||||
Quatf q_att(vehicle_attitude.q);
|
||||
Vector3f rotated_velocity_setpoint = q_att.rotateVector(velocity_setpoint);
|
||||
|
||||
Vector3f p_control_output = Vector3f(0,
|
||||
0,
|
||||
_param_pose_gain_z.get() * (pos_des(2) - vlocal_pos.z));
|
||||
//potential d controller missing
|
||||
Vector3f rotated_input = q_att.rotateVectorInverse(p_control_output);//rotate the coord.sys (from global to body)
|
||||
// Generate target quaternion
|
||||
Eulerf euler_sp(roll_setpoint, pitch_setpoint, yaw_setpoint);
|
||||
Quatf q_sp = euler_sp;
|
||||
|
||||
publish_attitude_setpoint(rotated_input(0) + pos_des(0), rotated_input(1) + pos_des(1), rotated_input(2),
|
||||
roll_des, pitch_des, yaw_des);
|
||||
// Normalize the quaternion to avoid numerical issues
|
||||
q_sp.normalize();
|
||||
|
||||
q_sp.copyTo(_trajectory_setpoint.quaternion);
|
||||
|
||||
_trajectory_setpoint.timestamp = hrt_absolute_time();
|
||||
|
||||
if (!_param_pos_mode.get()) {
|
||||
_trajectory_setpoint.position[0] = _trajectory_setpoint.position[0] + vx_sp * dt; // X in world frame
|
||||
_trajectory_setpoint.position[1] = _trajectory_setpoint.position[1] + vy_sp * dt; // Y in world frame
|
||||
_trajectory_setpoint.position[2] = _trajectory_setpoint.position[2] + vz_sp * dt; // Z in world frame
|
||||
|
||||
} else {
|
||||
_trajectory_setpoint.position[0] = _trajectory_setpoint.position[0] + rotated_velocity_setpoint(
|
||||
0) * dt; // X in body frame
|
||||
_trajectory_setpoint.position[1] = _trajectory_setpoint.position[1] + rotated_velocity_setpoint(
|
||||
1) * dt; // Y in body frame
|
||||
_trajectory_setpoint.position[2] = _trajectory_setpoint.position[2] + rotated_velocity_setpoint(
|
||||
2) * dt; // Z in body frame
|
||||
}
|
||||
}
|
||||
|
||||
void UUVPOSControl::reset_trajectory_setpoint(vehicle_local_position_s &vlocal_pos)
|
||||
{
|
||||
// Reset trajectory setpoint to current position and attitude
|
||||
_trajectory_setpoint.timestamp = hrt_absolute_time();
|
||||
_trajectory_setpoint.position[0] = vlocal_pos.x;
|
||||
_trajectory_setpoint.position[1] = vlocal_pos.y;
|
||||
_trajectory_setpoint.position[2] = vlocal_pos.z;
|
||||
_trajectory_setpoint.quaternion[0] = _vehicle_attitude.q[0];
|
||||
_trajectory_setpoint.quaternion[1] = _vehicle_attitude.q[1];
|
||||
_trajectory_setpoint.quaternion[2] = _vehicle_attitude.q[2];
|
||||
_trajectory_setpoint.quaternion[3] = _vehicle_attitude.q[3];
|
||||
}
|
||||
|
||||
void UUVPOSControl::Run()
|
||||
@@ -160,7 +271,6 @@ void UUVPOSControl::Run()
|
||||
/* check vehicle control mode for changes to publication state */
|
||||
_vcontrol_mode_sub.update(&_vcontrol_mode);
|
||||
|
||||
|
||||
/* update parameters from storage */
|
||||
parameters_update();
|
||||
|
||||
@@ -169,34 +279,58 @@ void UUVPOSControl::Run()
|
||||
|
||||
/* 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);
|
||||
_last_run = vlocal_pos.timestamp_sample;
|
||||
|
||||
/* Run geometric attitude controllers if NOT manual mode*/
|
||||
if (!_vcontrol_mode.flag_control_manual_enabled
|
||||
&& _vcontrol_mode.flag_control_attitude_enabled
|
||||
&& _vcontrol_mode.flag_control_rates_enabled) {
|
||||
// Update vehicle attitude
|
||||
_vehicle_attitude_sub.update(&_vehicle_attitude);
|
||||
|
||||
_vehicle_attitude_sub.update(&_vehicle_attitude);//get current vehicle attitude
|
||||
/* Run position or altitude mode from manual setpoints*/
|
||||
if (_vcontrol_mode.flag_control_manual_enabled
|
||||
&& (_vcontrol_mode.flag_control_altitude_enabled
|
||||
|| _vcontrol_mode.flag_control_position_enabled)
|
||||
&& _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);
|
||||
|
||||
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
|
||||
|| _vcontrol_mode.flag_control_position_enabled)
|
||||
&& _vcontrol_mode.flag_armed) {
|
||||
/* Autonomous position mode - no manual inputs are used */
|
||||
const bool altitude_only_flag = _vcontrol_mode.flag_control_altitude_enabled
|
||||
&& ! _vcontrol_mode.flag_control_position_enabled;
|
||||
|
||||
// get manual control setpoint
|
||||
_trajectory_setpoint_sub.update(&_trajectory_setpoint);
|
||||
|
||||
float roll_des = 0;
|
||||
float pitch_des = 0;
|
||||
float yaw_des = _trajectory_setpoint.yaw;
|
||||
pose_controller_6dof(Vector3f(_trajectory_setpoint.position), _vehicle_attitude,
|
||||
vlocal_pos, altitude_only_flag);
|
||||
|
||||
//stabilization controller(keep pos and hold depth + angle) vs position controller(global + yaw)
|
||||
if (_param_stabilization.get() == 0) {
|
||||
pose_controller_6dof(Vector3f(_trajectory_setpoint.position),
|
||||
roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
|
||||
|
||||
} else {
|
||||
stabilization_controller_6dof(Vector3f(_trajectory_setpoint.position),
|
||||
roll_des, pitch_des, yaw_des, _vehicle_attitude, vlocal_pos);
|
||||
}
|
||||
} else {
|
||||
// Reset if not in a valid mode (like attitude, rate, manual) to clear setpoint
|
||||
check_setpoint_validity(vlocal_pos);
|
||||
}
|
||||
}
|
||||
|
||||
/* Only publish if any of the proper modes are enabled */
|
||||
if (_vcontrol_mode.flag_control_manual_enabled ||
|
||||
_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
if (_vcontrol_mode.flag_control_position_enabled ||
|
||||
_vcontrol_mode.flag_control_altitude_enabled) {
|
||||
// Print attitude setpoint
|
||||
_att_sp_pub.publish(_attitude_setpoint);
|
||||
}
|
||||
|
||||
perf_end(_loop_perf);
|
||||
|
||||
@@ -59,8 +59,9 @@
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/trajectory_setpoint.h>
|
||||
#include <uORB/topics/trajectory_setpoint6dof.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
@@ -101,16 +102,20 @@ private:
|
||||
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
|
||||
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
|
||||
uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint6dof)};
|
||||
uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; /**< notification of manual control updates */
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)};
|
||||
|
||||
vehicle_attitude_s _vehicle_attitude{};
|
||||
trajectory_setpoint_s _trajectory_setpoint{};
|
||||
trajectory_setpoint6dof_s _trajectory_setpoint{};
|
||||
manual_control_setpoint_s _manual_control_setpoint{};
|
||||
vehicle_control_mode_s _vcontrol_mode{};
|
||||
vehicle_attitude_setpoint_s _attitude_setpoint{};
|
||||
|
||||
perf_counter_t _loop_perf;
|
||||
hrt_abstime _last_run{0};
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::UUV_GAIN_X_P>) _param_pose_gain_x,
|
||||
@@ -119,10 +124,14 @@ private:
|
||||
(ParamFloat<px4::params::UUV_GAIN_X_D>) _param_pose_gain_d_x,
|
||||
(ParamFloat<px4::params::UUV_GAIN_Y_D>) _param_pose_gain_d_y,
|
||||
(ParamFloat<px4::params::UUV_GAIN_Z_D>) _param_pose_gain_d_z,
|
||||
|
||||
(ParamInt<px4::params::UUV_INPUT_MODE>) _param_input_mode,
|
||||
(ParamInt<px4::params::UUV_STAB_MODE>) _param_stabilization,
|
||||
(ParamInt<px4::params::UUV_SKIP_CTRL>) _param_skip_ctrl
|
||||
(ParamInt<px4::params::UUV_STAB_MODE>) _param_stab_mode,
|
||||
(ParamFloat<px4::params::UUV_POS_STICK_DB>) _param_pos_stick_db,
|
||||
(ParamFloat<px4::params::UUV_PGM_VEL>) _param_pgm_vel,
|
||||
(ParamFloat<px4::params::UUV_SGM_ROLL>) _param_sgm_roll,
|
||||
(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
|
||||
)
|
||||
|
||||
void Run() override;
|
||||
@@ -136,10 +145,12 @@ private:
|
||||
*/
|
||||
void publish_attitude_setpoint(const float thrust_x, const float thrust_y, const float thrust_z,
|
||||
const float roll_des, const float pitch_des, const float yaw_des);
|
||||
void pose_controller_6dof(const Vector3f &pos_des,
|
||||
const float roll_des, const float pitch_des, const float yaw_des,
|
||||
vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos);
|
||||
void pose_controller_6dof(const Vector3f &pos_des, vehicle_attitude_s &vehicle_attitude,
|
||||
vehicle_local_position_s &vlocal_pos, bool altitude_mode);
|
||||
void stabilization_controller_6dof(const Vector3f &pos_des,
|
||||
const float roll_des, const float pitch_des, const float yaw_des,
|
||||
vehicle_attitude_s &vehicle_attitude, vehicle_local_position_s &vlocal_pos);
|
||||
void generate_trajectory_setpoint(vehicle_local_position_s &vlocal_pos, vehicle_attitude_s &vehicle_attitude, float dt);
|
||||
void reset_trajectory_setpoint(vehicle_local_position_s &vlocal_pos);
|
||||
void check_setpoint_validity(vehicle_local_position_s &vlocal_pos);
|
||||
};
|
||||
|
||||
@@ -90,8 +90,31 @@ PARAM_DEFINE_FLOAT(UUV_GAIN_Z_D, 0.2f);
|
||||
/**
|
||||
* Stabilization mode(1) or Position Control(0)
|
||||
*
|
||||
* @value 0 Position Control
|
||||
* @value 1 Stabilization Mode
|
||||
* @value 0 Tracks previous attitude setpoint
|
||||
* @value 1 Tracks horizontal attitude (allows yaw change)
|
||||
* @group UUV Position Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UUV_STAB_MODE, 1);
|
||||
|
||||
/**
|
||||
* Deadband for changing position setpoint
|
||||
*
|
||||
* @group UUV Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_POS_STICK_DB, 0.1f);
|
||||
|
||||
/**
|
||||
* Gain for position control velocity setpoint update
|
||||
*
|
||||
* @group UUV Position Control
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(UUV_PGM_VEL, 0.5f);
|
||||
|
||||
/**
|
||||
* Stabilization mode(1) or Position Control(0)
|
||||
*
|
||||
* @value 0 Moves position setpoint in world frame
|
||||
* @value 1 Moves position setpoint in body frame
|
||||
* @group UUV Position Control
|
||||
*/
|
||||
PARAM_DEFINE_INT32(UUV_POS_MODE, 1);
|
||||
|
||||
Reference in New Issue
Block a user