diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 42ac895f46..711427e2c5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -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 diff --git a/src/modules/rover_mecanum/RoverMecanum.cpp b/src/modules/rover_mecanum/RoverMecanum.cpp index 2bb3d23c3f..6c9538d6dd 100644 --- a/src/modules/rover_mecanum/RoverMecanum.cpp +++ b/src/modules/rover_mecanum/RoverMecanum.cpp @@ -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(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(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(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(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(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; diff --git a/src/modules/rover_mecanum/RoverMecanum.hpp b/src/modules/rover_mecanum/RoverMecanum.hpp index b2d89efbd3..7837d436dc 100644 --- a/src/modules/rover_mecanum/RoverMecanum.hpp +++ b/src/modules/rover_mecanum/RoverMecanum.hpp @@ -133,9 +133,10 @@ private: bool _armed{false}; DEFINE_PARAMETERS( - (ParamFloat) _param_rm_max_speed, - (ParamFloat) _param_rm_man_yaw_scale, - (ParamFloat) _param_rm_max_yaw_rate, - (ParamFloat) _param_pp_lookahd_max + (ParamFloat) _param_rm_max_speed, + (ParamFloat) _param_rm_wheel_track, + (ParamFloat) _param_rm_max_thr_yaw_r, + (ParamFloat) _param_rm_max_yaw_rate, + (ParamFloat) _param_pp_lookahd_max ) }; diff --git a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp index 87c9f86079..ecb0ea61c2 100644 --- a/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp +++ b/src/modules/rover_mecanum/RoverMecanumControl/RoverMecanumControl.cpp @@ -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 diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index a3d623820c..b72478d746 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -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); } diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index bc93fc5e03..fa1bd01e4b 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -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