mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
mecanum: add slew rates to yaw, yaw rate and speed setpoints
This commit is contained in:
chfriedrich98
chfriedrich98
parent
54abc59283
commit
5dcccd999c
@ -1,13 +1,17 @@
|
||||
uint64 timestamp # time since system start (microseconds)
|
||||
|
||||
float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
|
||||
float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
|
||||
float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output of the closed loop yaw controller
|
||||
float32 measured_yaw_rate # [rad/s] Measured yaw rate
|
||||
float32 measured_yaw # [rad] Measured yaw
|
||||
float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller
|
||||
float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller
|
||||
float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
|
||||
float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller
|
||||
float32 measured_forward_speed # [m/s] Measured speed in body x direction. Positiv: forwards, Negativ: backwards
|
||||
float32 adjusted_forward_speed_setpoint # [m/s] Speed setpoint after applying slew rate
|
||||
float32 measured_lateral_speed # [m/s] Measured speed in body y direction. Positiv: right, Negativ: left
|
||||
float32 adjusted_lateral_speed_setpoint # [m/s] Speed setpoint after applying slew rate
|
||||
float32 measured_yaw_rate # [rad/s] Measured yaw rate
|
||||
float32 clyaw_yaw_rate_setpoint # [rad/s] Yaw rate setpoint output by the closed loop yaw controller
|
||||
float32 adjusted_yaw_rate_setpoint # [rad/s] Yaw rate setpoint from the closed loop yaw controller
|
||||
float32 measured_yaw # [rad] Measured yaw
|
||||
float32 adjusted_yaw_setpoint # [rad] Yaw setpoint after applying slew rate
|
||||
float32 pid_yaw_rate_integral # Integral of the PID for the closed loop yaw rate controller
|
||||
float32 pid_yaw_integral # Integral of the PID for the closed loop yaw controller
|
||||
float32 pid_forward_throttle_integral # Integral of the PID for the closed loop forward speed controller
|
||||
float32 pid_lateral_throttle_integral # Integral of the PID for the closed loop lateral speed controller
|
||||
|
||||
# TOPICS rover_mecanum_status
|
||||
|
||||
@ -66,7 +66,7 @@ static constexpr float YAW_RATE_THRESHOLD =
|
||||
static constexpr float SPEED_THRESHOLD =
|
||||
0.1f; // [m/s] Threshold for the speed measurement to cut off measurement noise when the rover is standing still
|
||||
static constexpr float STICK_DEADZONE =
|
||||
0.3f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
|
||||
0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
|
||||
@ -61,9 +61,19 @@ void RoverMecanumControl::updateParams()
|
||||
_pid_lateral_throttle.setOutputLimit(1.f);
|
||||
|
||||
_max_yaw_rate = _param_rm_max_yaw_rate.get() * M_DEG_TO_RAD_F;
|
||||
_max_yaw_accel = _param_rm_max_yaw_accel.get() * M_DEG_TO_RAD_F;
|
||||
_pid_yaw.setGains(_param_rm_p_gain_yaw.get(), _param_rm_i_gain_yaw.get(), 0.f);
|
||||
_pid_yaw.setIntegralLimit(_max_yaw_rate);
|
||||
_pid_yaw.setOutputLimit(_max_yaw_rate);
|
||||
|
||||
// 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 RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const float vehicle_yaw_rate,
|
||||
@ -79,98 +89,191 @@ void RoverMecanumControl::computeMotorCommands(const float vehicle_yaw, const fl
|
||||
|
||||
// Closed loop yaw control
|
||||
if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_setpoint)) {
|
||||
_yaw_setpoint_with_yaw_rate_limit.update(matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint), dt);
|
||||
_rover_mecanum_status.adjusted_yaw_setpoint = matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState());
|
||||
_pid_yaw.setSetpoint(
|
||||
matrix::wrap_pi(_rover_mecanum_setpoint.yaw_setpoint - vehicle_yaw)); // error as setpoint to take care of wrapping
|
||||
matrix::wrap_pi(_yaw_setpoint_with_yaw_rate_limit.getState() -
|
||||
vehicle_yaw)); // error as setpoint to take care of wrapping
|
||||
_rover_mecanum_setpoint.yaw_rate_setpoint = _pid_yaw.update(0.f, dt);
|
||||
_rover_mecanum_status.clyaw_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
|
||||
|
||||
} else {
|
||||
_pid_yaw.resetIntegral();
|
||||
_yaw_setpoint_with_yaw_rate_limit.setForcedValue(vehicle_yaw);
|
||||
}
|
||||
|
||||
// Yaw rate control
|
||||
float speed_diff_normalized{0.f};
|
||||
|
||||
if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
|
||||
if (_param_rm_max_thr_yaw_r.get() > FLT_EPSILON) { // Feedforward
|
||||
const float speed_diff = _rover_mecanum_setpoint.yaw_rate_setpoint * _param_rm_wheel_track.get();
|
||||
speed_diff_normalized = math::interpolate<float>(speed_diff, -_param_rm_max_thr_yaw_r.get(),
|
||||
_param_rm_max_thr_yaw_r.get(), -1.f, 1.f);
|
||||
}
|
||||
|
||||
_pid_yaw_rate.setSetpoint(_rover_mecanum_setpoint.yaw_rate_setpoint);
|
||||
speed_diff_normalized = math::constrain(speed_diff_normalized +
|
||||
_pid_yaw_rate.update(vehicle_yaw_rate, dt),
|
||||
-1.f, 1.f); // Feedback
|
||||
if (PX4_ISFINITE(_rover_mecanum_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
|
||||
speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.yaw_rate_setpoint, vehicle_yaw_rate,
|
||||
_param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
|
||||
_pid_yaw_rate, false);
|
||||
_rover_mecanum_status.adjusted_yaw_rate_setpoint = _yaw_rate_with_accel_limit.getState();
|
||||
|
||||
} else { // Use normalized setpoint
|
||||
speed_diff_normalized = PX4_ISFINITE(_rover_mecanum_setpoint.speed_diff_setpoint_normalized) ? math::constrain(
|
||||
_rover_mecanum_setpoint.speed_diff_setpoint_normalized, -1.f, 1.f) : 0.f;
|
||||
speed_diff_normalized = calcNormalizedSpeedDiff(_rover_mecanum_setpoint.speed_diff_setpoint_normalized,
|
||||
vehicle_yaw_rate,
|
||||
_param_rm_max_thr_yaw_r.get(), _max_yaw_accel, _param_rm_wheel_track.get(), dt, _yaw_rate_with_accel_limit,
|
||||
_pid_yaw_rate, true);
|
||||
}
|
||||
|
||||
// Speed control
|
||||
float forward_throttle{0.f};
|
||||
float lateral_throttle{0.f};
|
||||
float forward_speed_normalized{0.f};
|
||||
float lateral_speed_normalized{0.f};
|
||||
|
||||
if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint)
|
||||
&& PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint)) { // Closed loop speed control
|
||||
// Closed loop forward speed control
|
||||
if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
|
||||
forward_throttle = math::interpolate<float>(_rover_mecanum_setpoint.forward_speed_setpoint,
|
||||
-_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
|
||||
}
|
||||
forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint,
|
||||
vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle,
|
||||
_param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false);
|
||||
lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint,
|
||||
vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle,
|
||||
_param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, false);
|
||||
_rover_mecanum_status.adjusted_forward_speed_setpoint = _forward_speed_setpoint_with_accel_limit.getState();
|
||||
_rover_mecanum_status.adjusted_lateral_speed_setpoint = _lateral_speed_setpoint_with_accel_limit.getState();
|
||||
|
||||
_pid_forward_throttle.setSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint);
|
||||
forward_throttle += _pid_forward_throttle.update(vehicle_forward_speed, dt);
|
||||
|
||||
// Closed loop lateral speed control
|
||||
if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
|
||||
lateral_throttle = math::interpolate<float>(_rover_mecanum_setpoint.lateral_speed_setpoint,
|
||||
-_param_rm_max_thr_spd.get(), _param_rm_max_thr_spd.get(), -1.f, 1.f);
|
||||
}
|
||||
} else if (PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized)
|
||||
&& PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized)) { // Use normalized setpoint
|
||||
forward_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.forward_speed_setpoint_normalized,
|
||||
vehicle_forward_speed, _param_rm_max_thr_spd.get(), _forward_speed_setpoint_with_accel_limit, _pid_forward_throttle,
|
||||
_param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true);
|
||||
lateral_speed_normalized = calcNormalizedSpeedSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized,
|
||||
vehicle_lateral_speed, _param_rm_max_thr_spd.get(), _lateral_speed_setpoint_with_accel_limit, _pid_lateral_throttle,
|
||||
_param_rm_max_accel.get(), _param_rm_max_decel.get(), dt, true);
|
||||
|
||||
_pid_lateral_throttle.setSetpoint(_rover_mecanum_setpoint.lateral_speed_setpoint);
|
||||
lateral_throttle += _pid_lateral_throttle.update(vehicle_lateral_speed, dt);
|
||||
|
||||
} else { // Use normalized setpoint
|
||||
forward_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.forward_speed_setpoint_normalized) ? math::constrain(
|
||||
_rover_mecanum_setpoint.forward_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
|
||||
lateral_throttle = PX4_ISFINITE(_rover_mecanum_setpoint.lateral_speed_setpoint_normalized) ? math::constrain(
|
||||
_rover_mecanum_setpoint.lateral_speed_setpoint_normalized, -1.f, 1.f) : 0.f;
|
||||
}
|
||||
|
||||
// Publish rover mecanum status (logging)
|
||||
rover_mecanum_status_s rover_mecanum_status{};
|
||||
rover_mecanum_status.timestamp = _timestamp;
|
||||
rover_mecanum_status.measured_forward_speed = vehicle_forward_speed;
|
||||
rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed;
|
||||
rover_mecanum_status.adjusted_yaw_rate_setpoint = _rover_mecanum_setpoint.yaw_rate_setpoint;
|
||||
rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
|
||||
rover_mecanum_status.measured_yaw = vehicle_yaw;
|
||||
rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
|
||||
rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
|
||||
rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
|
||||
rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
|
||||
_rover_mecanum_status_pub.publish(rover_mecanum_status);
|
||||
_rover_mecanum_status.timestamp = _timestamp;
|
||||
_rover_mecanum_status.measured_forward_speed = vehicle_forward_speed;
|
||||
_rover_mecanum_status.measured_lateral_speed = vehicle_lateral_speed;
|
||||
_rover_mecanum_status.measured_yaw_rate = vehicle_yaw_rate;
|
||||
_rover_mecanum_status.measured_yaw = vehicle_yaw;
|
||||
_rover_mecanum_status.pid_yaw_rate_integral = _pid_yaw_rate.getIntegral();
|
||||
_rover_mecanum_status.pid_yaw_integral = _pid_yaw.getIntegral();
|
||||
_rover_mecanum_status.pid_forward_throttle_integral = _pid_forward_throttle.getIntegral();
|
||||
_rover_mecanum_status.pid_lateral_throttle_integral = _pid_lateral_throttle.getIntegral();
|
||||
_rover_mecanum_status_pub.publish(_rover_mecanum_status);
|
||||
|
||||
// Publish to motors
|
||||
actuator_motors_s actuator_motors{};
|
||||
actuator_motors.reversible_flags = _param_r_rev.get();
|
||||
computeInverseKinematics(forward_throttle, lateral_throttle,
|
||||
computeInverseKinematics(forward_speed_normalized, lateral_speed_normalized,
|
||||
speed_diff_normalized).copyTo(actuator_motors.control);
|
||||
actuator_motors.timestamp = _timestamp;
|
||||
_actuator_motors_pub.publish(actuator_motors);
|
||||
|
||||
}
|
||||
|
||||
matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_throttle, float lateral_throttle,
|
||||
float RoverMecanumControl::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<float> &yaw_rate_with_accel_limit,
|
||||
PID &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 {
|
||||
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<float>(speed_diff, -max_thr_yaw_r,
|
||||
max_thr_yaw_r, -1.f, 1.f);
|
||||
}
|
||||
|
||||
_pid_yaw_rate.setSetpoint(yaw_rate_with_accel_limit.getState());
|
||||
speed_diff_normalized = math::constrain(speed_diff_normalized +
|
||||
_pid_yaw_rate.update(vehicle_yaw_rate, dt),
|
||||
-1.f, 1.f); // Feedback
|
||||
|
||||
|
||||
}
|
||||
|
||||
return math::constrain(speed_diff_normalized, -1.f, 1.f);
|
||||
|
||||
}
|
||||
|
||||
float RoverMecanumControl::calcNormalizedSpeedSetpoint(const float speed_setpoint,
|
||||
const float vehicle_speed, const float max_thr_spd, SlewRate<float> &speed_setpoint_with_accel_limit,
|
||||
PID &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(speed_setpoint) >= fabsf(speed_setpoint_with_accel_limit.getState())) {
|
||||
if (max_accel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
|
||||
speed_setpoint_with_accel_limit.setSlewRate(max_accel / slew_rate_normalization);
|
||||
speed_setpoint_with_accel_limit.update(speed_setpoint, dt);
|
||||
|
||||
} else {
|
||||
speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint);
|
||||
|
||||
}
|
||||
|
||||
} else if (max_decel > FLT_EPSILON && slew_rate_normalization > FLT_EPSILON) {
|
||||
speed_setpoint_with_accel_limit.setSlewRate(max_decel / slew_rate_normalization);
|
||||
speed_setpoint_with_accel_limit.update(speed_setpoint, dt);
|
||||
|
||||
} else {
|
||||
speed_setpoint_with_accel_limit.setForcedValue(speed_setpoint);
|
||||
}
|
||||
|
||||
// Calculate normalized forward speed setpoint
|
||||
float forward_speed_normalized{0.f};
|
||||
|
||||
if (normalized) {
|
||||
forward_speed_normalized = speed_setpoint_with_accel_limit.getState();
|
||||
|
||||
} else { // Closed loop speed control
|
||||
|
||||
if (_param_rm_max_thr_spd.get() > FLT_EPSILON) { // Feedforward
|
||||
forward_speed_normalized = math::interpolate<float>(speed_setpoint_with_accel_limit.getState(),
|
||||
-max_thr_spd, max_thr_spd,
|
||||
-1.f, 1.f);
|
||||
}
|
||||
|
||||
pid_throttle.setSetpoint(speed_setpoint_with_accel_limit.getState());
|
||||
forward_speed_normalized += pid_throttle.update(vehicle_speed, dt); // Feedback
|
||||
|
||||
}
|
||||
|
||||
return math::constrain(forward_speed_normalized, -1.f, 1.f);
|
||||
|
||||
}
|
||||
|
||||
matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_speed_normalized,
|
||||
float lateral_speed_normalized,
|
||||
float speed_diff)
|
||||
{
|
||||
// Prioritize ratio between forward and lateral speed over either magnitude
|
||||
float combined_speed = fabsf(forward_throttle) + fabsf(lateral_throttle);
|
||||
float combined_speed = fabsf(forward_speed_normalized) + fabsf(lateral_speed_normalized);
|
||||
|
||||
if (combined_speed > 1.f) {
|
||||
forward_throttle /= combined_speed;
|
||||
lateral_throttle /= combined_speed;
|
||||
forward_speed_normalized /= combined_speed;
|
||||
lateral_speed_normalized /= combined_speed;
|
||||
combined_speed = 1.f;
|
||||
}
|
||||
|
||||
@ -179,20 +282,21 @@ matrix::Vector4f RoverMecanumControl::computeInverseKinematics(float forward_thr
|
||||
|
||||
if (total_speed > 1.f) {
|
||||
const float excess_velocity = fabsf(total_speed - 1.f);
|
||||
const float forward_throttle_temp = forward_throttle - sign(forward_throttle) * 0.5f * excess_velocity;
|
||||
const float lateral_throttle_temp = lateral_throttle - sign(lateral_throttle) * 0.5f * excess_velocity;
|
||||
const float forward_throttle_temp = forward_speed_normalized - sign(forward_speed_normalized) * 0.5f * excess_velocity;
|
||||
const float lateral_throttle_temp = lateral_speed_normalized - sign(lateral_speed_normalized) * 0.5f * excess_velocity;
|
||||
|
||||
if (sign(forward_throttle_temp) == sign(forward_throttle) && sign(lateral_throttle) == sign(lateral_throttle_temp)) {
|
||||
forward_throttle = forward_throttle_temp;
|
||||
lateral_throttle = lateral_throttle_temp;
|
||||
if (sign(forward_throttle_temp) == sign(forward_speed_normalized)
|
||||
&& sign(lateral_speed_normalized) == sign(lateral_throttle_temp)) {
|
||||
forward_speed_normalized = forward_throttle_temp;
|
||||
lateral_speed_normalized = lateral_throttle_temp;
|
||||
|
||||
} else {
|
||||
forward_throttle = lateral_throttle = 0.f;
|
||||
forward_speed_normalized = lateral_speed_normalized = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
// Calculate motor commands
|
||||
const float input_data[3] = {forward_throttle, lateral_throttle, speed_diff};
|
||||
const float input_data[3] = {forward_speed_normalized, lateral_speed_normalized, speed_diff};
|
||||
const Matrix<float, 3, 1> input(input_data);
|
||||
const float m_data[12] = {1.f, -1.f, -1.f, 1.f, 1.f, 1.f, 1.f, 1.f, -1.f, 1.f, -1.f, 1.f};
|
||||
const Matrix<float, 4, 3> m(m_data);
|
||||
|
||||
@ -43,7 +43,8 @@
|
||||
#include <uORB/topics/rover_mecanum_setpoint.h>
|
||||
#include <uORB/topics/rover_mecanum_status.h>
|
||||
#include <uORB/topics/actuator_motors.h>
|
||||
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
#include <lib/slew_rate/SlewRateYaw.hpp>
|
||||
|
||||
// Standard libraries
|
||||
#include <lib/pid/PID.hpp>
|
||||
@ -89,6 +90,39 @@ 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<float> &yaw_rate_with_accel_limit, PID &pid_yaw_rate, bool normalized);
|
||||
/**
|
||||
* @brief Compute normalized speed setpoint and apply slew rates.
|
||||
* to the speed setpoint and doing closed loop speed control if enabled.
|
||||
* @param speed_setpoint Speed setpoint [m/s].
|
||||
* @param vehicle_speed Actual speed [m/s].
|
||||
* @param max_thr_spd Speed the rover drives at maximum throttle [m/s].
|
||||
* @param 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 speed setpoint is already normalized.
|
||||
* @return Normalized speed setpoint with applied slew rates [-1, 1].
|
||||
*/
|
||||
float calcNormalizedSpeedSetpoint(float speed_setpoint, float vehicle_forward_speed, float max_thr_spd,
|
||||
SlewRate<float> &speed_setpoint_with_accel_limit, PID &pid_throttle, float max_accel, float max_decel,
|
||||
float dt, bool normalized);
|
||||
|
||||
/**
|
||||
* @brief Turn normalized speed setpoints into normalized motor commands.
|
||||
*
|
||||
@ -105,30 +139,39 @@ private:
|
||||
// uORB publications
|
||||
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
|
||||
uORB::Publication<rover_mecanum_status_s> _rover_mecanum_status_pub{ORB_ID(rover_mecanum_status)};
|
||||
rover_mecanum_status_s _rover_mecanum_status{};
|
||||
|
||||
// Variables
|
||||
rover_mecanum_setpoint_s _rover_mecanum_setpoint{};
|
||||
hrt_abstime _timestamp{0};
|
||||
float _max_yaw_rate{0.f};
|
||||
float _max_yaw_accel{0.f};
|
||||
|
||||
// Controllers
|
||||
PID _pid_forward_throttle; // PID for the closed loop forward speed control
|
||||
PID _pid_lateral_throttle; // PID for the closed loop lateral speed control
|
||||
PID _pid_yaw; // PID for the closed loop yaw control
|
||||
PID _pid_yaw_rate; // PID for the closed loop yaw rate control
|
||||
SlewRate<float> _forward_speed_setpoint_with_accel_limit{0.f};
|
||||
SlewRate<float> _lateral_speed_setpoint_with_accel_limit{0.f};
|
||||
SlewRate<float> _yaw_rate_with_accel_limit{0.f};
|
||||
SlewRateYaw<float> _yaw_setpoint_with_yaw_rate_limit;
|
||||
|
||||
// Parameters
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
|
||||
(ParamFloat<px4::params::RM_MAX_THR_SPD>) _param_rm_max_thr_spd,
|
||||
(ParamFloat<px4::params::RM_WHEEL_TRACK>) _param_rm_wheel_track,
|
||||
(ParamFloat<px4::params::RM_MAX_THR_SPD>) _param_rm_max_thr_spd,
|
||||
(ParamFloat<px4::params::RM_MAX_ACCEL>) _param_rm_max_accel,
|
||||
(ParamFloat<px4::params::RM_MAX_DECEL>) _param_rm_max_decel,
|
||||
(ParamFloat<px4::params::RM_MAX_THR_YAW_R>) _param_rm_max_thr_yaw_r,
|
||||
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
|
||||
(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
|
||||
(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
|
||||
(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
|
||||
(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
|
||||
(ParamFloat<px4::params::RM_YAW_P>) _param_rm_p_gain_yaw,
|
||||
(ParamFloat<px4::params::RM_YAW_I>) _param_rm_i_gain_yaw,
|
||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||
(ParamFloat<px4::params::RM_MAX_YAW_RATE>) _param_rm_max_yaw_rate,
|
||||
(ParamFloat<px4::params::RM_MAX_YAW_ACCEL>) _param_rm_max_yaw_accel,
|
||||
(ParamFloat<px4::params::RM_YAW_RATE_P>) _param_rm_yaw_rate_p,
|
||||
(ParamFloat<px4::params::RM_YAW_RATE_I>) _param_rm_yaw_rate_i,
|
||||
(ParamFloat<px4::params::RM_SPEED_P>) _param_rm_p_gain_speed,
|
||||
(ParamFloat<px4::params::RM_SPEED_I>) _param_rm_i_gain_speed,
|
||||
(ParamFloat<px4::params::RM_YAW_P>) _param_rm_p_gain_yaw,
|
||||
(ParamFloat<px4::params::RM_YAW_I>) _param_rm_i_gain_yaw,
|
||||
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
|
||||
)
|
||||
};
|
||||
|
||||
@ -29,6 +29,22 @@ parameters:
|
||||
decimal: 2
|
||||
default: 90
|
||||
|
||||
RM_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
|
||||
|
||||
RM_MAX_THR_YAW_R:
|
||||
description:
|
||||
short: Yaw rate turning left/right wheels at max speed in opposite directions
|
||||
@ -160,6 +176,21 @@ parameters:
|
||||
decimal: 2
|
||||
default: 0.5
|
||||
|
||||
RM_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
|
||||
|
||||
RM_MISS_VEL_GAIN:
|
||||
description:
|
||||
short: Tuning parameter for the velocity reduction during waypoint transition
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user