dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Differential Drive: Added parameter & logic to set the minium turning speed in the turning state

This commit is contained in:
PerFrivik 2024-03-13 13:58:37 +01:00
parent 4a553938fb
commit d0aab458e5
3 changed files with 34 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
View File

@ -64,7 +64,7 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0), float desired_heading = get_bearing_to_next_waypoint(global_position(0), global_position(1), current_waypoint(0),
current_waypoint(1)); current_waypoint(1));
float heading_error = matrix::wrap_pi(desired_heading - yaw); _heading_error = matrix::wrap_pi(desired_heading - yaw);
if (_current_waypoint != current_waypoint) { if (_current_waypoint != current_waypoint) {
_currentState = GuidanceState::TURNING; _currentState = GuidanceState::TURNING;
@ -78,21 +78,29 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
float desired_speed = 0.f; float desired_speed = 0.f;
switch (_currentState) { switch (_currentState) {
case GuidanceState::TURNING: case GuidanceState::TURNING: {
desired_speed = 0.f; desired_speed = 0.f;
if (fabsf(heading_error) < 0.05f) { if (fabsf(_heading_error) < 0.05f) {
_currentState = GuidanceState::DRIVING; _currentState = GuidanceState::DRIVING;
}
float minTurningSpeed = _param_rdd_min_turning_speed.get();
// Make sure we do not get stuck while turning as the error gets too small
if (fabsf(_heading_error) < minTurningSpeed) {
_heading_error = (_heading_error > 0) ? minTurningSpeed : -minTurningSpeed;
}
break;
} }
break;
case GuidanceState::DRIVING: { case GuidanceState::DRIVING: {
const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(), const float max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_rdd_max_jerk.get(),
_param_rdd_max_accel.get(), distance_to_next_wp, 0.0f); _param_rdd_max_accel.get(), distance_to_next_wp, 0.0f);
_forwards_velocity_smoothing.updateDurations(max_velocity); _forwards_velocity_smoothing.updateDurations(max_velocity);
_forwards_velocity_smoothing.updateTraj(dt); _forwards_velocity_smoothing.updateTraj(dt);
desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.8f, desired_speed = math::interpolate<float>(abs(_heading_error), 0.1f, 0.8f,
_forwards_velocity_smoothing.getCurrentVelocity(), 0.0f); _forwards_velocity_smoothing.getCurrentVelocity(), 0.0f);
break; break;
} }
@ -100,15 +108,15 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
case GuidanceState::GOAL_REACHED: case GuidanceState::GOAL_REACHED:
// temporary till I find a better way to stop the vehicle // temporary till I find a better way to stop the vehicle
desired_speed = 0.f; desired_speed = 0.f;
heading_error = 0.f; _heading_error = 0.f;
angular_velocity = 0.f; angular_velocity = 0.f;
_desired_angular_velocity = 0.f; _desired_angular_velocity = 0.f;
break; break;
} }
// Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint. // Compute the desired angular velocity relative to the heading error, to steer the vehicle towards the next waypoint.
float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0, float angular_velocity_pid = pid_calculate(&_heading_p_controller, _heading_error, angular_velocity, 0,
dt) + heading_error; dt) + _heading_error;
differential_drive_setpoint_s output{}; differential_drive_setpoint_s output{};
output.speed = math::constrain(desired_speed, -_max_speed, _max_speed); output.speed = math::constrain(desired_speed, -_max_speed, _max_speed);

5
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
View File

@ -124,6 +124,8 @@ private:
float _max_speed; ///< The maximum speed. float _max_speed; ///< The maximum speed.
float _max_angular_velocity; ///< The maximum angular velocity. float _max_angular_velocity; ///< The maximum angular velocity.
float _heading_error{0.0f};
matrix::Vector2d _current_waypoint; ///< The current waypoint. matrix::Vector2d _current_waypoint; ///< The current waypoint.
VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion. VelocitySmoothing _forwards_velocity_smoothing; ///< The velocity smoothing for forward motion.
@ -135,6 +137,7 @@ private:
(ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading, (ParamFloat<px4::params::RDD_P_HEADING>) _param_rdd_p_gain_heading,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad, (ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk, (ParamFloat<px4::params::RDD_MAX_JERK>) _param_rdd_max_jerk,
(ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel (ParamFloat<px4::params::RDD_MAX_ACCEL>) _param_rdd_max_accel,
(ParamFloat<px4::params::RDD_MIN_TRN_VEL>) _param_rdd_min_turning_speed
) )
}; };

11
src/modules/differential_drive/module.yaml
View File

@ -120,3 +120,14 @@ parameters:
increment: 0.01 increment: 0.01
decimal: 2 decimal: 2
default: 0.5 default: 0.5
RDD_MIN_TRN_VEL:
description:
short: Minimun turning speed
long: Minimum turning speed required to ensure vehicle turns properly, preventing no-turn situations caused by minor heading errors or motor limitations at low speeds.
type: float
unit: rad/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 0.2