fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 01:13:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

differential: Add stabilized and position mode (#23669)

Browse Source 
* differential: add position and stabilized mode

* differential: add hardcoded stick input deadzones
This commit is contained in:
chfriedrich98
2024-09-16 12:09:51 +02:00
committed by GitHub
parent 81747f35bb
commit 2fd4150b38
9 changed files with 136 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/pure_pursuit/PurePursuit.cpp
+7 -9
View File
@@ -78,18 +78,16 @@ float PurePursuit::calcDesiredHeading(const Vector2f &curr_wp_ned, const Vector2
const Vector2f prev_wp_to_curr_pos = curr_pos_ned - prev_wp_ned;
const Vector2f prev_wp_to_curr_wp_u = prev_wp_to_curr_wp.unit_or_zero();
const Vector2f distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) *
prev_wp_to_curr_wp_u; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
const Vector2f curr_pos_to_path = distance_on_line_segment -
prev_wp_to_curr_pos; // Shortest vector from the current position to the path
_distance_on_line_segment = (prev_wp_to_curr_pos * prev_wp_to_curr_wp_u) * prev_wp_to_curr_wp_u;
_curr_pos_to_path = _distance_on_line_segment - prev_wp_to_curr_pos;
if (curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
return atan2f(curr_pos_to_path(1), curr_pos_to_path(0));
if (_curr_pos_to_path.norm() > _lookahead_distance) { // Target closest point on path if there is no intersection point
return atan2f(_curr_pos_to_path(1), _curr_pos_to_path(0));
}
const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(curr_pos_to_path.norm(),
2.f)); // Length of the vector from the endpoint of distance_on_line_segment to the intersection point
const Vector2f prev_wp_to_intersection_point = distance_on_line_segment + line_extension *
const float line_extension = sqrt(powf(_lookahead_distance, 2.f) - powf(_curr_pos_to_path.norm(),
2.f)); // Length of the vector from the endpoint of _distance_on_line_segment to the intersection point
const Vector2f prev_wp_to_intersection_point = _distance_on_line_segment + line_extension *
prev_wp_to_curr_wp_u;
const Vector2f curr_pos_to_intersection_point = prev_wp_to_intersection_point - prev_wp_to_curr_pos;
return atan2f(curr_pos_to_intersection_point(1), curr_pos_to_intersection_point(0));
src/lib/pure_pursuit/PurePursuit.hpp
+5 -1
View File
@@ -103,6 +103,8 @@ public:
float vehicle_speed);
float getLookaheadDistance() {return _lookahead_distance;};
float getCrosstrackError() {return _curr_pos_to_path.norm();};
float getDistanceOnLineSegment() {return _distance_on_line_segment.norm();};
protected:
/**
@@ -122,5 +124,7 @@ protected:
float lookahead_min{1.f};
} _params{};
private:
float _lookahead_distance{0.f};
float _lookahead_distance{0.f}; // Radius of the circle around the vehicle
Vector2f _distance_on_line_segment{}; // Projection of prev_wp_to_curr_pos onto prev_wp_to_curr_wp
Vector2f _curr_pos_to_path{}; // Shortest vector from the current position to the path
};