From db8e31eaecc4d44067a89bc9e20a37bacf44277a Mon Sep 17 00:00:00 2001 From: Balduin Date: Fri, 5 Dec 2025 18:02:30 +0100 Subject: [PATCH] differential: replace previous waypoint type == idle check with validity check --- .../DifferentialAutoMode/DifferentialAutoMode.cpp | 9 +++++---- .../DifferentialAutoMode/DifferentialAutoMode.hpp | 3 ++- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp index ebd60e9d99..b63047ef37 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp @@ -51,7 +51,8 @@ void DifferentialAutoMode::autoControl() if (_position_setpoint_triplet_sub.updated()) { position_setpoint_triplet_s position_setpoint_triplet{}; _position_setpoint_triplet_sub.copy(&position_setpoint_triplet); - int curr_wp_type = position_setpoint_triplet.current.type; + const int curr_wp_type = position_setpoint_triplet.current.type; + const bool curr_wp_valid = position_setpoint_triplet.current.valid; vehicle_local_position_s vehicle_local_position{}; _vehicle_local_position_sub.copy(&vehicle_local_position); @@ -85,7 +86,7 @@ void DifferentialAutoMode::autoControl() 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, waypoint_transition_angle, - _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type); + _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type, curr_wp_valid); rover_position_setpoint.cruising_speed = cruising_speed; rover_position_setpoint.yaw = NAN; _rover_position_setpoint_pub.publish(rover_position_setpoint); @@ -93,11 +94,11 @@ void DifferentialAutoMode::autoControl() } float DifferentialAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, - const float max_speed, const float trans_drv_trn, const float speed_red, int curr_wp_type) + const float max_speed, const float trans_drv_trn, const float speed_red, const int curr_wp_type, const bool curr_wp_valid) { // Upcoming stop if (!PX4_ISFINITE(waypoint_transition_angle) || waypoint_transition_angle < M_PI_F - trans_drv_trn - || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND) { + || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || !curr_wp_valid) { return 0.f; } diff --git a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp index 92cbce1653..6b7d6cbc4a 100644 --- a/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp +++ b/src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp @@ -81,10 +81,11 @@ private: * @param trans_drv_trn Heading error threshold to switch from driving to turning [rad]. * @param speed_red Tuning parameter for the speed reduction during waypoint transition. * @param curr_wp_type Type of the current waypoint. + * @param curr_wp_valid Validity flag of the current waypoint. * @return Speed setpoint [m/s]. */ float arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle, const float max_speed, - const float trans_drv_trn, const float speed_red, int curr_wp_type); + const float trans_drv_trn, const float speed_red, const int curr_wp_type, const bool curr_wp_valid); // uORB subscriptions uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};