diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index 26429add58e5..4bf8815520af 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -16,11 +16,12 @@ param set-default RD_WHEEL_TRACK 0.3 param set-default RD_MAN_YAW_SCALE 0.1 param set-default RD_MAX_ACCEL 6 param set-default RD_MAX_JERK 30 -param set-default RD_MAX_SPEED 7 -param set-default RD_YAW_RATE_P 5 +param set-default RD_MAX_THR_YAW_R 5 +param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0 +param set-default RD_MAX_THR_SPD 7 param set-default RD_SPEED_P 1 param set-default RD_SPEED_I 0 param set-default RD_MAX_YAW_RATE 180 diff --git a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover index 1ea26ad6bff0..d00063c573c3 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover +++ b/ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover @@ -25,12 +25,13 @@ param set-default RBCLW_REV 1 # reverse right wheels param set-default RD_WHEEL_TRACK 0.3 param set-default RD_MAN_YAW_SCALE 1 param set-default RD_MAX_ACCEL 5 -param set-default RD_MAX_JERK 50 -param set-default RD_MAX_SPEED 2 +param set-default RD_MAX_JERK 10 +param set-default RD_MAX_THR_YAW_R 4 param set-default RD_YAW_RATE_P 0.1 param set-default RD_YAW_RATE_I 0 param set-default RD_YAW_P 5 param set-default RD_YAW_I 0 +param set-default RD_MAX_THR_SPD 2 param set-default RD_SPEED_P 0.5 param set-default RD_SPEED_I 0.1 param set-default RD_MAX_YAW_RATE 300 diff --git a/msg/RoverDifferentialStatus.msg b/msg/RoverDifferentialStatus.msg index 4d3d54eb9909..808da3dc0d4f 100644 --- a/msg/RoverDifferentialStatus.msg +++ b/msg/RoverDifferentialStatus.msg @@ -1,11 +1,13 @@ 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 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 speed controller +float32 actual_speed # [m/s] Actual forward speed of the rover +float32 actual_yaw # [rad] Actual yaw of the rover +float32 actual_yaw_rate # [rad/s] Actual yaw rate of the rover +float32 desired_yaw_rate # [rad/s] Yaw rate setpoint for the closed loop yaw rate controller +float32 forward_speed_normalized # [-1, 1] Normalized forward speed setpoint +float32 speed_diff_normalized # [-1, 1] Normalized speed difference setpoint between the left and right motor +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 speed controller # TOPICS rover_differential_status diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index a7019e3a8470..7ec57fb176bc 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -92,9 +92,13 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con float speed_diff_normalized{0.f}; if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get(); // Feedforward - speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_speed.get(), - _param_rd_max_speed.get(), -1.f, 1.f); + if (_param_rd_wheel_track.get() > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward + const float speed_diff = _rover_differential_setpoint.yaw_rate_setpoint * _param_rd_wheel_track.get() / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -_param_rd_max_thr_yaw_r.get(), + _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); + } + speed_diff_normalized = math::constrain(speed_diff_normalized + pid_calculate(&_pid_yaw_rate, _rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, 0, dt), -1.f, 1.f); // Feedback @@ -105,30 +109,34 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con } // Speed control - float throttle{0.f}; + float forward_speed_normalized{0.f}; if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control - if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feedforward - throttle += math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, - 0.f, _param_rd_max_speed.get(), - 0.f, 1.f); + if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward + forward_speed_normalized = math::interpolate(_rover_differential_setpoint.forward_speed_setpoint, + -_param_rd_max_thr_spd.get(), _param_rd_max_thr_spd.get(), + -1.f, 1.f); } - throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0, - dt); // Feedback + forward_speed_normalized = math::constrain(forward_speed_normalized + pid_calculate(&_pid_throttle, + _rover_differential_setpoint.forward_speed_setpoint, + vehicle_forward_speed, 0, + dt), -1.f, 1.f); // Feedback } 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; + forward_speed_normalized = 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) rover_differential_status_s rover_differential_status{}; rover_differential_status.timestamp = _timestamp; rover_differential_status.actual_speed = vehicle_forward_speed; - rover_differential_status.actual_yaw_deg = M_RAD_TO_DEG_F * vehicle_yaw; - rover_differential_status.desired_yaw_rate_deg_s = M_RAD_TO_DEG_F * _rover_differential_setpoint.yaw_rate_setpoint; - rover_differential_status.actual_yaw_rate_deg_s = M_RAD_TO_DEG_F * vehicle_yaw_rate; + rover_differential_status.actual_yaw = vehicle_yaw; + rover_differential_status.desired_yaw_rate = _rover_differential_setpoint.yaw_rate_setpoint; + rover_differential_status.actual_yaw_rate = vehicle_yaw_rate; + rover_differential_status.forward_speed_normalized = forward_speed_normalized; + rover_differential_status.speed_diff_normalized = speed_diff_normalized; rover_differential_status.pid_yaw_rate_integral = _pid_yaw_rate.integral; rover_differential_status.pid_throttle_integral = _pid_throttle.integral; rover_differential_status.pid_yaw_integral = _pid_yaw.integral; @@ -137,7 +145,7 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con // Publish to motors actuator_motors_s actuator_motors{}; actuator_motors.reversible_flags = _param_r_rev.get(); - computeInverseKinematics(throttle, speed_diff_normalized).copyTo(actuator_motors.control); + computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp index 9f49186ad8b6..a5eb08135f50 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -113,7 +113,8 @@ class RoverDifferentialControl : public ModuleParams // Parameters DEFINE_PARAMETERS( (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_max_thr_spd, + (ParamFloat) _param_rd_max_thr_yaw_r, (ParamFloat) _param_rd_max_yaw_rate, (ParamFloat) _param_rd_yaw_rate_p, (ParamFloat) _param_rd_yaw_rate_i, diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index 848a7f513acf..c56830ad08a2 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -216,7 +216,7 @@ void RoverDifferentialGuidance::updateWaypoints() // Waypoint cruising speed if (position_setpoint_triplet.current.cruising_speed > 0.f) { - _max_forward_speed = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_rd_max_speed.get()); + _max_forward_speed = position_setpoint_triplet.current.cruising_speed; } else { _max_forward_speed = _param_rd_miss_spd_def.get(); diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index fd3a9485a04e..d2b9862b2cc0 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -140,7 +140,6 @@ class RoverDifferentialGuidance : public ModuleParams // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_max_speed, (ParamFloat) _param_nav_acc_rad, (ParamFloat) _param_rd_max_jerk, (ParamFloat) _param_rd_max_accel, diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 6602497685db..8ef9f62f5e30 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -113,10 +113,14 @@ parameters: decimal: 2 default: 0.5 - RD_MAX_SPEED: + RD_MAX_THR_SPD: description: - short: Maximum speed the rover can drive - long: This parameter is used to map desired speeds to normalized motor commands. + short: Speed the rover drives at maximum throttle + long: | + This parameter is used to calculate the feedforward term of the closed loop speed control which linearly + maps desired speeds to normalized motor commands [-1. 1]. + A good starting point is the observed ground speed when the rover drives at maximum throttle in manual mode. + Increase this parameter if the rover is faster than the setpoint, and decrease if the rover is slower. type: float unit: m/s min: 0 @@ -138,6 +142,23 @@ parameters: decimal: 2 default: 90 + RD_MAX_THR_YAW_R: + description: + short: Yaw rate turning left/right wheels at max speed in opposite directions + long: | + This parameter is used to calculate the feedforward term of the closed loop yaw rate control. + The controller first calculates the required speed difference between the left and right motor to achieve the desired yaw rate. + This desired speed difference is then linearly mapped to normalized motor commands. + A good starting point is twice the speed the rover drives at maximum throttle (RD_MAX_THR_SPD)). + Increase this parameter if the rover turns faster than the setpoint, and decrease if the rover turns slower. + type: float + unit: m/s + min: 0 + max: 100 + increment: 0.01 + decimal: 2 + default: 2 + RD_MISS_SPD_DEF: description: short: Default forward speed for the rover during auto modes