Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-31 02:16:53 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

some cosmetics

Browse Source 
Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2026-04-21 15:50:08 +03:00
parent 58a62340d9
commit e40a491dec
2 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/GeofenceBreachAvoidance/geofence_avoidance_planner.h
+4 -2
View File
@@ -47,6 +47,7 @@ static constexpr int kMaxNodes = 100;
static constexpr int kCircleApproxVertices = 8; static constexpr int kCircleApproxVertices = 8;
struct PlannedPath { struct PlannedPath {
// does not include the start and the end, just intermediate points
static constexpr int kMaxPathPoints = 32; static constexpr int kMaxPathPoints = 32;
matrix::Vector2<double> points[kMaxPathPoints]; matrix::Vector2<double> points[kMaxPathPoints];
int num_points{0}; int num_points{0};
@@ -64,19 +65,20 @@ struct PlannedPath {
} }
matrix::Vector2<double> getNextPoint() matrix::Vector2<double> getAndPopCurrentPoint()
{ {
if (current_index < num_points) { if (current_index < num_points) {
return points[current_index++]; return points[current_index++];
} else { } else {
return points[num_points - 1]; return matrix::Vector2d{NAN, NAN};
} }
} }
matrix::Vector2d getCurrentPoint() matrix::Vector2d getCurrentPoint()
{ {
// returns the current points but does not increase the index
if (current_index < num_points) { if (current_index < num_points) {
return points[current_index]; return points[current_index];
} }
src/modules/navigator/rtl_direct.cpp
+8 -1
View File
@@ -250,7 +250,14 @@ void RtlDirect::set_rtl_item()
case RTLState::MOVE_TO_INTERMEDIATE_POINT: { case RTLState::MOVE_TO_INTERMEDIATE_POINT: {
const matrix::Vector2d point = _geofence_aware_return_path.getNextPoint(); matrix::Vector2d point = _geofence_aware_return_path.getAndPopCurrentPoint();
if (point.isAllNan()) {
// should never happen
point(0) = _land_approach.lat;
point(1) = _land_approach.lon;
}
PositionYawSetpoint pos_yaw_sp { PositionYawSetpoint pos_yaw_sp {
.lat = point(0), .lat = point(0),
.lon = point(1), .lon = point(1),