Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

Fixed guidance logic and added feedforward term to compute the angular velocity

Browse Source
This commit is contained in:
PerFrivik
2024-01-26 09:34:13 +01:00
committed by Matthias Grob
parent 396ef222ee
commit d197d94889
2 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.cpp
+11 -9
View File
@@ -72,16 +72,10 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
_forwards_velocity_smoothing.updateDurations(max_velocity); _forwards_velocity_smoothing.updateDurations(max_velocity);
_forwards_velocity_smoothing.updateTraj(dt); _forwards_velocity_smoothing.updateTraj(dt);
if (_next_waypoint != next_waypoint) { if (_current_waypoint != current_waypoint) {
if (fabsf(heading_error) < 0.1f) {
_currentState = GuidanceState::DRIVING;
} else {
_currentState = GuidanceState::TURNING; _currentState = GuidanceState::TURNING;
} }
}
// Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly. // Make rover stop when it arrives at the last waypoint instead of loitering and driving around weirdly.
if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) { if ((current_waypoint == next_waypoint) && distance_to_next_wp <= _param_nav_acc_rad.get()) {
_currentState = GuidanceState::GOAL_REACHED; _currentState = GuidanceState::GOAL_REACHED;
@@ -92,10 +86,15 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
switch (_currentState) { switch (_currentState) {
case GuidanceState::TURNING: case GuidanceState::TURNING:
desired_speed = 0.f; desired_speed = 0.f;
if (fabsf(heading_error) < 0.05f) {
_currentState = GuidanceState::DRIVING;
}
break; break;
case GuidanceState::DRIVING: case GuidanceState::DRIVING:
desired_speed = math::interpolate<float>(abs(heading_error), 0.1f, 0.4f, 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;
@@ -109,7 +108,8 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
} }
// 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, dt); float angular_velocity_pid = pid_calculate(&_heading_p_controller, heading_error, angular_velocity, 0,
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);
@@ -117,6 +117,8 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true; output.closed_loop_speed_control = output.closed_loop_yaw_rate_control = true;
output.timestamp = hrt_absolute_time(); output.timestamp = hrt_absolute_time();
_differential_drive_setpoint_pub.publish(output); _differential_drive_setpoint_pub.publish(output);
_current_waypoint = current_waypoint;
} }
void DifferentialDriveGuidance::updateParams() void DifferentialDriveGuidance::updateParams()
src/modules/differential_drive/DifferentialDriveGuidance/DifferentialDriveGuidance.hpp
+1 -1
View File
@@ -125,7 +125,7 @@ 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.
matrix::Vector2d _next_waypoint; ///< The next 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.
PositionSmoothing _position_smoothing; ///< The position smoothing. PositionSmoothing _position_smoothing; ///< The position smoothing.