Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-23 22:58:10 +08:00
Code Issues Actions 15 Packages Projects Releases Wiki Activity

mecanum: deprecate RM_MAN_YAW_SCALE

Browse Source
This commit is contained in:
chfriedrich98
2024-11-12 16:29:36 +01:00
committed by chfriedrich98
parent 6cce443005
commit 54abc59283
6 changed files with 33 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum
+8 -9
View File
@@ -13,7 +13,6 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge
# Rover parameters
param set-default RM_WHEEL_TRACK 0.3
param set-default RM_MAN_YAW_SCALE 0.1
param set-default RM_YAW_RATE_I 0
param set-default RM_YAW_RATE_P 0.01
param set-default RM_MAX_ACCEL 3
@@ -41,23 +40,23 @@ param set-default SENS_EN_ARSPDSIM 0
# Actuator mapping
param set-default SIM_GZ_WH_FUNC1 102 # right wheel front
param set-default SIM_GZ_WH_MIN1 0
param set-default SIM_GZ_WH_MAX1 200
param set-default SIM_GZ_WH_MIN1 70
param set-default SIM_GZ_WH_MAX1 130
param set-default SIM_GZ_WH_DIS1 100
param set-default SIM_GZ_WH_FUNC2 101 # left wheel front
param set-default SIM_GZ_WH_MIN2 0
param set-default SIM_GZ_WH_MAX2 200
param set-default SIM_GZ_WH_MIN2 70
param set-default SIM_GZ_WH_MAX2 130
param set-default SIM_GZ_WH_DIS2 100
param set-default SIM_GZ_WH_FUNC3 104 # right wheel back
param set-default SIM_GZ_WH_MIN3 0
param set-default SIM_GZ_WH_MAX3 200
param set-default SIM_GZ_WH_MIN3 70
param set-default SIM_GZ_WH_MAX3 130
param set-default SIM_GZ_WH_DIS3 100
param set-default SIM_GZ_WH_FUNC4 103 # left wheel back
param set-default SIM_GZ_WH_MIN4 0
param set-default SIM_GZ_WH_MAX4 200
param set-default SIM_GZ_WH_MIN4 70
param set-default SIM_GZ_WH_MAX4 130
param set-default SIM_GZ_WH_DIS4 100
param set-default SIM_GZ_WH_REV 10
src/modules/rover_mecanum/RoverMecanum.cpp
+17 -5
View File
@@ -81,7 +81,19 @@ void RoverMecanum::Run()
rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
rover_mecanum_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.yaw * _param_rm_man_yaw_scale.get();
if (_max_yaw_rate > FLT_EPSILON && _param_rm_max_thr_yaw_r.get() > FLT_EPSILON) {
const float scaled_yaw_rate_input = math::interpolate<float>(manual_control_setpoint.yaw,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
const float speed_diff = scaled_yaw_rate_input * _param_rm_wheel_track.get() / 2.f;
rover_mecanum_setpoint.speed_diff_setpoint_normalized = math::interpolate<float>(speed_diff,
-_param_rm_max_thr_yaw_r.get(), _param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
} else {
rover_mecanum_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.yaw;
}
rover_mecanum_setpoint.yaw_setpoint = NAN;
_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
@@ -100,7 +112,7 @@ void RoverMecanum::Run()
rover_mecanum_setpoint.lateral_speed_setpoint_normalized = manual_control_setpoint.roll;
rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.yaw,
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
rover_mecanum_setpoint.yaw_setpoint = NAN;
_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
}
@@ -119,7 +131,7 @@ void RoverMecanum::Run()
rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
STICK_DEADZONE),
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
rover_mecanum_setpoint.yaw_setpoint = NAN;
if (fabsf(rover_mecanum_setpoint.yaw_rate_setpoint) > FLT_EPSILON
@@ -157,7 +169,7 @@ void RoverMecanum::Run()
rover_mecanum_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
STICK_DEADZONE),
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
rover_mecanum_setpoint.yaw_setpoint = NAN;
// Reset cruise control if the manual input changes
@@ -226,7 +238,7 @@ void RoverMecanum::Run()
rover_mecanum_setpoint.lateral_speed_setpoint = NAN;
rover_mecanum_setpoint.lateral_speed_setpoint_normalized = 0.f;
rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
rover_mecanum_setpoint.yaw_rate_setpoint_normalized = 0.f;
rover_mecanum_setpoint.speed_diff_setpoint_normalized = 0.f;
rover_mecanum_setpoint.yaw_setpoint = NAN;
_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
break;
src/modules/rover_mecanum/RoverMecanum.hpp
+5 -4
View File
@@ -133,9 +133,10 @@ private:
bool _armed{false};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
(ParamFloat<px4::params::RM_MAN_YAW_SCALE>) _param_rm_man_yaw_scale,
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
(ParamFloat<px4::params::RM_MAX_SPEED>) _param_rm_max_speed,
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
)
};
src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp
+2 -2
View File
@@ -103,8 +103,8 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
-1.f, 1.f); // Feedback
} else { // Use normalized setpoint
speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint_normalized) ? math::constrain(
_rover_mecanum_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f;
speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized) ? math::constrain(
_rover_mecanum_setpoint.speed_diff_setpoint_normalized, -1.f, 1.f) : 0.f;
}
// Speed control
src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp
+1 -1
View File
@@ -110,7 +110,7 @@ void RoverMecanumGuidance::computeGuidance(const float yaw, const int nav_state)
rover_mecanum_setpoint.lateral_speed_setpoint = desired_velocity(1);
rover_mecanum_setpoint.lateral_speed_setpoint_normalized = NAN;
rover_mecanum_setpoint.yaw_rate_setpoint = NAN;
rover_mecanum_setpoint.yaw_rate_setpoint_normalized = NAN;
rover_mecanum_setpoint.speed_diff_setpoint_normalized = NAN;
rover_mecanum_setpoint.yaw_setpoint = _desired_yaw;
_rover_mecanum_setpoint_pub.publish(rover_mecanum_setpoint);
}
src/modules/rover_mecanum/module.yaml
-13
View File
@@ -4,19 +4,6 @@ parameters:
- group: Rover Mecanum
definitions:
RM_MAN_YAW_SCALE:
description:
short: Manual yaw rate scale
long: |
In Manual mode the setpoint for the yaw rate received from the control stick
is scaled by this value.
type: float
min: 0.01
max: 1
increment: 0.01
decimal: 2
default: 1
RM_WHEEL_TRACK:
description:
short: Wheel track