From 2c76eb214a78a493dc9e1aac76e988cf7a0ec2b3 Mon Sep 17 00:00:00 2001 From: chfriedrich98 Date: Thu, 29 Aug 2024 13:44:28 +0200 Subject: [PATCH] differential: update setpoint message and add clarifications --- msg/RoverDifferentialSetpoint.msg | 11 ++++---- msg/RoverDifferentialStatus.msg | 7 +++-- .../rover_differential/RoverDifferential.cpp | 26 ++++++++--------- .../RoverDifferentialControl.cpp | 28 +++++++++---------- .../RoverDifferentialGuidance.cpp | 5 ++-- 5 files changed, 35 insertions(+), 42 deletions(-) diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg index 69c73a670c1e..b16f712828d8 100644 --- a/msg/RoverDifferentialSetpoint.msg +++ b/msg/RoverDifferentialSetpoint.msg @@ -1,10 +1,9 @@ uint64 timestamp # time since system start (microseconds) -float32 forward_speed_setpoint # [m/s or normalized [-1, 1]] Desired forward speed for the rover -float32 yaw_rate_setpoint # [rad/s or normalized [-1, 1]] Desired yaw rate for the rover -float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover -bool closed_loop_speed # Use closed loop velocity controller -bool closed_loop_yaw_rate # Use closed loop yaw rate controller -bool closed_loop_yaw # Use closed loop yaw controller +float32 forward_speed_setpoint # [m/s] Desired forward speed for the rover +float32 forward_speed_setpoint_normalized # [-1, 1] Normalized forward speed for the rover +float32 yaw_rate_setpoint # [rad/s] Desired yaw rate for the rover (Overriden by yaw controller if yaw_setpoint is used) +float32 yaw_rate_setpoint_normalized # [-1, 1] Normalized yaw rate for the rover +float32 yaw_setpoint # [rad] Desired yaw (heading) for the rover # TOPICS rover_differential_setpoint diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg index d608cdf2b750..4d3d54eb9909 100644 --- a/msg/RoverDifferentialStatus.msg +++ b/msg/RoverDifferentialStatus.msg @@ -2,9 +2,10 @@ uint64 timestamp # time since system start (microseconds) float32 actual_speed # [m/s] Actual forward speed of the rover float32 actual_yaw_deg # [deg] Actual yaw of the rover -float32 desired_yaw_rate_deg_s # [deg/s] Desired yaw rate (deviates from the yaw_rate_setpoint if the yaw setpoint is used) float32 actual_yaw_rate_deg_s # [deg/s] Actual yaw rate of the rover +float32 desired_yaw_rate_deg_s # [deg/s] Yaw rate setpoint for the closed loop yaw rate controller +float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller -float32 pid_throttle_integral # Integral of the PID for the closed loop yaw controller -float32 pid_yaw_integral # Integral of the PID for the closed loop heading controller +float32 pid_throttle_integral # Integral of the PID for the closed loop speed controller + # TOPICS rover_differential_status diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index 9ab82432dd40..20cbf7d9e3e2 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -73,12 +73,11 @@ void RoverDifferential::Run() if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { rover_differential_setpoint_s rover_differential_setpoint{}; rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle; - rover_differential_setpoint.yaw_rate_setpoint = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; rover_differential_setpoint.yaw_setpoint = NAN; - rover_differential_setpoint.closed_loop_yaw_rate = false; - rover_differential_setpoint.closed_loop_yaw = false; - rover_differential_setpoint.closed_loop_speed = false; + rover_differential_setpoint.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + rover_differential_setpoint.yaw_rate_setpoint = NAN; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } } break; @@ -89,13 +88,12 @@ void RoverDifferential::Run() if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) { rover_differential_setpoint_s rover_differential_setpoint{}; rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = manual_control_setpoint.throttle; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(manual_control_setpoint.roll, -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; rover_differential_setpoint.yaw_setpoint = NAN; - rover_differential_setpoint.closed_loop_yaw_rate = true; - rover_differential_setpoint.closed_loop_yaw = false; - rover_differential_setpoint.closed_loop_speed = false; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } @@ -108,13 +106,11 @@ void RoverDifferential::Run() default: // Unimplemented nav states will stop the rover rover_differential_setpoint_s rover_differential_setpoint{}; - rover_differential_setpoint.timestamp = timestamp; - rover_differential_setpoint.forward_speed_setpoint = 0.f; - rover_differential_setpoint.yaw_rate_setpoint = 0.f; + rover_differential_setpoint.forward_speed_setpoint = NAN; + rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f; + rover_differential_setpoint.yaw_rate_setpoint = NAN; + rover_differential_setpoint.yaw_rate_setpoint_normalized = 0.f; rover_differential_setpoint.yaw_setpoint = NAN; - rover_differential_setpoint.closed_loop_yaw_rate = false; - rover_differential_setpoint.closed_loop_yaw = false; - rover_differential_setpoint.closed_loop_speed = false; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); break; } diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index cced23aa454c..c1f96c0ce066 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -83,7 +83,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con _rover_differential_setpoint_sub.update(&_rover_differential_setpoint); // Closed loop yaw control (Overrides yaw rate setpoint) - if (_rover_differential_setpoint.closed_loop_yaw && PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { + if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { if (fabsf(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw) < FLT_EPSILON) { _rover_differential_setpoint.yaw_rate_setpoint = 0.f; pid_reset_integral(&_pid_yaw); @@ -97,8 +97,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con // Yaw rate control float speed_diff_normalized{0.f}; - if (_rover_differential_setpoint.closed_loop_yaw_rate - && PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control + if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control if (fabsf(_rover_differential_setpoint.yaw_rate_setpoint) < FLT_EPSILON) { speed_diff_normalized = 0.f; pid_reset_integral(&_pid_yaw_rate); @@ -112,16 +111,15 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con -1.f, 1.f); // Feedback } - } else { // Interpret yaw rate setpoint as already normalized - speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint) ? - math::constrain(_rover_differential_setpoint.yaw_rate_setpoint, -1.f, 1.f) : 0.f; + } else { // Use normalized setpoint + speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ? + math::constrain(_rover_differential_setpoint.yaw_rate_setpoint_normalized, -1.f, 1.f) : 0.f; } // Speed control float throttle{0.f}; - if (_rover_differential_setpoint.closed_loop_speed - && PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control + if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control if (fabsf(_rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { pid_reset_integral(&_pid_throttle); @@ -136,9 +134,9 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con } } - } else { // Interpret speed setpoint as already normalized - throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint) ? - math::constrain(_rover_differential_setpoint.forward_speed_setpoint, -1.f, 1.f) : 0.f; + } else { // Use normalized setpoint + throttle = PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized) ? + math::constrain(_rover_differential_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f; } // Publish rover differential status (logging) @@ -165,11 +163,11 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con matrix::Vector2f RoverDifferentialControl::computeInverseKinematics(float forward_speed_normalized, const float speed_diff_normalized) { - float combined_velocity = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); + float max_motor_command = fabsf(forward_speed_normalized) + fabsf(speed_diff_normalized); - if (combined_velocity > 1.0f) { // Prioritize yaw rate - float excess_velocity = fabsf(combined_velocity - 1.0f); - forward_speed_normalized -= sign(forward_speed_normalized) * excess_velocity; + if (max_motor_command > 1.0f) { // Prioritize yaw rate if a normalized motor command exceeds limit of 1 + float excess = fabsf(max_motor_command - 1.0f); + forward_speed_normalized -= sign(forward_speed_normalized) * excess; } // Calculate the left and right wheel speeds diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index 11e9c20d27ef..848a7f513acf 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -128,11 +128,10 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f rover_differential_setpoint_s rover_differential_setpoint{}; rover_differential_setpoint.timestamp = timestamp; rover_differential_setpoint.forward_speed_setpoint = desired_forward_speed; + rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; rover_differential_setpoint.yaw_rate_setpoint = NAN; + rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; rover_differential_setpoint.yaw_setpoint = desired_yaw; - rover_differential_setpoint.closed_loop_yaw_rate = true; - rover_differential_setpoint.closed_loop_yaw = true; - rover_differential_setpoint.closed_loop_speed = true; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); }