fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-12 04:53:34 +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.start_ned[0] = _prev_wp_ned(0);
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);
rover_position_setpoint.cruising_speed = _cruising_speed;
rover_position_setpoint.yaw = NAN;
@@ -90,6 +90,8 @@ void AckermannAutoMode::updateWaypointsAndAcceptanceRadius()
position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
_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,
_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,
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)
|| 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
} 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 acc_rad Acceptance radius of the current waypoint [m].
* @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 max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float arrivalSpeed(float cruising_speed, float min_speed, float acc_rad, int curr_wp_type,
float waypoint_transition_angle, float max_yaw_rate);
float arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad, const int curr_wp_type, const bool curr_wp_valid,
const float waypoint_transition_angle, const float max_yaw_rate);
// uORB subscriptions
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -125,6 +126,7 @@ private:
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.
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_POSITION};
int _curr_wp_valid{false};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,