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

differential: fix closed loop control

Browse Source 
removed thresholds for closed loop setpoints and added minimum thresholds for yaw rate and speed measurements instead to avoid moving due to measurement noise
This commit is contained in:
chfriedrich98
2024-09-05 13:50:00 +02:00
committed by chfriedrich98
parent c94c1ce4d2
commit 5d8a107925
3 changed files with 23 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_differential/RoverDifferential.cpp
+2 -2
View File
@@ -142,7 +142,7 @@ void RoverDifferential::updateSubscriptions()
if (_vehicle_angular_velocity_sub.updated()) {
vehicle_angular_velocity_s vehicle_angular_velocity{};
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
_vehicle_yaw_rate = vehicle_angular_velocity.xyz[2];
_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f;
}
if (_vehicle_attitude_sub.updated()) {
@@ -157,7 +157,7 @@ void RoverDifferential::updateSubscriptions()
_vehicle_local_position_sub.copy(&vehicle_local_position);
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_vehicle_forward_speed = velocity_in_body_frame(0);
_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
}
}
src/modules/rover_differential/RoverDifferential.hpp
+6
View File
@@ -113,6 +113,12 @@ private:
int _nav_state{0};
bool _armed{false};
// Thresholds to avoid moving at rest due to measurement noise
static constexpr float YAW_RATE_THRESHOLD =
0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
static constexpr float SPEED_THRESHOLD =
0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
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
src/modules/rover_differential/RoverDifferentialControl/RoverDifferentialControl.cpp
+15 -32
View File
@@ -84,32 +84,20 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
// Closed loop yaw control (Overrides yaw rate setpoint)
if (PX4_ISFINITE(_rover_differential_setpoint.yaw_setpoint)) {
if (fabsf(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw) < FLT_EPSILON) {
_rover_differential_setpoint.yaw_rate_setpoint = 0.f;
pid_reset_integral(&_pid_yaw);
} else {
const float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
}
float heading_error = matrix::wrap_pi(_rover_differential_setpoint.yaw_setpoint - vehicle_yaw);
_rover_differential_setpoint.yaw_rate_setpoint = pid_calculate(&_pid_yaw, heading_error, 0, 0, dt);
}
// Yaw rate control
float speed_diff_normalized{0.f};
if (PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint)) { // Closed loop yaw rate control
if (fabsf(_rover_differential_setpoint.yaw_rate_setpoint) < FLT_EPSILON) {
speed_diff_normalized = 0.f;
pid_reset_integral(&_pid_yaw_rate);
} else {
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);
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
}
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);
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
} else { // Use normalized setpoint
speed_diff_normalized = PX4_ISFINITE(_rover_differential_setpoint.yaw_rate_setpoint_normalized) ?
@@ -120,20 +108,15 @@ void RoverDifferentialControl::computeMotorCommands(const float vehicle_yaw, con
float throttle{0.f};
if (PX4_ISFINITE(_rover_differential_setpoint.forward_speed_setpoint)) { // Closed loop speed control
if (fabsf(_rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) {
pid_reset_integral(&_pid_throttle);
} else {
throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
dt);
if (_param_rd_max_speed.get() > FLT_EPSILON) { // Feed-forward term
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_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);
}
throttle = pid_calculate(&_pid_throttle, _rover_differential_setpoint.forward_speed_setpoint, vehicle_forward_speed, 0,
dt); // 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;