mecanum: deprecate RM_MAN_YAW_SCALE

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

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<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

@@ -133,9 +133,10 @@ class RoverMecanum : public ModuleBase<RoverMecanum>, public ModuleParams,
 	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

@@ -116,8 +116,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

@@ -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

@@ -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