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 e0968a3db5..42ac895f46 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 @@ -24,7 +24,6 @@ param set-default RM_MAX_THR_YAW_R 7.5 param set-default RM_YAW_P 5 param set-default RM_YAW_I 0.1 param set-default RM_MAX_YAW_RATE 180 -param set-default RM_MISS_SPD_DEF 3 param set-default RM_MISS_VEL_GAIN 1 param set-default RM_SPEED_I 0.01 param set-default RM_SPEED_P 0.1 diff --git a/msg/RoverMecanumSetpoint.msg b/msg/RoverMecanumSetpoint.msg index 8718c039a7..0cc9415be1 100644 --- a/msg/RoverMecanumSetpoint.msg +++ b/msg/RoverMecanumSetpoint.msg @@ -5,7 +5,7 @@ float32 forward_speed_setpoint_normalized # [-1, 1] Desired normalized forward s float32 lateral_speed_setpoint # [m/s] Desired lateral speed float32 lateral_speed_setpoint_normalized # [-1, 1] Desired normalized lateral speed float32 yaw_rate_setpoint # [rad/s] Desired yaw rate -float32 yaw_rate_setpoint_normalized # [-1, 1] Desired normalized yaw rate +float32 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels float32 yaw_setpoint # [rad] Desired yaw (heading) # TOPICS rover_mecanum_setpoint diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp index 4ffab54919..a3d623820c 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.cpp @@ -41,7 +41,7 @@ using namespace time_literals; RoverMecanumGuidance::RoverMecanumGuidance(ModuleParams *parent) : ModuleParams(parent) { updateParams(); - _max_velocity_magnitude = _param_rm_miss_spd_def.get(); + _max_velocity_magnitude = _param_rm_max_speed.get(); _rover_mecanum_guidance_status_pub.advertise(); } @@ -195,13 +195,9 @@ void RoverMecanumGuidance::updateWaypoints() _waypoint_transition_angle = acosf(cosin); // Waypoint cruising speed - if (position_setpoint_triplet.current.cruising_speed > FLT_EPSILON) { - _max_velocity_magnitude = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, - _param_rm_max_speed.get()); - - } else { - _max_velocity_magnitude = _param_rm_miss_spd_def.get(); - } + _max_velocity_magnitude = position_setpoint_triplet.current.cruising_speed > FLT_EPSILON ? math::constrain( + position_setpoint_triplet.current.cruising_speed, 0.f, + _param_rm_max_speed.get()) : _param_rm_max_speed.get(); // Waypoint yaw setpoint if (PX4_ISFINITE(position_setpoint_triplet.current.yaw)) { diff --git a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp index 509c2251b4..0970b83646 100644 --- a/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp +++ b/src/modules/rover_mecanum/RoverMecanumGuidance/RoverMecanumGuidance.hpp @@ -133,7 +133,6 @@ private: (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rm_max_jerk, (ParamFloat) _param_rm_max_accel, - (ParamFloat) _param_rm_miss_spd_def, (ParamFloat) _param_rm_max_yaw_rate, (ParamFloat) _param_rm_miss_vel_gain ) diff --git a/src/modules/rover_mecanum/module.yaml b/src/modules/rover_mecanum/module.yaml index 92f6600420..bc93fc5e03 100644 --- a/src/modules/rover_mecanum/module.yaml +++ b/src/modules/rover_mecanum/module.yaml @@ -173,17 +173,6 @@ parameters: decimal: 2 default: 0.5 - RM_MISS_SPD_DEF: - description: - short: Default rover speed during a mission - type: float - unit: m/s - min: 0 - max: 100 - increment: 0.01 - decimal: 2 - default: 1 - RM_MISS_VEL_GAIN: description: short: Tuning parameter for the velocity reduction during waypoint transition