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

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -72,16 +72,10 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
_forwards_velocity_smoothing.updateDurations(max_velocity);
_forwards_velocity_smoothing.updateTraj(dt);
if (_next_waypoint != next_waypoint) {
if (fabsf(heading_error) < 0.1f) {
_currentState = GuidanceState::DRIVING;
} else {
if (_current_waypoint != current_waypoint) {
_currentState = GuidanceState::TURNING;
}
}
// 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()) {
_currentState = GuidanceState::GOAL_REACHED;
@ -92,10 +86,15 @@ void DifferentialDriveGuidance::computeGuidance(float yaw, float angular_velocit
switch (_currentState) {
case GuidanceState::TURNING:
desired_speed = 0.f;
if (fabsf(heading_error) < 0.05f) {
_currentState = GuidanceState::DRIVING;
}
break;
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);
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.
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{};
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.timestamp = hrt_absolute_time();
_differential_drive_setpoint_pub.publish(output);
_current_waypoint = current_waypoint;
}
void DifferentialDriveGuidance::updateParams()

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

@ -125,7 +125,7 @@ private:
float _max_speed; ///< The maximum speed.
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.
PositionSmoothing _position_smoothing; ///< The position smoothing.