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

rover: auto mode stopping conditions

Browse Source
This commit is contained in:
chfriedrich98
2025-03-25 10:38:30 +01:00
committed by chfriedrich98
parent 735777862d
commit c2706f5406
8 changed files with 192 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp
+49 -57
View File
@@ -131,12 +131,6 @@ void AckermannPosVelControl::updateSubscriptions()
_vehicle_speed_body_y = fabsf(velocity_in_body_frame(1)) > _param_ro_speed_th.get() ? velocity_in_body_frame(1) : 0.f;
}
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
_vehicle_status_sub.copy(&vehicle_status);
_nav_state = vehicle_status.nav_state;
}
}
void AckermannPosVelControl::generateAttitudeSetpoint()
@@ -254,7 +248,9 @@ void AckermannPosVelControl::offboardVelocityMode()
void AckermannPosVelControl::autoPositionMode()
{
updateAutoSubscriptions();
if (_position_setpoint_triplet_sub.updated()) {
updateWaypointsAndAcceptanceRadius();
}
// Distances to waypoints
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
@@ -262,17 +258,25 @@ void AckermannPosVelControl::autoPositionMode()
const float distance_to_curr_wp = sqrt(powf(_curr_pos_ned(0) - _curr_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _curr_wp_ned(1), 2));
if (_nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { // Check RTL arrival
_mission_finished = distance_to_curr_wp < _param_nav_acc_rad.get();
// Check stopping conditions
bool auto_stop{false};
if (_curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|| _curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE
|| !_next_wp_ned.isAllFinite()) { // Check stopping conditions
auto_stop = distance_to_curr_wp < _param_nav_acc_rad.get();
}
if (_mission_finished) {
if (auto_stop) {
_speed_body_x_setpoint = 0.f;
rover_rate_setpoint_s rover_rate_setpoint{};
rover_rate_setpoint.timestamp = _timestamp;
rover_rate_setpoint.yaw_rate_setpoint = 0.f;
_rover_rate_setpoint_pub.publish(rover_rate_setpoint);
} else { // Regular guidance algorithm
_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, _min_speed, distance_to_prev_wp, distance_to_curr_wp,
_acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _nav_state,
_acceptance_radius, _prev_acceptance_radius, _param_ro_decel_limit.get(), _param_ro_jerk_limit.get(), _curr_wp_type,
_waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
@@ -284,25 +288,7 @@ void AckermannPosVelControl::autoPositionMode()
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
void AckermannPosVelControl::updateAutoSubscriptions()
{
if (_home_position_sub.updated()) {
home_position_s home_position{};
_home_position_sub.copy(&home_position);
_home_position = Vector2d(home_position.lat, home_position.lon);
}
if (_position_setpoint_triplet_sub.updated()) {
updateWaypointsAndAcceptanceRadius();
}
if (_mission_result_sub.updated()) {
mission_result_s mission_result{};
_mission_result_sub.copy(&mission_result);
_mission_finished = mission_result.finished;
}
}
@@ -310,9 +296,10 @@ void AckermannPosVelControl::updateWaypointsAndAcceptanceRadius()
{
position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
_curr_wp_type = position_setpoint_triplet.current.type;
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_curr_pos_ned, _home_position, _global_ned_proj_ref);
_curr_pos_ned, _global_ned_proj_ref);
_prev_waypoint_transition_angle = _waypoint_transition_angle;
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
@@ -339,13 +326,16 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi
{
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
float acceptance_radius = default_acceptance_radius;
const float theta = waypoint_transition_angle / 2.f;
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
acceptance_radius_max);
if (PX4_ISFINITE(_waypoint_transition_angle)) {
const float theta = waypoint_transition_angle / 2.f;
const float min_turning_radius = wheel_base / sinf(max_steer_angle);
const float acceptance_radius_temp = min_turning_radius / tanf(theta);
const float acceptance_radius_temp_scaled = acceptance_radius_gain *
acceptance_radius_temp; // Scale geometric ideal acceptance radius to account for kinematic and dynamic effects
acceptance_radius = math::constrain<float>(acceptance_radius_temp_scaled, default_acceptance_radius,
acceptance_radius_max);
}
// Publish updated acceptance radius
position_controller_status_s pos_ctrl_status{};
@@ -357,7 +347,7 @@ float AckermannPosVelControl::updateAcceptanceRadius(const float waypoint_transi
float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float miss_speed_min,
const float distance_to_prev_wp, const float distance_to_curr_wp, const float acc_rad,
const float prev_acc_rad, const float max_decel, const float max_jerk, const int nav_state,
const float prev_acc_rad, const float max_decel, const float max_jerk, const int curr_wp_type,
const float waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_speed)
{
// Catch improper values
@@ -365,13 +355,24 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons
return cruising_speed;
}
// Upcoming stop
if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!PX4_ISFINITE(waypoint_transition_angle)
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE)) {
const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
max_decel, distance_to_curr_wp, 0.f);
return math::min(straight_line_speed, cruising_speed);
}
// Cornering slow down effect
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON) {
if (distance_to_prev_wp <= prev_acc_rad && prev_acc_rad > FLT_EPSILON && PX4_ISFINITE(prev_waypoint_transition_angle)) {
const float turning_circle = prev_acc_rad * tanf(prev_waypoint_transition_angle / 2.f);
const float cornering_speed = _max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
} else if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON) {
}
if (distance_to_curr_wp <= acc_rad && acc_rad > FLT_EPSILON && PX4_ISFINITE(waypoint_transition_angle)) {
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
const float cornering_speed = _max_yaw_rate * turning_circle;
return math::constrain(cornering_speed, miss_speed_min, cruising_speed);
@@ -380,26 +381,17 @@ float AckermannPosVelControl::calcSpeedSetpoint(const float cruising_speed, cons
// Straight line speed
if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && acc_rad > FLT_EPSILON) {
float straight_line_speed{0.f};
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
max_decel, distance_to_curr_wp, 0.f);
} else {
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
float cornering_speed = _max_yaw_rate * turning_circle;
cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
}
const float turning_circle = acc_rad * tanf(waypoint_transition_angle / 2.f);
float cornering_speed = _max_yaw_rate * turning_circle;
cornering_speed = math::constrain(cornering_speed, miss_speed_min, cruising_speed);
const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
max_decel, distance_to_curr_wp - acc_rad, cornering_speed);
return math::min(straight_line_speed, cruising_speed);
} else {
return cruising_speed;
}
return cruising_speed; // Fallthrough
}
bool AckermannPosVelControl::runSanityChecks()
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp
+4 -17
View File
@@ -58,12 +58,10 @@
#include <uORB/topics/trajectory_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/offboard_control_mode.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/pure_pursuit_status.h>
using namespace matrix;
@@ -125,11 +123,6 @@ private:
*/
void autoPositionMode();
/**
* @brief Update uORB subscriptions used for auto modes.
*/
void updateAutoSubscriptions();
/**
* @brief Update global/NED waypoint coordinates and acceptance radius.
*/
@@ -161,14 +154,14 @@ private:
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
* @param max_decel Maximum allowed deceleration [m/s^2].
* @param max_jerk Maximum allowed jerk [m/s^3].
* @param nav_state Current nav_state of the rover.
* @param curr_wp_type Type of the current waypoint.
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param prev_waypoint_transition_angle Previous angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_speed Maximum speed setpoint [m/s]
* @return Speed setpoint [m/s].
*/
float calcSpeedSetpoint(float cruising_speed, float miss_speed_min, float distance_to_prev_wp,
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int nav_state,
float distance_to_curr_wp, float acc_rad, float prev_acc_rad, float max_decel, float max_jerk, int curr_wp_type,
float waypoint_transition_angle, float prev_waypoint_transition_angle, float max_speed);
/**
@@ -185,10 +178,6 @@ private:
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _position_setpoint_triplet_sub{ORB_ID(position_setpoint_triplet)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Subscription _local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _mission_result_sub{ORB_ID(mission_result)};
uORB::Subscription _home_position_sub{ORB_ID(home_position)};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
@@ -213,13 +202,11 @@ private:
float _speed_body_x_setpoint{0.f};
float _min_speed{0.f}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
float _dt{0.f};
int _nav_state{0};
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
bool _course_control{false}; // Indicates if the rover is doing course control in manual position mode.
bool _mission_finished{false};
bool _prev_param_check_passed{true};
// Waypoint variables
Vector2d _home_position{};
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};