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

rover: reduce speed based on course error

Browse Source
This commit is contained in:
chfriedrich98
2025-06-18 08:35:48 +02:00
committed by chfriedrich98
parent 5a430f0ba6
commit eed966a1c6
21 changed files with 123 additions and 172 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover
+2 -1
View File
@@ -35,10 +35,11 @@ param set-default RO_YAW_DECEL_LIM 1000
# Rover Attitude Control Parameters
param set-default RO_YAW_P 5
# Rover Position Control Parameters
# Rover Velocity Control Parameters
param set-default RO_SPEED_LIM 2
param set-default RO_SPEED_I 0.01
param set-default RO_SPEED_P 0.1
param set-defatul RO_SPEED_RED 1
# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann
+2 -1
View File
@@ -34,10 +34,11 @@ param set-default RO_YAW_RATE_LIM 180
# Rover Attitude Control Parameters
param set-default RO_YAW_P 3
# Rover Position Control Parameters
# Rover Velocity Control Parameters
param set-default RO_SPEED_LIM 3
param set-default RO_SPEED_I 0.1
param set-default RO_SPEED_P 1
param set-default RO_SPEED_RED 1
# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum
+2 -2
View File
@@ -16,7 +16,6 @@ param set-default NAV_ACC_RAD 0.5
# Mecanum Parameters
param set-default RM_WHEEL_TRACK 0.3
param set-default RM_MAX_THR_YAW_R 1.2
param set-default RM_MISS_SPD_GAIN 1
# Rover Control Parameters
param set-default RO_ACCEL_LIM 3
@@ -34,10 +33,11 @@ param set-default RO_YAW_DECEL_LIM 1000
# Rover Attitude Control Parameters
param set-default RO_YAW_P 5
# Rover Position Control Parameters
# Rover Velocity Control Parameters
param set-default RO_SPEED_LIM 2
param set-default RO_SPEED_I 0.5
param set-default RO_SPEED_P 1
param set-default RO_SPEED_RED 1
# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 0.5
ROMFS/px4fmu_common/init.d/airframes/50001_aion_robotics_r1_rover
+2 -1
View File
@@ -46,10 +46,11 @@ param set-default RO_YAW_DECEL_LIM 600
# Rover Attitude Control Parameters
param set-default RO_YAW_P 2.5
# Rover Position Control Parameters
# Rover Velocity Control Parameters
param set-default RO_SPEED_LIM 1.6
param set-default RO_SPEED_I 0.01
param set-default RO_SPEED_P 0.1
param set-default RO_SPEED_RED 1
# Pure Pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
ROMFS/px4fmu_common/init.d/airframes/51001_axial_scx10_2_trail_honcho
+2 -1
View File
@@ -37,10 +37,11 @@ param set-default RO_YAW_RATE_LIM 120
# Rover Attitude Control Parameters
param set-default RO_YAW_P 2.5
# Rover Position Control Parameters
# Rover Velocity Control Parameters
param set-default RO_SPEED_LIM 2.5
param set-default RO_SPEED_I 0.01
param set-default RO_SPEED_P 0.1
param set-default RO_SPEED_RED 1
# Pure pursuit parameters
param set-default PP_LOOKAHD_GAIN 1
src/lib/rover_control/rovercontrol_params.c
+18
View File
@@ -253,3 +253,21 @@ PARAM_DEFINE_FLOAT(RO_JERK_LIM, -1.f);
* @group Rover Velocity Control
*/
PARAM_DEFINE_FLOAT(RO_SPEED_TH, 0.1f);
/**
* Tuning parameter for the speed reduction based on the course error
*
* Reduced_speed = RO_MAX_THR_SPEED * (1 - normalized_course_error * RO_SPEED_RED)
* The normalized course error is the angle between the current course and the bearing setpoint
* interpolated from [0, 180] -> [0, 1].
* Higher value -> More speed reduction.
* Note: This is also used to calculate the speed at which the vehicle arrives at a waypoint in auto modes.
* Set to -1 to disable bearing error based speed reduction.
*
* @min -1
* @max 100
* @increment 0.01
* @decimal 2
* @group Rover Velocity Control
*/
PARAM_DEFINE_FLOAT(RO_SPEED_RED, -1.f);
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.cpp
+12 -2
View File
@@ -46,6 +46,8 @@ AckermannVelControl::AckermannVelControl(ModuleParams *parent) : ModuleParams(pa
void AckermannVelControl::updateParams()
{
ModuleParams::updateParams();
_max_yaw_rate = _param_ro_yaw_rate_limit.get() * M_DEG_TO_RAD_F;
_min_speed = _param_ra_wheel_base.get() * _max_yaw_rate / tanf(_param_ra_max_str_ang.get());
// Set up PID controller
_pid_speed.setGains(_param_ro_speed_p.get(), _param_ro_speed_i.get(), 0.f);
@@ -57,6 +59,7 @@ void AckermannVelControl::updateParams()
_adjusted_speed_setpoint.setSlewRate(_param_ro_accel_limit.get());
}
}
void AckermannVelControl::updateVelControl()
@@ -66,6 +69,7 @@ void AckermannVelControl::updateVelControl()
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
float max_speed = _param_ro_speed_limit.get();
// Attitude Setpoint
if (PX4_ISFINITE(_bearing_setpoint)) {
@@ -73,12 +77,18 @@ void AckermannVelControl::updateVelControl()
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float course_error = fabsf(matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw));
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), _min_speed, max_speed);
}
}
// Throttle Setpoint
if (PX4_ISFINITE(_speed_setpoint)) {
const float speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
const float speed_setpoint = math::constrain(_speed_setpoint, -max_speed, max_speed);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed,
src/modules/rover_ackermann/AckermannVelControl/AckermannVelControl.hpp
+8 -2
View File
@@ -112,9 +112,11 @@ private:
hrt_abstime _timestamp{0};
Quatf _vehicle_attitude_quaternion{};
float _vehicle_speed{0.f}; // [m/s] Positiv: Forwards, Negativ: Backwards
float _vehicle_yaw{0.f};
float _vehicle_yaw{0.f}; // [rad] Yaw angle of the vehicle
float _speed_setpoint{NAN};
float _bearing_setpoint{NAN};
float _min_speed{NAN};
float _max_yaw_rate{NAN};
// Controllers
PID _pid_speed;
@@ -128,6 +130,10 @@ private:
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RA_WHEEL_BASE>) _param_ra_wheel_base,
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang
)
};
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.cpp
+29 -70
View File
@@ -54,49 +54,35 @@ void AutoMode::updateParams()
void AutoMode::autoControl()
{
if (_vehicle_attitude_sub.updated()) {
vehicle_attitude_s vehicle_attitude{};
_vehicle_attitude_sub.copy(&vehicle_attitude);
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
}
if (_position_setpoint_triplet_sub.updated()) {
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
if (_vehicle_local_position_sub.updated()) {
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_sub.copy(&vehicle_local_position);
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
vehicle_local_position.ref_timestamp);
}
if (!_global_ned_proj_ref.isInitialized()
|| (_global_ned_proj_ref.getProjectionReferenceTimestamp() != vehicle_local_position.ref_timestamp)) {
_global_ned_proj_ref.initReference(vehicle_local_position.ref_lat, vehicle_local_position.ref_lon,
vehicle_local_position.ref_timestamp);
_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 (_position_setpoint_triplet_sub.updated()) {
updateWaypointsAndAcceptanceRadius();
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
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, _min_speed, _acceptance_radius, _curr_wp_type,
_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = _cruising_speed;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
// Distances to waypoints
const float distance_to_prev_wp = sqrt(powf(_curr_pos_ned(0) - _prev_wp_ned(0),
2) + powf(_curr_pos_ned(1) - _prev_wp_ned(1), 2));
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));
rover_position_setpoint_s rover_position_setpoint{};
rover_position_setpoint.timestamp = hrt_absolute_time();
rover_position_setpoint.position_ned[0] = _curr_wp_ned(0);
rover_position_setpoint.position_ned[1] = _curr_wp_ned(1);
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, _min_speed, _acceptance_radius, _curr_wp_type,
_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.cruising_speed = cruisingSpeed(_cruising_speed, _min_speed, distance_to_prev_wp,
distance_to_curr_wp, _acceptance_radius, _prev_acceptance_radius, _waypoint_transition_angle,
_prev_waypoint_transition_angle, _max_yaw_rate);
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
}
void AutoMode::updateWaypointsAndAcceptanceRadius()
@@ -108,12 +94,9 @@ void AutoMode::updateWaypointsAndAcceptanceRadius()
RoverControl::globalToLocalSetpointTriplet(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, position_setpoint_triplet,
_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);
// Update acceptance radius
_prev_acceptance_radius = _acceptance_radius;
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
_acceptance_radius = updateAcceptanceRadius(_waypoint_transition_angle, _param_nav_acc_rad.get(),
_param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_str_ang.get());
@@ -152,7 +135,7 @@ float AutoMode::updateAcceptanceRadius(const float waypoint_transition_angle,
return acceptance_radius;
}
float AutoMode::arrivalSpeed(const float cruising_speed, const float miss_speed_min, const float acc_rad,
float AutoMode::arrivalSpeed(const float cruising_speed, const float min_speed, const float acc_rad,
const int curr_wp_type, const float waypoint_transition_angle, const float max_yaw_rate)
{
if (!PX4_ISFINITE(waypoint_transition_angle)
@@ -160,37 +143,13 @@ float AutoMode::arrivalSpeed(const float cruising_speed, const float miss_speed_
|| curr_wp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
return 0.f; // Stop at the waypoint
} else {
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); // Slow down for cornering
}
}
float AutoMode::cruisingSpeed(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 waypoint_transition_angle, const float prev_waypoint_transition_angle, const float max_yaw_rate)
{
// Catch improper values
if (miss_speed_min < -FLT_EPSILON || miss_speed_min > cruising_speed) {
return cruising_speed;
}
// Cornering slow down effect
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);
}
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);
} else if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(
M_PI_F - waypoint_transition_angle,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
return math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), min_speed,
cruising_speed); // Slow down for cornering
}
return cruising_speed; // Fallthrough
}
src/modules/rover_ackermann/DriveModes/AutoMode/AutoMode.hpp
+5 -26
View File
@@ -43,7 +43,6 @@
// uORB includes
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_controller_status.h>
@@ -96,35 +95,17 @@ private:
/**
* @brief Calculate the speed at which the rover should arrive at the current waypoint based on the upcoming corner.
* @param cruising_speed Cruising speed [m/s].
* @param miss_speed_min Minimum speed setpoint [m/s].
* @param min_speed Minimum speed setpoint [m/s].
* @param acc_rad Acceptance radius of the current waypoint [m].
* @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 max_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float arrivalSpeed(float cruising_speed, float miss_speed_min, float acc_rad, int curr_wp_type,
float arrivalSpeed(float cruising_speed, float min_speed, float acc_rad, int curr_wp_type,
float waypoint_transition_angle, float max_yaw_rate);
/**
* @brief Calculate the cruising speed setpoint. During cornering the speed is restricted based on the radius of the corner.
* @param cruising_speed Cruising speed [m/s].
* @param miss_speed_min Minimum speed setpoint [m/s].
* @param distance_to_prev_wp Distance to the previous waypoint [m].
* @param distance_to_curr_wp Distance to the current waypoint [m].
* @param acc_rad Acceptance radius of the current waypoint [m].
* @param prev_acc_rad Acceptance radius of the previous waypoint [m].
* @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_yaw_rate Maximum yaw rate setpoint [rad/s]
* @return Speed setpoint [m/s].
*/
float cruisingSpeed(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 waypoint_transition_angle,
float prev_waypoint_transition_angle, float max_yaw_rate);
// uORB subscriptions
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)};
@@ -134,18 +115,14 @@ private:
// Variables
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
Quatf _vehicle_attitude_quaternion{};
Vector2f _curr_wp_ned{NAN, NAN};
Vector2f _prev_wp_ned{NAN, NAN};
Vector2f _next_wp_ned{NAN, NAN};
Vector2f _curr_pos_ned{NAN, NAN};
float _acceptance_radius{0.5f};
float _prev_acceptance_radius{0.5f};
float _cruising_speed{0.f};
float _waypoint_transition_angle{0.f}; // Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
float _prev_waypoint_transition_angle{0.f}; // Previous Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
float _max_yaw_rate{NAN};
float _vehicle_yaw{NAN};
float _min_speed{NAN}; // Speed at which the maximum yaw rate limit is enforced given the maximum steer angle and wheel base.
int _curr_wp_type{position_setpoint_s::SETPOINT_TYPE_IDLE};
@@ -156,6 +133,8 @@ private:
(ParamFloat<px4::params::RA_MAX_STR_ANG>) _param_ra_max_str_ang,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad,
(ParamFloat<px4::params::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed
)
};
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.cpp
+4 -4
View File
@@ -85,7 +85,7 @@ void DifferentialAutoMode::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_rd_trans_drv_trn.get(), _param_rd_miss_spd_gain.get(), curr_wp_type);
_param_ro_speed_limit.get(), _param_rd_trans_drv_trn.get(), _param_ro_speed_red.get(), curr_wp_type);
rover_position_setpoint.cruising_speed = cruising_speed;
rover_position_setpoint.yaw = NAN;
_rover_position_setpoint_pub.publish(rover_position_setpoint);
@@ -93,7 +93,7 @@ void DifferentialAutoMode::autoControl()
}
float DifferentialAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
const float max_speed, const float trans_drv_trn, const float miss_spd_gain, int curr_wp_type)
const float max_speed, const float trans_drv_trn, const float speed_red, int curr_wp_type)
{
// Upcoming stop
if (!PX4_ISFINITE(waypoint_transition_angle) || waypoint_transition_angle < M_PI_F - trans_drv_trn
@@ -102,8 +102,8 @@ float DifferentialAutoMode::arrivalSpeed(const float cruising_speed, const float
}
// Straight line speed
if (miss_spd_gain > FLT_EPSILON) {
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle,
if (speed_red > FLT_EPSILON) {
const float speed_reduction = math::constrain(speed_red * math::interpolate(M_PI_F - waypoint_transition_angle,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
return max_speed * (1.f - speed_reduction);
}
src/modules/rover_differential/DifferentialDriveModes/DifferentialAutoMode/DifferentialAutoMode.hpp
+4 -4
View File
@@ -79,12 +79,12 @@ private:
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @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 speed_red Tuning parameter for the speed reduction during waypoint transition.
* @param curr_wp_type Type 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 trans_drv_trn, const float miss_spd_gain, int curr_wp_type);
const float trans_drv_trn, const float speed_red, int curr_wp_type);
// uORB subscriptions
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -95,7 +95,7 @@ private:
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn,
(ParamFloat<px4::params::RD_MISS_SPD_GAIN>) _param_rd_miss_spd_gain
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red,
(ParamFloat<px4::params::RD_TRANS_DRV_TRN>) _param_rd_trans_drv_trn
)
};
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.cpp
+13 -5
View File
@@ -60,11 +60,13 @@ void DifferentialVelControl::updateParams()
void DifferentialVelControl::updateVelControl()
{
updateSubscriptions();
const hrt_abstime timestamp_prev = _timestamp;
_timestamp = hrt_absolute_time();
const float dt = math::constrain(_timestamp - timestamp_prev, 1_ms, 5000_ms) * 1e-6f;
float max_speed = _param_ro_speed_limit.get();
updateSubscriptions();
// Attitude Setpoint
if (PX4_ISFINITE(_bearing_setpoint)) {
@@ -72,11 +74,18 @@ void DifferentialVelControl::updateVelControl()
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _bearing_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
if (_param_ro_speed_red.get() > FLT_EPSILON) {
const float course_error = fabsf(matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw));
const float speed_reduction = math::constrain(_param_ro_speed_red.get() * math::interpolate(course_error,
0.f, M_PI_F, 0.f, 1.f), 0.f, 1.f);
max_speed = math::constrain(_param_ro_max_thr_speed.get() * (1.f - speed_reduction), 0.f, max_speed);
}
}
// Throttle Setpoint
if (PX4_ISFINITE(_speed_setpoint)) {
const float speed_setpoint = calcSpeedSetpoint();
const float speed_setpoint = calcSpeedSetpoint(max_speed);
rover_throttle_setpoint_s rover_throttle_setpoint{};
rover_throttle_setpoint.timestamp = _timestamp;
rover_throttle_setpoint.throttle_body_x = RoverControl::speedControl(_adjusted_speed_setpoint, _pid_speed,
@@ -126,7 +135,7 @@ void DifferentialVelControl::updateSubscriptions()
}
float DifferentialVelControl::calcSpeedSetpoint()
float DifferentialVelControl::calcSpeedSetpoint(const float max_speed)
{
const float heading_error = matrix::wrap_pi(_bearing_setpoint - _vehicle_yaw);
@@ -140,8 +149,7 @@ float DifferentialVelControl::calcSpeedSetpoint()
float speed_setpoint = 0.f;
if (_current_state == DrivingState::DRIVING) {
speed_setpoint = math::constrain(_speed_setpoint, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
speed_setpoint = math::constrain(_speed_setpoint, -max_speed, max_speed);
const float speed_setpoint_normalized = math::interpolate<float>(speed_setpoint,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
src/modules/rover_differential/DifferentialVelControl/DifferentialVelControl.hpp
+4 -3
View File
@@ -108,9 +108,10 @@ private:
/**
* @brief Calculate the speed setpoint based on the current state.
* @param max_speed Maximum speed limit [m/s].
* @return Speed setpoint.
*/
float calcSpeedSetpoint();
float calcSpeedSetpoint(float max_speed);
// uORB subscriptions
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
@@ -147,7 +148,7 @@ private:
(ParamFloat<px4::params::RO_DECEL_LIM>) _param_ro_decel_limit,
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red
)
};
src/modules/rover_differential/module.yaml
-17
View File
@@ -61,20 +61,3 @@ parameters:
increment: 0.01
decimal: 3
default: 0.174533
RD_MISS_SPD_GAIN:
description:
short: Tuning parameter for the speed reduction during waypoint transition
long: |
The waypoint transition speed is calculated as:
Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
The normalized transition angle is the angle between the line segment from prev-curr WP and curr-next WP
interpolated from [0, 180] -> [0, 1].
Higher value -> More speed reduction during waypoint transitions.
Set to -1 to disable any speed reduction during waypoint transition.
type: float
min: -1
max: 100
increment: 0.01
decimal: 2
default: -1
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.cpp
+4 -4
View File
@@ -85,7 +85,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_rm_miss_spd_gain.get(), curr_wp_type);
_param_ro_speed_limit.get(), _param_ro_speed_red.get(), curr_wp_type);
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,7 +94,7 @@ void MecanumAutoMode::autoControl()
}
float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float waypoint_transition_angle,
const float max_speed, const float miss_spd_gain, int curr_wp_type)
const float max_speed, const float speed_red, int curr_wp_type)
{
// Upcoming stop
if (!PX4_ISFINITE(waypoint_transition_angle) || curr_wp_type == position_setpoint_s::SETPOINT_TYPE_LAND
@@ -103,8 +103,8 @@ float MecanumAutoMode::arrivalSpeed(const float cruising_speed, const float wayp
}
// Straight line speed
if (miss_spd_gain > FLT_EPSILON) {
const float speed_reduction = math::constrain(miss_spd_gain * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
if (speed_red > FLT_EPSILON) {
const float speed_reduction = math::constrain(speed_red * math::interpolate(M_PI_F - waypoint_transition_angle, 0.f,
M_PI_F, 0.f, 1.f), 0.f, 1.f);
return max_speed * (1.f - speed_reduction);
}
src/modules/rover_mecanum/MecanumDriveModes/MecanumAutoMode/MecanumAutoMode.hpp
+4 -4
View File
@@ -78,12 +78,12 @@ private:
* @param cruising_speed Cruising speed [m/s].
* @param waypoint_transition_angle Angle between the prevWP-currWP and currWP-nextWP line segments [rad]
* @param max_speed Maximum speed setpoint [m/s]
* @param miss_spd_gain Tuning parameter for the speed reduction during waypoint transition.
* @param speed_red Tuning parameter for the speed reduction during waypoint transition.
* @param curr_wp_type Type 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 miss_spd_gain, int curr_wp_type);
const float speed_red, int curr_wp_type);
// uORB subscriptions
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
@@ -93,7 +93,7 @@ private:
uORB::Publication<rover_position_setpoint_s> _rover_position_setpoint_pub{ORB_ID(rover_position_setpoint)};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RM_MISS_SPD_GAIN>) _param_rm_miss_spd_gain
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_RED>) _param_ro_speed_red
)
};
src/modules/rover_mecanum/MecanumPosControl/MecanumPosControl.hpp
-2
View File
@@ -113,7 +113,6 @@ private:
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RM_MISS_SPD_GAIN>) _param_rm_miss_spd_gain,
(ParamFloat<px4::params::RM_COURSE_CTL_TH>) _param_rm_course_ctl_th,
(ParamFloat<px4::params::RO_MAX_THR_SPEED>) _param_ro_max_thr_speed,
(ParamFloat<px4::params::RO_SPEED_P>) _param_ro_speed_p,
@@ -130,6 +129,5 @@ private:
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::RO_YAW_P>) _param_ro_yaw_p,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad
)
};
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.cpp
+8 -5
View File
@@ -129,15 +129,18 @@ void MecanumVelControl::updateSubscriptions()
rover_velocity_setpoint_s rover_velocity_setpoint;
_rover_velocity_setpoint_sub.copy(&rover_velocity_setpoint);
const float speed_setpoint = math::constrain(rover_velocity_setpoint.speed, -_param_ro_speed_limit.get(),
_param_ro_speed_limit.get());
if (PX4_ISFINITE(rover_velocity_setpoint.speed) && PX4_ISFINITE(rover_velocity_setpoint.bearing)) {
const Vector3f velocity_in_local_frame(rover_velocity_setpoint.speed * cosf(rover_velocity_setpoint.bearing),
rover_velocity_setpoint.speed * sinf(rover_velocity_setpoint.bearing), 0.f);
const Vector3f velocity_in_local_frame(speed_setpoint * cosf(rover_velocity_setpoint.bearing),
speed_setpoint * sinf(rover_velocity_setpoint.bearing), 0.f);
const Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
_speed_x_setpoint = velocity_in_body_frame(0);
_speed_y_setpoint = velocity_in_body_frame(1);
} else if (PX4_ISFINITE(rover_velocity_setpoint.speed)) {
_speed_x_setpoint = rover_velocity_setpoint.speed;
_speed_x_setpoint = speed_setpoint;
_speed_y_setpoint = 0.f;
} else {
@@ -157,6 +160,8 @@ Vector2f MecanumVelControl::calcSpeedSetpoint()
_normalized_speed_diff = rover_steering_setpoint.normalized_steering_setpoint;
}
Vector2f speed_setpoint = Vector2f(_speed_x_setpoint, _speed_y_setpoint);
float speed_x_setpoint_normalized = math::interpolate<float>(_speed_x_setpoint,
-_param_ro_max_thr_speed.get(), _param_ro_max_thr_speed.get(), -1.f, 1.f);
@@ -166,8 +171,6 @@ Vector2f MecanumVelControl::calcSpeedSetpoint()
const float total_speed = fabsf(speed_x_setpoint_normalized) + fabsf(speed_y_setpoint_normalized) + fabsf(
_normalized_speed_diff);
Vector2f speed_setpoint = Vector2f(_speed_x_setpoint, _speed_y_setpoint);
if (total_speed > 1.f) {
const float theta = atan2f(fabsf(speed_y_setpoint_normalized), fabsf(speed_x_setpoint_normalized));
const float magnitude = (1.f - fabsf(_normalized_speed_diff)) / (sinf(theta) + cosf(theta));
src/modules/rover_mecanum/MecanumVelControl/MecanumVelControl.hpp
-1
View File
@@ -141,6 +141,5 @@ private:
(ParamFloat<px4::params::RO_JERK_LIM>) _param_ro_jerk_limit,
(ParamFloat<px4::params::RO_SPEED_LIM>) _param_ro_speed_limit,
(ParamFloat<px4::params::RO_SPEED_TH>) _param_ro_speed_th
)
};
src/modules/rover_mecanum/module.yaml
-17
View File
@@ -32,23 +32,6 @@ parameters:
decimal: 2
default: 0
RM_MISS_SPD_GAIN:
description:
short: Tuning parameter for the speed reduction during waypoint transition
long: |
The waypoint transition speed is calculated as:
Transition_speed = Maximum_speed * (1 - normalized_transition_angle * RM_MISS_VEL_GAIN)
The normalized transition angle is the angle between the line segment from prev-curr waypoint and
curr-next waypoint interpolated from [0, 180] -> [0, 1].
Higher value -> More speed reduction during waypoint transitions.
Set to -1 to disable any speed reduction during waypoint transition.
type: float
min: -1
max: 100
increment: 0.01
decimal: 2
default: -1
RM_COURSE_CTL_TH:
description:
short: Threshold to update course control in manual position mode