fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

differential: add slew rates for setpoints, deprecate RD_MAN_YAW_SCALE and update airframe tuning

Browse Source
This commit is contained in:
chfriedrich98 2024-09-17 10:15:36 +02:00 committed by chfriedrich98
parent fbbfcdb7a6
commit 4f00df60ae
12 changed files with 300 additions and 125 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
View File

@ -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

1
ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower
View File

@ -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

32
ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
View File

@ -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

2
msg/RoverDifferentialSetpoint.msg
View File

@ -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

19
msg/RoverDifferentialStatus.msg
View File

@ -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

24
src/modules/rover_differential/RoverDifferential.cpp
View File

@ -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<float>(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<float>(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<float>(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<float>(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<float>(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;

9
src/modules/rover_differential/RoverDifferential.hpp
View File

@ -129,9 +129,10 @@ private:
Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_MAN_YAW_SCALE>) _param_rd_man_yaw_scale,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max
)
};

175
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp
View File

@ -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<float>(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<float>(_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<float> &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<float>(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<float> &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<float>(_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);
}

72
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp
View File

@ -43,7 +43,8 @@
#include <uORB/topics/rover_differential_setpoint.h>
#include <uORB/topics/rover_differential_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.h>
@ -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<float> &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<float> &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_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<rover_differential_status_s> _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<float> _forward_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::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
(ParamFloat<px4::params::RD_MAX_DECEL>) _param_rd_max_decel,
(ParamFloat<px4::params::RD_MAX_THR_YAW_R>) _param_rd_max_thr_yaw_r,
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
(ParamFloat<px4::params::RD_YAW_P>) _param_rd_p_gain_yaw,
(ParamFloat<px4::params::RD_YAW_I>) _param_rd_i_gain_yaw,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
(ParamFloat<px4::params::RD_MAX_YAW_RATE>) _param_rd_max_yaw_rate,
(ParamFloat<px4::params::RD_MAX_YAW_ACCEL>) _param_rd_max_yaw_accel,
(ParamFloat<px4::params::RD_YAW_RATE_P>) _param_rd_yaw_rate_p,
(ParamFloat<px4::params::RD_YAW_RATE_I>) _param_rd_yaw_rate_i,
(ParamFloat<px4::params::RD_SPEED_P>) _param_rd_p_gain_speed,
(ParamFloat<px4::params::RD_SPEED_I>) _param_rd_i_gain_speed,
(ParamFloat<px4::params::RD_YAW_P>) _param_rd_p_gain_yaw,
(ParamFloat<px4::params::RD_YAW_I>) _param_rd_i_gain_yaw,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};

6
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
View File

@ -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);
}

10
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
View File

@ -140,11 +140,11 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
(ParamFloat<px4::params::RD_MAX_ACCEL>) _param_rd_max_accel,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RD_MAX_JERK>) _param_rd_max_jerk,
(ParamFloat<px4::params::RD_MAX_DECEL>) _param_rd_max_decel,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MISS_SPD_DEF>) _param_rd_miss_spd_def,
(ParamFloat<px4::params::RD_TRANS_TRN_DRV>) _param_rd_trans_trn_drv,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn

44
src/modules/rover_differential/module.yaml
View File

@ -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