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

ackermann: fix naming conventions

Browse Source
This commit is contained in:
chfriedrich98
2024-07-19 14:43:40 +02:00
committed by Mathieu Bresciani
parent f8bebd9e41
commit aa0dda7443
3 changed files with 39 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/rover_ackermann/RoverAckermann.cpp
+1 -1
View File
@@ -112,7 +112,7 @@ void RoverAckermann::Run()
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
_motor_setpoint = _ackermann_guidance.purePursuit(_nav_state); _motor_setpoint = _ackermann_guidance.computeGuidance(_nav_state);
break; break;
default: // Unimplemented nav states will stop the rover default: // Unimplemented nav states will stop the rover
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.cpp
+22 -26
View File
@@ -56,7 +56,7 @@ void RoverAckermannGuidance::updateParams()
1); // Output limit 1); // Output limit
} }
RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const int nav_state) RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::computeGuidance(const int nav_state)
{ {
// Initializations // Initializations
float desired_speed{0.f}; float desired_speed{0.f};
@@ -76,12 +76,12 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
vehicle_local_position_s local_position{}; vehicle_local_position_s local_position{};
_local_position_sub.copy(&local_position); _local_position_sub.copy(&local_position);
if (!_global_local_proj_ref.isInitialized() if (!_global_ned_proj_ref.isInitialized()
|| (_global_local_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) { || (_global_ned_proj_ref.getProjectionReferenceTimestamp() != local_position.ref_timestamp)) {
_global_local_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp); _global_ned_proj_ref.initReference(local_position.ref_lat, local_position.ref_lon, local_position.ref_timestamp);
} }
_curr_pos_local = Vector2f(local_position.x, local_position.y); _curr_pos_ned = Vector2f(local_position.x, local_position.y);
const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz}; const Vector3f rover_velocity = {local_position.vx, local_position.vy, local_position.vz};
actual_speed = rover_velocity.norm(); actual_speed = rover_velocity.norm();
} }
@@ -162,7 +162,7 @@ RoverAckermannGuidance::motor_setpoint RoverAckermannGuidance::purePursuit(const
} }
// Calculate desired steering // Calculate desired steering
desired_steering = calcDesiredSteering(_curr_wp_local, _prev_wp_local, _curr_pos_local, _param_ra_lookahd_gain.get(), 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); _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(), desired_steering = math::constrain(desired_steering, -_param_ra_max_steer_angle.get(),
_param_ra_max_steer_angle.get()); _param_ra_max_steer_angle.get());
@@ -233,16 +233,16 @@ void RoverAckermannGuidance::updateWaypoints()
_next_wp = _home_position; // Enables corner slow down with RTL _next_wp = _home_position; // Enables corner slow down with RTL
} }
// Local waypoint coordinates // NED waypoint coordinates
_curr_wp_local = _global_local_proj_ref.project(_curr_wp(0), _curr_wp(1)); _curr_wp_ned = _global_ned_proj_ref.project(_curr_wp(0), _curr_wp(1));
_prev_wp_local = _global_local_proj_ref.project(_prev_wp(0), _prev_wp(1)); _prev_wp_ned = _global_ned_proj_ref.project(_prev_wp(0), _prev_wp(1));
_next_wp_local = _global_local_proj_ref.project(_next_wp(0), _next_wp(1)); _next_wp_ned = _global_ned_proj_ref.project(_next_wp(0), _next_wp(1));
// Update acceptance radius // Update acceptance radius
_prev_acceptance_radius = _acceptance_radius; _prev_acceptance_radius = _acceptance_radius;
if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) { if (_param_ra_acc_rad_max.get() >= _param_nav_acc_rad.get()) {
_acceptance_radius = updateAcceptanceRadius(_curr_wp_local, _prev_wp_local, _next_wp_local, _param_nav_acc_rad.get(), _acceptance_radius = updateAcceptanceRadius(_curr_wp_ned, _prev_wp_ned, _next_wp_ned, _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_steer_angle.get()); _param_ra_acc_rad_gain.get(), _param_ra_acc_rad_max.get(), _param_ra_wheel_base.get(), _param_ra_max_steer_angle.get());
} else { } else {
@@ -250,19 +250,19 @@ void RoverAckermannGuidance::updateWaypoints()
} }
} }
float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain, const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain,
const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle) const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle)
{ {
// Setup variables // Setup variables
const Vector2f curr_to_prev_wp_local = prev_wp_local - curr_wp_local; const Vector2f curr_to_prev_wp_ned = prev_wp_ned - curr_wp_ned;
const Vector2f curr_to_next_wp_local = next_wp_local - curr_wp_local; const Vector2f curr_to_next_wp_ned = next_wp_ned - curr_wp_ned;
float acceptance_radius = default_acceptance_radius; float acceptance_radius = default_acceptance_radius;
// Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment // Calculate acceptance radius s.t. the rover cuts the corner tangential to the current and next line segment
if (curr_to_next_wp_local.norm() > FLT_EPSILON && curr_to_prev_wp_local.norm() > FLT_EPSILON) { if (curr_to_next_wp_ned.norm() > FLT_EPSILON && curr_to_prev_wp_ned.norm() > FLT_EPSILON) {
const float theta = acosf((curr_to_prev_wp_local * curr_to_next_wp_local) / (curr_to_prev_wp_local.norm() * const float theta = acosf((curr_to_prev_wp_ned * curr_to_next_wp_ned) / (curr_to_prev_wp_ned.norm() *
curr_to_next_wp_local.norm())) / 2.f; curr_to_next_wp_ned.norm())) / 2.f;
const float min_turning_radius = wheel_base / sinf(max_steer_angle); 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 = min_turning_radius / tanf(theta);
const float acceptance_radius_temp_scaled = acceptance_radius_gain * const float acceptance_radius_temp_scaled = acceptance_radius_gain *
@@ -279,14 +279,14 @@ float RoverAckermannGuidance::updateAcceptanceRadius(const Vector2f &curr_wp_loc
return acceptance_radius; return acceptance_radius;
} }
float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &curr_pos_local, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const Vector2f &curr_pos_ned, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max,
const float &wheel_base, const float &desired_speed, const float &vehicle_yaw) const float &wheel_base, const float &desired_speed, const float &vehicle_yaw)
{ {
// Calculate desired steering to reach lookahead point // Calculate desired steering to reach lookahead point
const float lookahead_distance = math::constrain(lookahead_gain * desired_speed, const float lookahead_distance = math::constrain(lookahead_gain * desired_speed,
lookahead_min, lookahead_max); lookahead_min, lookahead_max);
const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_local, prev_wp_local, curr_pos_local, const float desired_heading = _pure_pursuit.calcDesiredHeading(curr_wp_ned, prev_wp_ned, curr_pos_ned,
lookahead_distance); lookahead_distance);
const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw); const float heading_error = matrix::wrap_pi(desired_heading - vehicle_yaw);
// For logging // For logging
@@ -296,12 +296,8 @@ float RoverAckermannGuidance::calcDesiredSteering(const Vector2f &curr_wp_local,
if (math::abs_t(heading_error) <= M_PI_2_F) { if (math::abs_t(heading_error) <= M_PI_2_F) {
return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance); return atanf(2 * wheel_base * sinf(heading_error) / lookahead_distance);
} else if (heading_error > FLT_EPSILON) {
return atanf(2 * wheel_base * (1.0f + sinf(heading_error - M_PI_2_F)) /
lookahead_distance);
} else { } else {
return atanf(2 * wheel_base * (-1.0f + sinf(heading_error + M_PI_2_F)) / return atanf(2 * wheel_base * (sign(heading_error) * 1.0f + sinf(heading_error - sign(heading_error) * M_PI_2_F)) /
lookahead_distance); lookahead_distance);
} }
src/modules/rover_ackermann/RoverAckermannGuidance/RoverAckermannGuidance.hpp
+16 -16
View File
@@ -82,10 +82,10 @@ public:
* @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering. * @brief Compute guidance for ackermann rover and return motor_setpoint for throttle and steering.
* @param nav_state Vehicle navigation state * @param nav_state Vehicle navigation state
*/ */
motor_setpoint purePursuit(const int nav_state); motor_setpoint computeGuidance(const int nav_state);
/** /**
* @brief Update global/local waypoint coordinates and acceptance radius * @brief Update global/NED waypoint coordinates and acceptance radius
*/ */
void updateWaypoints(); void updateWaypoints();
@@ -93,24 +93,24 @@ public:
* @brief Returns and publishes the acceptance radius for current waypoint based on the angle between a line segment * @brief Returns and publishes the acceptance radius for current waypoint based on the angle between a line segment
* from the previous to the current waypoint/current to the next waypoint and maximum steer angle of * from the previous to the current waypoint/current to the next waypoint and maximum steer angle of
* the vehicle. * the vehicle.
* @param curr_wp_local Current waypoint in local frame. * @param curr_wp_ned Current waypoint in NED frame.
* @param prev_wp_local Previous waypoint in local frame. * @param prev_wp_ned Previous waypoint in NED frame.
* @param next_wp_local Next waypoint in local frame. * @param next_wp_ned Next waypoint in NED frame.
* @param default_acceptance_radius Default acceptance radius for waypoints. * @param default_acceptance_radius Default acceptance radius for waypoints.
* @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners. * @param acceptance_radius_gain Scaling of the geometric optimal acceptance radius for the rover to cut corners.
* @param acceptance_radius_max Maximum value for the acceptance radius. * @param acceptance_radius_max Maximum value for the acceptance radius.
* @param wheel_base Rover wheelbase. * @param wheel_base Rover wheelbase.
* @param max_steer_angle Rover maximum steer angle. * @param max_steer_angle Rover maximum steer angle.
*/ */
float updateAcceptanceRadius(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, float updateAcceptanceRadius(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned,
const Vector2f &next_wp_local, const float &default_acceptance_radius, const float &acceptance_radius_gain, const Vector2f &next_wp_ned, const float &default_acceptance_radius, const float &acceptance_radius_gain,
const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle); const float &acceptance_radius_max, const float &wheel_base, const float &max_steer_angle);
/** /**
* @brief Calculate and return desired steering input * @brief Calculate and return desired steering input
* @param curr_wp_local Current waypoint in local frame. * @param curr_wp_ned Current waypoint in NED frame.
* @param prev_wp_local Previous waypoint in local frame. * @param prev_wp_ned Previous waypoint in NED frame.
* @param curr_pos_local Current position of the vehicle in local frame. * @param curr_pos_ned Current position of the vehicle in NED frame.
* @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller. * @param lookahead_gain Tuning parameter for the lookahead distance pure pursuit controller.
* @param lookahead_min Minimum lookahead distance. * @param lookahead_min Minimum lookahead distance.
* @param lookahead_max Maximum lookahead distance. * @param lookahead_max Maximum lookahead distance.
@@ -118,7 +118,7 @@ public:
* @param desired_speed Desired speed for the rover. * @param desired_speed Desired speed for the rover.
* @param vehicle_yaw Current yaw of the rover. * @param vehicle_yaw Current yaw of the rover.
*/ */
float calcDesiredSteering(const Vector2f &curr_wp_local, const Vector2f &prev_wp_local, const Vector2f &curr_pos_local, float calcDesiredSteering(const Vector2f &curr_wp_ned, const Vector2f &prev_wp_ned, const Vector2f &curr_pos_ned,
const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base, const float &lookahead_gain, const float &lookahead_min, const float &lookahead_max, const float &wheel_base,
const float &desired_speed, const float &vehicle_yaw); const float &desired_speed, const float &vehicle_yaw);
@@ -143,12 +143,12 @@ private:
rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{}; rover_ackermann_guidance_status_s _rover_ackermann_guidance_status{};
MapProjection _global_local_proj_ref{}; // Transform global to local coordinates. MapProjection _global_ned_proj_ref{}; // Transform global to NED coordinates.
PurePursuit _pure_pursuit; // Pure pursuit library PurePursuit _pure_pursuit; // Pure pursuit library
// Rover variables // Rover variables
Vector2d _curr_pos{}; Vector2d _curr_pos{};
Vector2f _curr_pos_local{}; Vector2f _curr_pos_ned{};
PID_t _pid_throttle; PID_t _pid_throttle;
hrt_abstime _timestamp{0}; hrt_abstime _timestamp{0};
@@ -157,9 +157,9 @@ private:
Vector2d _next_wp{}; Vector2d _next_wp{};
Vector2d _prev_wp{}; Vector2d _prev_wp{};
Vector2d _home_position{}; Vector2d _home_position{};
Vector2f _curr_wp_local{}; Vector2f _curr_wp_ned{};
Vector2f _prev_wp_local{}; Vector2f _prev_wp_ned{};
Vector2f _next_wp_local{}; Vector2f _next_wp_ned{};
float _acceptance_radius{0.5f}; float _acceptance_radius{0.5f};
float _prev_acceptance_radius{0.5f}; float _prev_acceptance_radius{0.5f};