fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 11:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ackermann: added delay comand support (#23445)

Browse Source
This commit is contained in:
sbtjagu
2024-07-25 17:13:30 +02:00
committed by GitHub
parent b76c1c97b3
commit 85b6b0a406
2 changed files with 39 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
+38 -31
View File
@@ -125,47 +125,54 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(c
_curr_wp(0),
_curr_wp(1));
if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get()
&& _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect
if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius;
desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
if (distance_to_curr_wp > _acceptance_radius) {
if (_param_ra_miss_vel_min.get() > 0.f && _param_ra_miss_vel_min.get() < _param_ra_miss_vel_def.get()
&& _param_ra_miss_vel_gain.get() > FLT_EPSILON) { // Cornering slow down effect
if (distance_to_prev_wp <= _prev_acceptance_radius && _prev_acceptance_radius > FLT_EPSILON) {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _prev_acceptance_radius;
desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
} else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
} else if (distance_to_curr_wp <= _acceptance_radius && _acceptance_radius > FLT_EPSILON) {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
desired_speed = math::constrain(cornering_speed, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
} else { // Straight line speed
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON
&& _acceptance_radius > FLT_EPSILON) {
float max_velocity{0.f};
} else { // Straight line speed
if (_param_ra_max_accel.get() > FLT_EPSILON && _param_ra_max_jerk.get() > FLT_EPSILON
&& _acceptance_radius > FLT_EPSILON) {
float max_velocity{0.f};
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(),
_param_ra_max_accel.get(), distance_to_curr_wp, 0.f);
if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) {
max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(),
_param_ra_max_accel.get(), distance_to_curr_wp, 0.f);
} else {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(),
_param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed);
}
desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
} else {
const float cornering_speed = _param_ra_miss_vel_gain.get() / _acceptance_radius;
max_velocity = math::trajectory::computeMaxSpeedFromDistance(_param_ra_max_jerk.get(),
_param_ra_max_accel.get(), distance_to_curr_wp - _acceptance_radius, cornering_speed);
desired_speed = _param_ra_miss_vel_def.get();
}
desired_speed = math::constrain(max_velocity, _param_ra_miss_vel_min.get(), _param_ra_miss_vel_def.get());
} else {
desired_speed = _param_ra_miss_vel_def.get();
}
} else {
desired_speed = _param_ra_miss_vel_def.get();
}
} else {
desired_speed = _param_ra_miss_vel_def.get();
}
// Calculate desired steering
desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(),
_param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw);
desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(),
_param_ra_max_steer_angle.get());
_prev_desired_steering = desired_steering;
// Calculate desired steering
desired_steering = calcDesiredSteering(_curr_wp_ned, _prev_wp_ned, _curr_pos_ned, _param_ra_lookahd_gain.get(),
_param_ra_lookahd_min.get(), _param_ra_lookahd_max.get(), _param_ra_wheel_base.get(), desired_speed, vehicle_yaw);
desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(),
_param_ra_max_steer_angle.get());
} else { // Catch delay command
desired_steering = _prev_desired_steering; // Avoid steering on the spot
desired_speed = 0.f;
}
}
// Throttle PID
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
+1
View File
@@ -151,6 +151,7 @@ private:
Vector2f _curr_pos_ned{};
PID_t _pid_throttle;
hrt_abstime _timestamp{0};
float _prev_desired_steering{0.f};
// Waypoint variables
Vector2d _curr_wp{};