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

rover: auto mode stopping conditions

Browse Source
This commit is contained in:
chfriedrich98 2025-03-12 12:04:21 +01:00 committed by chfriedrich98
parent 735777862d
commit c2706f5406
8 changed files with 192 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/rover_control/RoverControl.cpp
View File

@ -215,8 +215,7 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
}
void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
MapProjection &global_ned_proj_ref)
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, MapProjection &global_ned_proj_ref)
{
if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
@ -241,8 +240,7 @@ void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned,
next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
} else {
next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN,
NAN); // Enables corner slow down with RTL
next_wp_ned = Vector2f(NAN, NAN);
}
}

3
src/lib/rover_control/RoverControl.hpp
View File

@ -118,11 +118,10 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
* @param next_wp_ned Next waypoint in NED frame (Updated by this function)
* @param position_setpoint_triplet Position Setpoint Triplet
* @param curr_pos Current position of the rover in global frame
* @param home_pos Home position in global frame
* @param global_ned_proj_ref Global to ned projection
*/
void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned,
MapProjection &global_ned_proj_ref);
/**

106
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp
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()

21
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp
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{};

124
src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.cpp
View File

@ -145,12 +145,6 @@ void DifferentialPosVelControl::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 DifferentialPosVelControl::generateAttitudeSetpoint()
@ -268,14 +262,32 @@ void DifferentialPosVelControl::offboardVelocityMode()
void DifferentialPosVelControl::autoPositionMode()
{
updateAutoSubscriptions();
if (_position_setpoint_triplet_sub.updated()) {
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, _global_ned_proj_ref);
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
// Waypoint cruising speed
_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
}
// Distances to waypoints
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();
}
// State machine
@ -287,7 +299,7 @@ void DifferentialPosVelControl::autoPositionMode()
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float heading_error = matrix::wrap_pi(yaw_setpoint - _vehicle_yaw);
if (!_mission_finished && distance_to_curr_wp > _param_nav_acc_rad.get()) {
if (!auto_stop) {
if (_currentState == GuidanceState::STOPPED) {
_currentState = GuidanceState::DRIVING;
}
@ -309,80 +321,66 @@ void DifferentialPosVelControl::autoPositionMode()
// Calculate desired speed in body x direction
_speed_body_x_setpoint = calcSpeedSetpoint(_cruising_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(),
_param_rd_miss_spd_gain.get());
_param_rd_miss_spd_gain.get(), _curr_wp_type);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} break;
case GuidanceState::SPOT_TURNING:
if (fabsf(_vehicle_speed_body_x) > 0.f) {
yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop
case GuidanceState::SPOT_TURNING: {
_speed_body_x_setpoint = 0.f;
}
if (fabsf(_vehicle_speed_body_x) > 0.f) {
yaw_setpoint = _vehicle_yaw; // Wait for the rover to stop
_speed_body_x_setpoint = 0.f;
break;
}
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} break;
case GuidanceState::STOPPED:
default:
yaw_setpoint = _vehicle_yaw;
_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);
break;
}
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
void DifferentialPosVelControl::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()) {
position_setpoint_triplet_s position_setpoint_triplet{};
_position_setpoint_triplet_sub.copy(&position_setpoint_triplet);
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_curr_pos_ned, _home_position, _global_ned_proj_ref);
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
// Waypoint cruising speed
_cruising_speed = position_setpoint_triplet.current.cruising_speed > 0.f ? math::constrain(
position_setpoint_triplet.current.cruising_speed, 0.f, _param_ro_speed_limit.get()) : _param_ro_speed_limit.get();
}
if (_mission_result_sub.updated()) {
mission_result_s mission_result{};
_mission_result_sub.copy(&mission_result);
_mission_finished = mission_result.finished;
}
}
float DifferentialPosVelControl::calcSpeedSetpoint(const float cruising_speed, const float distance_to_curr_wp,
const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
const float trans_drv_trn, const float miss_spd_gain)
const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type)
{
float speed_body_x_setpoint = cruising_speed;
if (_waypoint_transition_angle < M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON) {
speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp, 0.0f);
} else if (_waypoint_transition_angle >= M_PI_F - trans_drv_trn && max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON
&& miss_spd_gain > FLT_EPSILON) {
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
speed_body_x_setpoint = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp,
max_speed * (1.f - speed_reduction));
// Upcoming stop
if (max_decel > FLT_EPSILON && max_jerk > FLT_EPSILON && (!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_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);
}
return math::constrain(speed_body_x_setpoint, -cruising_speed, cruising_speed);
// Straight line speed
if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) {
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - _waypoint_transition_angle,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
const float straight_line_speed = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
distance_to_curr_wp,
max_speed * (1.f - speed_reduction));
return math::min(straight_line_speed, cruising_speed);
}
return cruising_speed; // Fallthrough
}

22
src/modules/rover_differential/DifferentialPosVelControl/DifferentialPosVelControl.hpp
View File

@ -59,12 +59,9 @@
#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>
using namespace matrix;
@ -134,11 +131,6 @@ private:
*/
void autoPositionMode();
/**
* @brief Update uORB subscriptions used for auto modes.
*/
void updateAutoSubscriptions();
/**
* @brief Calculate the speed setpoint. During waypoint transition the speed is restricted to
* Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
@ -152,10 +144,11 @@ private:
* @param max_speed Maximum speed setpoint [m/s]
* @param trans_drv_trn Heading error threshold to switch from driving to turning [rad].
* @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
* @param curr_wp_type Type of the current waypoint.
* @return Speed setpoint [m/s].
*/
float calcSpeedSetpoint(float cruising_speed, float distance_to_curr_wp, float max_decel, float max_jerk,
float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain);
float waypoint_transition_angle, float max_speed, float trans_drv_trn, float miss_spd_gain, int curr_wp_type);
/**
@ -172,10 +165,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)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
@ -186,7 +175,6 @@ private:
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
@ -201,12 +189,10 @@ private:
float _max_yaw_rate{0.f};
float _speed_body_x_setpoint{0.f};
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};
// Waypoint variables
Vector2d _home_position{};
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};

127
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp
View File

@ -165,12 +165,6 @@ void MecanumPosVelControl::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 MecanumPosVelControl::generateAttitudeSetpoint()
@ -313,48 +307,13 @@ void MecanumPosVelControl::offboardVelocityMode()
void MecanumPosVelControl::autoPositionMode()
{
updateAutoSubscriptions();
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();
}
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
_nav_state);
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
velocity_magnitude);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
Vector2f desired_velocity(0.f, 0.f);
_speed_body_x_setpoint = _mission_finished ? 0.f : velocity_magnitude * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = _mission_finished ? 0.f : velocity_magnitude * sinf(bearing_setpoint_body_frame);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _auto_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
void MecanumPosVelControl::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()) {
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);
_waypoint_transition_angle = RoverControl::calcWaypointTransitionAngle(_prev_wp_ned, _curr_wp_ned, _next_wp_ned);
@ -371,38 +330,72 @@ void MecanumPosVelControl::updateAutoSubscriptions()
}
}
if (_mission_result_sub.updated()) {
mission_result_s mission_result{};
_mission_result_sub.copy(&mission_result);
_mission_finished = mission_result.finished;
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));
// 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 (auto_stop) {
_speed_body_x_setpoint = 0.f;
_speed_body_y_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
const float velocity_magnitude = calcVelocityMagnitude(_auto_speed, distance_to_curr_wp, _param_ro_decel_limit.get(),
_param_ro_jerk_limit.get(), _waypoint_transition_angle, _param_ro_speed_limit.get(), _param_rm_miss_spd_gain.get(),
_curr_wp_type);
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float bearing_setpoint = PurePursuit::calcTargetBearing(pure_pursuit_status, _param_pp_lookahd_gain.get(),
_param_pp_lookahd_max.get(), _param_pp_lookahd_min.get(), _curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
velocity_magnitude);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
Vector2f desired_velocity(0.f, 0.f);
_speed_body_x_setpoint = velocity_magnitude * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude * sinf(bearing_setpoint_body_frame);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _auto_yaw;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
float MecanumPosVelControl::calcVelocityMagnitude(const float auto_speed, const float distance_to_curr_wp,
const float max_decel, const float max_jerk, const float waypoint_transition_angle, const float max_speed,
const float miss_spd_gain, const int nav_state)
const float miss_spd_gain, const int curr_wp_type)
{
float velocity_magnitude{auto_speed};
if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON
&& miss_spd_gain > FLT_EPSILON) {
float max_velocity_magnitude = velocity_magnitude;
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
max_decel, distance_to_curr_wp, 0.f);
} else if (PX4_ISFINITE(waypoint_transition_angle)) {
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
M_PI_F, 0.f, 1.f), 0.f, 1.f);
max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel, distance_to_curr_wp,
max_speed * (1.f - speed_reduction));
}
velocity_magnitude = math::constrain(max_velocity_magnitude, -auto_speed, auto_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 max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk,
max_decel, distance_to_curr_wp, 0.f);
return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
}
return velocity_magnitude;
// Straight line speed
if (max_jerk > FLT_EPSILON && max_decel > FLT_EPSILON && miss_spd_gain > FLT_EPSILON) {
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
M_PI_F, 0.f, 1.f), 0.f, 1.f);
const float max_velocity_magnitude = math::trajectory::computeMaxSpeedFromDistance(max_jerk, max_decel,
distance_to_curr_wp,
max_speed * (1.f - speed_reduction));
return math::constrain(max_velocity_magnitude, -auto_speed, auto_speed);
}
return auto_speed; // Fallthrough
}
bool MecanumPosVelControl::runSanityChecks()

34
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp
View File

@ -59,12 +59,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 Calculate the velocity magnitude setpoint. During waypoint transition the speed is restricted to
* Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN).
@ -142,11 +135,11 @@ private:
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_speed Maximum velocity magnitude setpoint [m/s]
* @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
* @param nav_state Vehicle navigation state
* @param curr_wp_type Type of the current waypoint.
* @return Velocity magnitude setpoint [m/s].
*/
float calcVelocityMagnitude(float auto_speed, float distance_to_curr_wp, float max_decel, float max_jerk,
float waypoint_transition_angle, float max_speed, float miss_spd_gain, int nav_state);
float waypoint_transition_angle, float max_speed, float miss_spd_gain, int curr_wp_type);
/**
* @brief Check if the necessary parameters are set.
@ -162,10 +155,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)};
uORB::Subscription _rover_steering_setpoint_sub{ORB_ID(rover_steering_setpoint)};
vehicle_control_mode_s _vehicle_control_mode{};
offboard_control_mode_s _offboard_control_mode{};
@ -176,19 +165,14 @@ private:
uORB::Publication<rover_throttle_setpoint_s> _rover_throttle_setpoint_pub{ORB_ID(rover_throttle_setpoint)};
uORB::Publication<rover_attitude_setpoint_s> _rover_attitude_setpoint_pub{ORB_ID(rover_attitude_setpoint)};
uORB::Publication<rover_velocity_status_s> _rover_velocity_status_pub{ORB_ID(rover_velocity_status)};
uORB::Publication<position_controller_status_s> _position_controller_status_pub{ORB_ID(position_controller_status)};
uORB::Publication<pure_pursuit_status_s> _pure_pursuit_status_pub{ORB_ID(pure_pursuit_status)};
// Variables
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
Vector2d _home_position{};
Vector2f _curr_pos_ned{};
Vector2f _pos_ctl_course_direction{};
Vector2f _pos_ctl_start_position_ned{};
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};
float _vehicle_speed_body_x{0.f};
float _vehicle_speed_body_y{0.f};
float _vehicle_yaw{0.f};
@ -199,11 +183,15 @@ private:
float _dt{0.f};
float _auto_speed{0.f};
float _auto_yaw{0.f};
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
int _nav_state{0};
bool _mission_finished{false};
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
bool _prev_param_check_passed{true};
// Waypoint variables
Vector2f _curr_wp_ned{};
Vector2f _prev_wp_ned{};
Vector2f _next_wp_ned{};
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
// Controllers
PID _pid_speed_x;
PID _pid_speed_y;