Skip to content

Mecanum: Seperate velocity control #24647

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 2 commits into from
May 5, 2025
Merged

Mecanum: Seperate velocity control #24647

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
1e3ca6f
mecanum: separate velocity control
chfriedrich98 Mar 21, 2025
d9d6848
mecanum: streamline flow of information
chfriedrich98 Apr 29, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions msg/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -131,6 +131,7 @@ set(msg_files
ManualControlSwitches.msg
MavlinkLog.msg
MavlinkTunnel.msg
MecanumVelocitySetpoint.msg
MessageFormatRequest.msg
MessageFormatResponse.msg
Mission.msg
Expand Down
5 changes: 5 additions & 0 deletions msg/MecanumVelocitySetpoint.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
uint64 timestamp # time since system start (microseconds)

float32 speed # [m/s] [-inf, inf] Speed setpoint
float32 bearing # [rad] [-pi, pi] from North.
float32 yaw # [rad] [-pi, pi] (Optional, defaults to current vehicle yaw) Vehicle yaw setpoint in NED frame
1 change: 1 addition & 0 deletions src/modules/logger/logged_topics.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,6 +93,7 @@ void LoggedTopics::add_default_topics()
add_optional_topic("magnetometer_bias_estimate", 200);
add_topic("manual_control_setpoint", 200);
add_topic("manual_control_switches");
add_optional_topic("mecanum_velocity_setpoint", 100);
add_topic("mission_result");
add_topic("navigator_mission_item");
add_topic("navigator_status");
Expand Down
6 changes: 4 additions & 2 deletions src/modules/rover_mecanum/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,8 @@

add_subdirectory(MecanumRateControl)
add_subdirectory(MecanumAttControl)
add_subdirectory(MecanumPosVelControl)
add_subdirectory(MecanumVelControl)
add_subdirectory(MecanumPosControl)

px4_add_module(
MODULE modules__rover_mecanum
Expand All @@ -44,7 +45,8 @@ px4_add_module(
DEPENDS
MecanumRateControl
MecanumAttControl
MecanumPosVelControl
MecanumVelControl
MecanumPosControl
px4_work_queue
rover_control
pure_pursuit
Expand Down
37 changes: 14 additions & 23 deletions src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,7 @@ void MecanumAttControl::updateAttControl()
if (_vehicle_control_mode.flag_control_attitude_enabled && _vehicle_control_mode.flag_armed && runSanityChecks()) {

if (_vehicle_control_mode.flag_control_manual_enabled || _vehicle_control_mode.flag_control_offboard_enabled) {
generateAttitudeSetpoint();
generateAttitudeAndThrottleSetpoint();
}

generateRateSetpoint();
Expand All @@ -92,12 +92,12 @@ void MecanumAttControl::updateAttControl()
rover_attitude_status_s rover_attitude_status;
rover_attitude_status.timestamp = _timestamp;
rover_attitude_status.measured_yaw = _vehicle_yaw;
rover_attitude_status.adjusted_yaw_setpoint = _adjusted_yaw_setpoint.getState();
rover_attitude_status.adjusted_yaw_setpoint = matrix::wrap_pi(_adjusted_yaw_setpoint.getState());
_rover_attitude_status_pub.publish(rover_attitude_status);

}

void MecanumAttControl::generateAttitudeSetpoint()
void MecanumAttControl::generateAttitudeAndThrottleSetpoint()
{
const bool stab_mode_enabled = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_position_enabled && _vehicle_control_mode.flag_control_attitude_enabled;
Expand All @@ -113,20 +113,19 @@ void MecanumAttControl::generateAttitudeSetpoint()
rover_throttle_setpoint.throttle_body_y = manual_control_setpoint.roll;
_rover_throttle_setpoint_pub.publish(rover_throttle_setpoint);

const float yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
const float yaw_delta = math::interpolate<float>(math::deadzone(manual_control_setpoint.yaw,
_param_ro_yaw_stick_dz.get()), -1.f, 1.f, -_max_yaw_rate / _param_ro_yaw_p.get(),
_max_yaw_rate / _param_ro_yaw_p.get());

if (fabsf(yaw_rate_setpoint) > FLT_EPSILON) { // Closed loop yaw rate control
if (fabsf(yaw_delta) > FLT_EPSILON) { // Closed loop yaw rate control
_stab_yaw_setpoint = NAN;
_adjusted_yaw_setpoint.setForcedValue(0.f);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = yaw_rate_setpoint;
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);

} else if (fabsf(rover_throttle_setpoint.throttle_body_x) > FLT_EPSILON
|| fabsf(rover_throttle_setpoint.throttle_body_y) >
FLT_EPSILON) { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
const float yaw_setpoint = matrix::wrap_pi(_vehicle_yaw + yaw_delta);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);

} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
if (!PX4_ISFINITE(_stab_yaw_setpoint)) {
_stab_yaw_setpoint = _vehicle_yaw;
}
Expand All @@ -136,16 +135,8 @@ void MecanumAttControl::generateAttitudeSetpoint()
rover_attitude_setpoint.yaw_setpoint = _stab_yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);

} else { // Reset yaw control and yaw rate setpoint
_stab_yaw_setpoint = NAN;
_adjusted_yaw_setpoint.setForcedValue(0.f);
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = 0.f;
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
}


}

} else if (_vehicle_control_mode.flag_control_offboard_enabled) { // Offboard attitude control
Expand Down
6 changes: 3 additions & 3 deletions src/modules/rover_mecanum/MecanumAttControl/MecanumAttControl.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,10 +84,10 @@ class MecanumAttControl : public ModuleParams

private:
/**
* @brief Generate and publish roverAttitudeSetpoint from manualControlSetpoint (Stab Mode)
* @brief Generate and publish roverAttitudeSetpoint and roverThrottleSetpoint from manualControlSetpoint (Stab Mode)
* or trajectorySetpoint (Offboard attitude control).
*/
void generateAttitudeSetpoint();
void generateAttitudeAndThrottleSetpoint();

/**
* @brief Generate and publish roverRateSetpoint from roverAttitudeSetpoint.
Expand Down Expand Up @@ -126,7 +126,7 @@ class MecanumAttControl : public ModuleParams
float _vehicle_yaw{0.f};
float _dt{0.f};
float _max_yaw_rate{0.f};
float _stab_yaw_setpoint{0.f}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
float _stab_yaw_setpoint{NAN}; // Yaw setpoint for stab mode, NAN if yaw rate is manually controlled [rad]
bool _prev_param_check_passed{true};

// Controllers
Expand Down
10 changes: 5 additions & 5 deletions ...canum/MecanumPosVelControl/CMakeLists.txt → ..._mecanum/MecanumPosControl/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,10 @@
#
############################################################################

px4_add_library(MecanumPosVelControl
MecanumPosVelControl.cpp
px4_add_library(MecanumPosControl
MecanumPosControl.cpp
)

target_link_libraries(MecanumPosVelControl PUBLIC PID)
target_link_libraries(MecanumPosVelControl PUBLIC pure_pursuit)
target_include_directories(MecanumPosVelControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(MecanumPosControl PUBLIC PID)
target_link_libraries(MecanumPosControl PUBLIC pure_pursuit)
target_include_directories(MecanumPosControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
Loading
Loading