fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 18:13:35 +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_ackermann/AckermannPosVelControl/AckermannPosVelControl.cpp
+22 -7
View File
@@ -41,6 +41,7 @@ AckermannPosVelControl::AckermannPosVelControl(ModuleParams *parent) : ModulePar
_rover_throttle_setpoint_pub.advertise();
_rover_attitude_setpoint_pub.advertise();
_rover_velocity_status_pub.advertise();
_pure_pursuit_status_pub.advertise();
updateParams();
}
@@ -187,12 +188,16 @@ void AckermannPosVelControl::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 + sign(_speed_body_x_setpoint) *
vector_scaling * _pos_ctl_course_direction;
float yaw_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;
float yaw_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, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
yaw_setpoint = _speed_body_x_setpoint > FLT_EPSILON ? yaw_setpoint : matrix::wrap_pi(yaw_setpoint + M_PI_F);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
@@ -215,10 +220,15 @@ void AckermannPosVelControl::offboardPositionMode()
const float speed_setpoint = math::trajectory::computeMaxSpeedFromDistance(_param_ro_jerk_limit.get(),
_param_ro_decel_limit.get(), distance_to_target, 0.f);
_speed_body_x_setpoint = math::min(speed_setpoint, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_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, fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned, _curr_pos_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
} else {
@@ -264,10 +274,15 @@ void AckermannPosVelControl::autoPositionMode()
_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,
_waypoint_transition_angle, _prev_waypoint_transition_angle, _param_ro_speed_limit.get());
pure_pursuit_status_s pure_pursuit_status{};
pure_pursuit_status.timestamp = _timestamp;
const float yaw_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,
fabsf(_speed_body_x_setpoint));
_pure_pursuit_status_pub.publish(pure_pursuit_status);
rover_attitude_setpoint_s rover_attitude_setpoint{};
rover_attitude_setpoint.timestamp = _timestamp;
rover_attitude_setpoint.yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(_curr_wp_ned, _prev_wp_ned,
_curr_pos_ned, fabsf(_speed_body_x_setpoint));
rover_attitude_setpoint.yaw_setpoint = yaw_setpoint;
_rover_attitude_setpoint_pub.publish(rover_attitude_setpoint);
}
}
src/modules/rover_ackermann/AckermannPosVelControl/AckermannPosVelControl.hpp
+8 -5
View File
@@ -64,6 +64,7 @@
#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;
@@ -192,11 +193,12 @@ private:
offboard_control_mode_s _offboard_control_mode{};
// uORB publications
uORB::Publication<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
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<rover_rate_setpoint_s> _rover_rate_setpoint_pub{ORB_ID(rover_rate_setpoint)};
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};
@@ -232,7 +234,6 @@ private:
SlewRate<float> _speed_setpoint;
// Class Instances
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates
DEFINE_PARAMETERS(
@@ -245,7 +246,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::RA_ACC_RAD_MAX>) _param_ra_acc_rad_max,
(ParamFloat<px4::params::RA_ACC_RAD_GAIN>) _param_ra_acc_rad_gain,
src/modules/rover_ackermann/CMakeLists.txt
+1
View File
@@ -47,6 +47,7 @@ px4_add_module(
AckermannPosVelControl
px4_work_queue
rover_control
pure_pursuit
MODULE_CONFIG
module.yaml
)