fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-29 02:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

pure_pursuit: update library

Browse Source
This commit is contained in:
chfriedrich98
2025-03-10 11:14:00 +01:00
committed by chfriedrich98
parent fec240efba
commit d1b0be18b2
16 changed files with 277 additions and 189 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_mecanum/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
MecanumPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.cpp
+19 -6
View File
@@ -41,6 +41,7 @@ MecanumPosVelControl::MecanumPosVelControl(ModuleParams *parent) : ModuleParams(
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -235,11 +236,15 @@ void MecanumPosVelControl::manualPositionMode()
}
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
const Vector2f start_to_curr_pos = _curr_pos_ned - _pos_ctl_start_position_ned;
const float vector_scaling = fabsf(start_to_curr_pos * _pos_ctl_course_direction) + _param_pp_lookahd_max.get();
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + vector_scaling * _pos_ctl_course_direction;
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _pos_ctl_start_position_ned,
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(), target_waypoint_ned, _pos_ctl_start_position_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
@@ -271,8 +276,12 @@ void MecanumPosVelControl::offboardPositionMode()
const float velocity_magnitude_setpoint = math::min(math::trajectory::computeMaxSpeedFromDistance(
_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f), _param_ro_speed_limit.get());
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
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(), target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, velocity_magnitude_setpoint);
_pure_pursuit_status_pub.publish(pure_pursuit_status);
const float bearing_setpoint_body_frame = matrix::wrap_pi(bearing_setpoint - _vehicle_yaw);
_speed_body_x_setpoint = velocity_magnitude_setpoint * cosf(bearing_setpoint_body_frame);
_speed_body_y_setpoint = velocity_magnitude_setpoint * sinf(bearing_setpoint_body_frame);
@@ -316,8 +325,12 @@ void MecanumPosVelControl::autoPositionMode()
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);
const float bearing_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned,
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);
src/modules/rover_mecanum/MecanumPosVelControl/MecanumPosVelControl.hpp
+3 -1
View File
@@ -177,6 +177,7 @@ private:
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};
@@ -210,7 +211,6 @@ private:
SlewRate<float> _speed_y_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
@@ -225,7 +225,9 @@ 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,
(ParamFloat<px4::params::PP_LOOKAHD_GAIN>) _param_pp_lookahd_gain,
(ParamFloat<px4::params::PP_LOOKAHD_MAX>) _param_pp_lookahd_max,
(ParamFloat<px4::params::PP_LOOKAHD_MIN>) _param_pp_lookahd_min,
(ParamFloat<px4::params::RO_YAW_RATE_LIM>) _param_ro_yaw_rate_limit,
(ParamFloat<px4::params::NAV_ACC_RAD>) _param_nav_acc_rad