d7e749d203
integrate dijkstra lib into avoidance planner
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-05-07 10:39:46 +03:00
0786191a5c
switched to functional unit test to fix compilation error
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-05-07 10:39:17 +03:00
e5b4e9c52a
support symmetric cost in order to save memory
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-05-05 10:31:06 +03:00
cce490e6f5
geofence_avoidance_planner: simplify shortest path logic even further
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-05-05 10:31:06 +03:00
e414e15421
geofence_avoidance_planner: save on stack space by simplifying calculations
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-05-05 10:31:06 +03:00
12b3acd5b4
make search even more efficient by combining two loops
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-30 14:03:07 +03:00
a3cec7a51d
feat(lib): dijkstra path planner library
...
- Takes in buffer of all edge costs
- fills the best_cost buffer for all reachable nodes (unreachable =
infinite cost)
Usage:
- Precompute edge costs (polygon expansion, visibility all separate
from planning algorithm)
- solve
- To find best cost from any position: do one more visibility check
from that position to all reachable nodes, return the one with
smallest total distance (current position to node to goal)
- Once waypoint reached, keep looking up next_node until goal reached
Todo:
- clean up API (raw float buffer really the best? heap allocated array?)
- make optimised version for symmetric cost? < half the data
2026-04-29 16:07:20 +02:00
c3a152fbf2
rtl_direct: send out message to inform that rtl is avoiding geofence
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-29 14:28:46 +03:00
1a7b579342
fix formatting
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-29 14:28:46 +03:00
40bf5b33a2
renamed MOVE_TO_INTERMEDIATE_POINT to AVOID_GEOFENCE
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-29 14:28:46 +03:00
6871caa6f9
invalidate start and destination when resetting the vertices
...
- the reference for the projection may change which will cause start and
destination positions to be invalid
- the position of the destination in the graph node may not be correct anymore
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-29 14:28:46 +03:00
a77093f0e7
rtl_direct: only update destination if it's invalid or the position actually
...
changed
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-29 14:28:46 +03:00
cace5b88dd
take rtl path into account for remaining rtl time estimation
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-29 14:28:46 +03:00
cac90d75bf
geofence utils test: simplify and extend
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-28 14:11:20 +03:00
2168374b58
geofence utils unit tests: improve test for SymmetricPairIndex
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-28 14:06:57 +03:00
3efb9ad3b4
perf(navigator): instrument geofence avoidance planner with perf counters
...
Adds five PC_ELAPSED counters around the components expected to dominate
RTL path planning cost so they show up in `perf` output:
rtl_planner: setup - update_vertices() (fence-load entry)
rtl_planner: setup distances - O(n^2) line-fence checks
rtl_planner: update start - O(n) line checks on RTL entry
rtl_planner: update destination
rtl_planner: plan path - Dijkstra in planPath()
Skips the no-work early-return paths (unhealthy state, invalid input) via
perf_cancel so reported numbers reflect actual work done.
2026-04-28 13:48:18 +03:00
e5b740cc82
fix(navigator): keep result as member and return by reference to use less stack
...
Declaring a local PlannedPath needs an additional 1608B of stack which
we don't typically have.
Making it a member of the planner means it is allocated on the heap.
2026-04-28 13:48:18 +03:00
53b73bb473
fix(geofence_avoidance_planner): fix double-promotion clang-tidy warning
...
Cast NAN to double when constructing Vector2d to avoid
-Wdouble-promotion.
2026-04-27 11:35:03 +02:00
e69e4432ec
rtl_direct: reversed changes in rtl time estimate regarding geofence
...
avoidance for now, needs more thought
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-27 07:01:23 +03:00
970d52f502
geofence: removed duplicate call to update_vertices
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-27 06:44:42 +03:00
b8a0b77e22
shorter comment
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-27 06:37:09 +03:00
21ae5057fd
added reference for symmetricPairIndex
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-27 06:33:52 +03:00
8ec3e2b890
include start into planned path to avoid corner cases when rtl is engaged
...
outside of geofence
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-24 17:34:19 +03:00
6108efbe75
if RTL direct is activated during a geofence violation, we use the last
...
recorded valid position as start position for the path planning
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-24 17:32:07 +03:00
ccd3608e50
refactor: instead of computing the distance between the nodes at each iteration,
...
precute them in advance. Separate vertices from start and destination
calculations.
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-24 14:36:33 +03:00
5a0b9a44f2
more comments
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-23 11:40:16 +03:00
f401181439
addressed some review comments, fixed bugs
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-23 11:31:08 +03:00
31792d773d
make geofence util functions more readable, added references and more
...
unti tests
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-23 11:30:26 +03:00
22d5617f10
geofence_utils: use shoelace formula to calculate proper inward vector
...
at vertice location
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-22 14:43:13 +03:00
e40a491dec
some cosmetics
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-21 15:50:08 +03:00
58a62340d9
fixed radius enlargement bug
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-21 15:49:58 +03:00
3e1f18e8de
added support for circular fences
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-21 15:17:29 +03:00
05c39e381c
geofence_utils: remove unused function
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-20 16:21:16 +03:00
8c49c202f0
fixed conversion
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-20 16:07:51 +03:00
66c704684f
removed unnecessary include
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-20 14:42:52 +03:00
1d6d641b2b
integrated geofence aware path planner into rtl direct navigation mode
...
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-20 11:53:45 +03:00
c5c83da12f
added a geofence avoidance planner, which returns a path from start to destination
...
avoiding geofence breaches
Signed-off-by: RomanBapst <bapstroman@gmail.com >
2026-04-20 11:48:34 +03:00
9b45769878
docs: auto-sync metadata [skip ci]
...
Co-Authored-By: PX4 BuildBot <bot@px4.io >
2026-04-20 07:02:22 +00:00
dbd3adc145
fix(airspeed_selector): fix invalid array access after airspeed failure
...
The array access
_airspeed_validator[valid_airspeed_index - 1]
is only valid if valid_airspeed_index > 0. After an airspeed failure, we
have valid_airspeed_index = -1 (see AirspeedSource enum definition) and
are thus accessing array element -2. This is UB and pollutes the log
(and potentially other modules) with garbage values.
Fix: Like already done for other fields, we initialise the values with
NaN and only populate (using the array access) when the airspeed source
is a real sensor, and thus valid.
2026-04-20 08:55:30 +02:00
4159d94ac1
docs: auto-sync metadata [skip ci]
...
Co-Authored-By: PX4 BuildBot <bot@px4.io >
2026-04-20 03:35:44 +00:00
52b2546046
fix(ekf2): correct EV orientation variance inflation and NE aiding check ( #26622 )
...
Replace dimensionally wrong max(pos/vel_var, orientation_var) floor
with physically correct additive variance from the cross-product
δp = δθ × p (and δv = δθ × v). The old code compared rad² directly
to m² or (m/s)², ignoring lever arm / speed.
Also include EV position (guarded by yaw_align) in
isNorthEastAidingActive() so that mag fusion doesn't spuriously
clear yaw_align when it stops during EV-only flight.
Supersedes #25703
2026-04-19 19:28:41 -08:00
e7e1a7ec88
docs: auto-sync metadata [skip ci]
...
Co-Authored-By: PX4 BuildBot <bot@px4.io >
2026-04-20 03:04:49 +00:00
dd4213310f
ekf2: fix published terrain reset delta ( #27130 )
2026-04-19 18:57:23 -08:00
385828fcb3
feat(astyle): add option to only format diff with respect to HEAD ( #27122 )
...
* feat(astyle): add make format_changed for diff only style fixes
`make format` is pretty slow because it always considers the entire
source tree. For developing on top of a clean state, considering only
files that differ from HEAD should be sufficient.
* docs(astyle): document new format_changed target
2026-04-19 18:30:21 -08:00
3169dc6b1b
fix(uxrce_dds_client): avoid stalls under high UDP load ( #26161 )
...
* uxrce_dds_client: avoid stalls under high UDP load
- Avoid double-close of transport fd on deinit\n- Reduce loop latency and drain inbound bursts\n- Batch best-effort output flush and handle buffer-full\n- Lower default uORB->DDS poll interval to allow higher /fmu/out rates
* Fix formatting of UxrceddsClient descriptor initialization
2026-04-19 18:26:42 -08:00
372369fc83
docs: auto-sync metadata [skip ci]
...
Co-Authored-By: PX4 BuildBot <bot@px4.io >
2026-04-20 02:15:40 +00:00
a182072792
fix(various): Fix hardfaults when running out of memory (nullptr check) ( #26516 )
...
Build all targets / Scan for Board Targets (push) Has been cancelled
Checks / Gate Checks [check_format] (push) Has been cancelled
Checks / Gate Checks [check_newlines] (push) Has been cancelled
Checks / Gate Checks [module_documentation] (push) Has been cancelled
Checks / Gate Checks [shellcheck_all] (push) Has been cancelled
Checks / Gate Checks [validate_module_configs] (push) Has been cancelled
Checks / Unit Tests (push) Has been cancelled
Static Analysis / Clang-Tidy (push) Has been cancelled
MacOS build / build (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:22.04) (push) Has been cancelled
Ubuntu environment build / Build and Test (ubuntu:24.04) (push) Has been cancelled
Container build / Set Tags and Variables (push) Has been cancelled
Docs - Orchestrator / T1: Detect Changes (push) Has been cancelled
Docs - Orchestrator / T2: Metadata Sync (push) Has been cancelled
Docs - Crowdin - Upload Guide sources (en) / upload-to-crowdin (push) Has been cancelled
Failsafe Simulator Build / build (failsafe_web) (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v5x (push) Has been cancelled
FLASH usage analysis / Analyzing px4_fmu-v6x (push) Has been cancelled
ITCM check / Checking nxp_mr-tropic (push) Has been cancelled
ITCM check / Checking nxp_tropic-community (push) Has been cancelled
ITCM check / Checking px4_fmu-v5x (push) Has been cancelled
ITCM check / Checking px4_fmu-v6xrt (push) Has been cancelled
MAVROS Tests / MAVROS Mission (push) Has been cancelled
MAVROS Tests / MAVROS Offboard (push) Has been cancelled
Python CI Checks / build (push) Has been cancelled
ROS Integration Tests / build (push) Has been cancelled
ROS Translation Node Tests / Build and test [humble] (push) Has been cancelled
ROS Translation Node Tests / Build and test [jazzy] (push) Has been cancelled
SITL Tests / Testing PX4 iris (push) Has been cancelled
Build all targets / Seed [${{ matrix.chip_family }}] (push) Has been cancelled
Build all targets / Build [${{ matrix.runner }}][${{ matrix.group }}] (push) Has been cancelled
Build all targets / Upload Artifacts (push) Has been cancelled
Container build / Build Container (amd64) (push) Has been cancelled
Container build / Build Container (arm64) (push) Has been cancelled
Container build / Deploy To Registry (push) Has been cancelled
Docs - Orchestrator / T2: PR Metadata (push) Has been cancelled
Docs - Orchestrator / T2: Link Check (push) Has been cancelled
Docs - Orchestrator / T3: Build Site (push) Has been cancelled
Docs - Orchestrator / T4: Deploy (push) Has been cancelled
FLASH usage analysis / Publish Results (push) Has been cancelled
Fuzzing / Fuzzing (push) Has been cancelled
SBOM Monthly Audit / audit (push) Has been cancelled
Handle stale issues and PRs / stale (push) Has been cancelled
* Fix hardfault in mavlink stream strcpy
* prevent more OOM hardfaults
* revert mavlink fallback
* delete _interpreter in mc_nn_control module
* fix build
---------
Co-authored-by: jonas <jonas.perolini@rigi.tech >
Co-authored-by: Julian Oes <julian@oes.ch >
2026-04-19 18:08:39 -08:00
300e368e65
docs: auto-sync metadata [skip ci]
...
Co-Authored-By: PX4 BuildBot <bot@px4.io >
2026-04-19 23:09:12 +00:00
7ab709c202
docs(i18n): PX4 guide translations (Crowdin) - zh-CN ( #27066 )
...
Co-authored-by: Crowdin Bot <support+bot@crowdin.com >
2026-04-20 09:01:48 +10:00
0a4070b5c2
docs: auto-sync metadata [skip ci]
...
Co-Authored-By: PX4 BuildBot <bot@px4.io >
2026-04-19 04:03:02 +00:00
RomanBapst
Balduin
PX4BuildBot
Balduin
Jacob Dahl
Eurus
Nanwan
Jonas Perolini