Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 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()) { if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _acceptance_radius) { if (_arrival_speed > FLT_EPSILON) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : distance_to_target -= _acceptance_radius; // shift target to the edge of the acceptance radius if arrival speed not zero
0.f; }
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _acceptance_radius : distance_to_target;
if (distance_to_target > _acceptance_radius || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), 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()); speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) {
@@ -130,6 +132,7 @@ void AckermannPosControl::updateSubscriptions()
_rover_position_setpoint_sub.copy(&_rover_position_setpoint); _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 = Vector2f(_rover_position_setpoint.start_ned[0], _rover_position_setpoint.start_ned[1]);
_start_ned = _start_ned.isAllFinite() ? _start_ned : _curr_pos_ned; _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{}; Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{}; Vector2f _curr_pos_ned{};
Vector2f _start_ned{}; Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f}; float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f}; float _max_yaw_rate{0.f};
float _acceptance_radius{0.f}; // Acceptance radius for the waypoint. 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(); 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]); const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (target_waypoint_ned.isAllFinite()) { if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) { if (_arrival_speed > FLT_EPSILON) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : distance_to_target -=
0.f; _param_nav_acc_rad.get(); // shift target to the edge of the acceptance radius if arrival speed not zero
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() : }
distance_to_target;
if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), 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()); speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { 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); _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() bool DifferentialPosControl::runSanityChecks()
src/modules/rover_differential/DifferentialPosControl/DifferentialPosControl.hpp
+1
View File
@@ -103,6 +103,7 @@ private:
// Variables // Variables
Vector2f _curr_pos_ned{}; Vector2f _curr_pos_ned{};
Vector2f _start_ned{}; Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f}; float _vehicle_yaw{0.f};
DEFINE_PARAMETERS( 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(); 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]); const Vector2f target_waypoint_ned(_rover_position_setpoint.position_ned[0], _rover_position_setpoint.position_ned[1]);
if (target_waypoint_ned.isAllFinite()) { if (target_waypoint_ned.isAllFinite()) {
float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm(); float distance_to_target = (target_waypoint_ned - _curr_pos_ned).norm();
if (distance_to_target > _param_nav_acc_rad.get()) { if (_arrival_speed > FLT_EPSILON) {
float arrival_speed = PX4_ISFINITE(_rover_position_setpoint.arrival_speed) ? _rover_position_setpoint.arrival_speed : distance_to_target -=
0.f; _param_nav_acc_rad.get(); // shift target to the edge of the acceptance radius if arrival speed not zero
const float distance = arrival_speed > 0.f + FLT_EPSILON ? distance_to_target - _param_nav_acc_rad.get() : }
distance_to_target;
if (distance_to_target > _param_nav_acc_rad.get() || _arrival_speed > FLT_EPSILON) {
float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(), 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()); speed_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
if (PX4_ISFINITE(_rover_position_setpoint.cruising_speed)) { 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); _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() bool MecanumPosControl::runSanityChecks()
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
+1
View File
@@ -105,6 +105,7 @@ private:
Quatf _vehicle_attitude_quaternion{}; Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_pos_ned{}; Vector2f _curr_pos_ned{};
Vector2f _start_ned{}; Vector2f _start_ned{};
float _arrival_speed{0.f};
float _vehicle_yaw{0.f}; float _vehicle_yaw{0.f};
float _max_yaw_rate{0.f}; float _max_yaw_rate{0.f};
float _yaw_setpoint{NAN}; float _yaw_setpoint{NAN};