fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 07:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ackermann: add speed waypoint support and fix delay detection (#23572)

Browse Source
This commit is contained in:
sbtjagu
2024-08-27 13:35:48 +02:00
committed by GitHub
parent 5fff1ad6d1
commit be4d0d351c
2 changed files with 30 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
+26 -18
View File
@@ -60,6 +60,14 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
{ {
updateSubscriptions(); 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 // Catch return to launch
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
_mission_finished = _distance_to_next_wp < _acceptance_radius; _mission_finished = _distance_to_next_wp < _acceptance_radius;
@@ -74,7 +82,8 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
_desired_speed = 0.f; _desired_speed = 0.f;
} else { // Regular guidance algorithm } 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, _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); _prev_acceptance_radius, _param_ra_max_accel.get(), _param_ra_max_jerk.get(), nav_state);
@@ -156,40 +165,32 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet); _position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
// Global waypoint coordinates // Global waypoint coordinates
Vector2d prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid _prev_wp = _curr_pos.isAllFinite() ? _curr_pos : Vector2d(0, 0); // Fallback if previous waypoint is invalid
Vector2d curr_wp(0, 0); _curr_wp = Vector2d(0, 0);
Vector2d next_wp = _home_position.isAllFinite() ? _home_position : Vector2d(0, 0); // Enables corner slow down with RTL _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) if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) { && 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) if (position_setpoint_triplet.previous.valid && PX4_ISFINITE(position_setpoint_triplet.previous.lat)
&& PX4_ISFINITE(position_setpoint_triplet.previous.lon)) { && 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) if (position_setpoint_triplet.next.valid && PX4_ISFINITE(position_setpoint_triplet.next.lat)
&& PX4_ISFINITE(position_setpoint_triplet.next.lon)) { && 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 // NED waypoint coordinates
_curr_wp_ned = _global_ned_proj_ref.project(curr_wp(0), curr_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)); _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)); _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
// Update acceptance radius // Update acceptance radius
_prev_acceptance_radius = _acceptance_radius; _prev_acceptance_radius = _acceptance_radius;
@@ -201,6 +202,13 @@ void RoverAckermannGuidance::updateWaypointsAndAcceptanceRadius()
} else { } else {
_acceptance_radius = _param_nav_acc_rad.get(); _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, float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
+4
View File
@@ -205,11 +205,15 @@ private:
Vector2f _curr_wp_ned{}; Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{}; Vector2f _prev_wp_ned{};
Vector2f _next_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_prev_wp{0.f};
float _distance_to_curr_wp{0.f}; float _distance_to_curr_wp{0.f};
float _distance_to_next_wp{0.f}; float _distance_to_next_wp{0.f};
float _acceptance_radius{0.5f}; float _acceptance_radius{0.5f};
float _prev_acceptance_radius{0.5f}; float _prev_acceptance_radius{0.5f};
float _wp_max_desired_vel{0.f};
bool _mission_finished{false}; bool _mission_finished{false};
// Parameters // Parameters