fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 09:50:35 +08:00
Code Issues 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
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