Skip to content

Commit

Permalink
ackermann: add speed waypoint support and fix delay detection (PX4#23572
Browse files Browse the repository at this point in the history 
)
  • Loading branch information
@sbtjagu
sbtjagu authored Aug 27, 2024
1 parent 5fff1ad commit be4d0d3
Show file tree
Hide file tree
Showing 2 changed files with 30 additions and 18 deletions.
44 changes: 26 additions & 18 deletions src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,14 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
{
updateSubscriptions();

// Distances to waypoints
_distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
_prev_wp(0), _prev_wp(1));
_distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
_curr_wp(0), _curr_wp(1));
_distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
_next_wp(0), _next_wp(1));

// Catch return to launch
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
_mission_finished = _distance_to_next_wp < _acceptance_radius;
Expand All @@ -74,7 +82,8 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
_desired_speed = 0.f;

} else { // Regular guidance algorithm
_desired_speed = calcDesiredSpeed(_param_ra_miss_vel_def.get(), _param_ra_miss_vel_min.get(),

_desired_speed = calcDesiredSpeed(_wp_max_desired_vel, _param_ra_miss_vel_min.get(),
_param_ra_miss_vel_gain.get(), _distance_to_prev_wp, _distance_to_curr_wp, _acceptance_radius,
_prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state);

Expand Down Expand Up @@ -156,40 +165,32 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);

// Global waypoint coordinates
Vector2d prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid
Vector2d curr_wp(0, 0);
Vector2d next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL
_prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid
_curr_wp = Vector2d(0, 0);
_next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL

if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);
_curr_wp = Vector2d(position_setpoint_triplet.current.lat, position_setpoint_triplet.current.lon);

}

if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
&& PX4_ISFINITE(position_setpoint_triplet.previous.lon)) {
prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);
_prev_wp = Vector2d(position_setpoint_triplet.previous.lat, position_setpoint_triplet.previous.lon);

}

if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
&& PX4_ISFINITE(position_setpoint_triplet.next.lon)) {
next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
_next_wp = Vector2d(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);

}

// Distances to waypoints
_distance_to_prev_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
prev_wp(0), prev_wp(1));
_distance_to_curr_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
curr_wp(0), curr_wp(1));
_distance_to_next_wp = get_distance_to_next_waypoint(_curr_pos(0), _curr_pos(1),
next_wp(0), next_wp(1));

// NED waypoint coordinates
_curr_wp_ned = _global_ned_proj_ref.project(curr_wp(0), curr_wp(1));
_prev_wp_ned = _global_ned_proj_ref.project(prev_wp(0), prev_wp(1));
_next_wp_ned = _global_ned_proj_ref.project(next_wp(0), next_wp(1));
_curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
_prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
_next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));

// Update acceptance radius
_prev_acceptance_radius = _acceptance_radius;
Expand All @@ -201,6 +202,13 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
} else {
_acceptance_radius = _param_nav_acc_rad.get();
}

if (position_setpoint_triplet.current.cruising_speed > 0.f) {
_wp_max_desired_vel = math::constrain(position_setpoint_triplet.current.cruising_speed, 0.f, _param_ra_max_speed.get());

} else {
_wp_max_desired_vel = _param_ra_miss_vel_def.get();
}
}

float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
Expand Down
4 changes: 4 additions & 0 deletions src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -205,11 +205,15 @@ class RoverAckermannGuidance : public ModuleParams
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};
Vector2d _curr_wp{};
Vector2d _prev_wp{};
Vector2d _next_wp{};
float _distance_to_prev_wp{0.f};
float _distance_to_curr_wp{0.f};
float _distance_to_next_wp{0.f};
float _acceptance_radius{0.5f};
float _prev_acceptance_radius{0.5f};
float _wp_max_desired_vel{0.f};
bool _mission_finished{false};

// Parameters
Expand Down

0 comments on commit be4d0d3

Please sign in to comment.