Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

differential: add individual parameters for speed and yaw rate feedforward

Browse Source
This commit is contained in:
chfriedrich98
2024-09-05 15:11:10 +02:00
committed by chfriedrich98
parent 5d8a107925
commit 741ea6b707
8 changed files with 66 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
+3 -2
View File
@@ -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
ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
+3 -2
View File
@@ -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
msg/RoverDifferentialStatus.msg
+9 -7
View File
@@ -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
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp
+24 -16
View File
@@ -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<float>(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<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
@@ -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<float>(_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<float>(_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);
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.hpp
+2 -1
View File
@@ -113,7 +113,8 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_WHEEL_TRACK>) _param_rd_wheel_track,
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(ParamFloat<px4::params::RD_MAX_THR_SPD>) _param_rd_max_thr_spd,
(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,
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.cpp
+1 -1
View File
@@ -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();
src/modules/rover_differential/RoverDifferentialGuidance/RoverDifferentialGuidance.hpp
-1
View File
@@ -140,7 +140,6 @@ private:
// Parameters
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RD_MAX_SPEED>) _param_rd_max_speed,
(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,
src/modules/rover_differential/module.yaml
+24 -3
View File
@@ -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