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

mecanum: 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 503f8d8fc7
commit 1433931fe0
2 changed files with 7 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp
+5 -4
View File
@@ -51,7 +51,8 @@ void MecanumAutoMode::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 MecanumAutoMode::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_ro_speed_red.get(), curr_wp_type);
_param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type, curr_wp_valid);
rover_position_setpoint.cruising_speed = cruising_speed;
rover_position_setpoint.yaw = PX4_ISFINITE(position_setpoint_triplet.current.yaw) ?
position_setpoint_triplet.current.yaw : NAN;
@@ -94,10 +95,10 @@ void MecanumAutoMode::autoControl()
}
float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
const float max_speed, const float speed_red, int curr_wp_type)
const float max_speed, const float speed_red, const int curr_wp_type, const bool curr_wp_valid)
{
// Upcoming stop
if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND || !curr_wp_valid) {
return 0.f;
}
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp
+2 -1
View File
@@ -80,10 +80,11 @@ private:
* @param max_speed Maximum speed setpoint [m/s]
* @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 speed_red, int curr_wp_type);
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)};