fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 13:03:34 +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-11 13:57:27 +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,