Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

rover: fix speed setpoint in position controller

Browse Source
This commit is contained in:
chfriedrich98
2025-07-15 10:13:28 +02:00
committed by chfriedrich98
parent 54f2652329
commit b8dacf5ae4
6 changed files with 42 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.cpp
+8 -5
View File
@@ -61,12 +61,14 @@ void AckermannPosControl::updatePosControl()
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _acceptance_radius) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
0.f;
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
if (_arrival_speed > FLT_EPSILON) {
distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero
}
if (distance_to_target > _acceptance_radius || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
_param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -130,6 +132,7 @@ void AckermannPosControl::updateSubscriptions()
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
}
}
src/modules/rover_ackermann/AckermannPosControl/AckermannPosControl.hpp
+1
View File
@@ -106,6 +106,7 @@ private:
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _acceptance_radius{0.f}; // Acceptance radius for the waypoint.
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.cpp
+15 -12
View File
@@ -54,24 +54,19 @@ void DifferentialPosControl::updatePosControl()
hrt_abstime timestamp = hrt_absolute_time();
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
}
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
0.f;
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() :
distance_to_target;
if (_arrival_speed > FLT_EPSILON) {
distance_to_target -=
_param_nav_acc_rad.get(); // shift target to the edge of the acceptance radius if arrival speed not zero
}
if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
_param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -119,6 +114,14 @@ void DifferentialPosControl::updateSubscriptions()
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
}
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
}
}
bool DifferentialPosControl::runSanityChecks()
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
+1
View File
@@ -103,6 +103,7 @@ private:
// Variables
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
DEFINE_PARAMETERS(
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.cpp
+16 -13
View File
@@ -56,26 +56,21 @@ void MecanumPosControl::updatePosControl()
hrt_abstime timestamp = hrt_absolute_time();
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
}
const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed :
0.f;
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() :
distance_to_target;
if (_arrival_speed > FLT_EPSILON) {
distance_to_target -=
_param_nav_acc_rad.get(); // shift target to the edge of the acceptance radius if arrival speed not zero
}
if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance, fabsf(arrival_speed));
_param_ro_decel_limit.get(), distance_to_target, fabsf(_arrival_speed));
speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -130,6 +125,14 @@ void MecanumPosControl::updateSubscriptions()
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
}
if (_rover_position_setpoint_sub.updated()) {
_rover_position_setpoint_sub.copy(&_rover_position_setpoint);
_start_ned = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned;
_yaw_setpoint = PX4_ISFINITE(_rover_position_setpoint.yaw) ? _rover_position_setpoint.yaw : _vehicle_yaw;
_arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : 0.f;
}
}
bool MecanumPosControl::runSanityChecks()
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
+1
View File
@@ -105,6 +105,7 @@ private:
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{};
Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f};
float _yaw_setpoint{NAN};