fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-04 01:01:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

ackermann: replace previous waypoint type == idle check with validity check

Browse Source
This commit is contained in:
Balduin
2026-01-14 13:59:45 +01:00
parent 1433931fe0
commit 9b0e1b8378
2 changed files with 9 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.cpp
+5 -3
View File
@@ -76,7 +76,7 @@ void AckermannAutoMode::autoControl()
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1); rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
rover_position_setpoint.start_ned[0] = _prev_wp_ned(0); rover_position_setpoint.start_ned[0] = _prev_wp_ned(0);
rover_position_setpoint.start_ned[1] = _prev_wp_ned(1); rover_position_setpoint.start_ned[1] = _prev_wp_ned(1);
rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type, rover_position_setpoint.arrival_speed = arrivalSpeed(_cruising_speed, _min_speed, _acceptance_radius, _curr_wp_type, _curr_wp_valid,
_waypoint_transition_angle, _max_yaw_rate); _waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = _cruising_speed; rover_position_setpoint.cruising_speed = _cruising_speed;
rover_position_setpoint.yaw = NAN; rover_position_setpoint.yaw = NAN;
@@ -90,6 +90,8 @@ void AckermannAutoMode::updateWaypointsAndAcceptanceRadius()
position_setpoint_triplet_s position_setpoint_triplet{}; position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet); _position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
_curr_wp_type = position_setpoint_triplet.current.type; _curr_wp_type = position_setpoint_triplet.current.type;
_curr_wp_valid = position_setpoint_triplet.current.valid;
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet, RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_curr_pos_ned, _global_ned_proj_ref); _curr_pos_ned, _global_ned_proj_ref);
@@ -136,10 +138,10 @@ float AckermannAutoMode::updateAcceptanceRadius(const float waypoint_transition_
} }
float AckermannAutoMode::arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad, float AckermannAutoMode::arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad,
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate) const int curr_wp_type, const bool curr_wp_valid, const float waypoint_transition_angle, const float max_yaw_rate)
{ {
if (!PX4_ISFINITE(waypoint_transition_angle) if (!PX4_ISFINITE(waypoint_transition_angle)
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND) { || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || !curr_wp_valid) {
return 0.f; // Stop at the waypoint return 0.f; // Stop at the waypoint
} else if (_param_ro_speed_red.get() > FLT_EPSILON) { } else if (_param_ro_speed_red.get() > FLT_EPSILON) {
src/modules/rover_ackermann/AckermannDriveModes/AckermannAutoMode/AckermannAutoMode.hpp
+4 -2
View File
@@ -98,12 +98,13 @@ private:
* @param min_speed Minimum speed setpoint [m/s]. * @param min_speed Minimum speed setpoint [m/s].
* @param acc_rad Acceptance radius of the current waypoint [m]. * @param acc_rad Acceptance radius of the current waypoint [m].
* @param curr_wp_type Type of the current waypoint. * @param curr_wp_type Type of the current waypoint.
* @param curr_wp_valid Validity flag of the current waypoint.
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad] * @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_yaw_rate Maximum yaw rate setpoint [rad/s] * @param max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s]. * @return Speed setpoint [m/s].
*/ */
float arrivalSpeed(float cruising_speed, float min_speed, float acc_rad, int curr_wp_type, float arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad, const int curr_wp_type, const bool curr_wp_valid,
float waypoint_transition_angle, float max_yaw_rate); const float waypoint_transition_angle, const float max_yaw_rate);
// uORB subscriptions // uORB subscriptions
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -125,6 +126,7 @@ private:
float _max_yaw_rate{NAN}; float _max_yaw_rate{NAN};
float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base. float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_POSITION}; int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_POSITION};
int _curr_wp_valid{false};
DEFINE_PARAMETERS( DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit, (ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,