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 b41d61355c..ffbdfad2f9 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 @@ -13,22 +13,23 @@ param set-default SIM_GZ_EN 1 # Gazebo bridge # Rover parameters 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_ACCEL 5 +param set-default RD_MAX_DECEL 10 param set-default RD_MAX_JERK 30 -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_MAX_THR_YAW_R 1.5 +param set-default RD_YAW_RATE_P 0.25 +param set-default RD_YAW_RATE_I 0.01 param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0 -param set-default RD_MAX_SPEED 5 -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_YAW_I 0.1 +param set-default RD_MAX_SPEED 2 +param set-default RD_MAX_THR_SPD 2.15 +param set-default RD_SPEED_P 0.1 +param set-default RD_SPEED_I 0.01 param set-default RD_MAX_YAW_RATE 180 -param set-default RD_MISS_SPD_DEF 5 +param set-default RD_MISS_SPD_DEF 2 param set-default RD_TRANS_DRV_TRN 0.349066 param set-default RD_TRANS_TRN_DRV 0.174533 +param set-default RD_MAX_YAW_ACCEL 1000 # Pure pursuit parameters param set-default PP_LOOKAHD_MAX 30 @@ -43,13 +44,13 @@ param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel -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 102 # left wheel -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_REV 1 # reverse right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 8a05e2a15e..5f2b635238 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -21,7 +21,6 @@ param set-default COM_ARM_WO_GPS 1 # Rover parameters param set-default RD_WHEEL_TRACK 0.9 -param set-default RD_MAN_YAW_SCALE 0.1 param set-default RD_YAW_RATE_I 0.1 param set-default RD_YAW_RATE_P 5 param set-default RD_MAX_ACCEL 1 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 901127f09b..859d589d6a 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 @@ -23,22 +23,24 @@ param set-default RBCLW_REV 1 # reverse right wheels # Rover parameters 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 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_MAX_ACCEL 3 +param set-default RD_MAX_DECEL 4 +param set-default RD_MAX_JERK 5 +param set-default RD_MAX_SPEED 1.6 +param set-default RD_MAX_THR_SPD 1.9 +param set-default RD_MAX_THR_YAW_R 0.7 +param set-default RD_MAX_YAW_ACCEL 600 +param set-default RD_MAX_YAW_RATE 250 +param set-default RD_MISS_SPD_DEF 1.5 +param set-default RD_SPEED_P 0.1 +param set-default RD_SPEED_I 0.01 +param set-default RD_TRANS_DRV_TRN 0.785398 +param set-default RD_TRANS_TRN_DRV 0.139626 param set-default RD_YAW_P 5 -param set-default RD_YAW_I 0 -param set-default RD_MAX_SPEED 1.8 -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 -param set-default RD_MISS_SPD_DEF 1.8 -param set-default RD_TRANS_DRV_TRN 0.349066 -param set-default RD_TRANS_TRN_DRV 0.174533 +param set-default RD_YAW_I 0.1 +param set-default RD_YAW_RATE_P 0.1 +param set-default RD_YAW_RATE_I 0.01 + # Pure pursuit parameters param set-default PP_LOOKAHD_MAX 10 diff --git a/msg/RoverDifferentialSetpoint.msg b/msg/RoverDifferentialSetpoint.msg index b16f712828..100cc6b3cd 100644 --- a/msg/RoverDifferentialSetpoint.msg +++ b/msg/RoverDifferentialSetpoint.msg @@ -3,7 +3,7 @@ uint64 timestamp # time since system start (microseconds) 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 speed_diff_setpoint_normalized # [-1, 1] Normalized speed difference between the left and right wheels 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 808da3dc0d..767474fb5b 100644 --- a/msg/RoverDifferentialStatus.msg +++ b/msg/RoverDifferentialStatus.msg @@ -1,13 +1,14 @@ uint64 timestamp # time since system start (microseconds) -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 +float32 measured_forward_speed # [m/s] Measured speed in body x direction. Forwards: positiv, Backwards: negativ +float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate +float32 measured_yaw # [rad] Measured yaw +float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate +float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller +float32 measured_yaw_rate # [rad/s] Measured yaw rate +float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw 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 # TOPICS rover_differential_status diff --git a/src/modules/rover_differential/RoverDifferential.cpp b/src/modules/rover_differential/RoverDifferential.cpp index a7a9e82d02..c5fe294cd3 100644 --- a/src/modules/rover_differential/RoverDifferential.cpp +++ b/src/modules/rover_differential/RoverDifferential.cpp @@ -63,7 +63,7 @@ void RoverDifferential::Run() updateSubscriptions(); - // Generate and publish attitude and velocity setpoints + // Generate and publish attitude, rate and speed setpoints hrt_abstime timestamp = hrt_absolute_time(); switch (_nav_state) { @@ -76,7 +76,19 @@ void RoverDifferential::Run() 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.yaw_rate_setpoint_normalized = manual_control_setpoint.roll * _param_rd_man_yaw_scale.get(); + + if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) { + const float scaled_yaw_rate_input = math::interpolate(manual_control_setpoint.roll, + -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); + const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f; + rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate(speed_diff, + -_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f); + + } else { + rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll; + + } + rover_differential_setpoint.yaw_rate_setpoint = NAN; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } @@ -92,7 +104,7 @@ void RoverDifferential::Run() 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.speed_diff_setpoint_normalized = NAN; rover_differential_setpoint.yaw_setpoint = NAN; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } @@ -109,7 +121,7 @@ void RoverDifferential::Run() rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle; rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; rover_differential_setpoint.yaw_setpoint = NAN; if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON @@ -144,7 +156,7 @@ void RoverDifferential::Run() rover_differential_setpoint.forward_speed_setpoint_normalized = NAN; rover_differential_setpoint.yaw_rate_setpoint = math::interpolate(math::deadzone(manual_control_setpoint.roll, STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate); - rover_differential_setpoint.yaw_rate_setpoint_normalized = NAN; + rover_differential_setpoint.speed_diff_setpoint_normalized = NAN; rover_differential_setpoint.yaw_setpoint = NAN; if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON @@ -191,7 +203,7 @@ void RoverDifferential::Run() 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.speed_diff_setpoint_normalized = 0.f; rover_differential_setpoint.yaw_setpoint = NAN; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); break; diff --git a/src/modules/rover_differential/RoverDifferential.hpp b/src/modules/rover_differential/RoverDifferential.hpp index 8d83519410..3fa8f4891c 100644 --- a/src/modules/rover_differential/RoverDifferential.hpp +++ b/src/modules/rover_differential/RoverDifferential.hpp @@ -129,9 +129,10 @@ private: Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode DEFINE_PARAMETERS( - (ParamFloat) _param_rd_man_yaw_scale, - (ParamFloat) _param_rd_max_yaw_rate, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_pp_lookahd_max + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_max_thr_yaw_r, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_pp_lookahd_max ) }; diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp index 7ec57fb176..deb0b9e37b 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp @@ -51,6 +51,9 @@ void RoverDifferentialControl::updateParams() { ModuleParams::updateParams(); _max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F; + _max_yaw_accel = _param_rd_max_yaw_accel.get() * M_DEG_TO_RAD_F; + + // Update PID pid_set_parameters(&_pid_yaw_rate, _param_rd_yaw_rate_p.get(), // Proportional gain _param_rd_yaw_rate_i.get(), // Integral gain @@ -69,6 +72,16 @@ void RoverDifferentialControl::updateParams() 0.f, // Derivative gain _max_yaw_rate, // Integral limit _max_yaw_rate); // Output limit + + // Update slew rates + if (_max_yaw_rate > FLT_EPSILON) { + _yaw_setpoint_with_yaw_rate_limit.setSlewRate(_max_yaw_rate); + } + + if (_max_yaw_accel > FLT_EPSILON) { + _yaw_rate_with_accel_limit.setSlewRate(_max_yaw_accel); + } + } void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate, @@ -84,63 +97,55 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con // Closed loop yaw control (Overrides yaw rate setpoint) if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) { - float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw); + _yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint), dt); + _rover_differential_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState()); + const float heading_error = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() - vehicle_yaw); _rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt); + _rover_differential_status.clyaw_yaw_rate_setpoint = _rover_differential_setpoint.yaw_rate_setpoint; + + } else { + _yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw); } // Yaw rate control float speed_diff_normalized{0.f}; if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control - 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 + speed_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.yaw_rate_setpoint, vehicle_yaw_rate, + _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, false); } 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_diff_normalized = calcNormalizedSpeedDiff(_rover_differential_setpoint.speed_diff_setpoint_normalized, + vehicle_yaw_rate, + _param_rd_max_thr_yaw_r.get(), _max_yaw_accel, _param_rd_wheel_track.get(), dt, _yaw_rate_with_accel_limit, + _pid_yaw_rate, true); } // Speed control float forward_speed_normalized{0.f}; - if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control - 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); - } + if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint, + vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, + _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, false); - 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 if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint_normalized)) { // Use normalized setpoint + forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_differential_setpoint.forward_speed_setpoint_normalized, + vehicle_forward_speed, _param_rd_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_throttle, + _param_rd_max_accel.get(), _param_rd_max_decel.get(), dt, true); - } else { // Use normalized setpoint - 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 = 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; - _rover_differential_status_pub.publish(rover_differential_status); + _rover_differential_status.timestamp = _timestamp; + _rover_differential_status.measured_forward_speed = vehicle_forward_speed; + _rover_differential_status.measured_yaw = vehicle_yaw; + _rover_differential_status.measured_yaw_rate = vehicle_yaw_rate; + _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; + _rover_differential_status_pub.publish(_rover_differential_status); // Publish to motors actuator_motors_s actuator_motors{}; @@ -148,6 +153,100 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con computeInverseKinematics(forward_speed_normalized, speed_diff_normalized).copyTo(actuator_motors.control); actuator_motors.timestamp = _timestamp; _actuator_motors_pub.publish(actuator_motors); +} + +float RoverDifferentialControl::calcNormalizedSpeedDiff(const float yaw_rate_setpoint, const float vehicle_yaw_rate, + const float max_thr_yaw_r, + const float max_yaw_accel, const float wheel_track, const float dt, SlewRate &yaw_rate_with_accel_limit, + PID_t &pid_yaw_rate, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_yaw_r > FLT_EPSILON ? max_thr_yaw_r : 0.f; + } + + if (max_yaw_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + yaw_rate_with_accel_limit.setSlewRate(max_yaw_accel / slew_rate_normalization); + yaw_rate_with_accel_limit.update(yaw_rate_setpoint, dt); + + } else { + yaw_rate_with_accel_limit.setForcedValue(yaw_rate_setpoint); + } + + // Transform yaw rate into speed difference + float speed_diff_normalized{0.f}; + + if (normalized) { + speed_diff_normalized = yaw_rate_with_accel_limit.getState(); + + } else { + _rover_differential_status.adjusted_yaw_rate_setpoint = yaw_rate_with_accel_limit.getState(); + + if (wheel_track > FLT_EPSILON && max_thr_yaw_r > FLT_EPSILON) { // Feedforward + const float speed_diff = yaw_rate_with_accel_limit.getState() * wheel_track / + 2.f; + speed_diff_normalized = math::interpolate(speed_diff, -max_thr_yaw_r, + max_thr_yaw_r, -1.f, 1.f); + } + + speed_diff_normalized += pid_calculate(&pid_yaw_rate, yaw_rate_with_accel_limit.getState(), vehicle_yaw_rate, 0, + dt); // Feedback + } + + return math::constrain(speed_diff_normalized, -1.f, 1.f); + +} + +float RoverDifferentialControl::calcNormalizedSpeedSetpoint(const float forward_speed_setpoint, + const float vehicle_forward_speed, const float max_thr_spd, SlewRate &forward_speed_setpoint_with_accel_limit, + PID_t &pid_throttle, const float max_accel, const float max_decel, const float dt, const bool normalized) +{ + float slew_rate_normalization{1.f}; + + if (normalized) { // Slew rate needs to be normalized if the setpoint is normalized + slew_rate_normalization = max_thr_spd > FLT_EPSILON ? max_thr_spd : 0.f; + } + + // Apply acceleration and deceleration limit + if (fabsf(forward_speed_setpoint) >= fabsf(forward_speed_setpoint_with_accel_limit.getState())) { + if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + forward_speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization); + forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); + + } else { + forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); + + } + + } else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) { + forward_speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization); + forward_speed_setpoint_with_accel_limit.update(forward_speed_setpoint, dt); + + } else { + forward_speed_setpoint_with_accel_limit.setForcedValue(forward_speed_setpoint); + } + + // Calculate normalized forward speed setpoint + float forward_speed_normalized{0.f}; + + if (normalized) { + forward_speed_normalized = _forward_speed_setpoint_with_accel_limit.getState(); + + } else { // Closed loop speed control + _rover_differential_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState(); + + if (_param_rd_max_thr_spd.get() > FLT_EPSILON) { // Feedforward + forward_speed_normalized = math::interpolate(_forward_speed_setpoint_with_accel_limit.getState(), + -max_thr_spd, max_thr_spd, + -1.f, 1.f); + } + + forward_speed_normalized += pid_calculate(&pid_throttle, _forward_speed_setpoint_with_accel_limit.getState(), + vehicle_forward_speed, 0, dt); // Feedback + } + + return math::constrain(forward_speed_normalized, -1.f, 1.f); } diff --git a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp index a5eb08135f..5a445e7882 100644 --- a/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp +++ b/src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp @@ -43,7 +43,8 @@ #include #include #include - +#include +#include // Standard libraries #include @@ -84,9 +85,42 @@ protected: void updateParams() override; private: + + /** + * @brief Compute normalized speed diff setpoint between the left and right wheels and apply slew rates. + * @param yaw_rate_setpoint Yaw rate setpoint [rad/s or normalized [-1, 1]]. + * @param vehicle_yaw_rate Measured yaw rate [rad/s]. + * @param max_thr_yaw_r Yaw rate turning left/right wheels at max speed in opposite directions [m/s]. + * @param max_yaw_accel Maximum allowed yaw acceleration for the rover [rad/s^2]. + * @param wheel_track Wheel track [m]. + * @param dt Time since last update [s]. + * @param yaw_rate_with_accel_limit Yaw rate slew rate. + * @param pid_yaw_rate Yaw rate PID + * @param normalized Indicates if the forward speed setpoint is already normalized. + * @return Normalized speed differece setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedDiff(float yaw_rate_setpoint, float vehicle_yaw_rate, float max_thr_yaw_r, float max_yaw_accel, + float wheel_track, float dt, SlewRate &yaw_rate_with_accel_limit, PID_t &pid_yaw_rate, bool normalized); + /** + * @brief Compute normalized forward speed setpoint and apply slew rates. + * to the forward speed setpoint and doing closed loop speed control if enabled. + * @param forward_speed_setpoint Forward speed setpoint [m/s]. + * @param vehicle_forward_speed Actual forward speed [m/s]. + * @param max_thr_spd Speed the rover drives at maximum throttle [m/s]. + * @param forward_speed_setpoint_with_accel_limit Speed slew rate. + * @param pid_throttle Throttle PID + * @param max_accel Maximum acceleration [m/s^2] + * @param max_decel Maximum deceleration [m/s^2] + * @param dt Time since last update [s]. + * @param normalized Indicates if the forward speed setpoint is already normalized. + * @return Normalized forward speed setpoint with applied slew rates [-1, 1]. + */ + float calcNormalizedSpeedSetpoint(float forward_speed_setpoint, float vehicle_forward_speed, float max_thr_spd, + SlewRate &forward_speed_setpoint_with_accel_limit, PID_t &pid_throttle, float max_accel, float max_decel, + float dt, bool normalized); + /** * @brief Compute normalized motor commands based on normalized setpoints. - * * @param forward_speed_normalized Normalized forward speed [-1, 1]. * @param speed_diff_normalized Speed difference between left and right wheels [-1, 1]. * @return matrix::Vector2f Motor velocities for the right and left motors [-1, 1]. @@ -99,29 +133,37 @@ private: // uORB publications uORB::PublicationMulti _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _rover_differential_status_pub{ORB_ID(rover_differential_status)}; + rover_differential_status_s _rover_differential_status{}; // Variables rover_differential_setpoint_s _rover_differential_setpoint{}; hrt_abstime _timestamp{0}; float _max_yaw_rate{0.f}; + float _max_yaw_accel{0.f}; // Controllers - PID_t _pid_throttle; // The PID controller for the closed loop speed control - PID_t _pid_yaw; // The PID controller for the closed loop yaw control - PID_t _pid_yaw_rate; // The PID controller for the closed loop yaw rate control + PID_t _pid_throttle; // Closed loop speed control + PID_t _pid_yaw; // Closed loop yaw control + PID_t _pid_yaw_rate; // Closed loop yaw rate control + SlewRate _forward_speed_setpoint_with_accel_limit{0.f}; + SlewRate _yaw_rate_with_accel_limit{0.f}; + SlewRateYaw _yaw_setpoint_with_yaw_rate_limit; // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_rd_wheel_track, - (ParamFloat) _param_rd_max_thr_spd, + (ParamFloat) _param_rd_wheel_track, + (ParamFloat) _param_rd_max_thr_spd, + (ParamFloat) _param_rd_max_accel, + (ParamFloat) _param_rd_max_decel, (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, - (ParamFloat) _param_rd_p_gain_speed, - (ParamFloat) _param_rd_i_gain_speed, - (ParamFloat) _param_rd_p_gain_yaw, - (ParamFloat) _param_rd_i_gain_yaw, - (ParamInt) _param_r_rev + (ParamFloat) _param_rd_max_yaw_rate, + (ParamFloat) _param_rd_max_yaw_accel, + (ParamFloat) _param_rd_yaw_rate_p, + (ParamFloat) _param_rd_yaw_rate_i, + (ParamFloat) _param_rd_p_gain_speed, + (ParamFloat) _param_rd_i_gain_speed, + (ParamFloat) _param_rd_p_gain_yaw, + (ParamFloat) _param_rd_i_gain_yaw, + (ParamInt) _param_r_rev ) }; diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp index 848a7f513a..400cd643f3 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp @@ -91,9 +91,9 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f desired_forward_speed = _max_forward_speed; if (_waypoint_transition_angle < M_PI_F - _param_rd_trans_drv_trn.get()) { - if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_accel.get() > FLT_EPSILON) { + if (_param_rd_max_jerk.get() > FLT_EPSILON && _param_rd_max_decel.get() > FLT_EPSILON) { desired_forward_speed = math::trajectory::computeMaxSpeedFromDistance(_param_rd_max_jerk.get(), - _param_rd_max_accel.get(), distance_to_curr_wp, 0.0f); + _param_rd_max_decel.get(), distance_to_curr_wp, 0.0f); desired_forward_speed = math::constrain(desired_forward_speed, -_max_forward_speed, _max_forward_speed); } } @@ -130,7 +130,7 @@ void RoverDifferentialGuidance::computeGuidance(const float vehicle_yaw, const f 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.speed_diff_setpoint_normalized = NAN; rover_differential_setpoint.yaw_setpoint = desired_yaw; _rover_differential_setpoint_pub.publish(rover_differential_setpoint); } diff --git a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp index 4b3815247e..c5d3a3f173 100644 --- a/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp +++ b/src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp @@ -140,11 +140,11 @@ private: // Parameters DEFINE_PARAMETERS( - (ParamFloat) _param_nav_acc_rad, - (ParamFloat) _param_rd_max_jerk, - (ParamFloat) _param_rd_max_accel, - (ParamFloat) _param_rd_max_speed, - (ParamFloat) _param_rd_miss_spd_def, + (ParamFloat) _param_nav_acc_rad, + (ParamFloat) _param_rd_max_jerk, + (ParamFloat) _param_rd_max_decel, + (ParamFloat) _param_rd_max_speed, + (ParamFloat) _param_rd_miss_spd_def, (ParamFloat) _param_rd_trans_trn_drv, (ParamFloat) _param_rd_trans_drv_trn diff --git a/src/modules/rover_differential/module.yaml b/src/modules/rover_differential/module.yaml index 3bad343f2e..d1de0cbddb 100644 --- a/src/modules/rover_differential/module.yaml +++ b/src/modules/rover_differential/module.yaml @@ -16,19 +16,6 @@ parameters: decimal: 3 default: 0.5 - RD_MAN_YAW_SCALE: - description: - short: Manual yaw rate scale - long: | - In manual mode the setpoint for the yaw rate received from the yaw stick - is scaled by this value. - type: float - min: 0.001 - max: 1 - increment: 0.01 - decimal: 3 - default: 1 - RD_YAW_P: description: short: Proportional gain for closed loop yaw controller @@ -113,6 +100,21 @@ parameters: decimal: 2 default: 0.5 + RD_MAX_DECEL: + description: + short: Maximum deceleration + long: | + Maximum decelaration is used to limit the deceleration of the rover. + Set to -1 to disable, causing the rover to decelerate as fast as possible. + Caution: This disables the slow down effect in auto modes. + type: float + unit: m/s^2 + min: -1 + max: 100 + increment: 0.01 + decimal: 2 + default: -1 + RD_MAX_SPEED: description: short: Maximum speed setpoint @@ -156,6 +158,22 @@ parameters: decimal: 2 default: 90 + RD_MAX_YAW_ACCEL: + description: + short: Maximum allowed yaw acceleration for the rover + long: | + This parameter is used to cap desired yaw acceleration. This is used to adjust incoming yaw rate setpoints + to a feasible yaw rate setpoint based on the physical limitation on how fast the yaw rate can change. + This leads to a smooth setpoint trajectory for the closed loop yaw rate controller to track. + Set to -1 to disable. + type: float + unit: deg/s^2 + min: -1 + max: 1000 + increment: 0.01 + decimal: 2 + default: -1 + RD_MAX_THR_YAW_R: description: short: Yaw rate turning left/right wheels at max speed in opposite directions