UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-24 15:27:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

feat(navigator): extend detect and avoid module to follow regulatory standards such as ASTM F3442 (#26815)

Browse Source 
* feat(navigator): extend detect and avoid module to follow regulatory standards such as ASTM F3442

* docs(docs): minor subedit

* refactor(navigator): reduce flash by grouping notif into same events

* docs(daa): Improve docs readability with ::: details blocks

* fix(navigator): single event when on ground with conflict

* refactor(boards): increase flash length from 4M - 128k to 5M - 128k

* rework(general): define DAA standard at build with new CONFIG_NAVIGATOR_ADSB_F3442

* clean(navigator): minor changes to clean the PR

* feat(boards): add CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC in visionTargetEstStatic.px4board

* rework(daa): reduce amount of abstractions and minor cleaning

* refactor(boards): allyes revert flash length to 4M-128K

* refactor(boards): allyes increase flash length to 5M-128K

* rework(daa): rework event notifications to improve clarity

* docs(docs): move details inside of ::: details block

* docs(docs): run npx prettier

* refactor(daa): avoid void mutator functions

* docs(docs): Improve Python helper to decode daa unique id

* rework(daa): move encoded id handling to the adsb lib and refactor on_active

* refactor(daa): naming and zero init

* fix(daa): move dataman dep from daa level to unit test level

* refactor(daa): define common daa_input and rework process_transponder_report

* fix(daa): remove stale todo

* refactor(daa): rename crosstrack_based_daa to crosstrack_standard

* refactor(daa): rename F34_ params to DAA_

* docs(docs): revert changes to autogenerated docs

* docs(docs): Add Detect And Avoid in index.md

* refactor(daa): update message on init failed

* refactor(daa): only publish most_urgent conflict once in on_active()

* refactor(daa): move conflict buffer handling in the adsb lib

* refactor(daa): move notifications into new class ConflictNotifier and only notify once per cycle

* refactor(daa): uninit _cycle_changes to store into .bss (and save flash)

* docs(docs): remove unused image link

* refactor(daa): move automated action policy to the adsb lib

* refactor(daa): move self detection into adsb lib as DaaTrafficFilter

* refactor(daa): minor changes in unit tests

* refactor(daa): merge DaaTrafficFilter and DaaEncoding

* fix(daa): fix CI by removing navigator from the DAA deps

* refactor(daa): reduce stack size

* fix(daa): advertise _fake_traffic_pub in constructor

* refactor(daa): Comments only,  simplify and reduce comments

* fix(daa): brief comment missing end star

* Disable CONFIG_NAVIGATOR_ADSB in ark_fpv_default board

* docs(docs): General clarifications and remove the 1.18 badge

* refactor(daa): cleaning

* refactor(daa): minor cleaning

* refactor(adsb): simplify conflict tracker assuming push_back cannot fail

* build(cmake): cleaner fix to protect regex alternations

* refactor(daa): Crosstrack, remove unit test requiring finite yaw (yaw not used by standard)

* refactor(daa): minor cleaning and fix callsign to uint64

---------

Co-authored-by: jonas <jonas.perolini@rigi.tech>
Co-authored-by: Hamish Willee <hamishwillee@gmail.com>
This commit is contained in:
Jonas Perolini
2026-07-14 00:14:42 +02:00
committed by GitHub
parent 7ca74a7cc9
commit 5ea3cf8cb9
69 changed files with 10547 additions and 1074 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
.github/workflows/checks.yml vendored
View File

@@ -77,6 +77,7 @@ jobs:
PX4_SBOM_DISABLE: 1
run: |
make tests
make tests_daa_crosstrack
make tests_vtest_moving
- uses: ./.github/actions/save-ccache

12
Makefile
View File

@@ -445,7 +445,7 @@ check_newlines:
# Testing
# --------------------------------------------------------------------
.PHONY: tests tests_vtest_moving tests_coverage tests_mission tests_mission_coverage tests_offboard
.PHONY: tests tests_daa_crosstrack tests_vtest_moving tests_coverage tests_mission tests_mission_coverage tests_offboard
.PHONY: rostest python_coverage
tests:
@@ -455,6 +455,16 @@ tests:
$(eval UBSAN_OPTIONS += color=always)
$(call cmake-build,px4_sitl_test)
# The default px4_sitl_test board builds the F3442 DAA standard.
# This target builds the crosstrack DAA standard (CONFIG_NAVIGATOR_ADSB_F3442=n).
tests_daa_crosstrack:
$(eval override CMAKE_ARGS += -DTESTFILTER=$(if $(TESTFILTER),$(TESTFILTER),'detect_and_avoid|DaaCrosstrack|AdsbConflict|DaaActionPolicy|DaaEncodedId'))
$(eval override CMAKE_ARGS += -DCMAKE_TESTING=ON)
$(eval ARGS += test_results)
$(eval ASAN_OPTIONS += color=always:check_initialization_order=1:detect_stack_use_after_return=1)
$(eval UBSAN_OPTIONS += color=always)
$(call cmake-build,px4_sitl_test-daa-crosstrack)
tests_vtest_moving:
$(eval override CMAKE_ARGS += -DTESTFILTER=$(if $(TESTFILTER),$(TESTFILTER),VTE))
$(eval override CMAKE_ARGS += -DCMAKE_TESTING=ON)

3
Tools/px4moduledoc/srcparser.py
View File

@@ -451,6 +451,9 @@ class SourceParser(object):
if command in ['start', 'stop', 'status']:
continue # handled in the base class
if command == 'help':
continue # universal meta-command, used as sub-argument
if not command in doc_commands:
print(("Warning: undocumented command '%s' in %s" %(command, scope)))
self._consistency_checks_failure = True

1
boards/ark/fpv/default.px4board
View File

@@ -63,6 +63,7 @@ CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_NAVIGATOR_GEOFENCE_AVOIDANCE=n
# CONFIG_NAVIGATOR_ADSB is not set
CONFIG_NUM_MISSION_ITMES_SUPPORTED=1000
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y

2
boards/px4/fmu-v6c/visionTargetEstStatic.px4board
View File

@@ -87,6 +87,8 @@ CONFIG_SYSTEMCMDS_VER=y
CONFIG_SYSTEMCMDS_WORK_QUEUE=y
CONFIG_MAVLINK_DIALECT="development"
CONFIG_NAVIGATOR_ADSB_F3442=y
CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC=y
CONFIG_MODULES_VISION_TARGET_ESTIMATOR=y
CONFIG_MODULES_LANDING_TARGET_ESTIMATOR=n
CONFIG_MODULES_ROVER_POS_CONTROL=n

2
boards/px4/fmu-v6xrt/nuttx-config/scripts/allyes-script.ld
View File

@@ -34,7 +34,7 @@
MEMORY
{
flash (rx) : ORIGIN = 0x30020000, LENGTH = 4M-128K /* We have 64M but we do not want to wait to program it all */
flash (rx) : ORIGIN = 0x30020000, LENGTH = 5M-128K /* We have 64M but we do not want to wait to program it all */
sram (rwx) : ORIGIN = 0x20240000, LENGTH = 2M-256k-512k
itcm (rwx) : ORIGIN = 0x00000000, LENGTH = 256K /* TODO FlexRAM partition */
dtcm (rwx) : ORIGIN = 0x20000000, LENGTH = 256K

2
boards/px4/sitl/default.px4board
View File

@@ -43,6 +43,8 @@ CONFIG_MODULES_MC_POS_CONTROL=y
CONFIG_MODULES_MC_RATE_CONTROL=y
CONFIG_MODULES_NAVIGATOR=y
CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF=y
CONFIG_NAVIGATOR_ADSB_F3442=y
CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC=y
CONFIG_NUM_MISSION_ITMES_SUPPORTED=10000
CONFIG_MODULES_PAYLOAD_DELIVERER=y
CONFIG_MODULES_RC_UPDATE=y

3
boards/px4/sitl/test-daa-crosstrack.px4board Normal file
View File

@@ -0,0 +1,3 @@
CONFIG_BOARD_NOLOCKSTEP=y
CONFIG_DRIVERS_DISTANCE_SENSOR_LIGHTWARE_LASER_SERIAL=y
CONFIG_NAVIGATOR_ADSB_F3442=n

1
docs/en/SUMMARY.md
View File

@@ -859,6 +859,7 @@
- [Log Encryption](dev_log/log_encryption.md)
- [Advanced Topics](advanced/index.md)
- [PX4 Metadata](advanced/px4_metadata.md)
- [Detect and Avoid](advanced_features/detect_and_avoid.md)
- [Package Delivery Architecture](advanced/package_delivery.md)
- [Camera Integration/Architecture](camera/camera_architecture.md)
- [Computer Vision](advanced/computer_vision.md)

1016
docs/en/advanced_features/detect_and_avoid.md Normal file
View File

File diff suppressed because it is too large Load Diff

1
docs/en/advanced_features/index.md
View File

@@ -3,6 +3,7 @@
This section contains topics related to some of the more advanced features of the PX4 autopilot:
- [Air Traffic Avoidance: ADS-B/FLARM/UTM](../peripherals/adsb_flarm.md)
- [Detect And Avoid](../advanced_features/detect_and_avoid.md)
- [Computer Vision](../computer_vision/index.md)
- [Collision Prevention](../computer_vision/collision_prevention.md)
- [Motion Capture (MoCap)](../computer_vision/motion_capture.md)

17
docs/en/config/safety.md
View File

@@ -276,13 +276,22 @@ The relevant parameters are shown below:
## Traffic Avoidance Failsafe
The Traffic Avoidance Failsafe allows PX4 to respond to transponder data (e.g. from [ADSB transponders](../advanced_features/traffic_avoidance_adsb.md)) during missions.
The Traffic Avoidance Failsafe allows PX4 to respond to [cooperative traffic reports](../peripherals/adsb_flarm.md).
The action parameters depend on the conflict model selected when the firmware is built.
The relevant parameters are shown below:
| Parameter | Description |
| ---------------------------------------------------------------------------- | ---------------------------------------------------------------- |
| [NAV_TRAFF_AVOID](../advanced_config/parameter_reference.md#NAV_TRAFF_AVOID) | Set the failsafe action: Disabled, Warn, Return mode, Land mode. |
| Parameter | Description |
| -------------------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------------------------- |
| [DAA_EN](../advanced_config/parameter_reference.md#DAA_EN) | Enables or disables Detect and Avoid. |
| [NAV_TRAFF_AVOID](../advanced_config/parameter_reference.md#NAV_TRAFF_AVOID) | Action for the Crosstrack model. |
| [DAA_LVL_LOW_ACT](../advanced_config/parameter_reference.md#DAA_LVL_LOW_ACT) | F3442-mode action for a `LOW` conflict. |
| [DAA_LVL_MED_ACT](../advanced_config/parameter_reference.md#DAA_LVL_MED_ACT) | F3442-mode action for a `MEDIUM` conflict. |
| [DAA_LVL_HIGH_ACT](../advanced_config/parameter_reference.md#DAA_LVL_HIGH_ACT) | F3442-mode action for a `HIGH` conflict. |
| [DAA_LVL_CRIT_ACT](../advanced_config/parameter_reference.md#DAA_LVL_CRIT_ACT) | F3442-mode action for a `CRITICAL` conflict. |
All action parameters use `Disabled`, `Warn only`, `Return mode`, `Land mode`, `Position Hold mode`, and `Terminate`.
See [Detect and Avoid](../advanced_features/detect_and_avoid.md) for conflict-model and action-transition details.
## Remote ID Failsafe

212
docs/en/peripherals/adsb_flarm.md
View File

@@ -1,12 +1,27 @@
# ADS-B/FLARM/UTM Receivers: Air Traffic Avoidance
PX4 supports simple air traffic avoidance in [missions](../flying/missions.md) using [ADS-B](https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast), [FLARM](https://en.wikipedia.org/wiki/FLARM), or [UTM](https://www.faa.gov/uas/advanced_operations/traffic_management) transponders that use the standard MAVLink interfaces.
PX4 can monitor cooperative air traffic reported by [ADS-B](https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast), [FLARM](https://en.wikipedia.org/wiki/FLARM), or [UTM](https://www.faa.gov/uas/advanced_operations/traffic_management) integrations.
When it detects a potential conflict, PX4 can warn the operator or request an action such as Hold, Return, Land, or Terminate.
If a potential collision is detected, PX4 can _warn_, immediately [land](../flight_modes_mc/land.md), or [return](../flight_modes_mc/return.md) (depending on the value of [NAV_TRAFF_AVOID](#NAV_TRAFF_AVOID)).
This page explains how to connect a supported receiver and configure traffic avoidance behavior.
It is most relevant for operations in shared airspace, particularly beyond visual line of sight (BVLOS), where the vehicle must maintain safe separation from manned aviation without an onboard pilot.
::: info
PX4 can only assess _cooperative_ traffic: aircraft that actively broadcast their position via ADS-B, FLARM, UTM, or a compatible integration.
If your operation does not involve that kind of traffic data, this page is unlikely to apply.
For details on conflict-detection logic, alert volumes, notifications, testing, and extension points, see [Detect and Avoid](../advanced_features/detect_and_avoid.md).
:::
## Supported Hardware
PX4 traffic avoidance works with ADS-B or FLARM products that supply transponder data using the MAVLink [ADSB_VEHICLE](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE) message, and UTM products that supply transponder data using the MAVLink [UTM_GLOBAL_POSITION](https://mavlink.io/en/messages/common.html#UTM_GLOBAL_POSITION) message.
PX4 traffic avoidance works directly with ADS-B and FLARM products that send MAVLink [`ADSB_VEHICLE`](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE).
DAA consumes PX4's `transponder_report` topic, so other traffic sources can be supported by an adapter that publishes compatible reports.
::: info
PX4 does not currently convert incoming MAVLink [`UTM_GLOBAL_POSITION`](https://mavlink.io/en/messages/common.html#UTM_GLOBAL_POSITION) messages into traffic reports.
A UTM integration therefore needs a separate `transponder_report` adapter.
:::
It has been tested with the following devices:
@@ -16,7 +31,7 @@ It has been tested with the following devices:
## Hardware Setup
Any of the devices can be connected to any free/unused serial port on the flight controller.
Most commonly they are connected to `TELEM2` (if this is not being use for some other purpose).
Most commonly they are connected to `TELEM2` (if this is not being used for some other purpose).
### PingRX Pro
@@ -25,7 +40,7 @@ The PingRX MAVLink port uses a JST ZHR-4 mating connector with pinout as shown b
| Pin | Signal | Volt |
| ------- | -------- | ------------ |
| 1 (red) | RX (IN) | +5V tolerant |
| 2 (blk) | TX (OUT) |
| 2 (blk) | TX (OUT) | |
| 3 (blk) | Power | +4 to 6V |
| 4 (blk) | GND | GND |
@@ -77,20 +92,97 @@ You will now find a new parameter called [SER_TEL2_BAUD](../advanced_config/para
### Configure Traffic Avoidance
Configure the action when there is a potential collision using the parameter below:
Traffic avoidance is included when the firmware is built with `CONFIG_NAVIGATOR_ADSB`.
The conflict model is also selected at build time:
| Parameter | Description |
| ----------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
| <a id="NAV_TRAFF_AVOID"></a>[NAV_TRAFF_AVOID](../advanced_config/parameter_reference.md#NAV_TRAFF_AVOID) | Enable traffic avoidance mode specify avoidance response. 0: Disable, 1: Warn only, 2: Return mode, 3: Land mode. |
| <a id="NAV_TRAFF_A_HOR"></a>[NAV_TRAFF_A_HOR](../advanced_config/parameter_reference.md#NAV_TRAFF_A_HOR) | Horizontal radius of cylinder around the vehicle that defines its airspace (i.e. the airspace in the ground plane). |
| <a id="NAV_TRAFF_A_VER"></a>[NAV_TRAFF_A_VER](../advanced_config/parameter_reference.md#NAV_TRAFF_A_VER) | Vertical height above and below vehicle of the cylinder that defines its airspace (also see [NAV_TRAFF_A_HOR](#NAV_TRAFF_A_HOR)). |
| <a id="NAV_TRAFF_COLL_T"></a>[NAV_TRAFF_COLL_T](../advanced_config/parameter_reference.md#NAV_TRAFF_COLL_T) | Collision time threshold. Avoidance will trigger if the estimated time until collision drops below this value (the estimated time is based on relative speed of traffic and UAV). |
- **Crosstrack mode** is used when `CONFIG_NAVIGATOR_ADSB_F3442` is disabled.
It raises one conflict level and action when the current vehicle is close to the traffic's predicted track, vertically close, and within a configured collision-time threshold.
This is the avoidance mode historically supported by PX4.
- **F3442 mode** is used when `CONFIG_NAVIGATOR_ADSB_F3442` is enabled.
It evaluates four alert tests derived from concepts in [ASTM F3442/F3442M-23](https://store.astm.org/f3442_f3442m-23.html) and supports a separate action for each result level.
::: warning
The F3442 mode processes cooperative traffic only and implements selected alert concepts and thresholds.
It does not by itself establish compliance with ASTM F3442/F3442M-23, which applies to the complete DAA system and its compliance evidence.
The implementation references the 2023 edition and has not been evaluated against the later [ASTM F3442-25](https://store.astm.org/f3442-25.html) edition.
:::
For the detailed behavior of each conflict model, see [Detect and Avoid > Conflict Standards](../advanced_features/detect_and_avoid.md#conflict-standards) and [Detect and Avoid > Automated Actions](../advanced_features/detect_and_avoid.md#automated-actions).
#### Enable Avoidance
Use [DAA_EN](../advanced_config/parameter_reference.md#DAA_EN) to enable or disable DAA at runtime.
#### Crosstrack
Use firmware built without `CONFIG_NAVIGATOR_ADSB_F3442` if you want the single-threshold traffic avoidance behavior.
| Parameter | Description |
| ----------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------- |
| <a id="NAV_TRAFF_AVOID"></a>[NAV_TRAFF_AVOID] | Action requested when the crosstrack threshold is breached. `0`: Disabled, `1`: Warn only, `2`: Return, `3`: Land, `4`: Hold, `5`: Terminate. |
| <a id="NAV_TRAFF_A_HOR"></a>[NAV_TRAFF_A_HOR] | Maximum absolute crosstrack distance from the projected traffic track. |
| <a id="NAV_TRAFF_A_VER"></a>[NAV_TRAFF_A_VER] | Maximum vertical separation from the traffic aircraft. |
| <a id="NAV_TRAFF_COLL_T"></a>[NAV_TRAFF_COLL_T] | Maximum conservative time-to-collision estimate. A conflict is raised only if the horizontal, vertical, and time conditions are all met. |
[NAV_TRAFF_AVOID]: ../advanced_config/parameter_reference.md#NAV_TRAFF_AVOID
[NAV_TRAFF_A_HOR]: ../advanced_config/parameter_reference.md#NAV_TRAFF_A_HOR
[NAV_TRAFF_A_VER]: ../advanced_config/parameter_reference.md#NAV_TRAFF_A_VER
[NAV_TRAFF_COLL_T]: ../advanced_config/parameter_reference.md#NAV_TRAFF_COLL_T
#### F3442
Use firmware built with `CONFIG_NAVIGATOR_ADSB_F3442` for staged alerting based on selected ASTM F3442/F3442M-23 concepts.
PX4 evaluates four conflict levels and maps each level to an action:
| Parameter | Description |
| ----------------------------------------------- | ------------------------------------------------- |
| <a id="DAA_LVL_LOW_ACT"></a>[DAA_LVL_LOW_ACT] | Action for the augmented well clear alert volume. |
| <a id="DAA_LVL_MED_ACT"></a>[DAA_LVL_MED_ACT] | Action for the augmented NMAC alert volume. |
| <a id="DAA_LVL_HIGH_ACT"></a>[DAA_LVL_HIGH_ACT] | Action for Loss of Well Clear (LoWC). |
| <a id="DAA_LVL_CRIT_ACT"></a>[DAA_LVL_CRIT_ACT] | Action for Near Mid-Air Collision (NMAC). |
[DAA_LVL_LOW_ACT]: ../advanced_config/parameter_reference.md#DAA_LVL_LOW_ACT
[DAA_LVL_MED_ACT]: ../advanced_config/parameter_reference.md#DAA_LVL_MED_ACT
[DAA_LVL_HIGH_ACT]: ../advanced_config/parameter_reference.md#DAA_LVL_HIGH_ACT
[DAA_LVL_CRIT_ACT]: ../advanced_config/parameter_reference.md#DAA_LVL_CRIT_ACT
F3442 mode evaluates four cylindrical alert tests in priority order.
A test is breached when both horizontal and vertical separation are inside its combined ownship (the current vehicle) plus traffic bounds.
| Item | Parameters | Meaning |
| -------------- | -------------------------------------- | ------------------------------------------------------------------------------------------------------- |
| `CRITICAL` | [DAA_LVL_CRIT_RAD], [DAA_LVL_CRIT_HGT] | Per-aircraft NMAC base radius and vertical bound. |
| `HIGH` | [DAA_LVL_HIGH_RAD], [DAA_LVL_HIGH_HGT] | Per-aircraft Well Clear base radius and vertical bound. |
| `MEDIUM` | [DAA_LVL_MED_TIME] | Expands the NMAC base volume using aircraft speed and the configured time margin. |
| `LOW` | [DAA_LVL_LOW_TIME] | Expands the Well Clear base volume using aircraft speed and the configured time margin. |
| Velocity input | [DAA_EN_DFLT_VEL], [DAA_DFLT_VEL] | Optional replacement for the reported traffic vertical speed. |
[DAA_LVL_CRIT_RAD]: ../advanced_config/parameter_reference.md#DAA_LVL_CRIT_RAD
[DAA_LVL_CRIT_HGT]: ../advanced_config/parameter_reference.md#DAA_LVL_CRIT_HGT
[DAA_LVL_HIGH_RAD]: ../advanced_config/parameter_reference.md#DAA_LVL_HIGH_RAD
[DAA_LVL_HIGH_HGT]: ../advanced_config/parameter_reference.md#DAA_LVL_HIGH_HGT
[DAA_LVL_MED_TIME]: ../advanced_config/parameter_reference.md#DAA_LVL_MED_TIME
[DAA_LVL_LOW_TIME]: ../advanced_config/parameter_reference.md#DAA_LVL_LOW_TIME
[DAA_EN_DFLT_VEL]: ../advanced_config/parameter_reference.md#DAA_EN_DFLT_VEL
[DAA_DFLT_VEL]: ../advanced_config/parameter_reference.md#DAA_DFLT_VEL
Changing an action parameter does not re-evaluate buffered conflicts immediately.
The new action is considered on a later change of the overall most-urgent conflict level, and automatic mode changes are only requested when that level increases.
These parameters use the same action scale:
`0`: Disabled, `1`: Warn only, `2`: Return, `3`: Land, `4`: Hold, `5`: Terminate.
Most users can start with the default F3442 volume parameters and tune them only if needed.
See [Detect and Avoid > F3442 Mode](../advanced_features/detect_and_avoid.md#f3442-mode), which also includes the zone-computation equations.
### Arming Check
PX4 can be configured to check for the presence of a traffic avoidance system (ADSB or FLARM transponder) before arming.
PX4 can be configured to check for the presence of a traffic avoidance system (for example an ADS-B or FLARM receiver) before arming.
This ensures that a traffic avoidance system is connected and functioning before flight.
This check only verifies that a traffic source is present. It is separate from DAA rejecting arming because active traffic already requires an automatic action; that behavior is described in [Detect and Avoid > Arming, Preflight, and Ground Behavior](../advanced_features/detect_and_avoid.md#arming-preflight-and-ground-behavior).
The check is configured using the [COM_ARM_TRAFF](../advanced_config/parameter_reference.md#COM_ARM_TRAFF) parameter:
| Value | Description |
@@ -103,103 +195,15 @@ The check is configured using the [COM_ARM_TRAFF](../advanced_config/parameter_r
When a traffic avoidance system is detected, the system tracks its presence with a 3-second timeout.
If the system is lost or regained, corresponding events are logged ("Traffic avoidance system lost" / "Traffic avoidance system regained").
## Implementation
## Testing
### ADSB/FLARM
PX4 listens for valid transponder reports during missions.
If a valid transponder report is received, PX4 first uses the traffic transponder information to estimate whether the traffic heading and height indicates there will be an intersection with the airspace of the UAV.
The UAV airspace consists of a surrounding cylinder defined by the radius [NAV_TRAFF_A_HOR](#NAV_TRAFF_A_HOR) and height [NAV_TRAFF_A_VER](#NAV_TRAFF_A_VER), with the UAV at it's center.
The traffic detector then checks if the time until intersection with the UAV airspace is below the [NAV_TRAFF_COLL_T](#NAV_TRAFF_COLL_T) threshold based on the relative speed.
If the both checks are true, the [Traffic Avoidance Failsafe](../config/safety.md#traffic-avoidance-failsafe) action is started, and the vehicle will either warn, land, or return.
The code can be found in `Navigator::check_traffic` ([/src/modules/navigator/navigator_main.cpp](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/navigator/navigator_main.cpp)).
PX4 will also forward the transponder data to a GCS if this has been configured for the MAVLink instance (this is recommended).
The last 10 Digits of the GUID is displayed as Drone identification.
### UTM
PX4 listens for `UTM_GLOBAL_POSITION` MAVLink messages during missions.
When a valid message is received, its validity flags, position and heading are mapped into the same `transponder_report` UORB topic used for _ADS-B traffic avoidance_.
The implementation is otherwise _exactly_ as described in the section above.
::: info
[UTM_GLOBAL_POSITION](https://mavlink.io/en/messages/common.html#UTM_GLOBAL_POSITION) contains additional fields that are not provided by an ADSB transponder (see [ADSB_VEHICLE](https://mavlink.io/en/messages/common.html#ADSB_VEHICLE)).
The current implementation simply drops the additional fields (including information about the vehicle's planned next waypoint).
:::
## Testing/Simulated ADSB Traffic
You can simulate ADS-B traffic for testing.
Note that this requires that you [Build PX4](../dev_setup/building_px4.md).
::: info
Simulated ADS-B traffic can trigger real failsafe actions.
Use with care in real flight!
:::
To enable this feature:
1. Uncomment the code in `AdsbConflict::run_fake_traffic()`([AdsbConflict.cpp](https://github.com/PX4/PX4-Autopilot/blob/main/src/lib/adsb/AdsbConflict.cpp#L342C1-L342C1)).
2. Rebuild and run PX4.
3. Execute the [`navigator fake_traffic` command](../modules/modules_controller.md#navigator) in the [QGroundControl MAVLink Shell](https://docs.qgroundcontrol.com/master/en/qgc-user-guide/analyze_view/mavlink_console.html) (or some other [PX4 Console or MAVLink shell](../debug/consoles.md), such as the PX4 simulator terminal).
The code in `run_fake_traffic()` is then executed.
You should see ADS-B warnings in the Console/MAVLink shell, and QGC should also show an ADS-B traffic popup.
By default `run_fake_traffic()` publishes a number of traffic messages (it calls [`AdsbConflict::fake_traffic()`](#fake-traffic-method) to emit each report).
These simulate ADS-B traffic where there may be a conflict, where there won't be a conflict, as well as spamming the traffic buffer.
::: details Information about the test methods
The relevant methods are defined in [AdsbConflict.cpp](https://github.com/PX4/PX4-Autopilot/blob/main/src/lib/adsb/AdsbConflict.cpp#L342C1-L342C1).
#### `run_fake_traffic()` method
The `run_fake_traffic()` method is run when the `navigator fake_traffic` command is called.
The method calls the `fake_traffic()` method to generate simulated transponder messages around the current vehicle position.
It passes in the current vehicle position, and information about the simulated traffic, such as callsign, distances, directions, altitude differences, velocities, and emitter types.
The (commented out) code in `run_fake_traffic()` simulates a number of different scenarios, including conflicts and non-conflicts, as well as spamming the traffic buffer.
#### `fake_traffic()` method
`AdsbConflict::fake_traffic()` is called by the [`run_fake_traffic()`](#run-fake-traffic-method) to create individual ADS-B transponder reports.
This takes several parameters, which specify the characteristics of the fake traffic:
- `callsign`: Callsign of the fake transponder.
- `distance`: Horizontal distance to the fake vehicle from the current vehicle.
- `direction`: Direction in NED from this vehicle to the fake in radians.
- `traffic_heading`: Travel direction of the traffic in NED in radians.
- `altitude_diff`: Altitude difference of the fake traffic. Positive is up.
- `hor_velocity`: Horizontal velocity of fake traffic, in m/s.
- `ver_velocity`: Vertical velocity of fake traffic, in m/s.
- `emitter_type`: Type of fake vehicle, as an enumerated value.
- `icao_address`: ICAO address.
- `lat_uav`: Lat of this vehicle (used to position fake traffic around vehicle)
- `on_uav`: Lon of this vehicle (used to position fake traffic around vehicle)
- `alt_uav`: Altitude of the vehicle (as reference - used to position fake traffic around vehicle)
The method creates a simulated transponder message near the vehicle, using following steps:
- Calculates the latitude and longitude of the traffic based on the UAV's position, distance, and direction.
- Computes the new altitude by adding the altitude difference to the UAV's altitude.
- Populates a [TransponderReport](../msg_docs/TransponderReport.md) topic with the simulated traffic data.
- If the board supports a Universally Unique Identifier (UUID), the method retrieves the UUID using `board_get_px4_guid` and copies it to the `uas_id` field of the structure.
Otherwise, it generates a simulated GUID.
- Publishes the simulated traffic message using `orb_publish`.
:::
To test your DAA configuration using simulated traffic, see [Detect and Avoid > Testing and Simulation](../advanced_features/detect_and_avoid.md#testing-and-simulation).
<!-- See also implementation PR: https://github.com/PX4/PX4-Autopilot/pull/21283 -->
<!-- See also bug to make this work without uncommenting: https://github.com/PX4/PX4-Autopilot/issues/21810 -->
## Further Information
- [Detect and Avoid](../advanced_features/detect_and_avoid.md)
- [MAVLink Peripherals](../peripherals/mavlink_peripherals.md)
- [Serial Port Configuration](../peripherals/serial_configuration.md)

6
msg/CMakeLists.txt
View File

@@ -289,6 +289,12 @@ set(msg_files
)
list(SORT msg_files)
if(CONFIG_NAVIGATOR_ADSB)
list(APPEND msg_files
DetectAndAvoid.msg
DetectAndAvoidMostUrgent.msg)
endif()
if(CONFIG_MODULES_VISION_TARGET_ESTIMATOR)
list(APPEND msg_files
TargetGnss.msg

34
msg/DetectAndAvoid.msg Normal file
View File

@@ -0,0 +1,34 @@
# Detect-and-avoid conflict assessment for one traffic aircraft.
#
# Detailed per-traffic output from navigator's `DetectAndAvoid` component.
# A sample is published when an evaluated report indicates a conflict or updates
# an aircraft already tracked as a conflict. New reports assessed at NONE are omitted.
#
# Unlike `detect_and_avoid_most_urgent`, this topic does not summarize the single
# active conflict driving DAA actions or prearm checks. It reports the conflict level
# and geometry for the specific traffic aircraft that was just evaluated.
#
# Published by: `navigator` (`DetectAndAvoid`)
# Used by: logging and tests
uint8 DAA_CONFLICT_LVL_NONE = 0
uint8 DAA_CONFLICT_LVL_LOW = 1
uint8 DAA_CONFLICT_LVL_MEDIUM = 2
uint8 DAA_CONFLICT_LVL_HIGH = 3
uint8 DAA_CONFLICT_LVL_CRITICAL = 4
uint8 UNIQUE_ID_ENCODING_ICAO = 0 # `unique_id` contains an ICAO address
uint8 UNIQUE_ID_ENCODING_ADSB_CALLSIGN = 1 # `unique_id` contains an ADS-B callsign packed into a `uint64`
uint8 UNIQUE_ID_ENCODING_UAS_ID = 2 # `unique_id` contains the reduced tail bytes of a UAS ID
uint8 ORB_QUEUE_LENGTH = 16
uint64 timestamp # [us] Time since system start
uint64 unique_id # [-] Encoded traffic identifier selected in priority order: ICAO address, ADS-B callsign, then reduced UAS ID tail bytes
uint8 unique_id_encoding # [-] Namespace used to decode `unique_id`
uint8 conflict_level # [-] Conflict level calculated for this traffic aircraft
float32 aircraft_dist # [m] Current 3D point-to-point range between ownship and the traffic aircraft
float32 aircraft_dist_hor # [m] [@frame NED] Horizontal separation metric. In Crosstrack mode this is the signed crosstrack distance when available, otherwise direct horizontal range
float32 aircraft_dist_vert # [m] [@frame NED] Vertical separation between ownship (the current vehicle) and the traffic aircraft
float32 expected_min_dist_time # [s] Conservative collision-time estimate from current 3D separation and the sum of ownship and traffic speed magnitudes

25
msg/DetectAndAvoidMostUrgent.msg Normal file
View File

@@ -0,0 +1,25 @@
# Detect-and-avoid summary for the most urgent active conflict.
#
# Aggregated DAA status from navigator's `DetectAndAvoid` component.
# It publishes the single active conflict that currently has the highest urgency
# after the per-traffic conflict buffer has been updated.
#
# Unlike `detect_and_avoid`, this topic is not published for every processed
# traffic report and does not include the detailed horizontal and vertical geometry.
# It is the topic used for overall DAA status, automatic-action decisions, and
# prearm checks.
#
# Published by: `navigator` (`DetectAndAvoid`)
# Used by: `commander` DAA arming checks, logging, and tests
uint8 UNIQUE_ID_ENCODING_ICAO = 0 # `unique_id` contains an ICAO address
uint8 UNIQUE_ID_ENCODING_ADSB_CALLSIGN = 1 # `unique_id` contains an ADS-B callsign packed into a `uint64`
uint8 UNIQUE_ID_ENCODING_UAS_ID = 2 # `unique_id` contains the reduced tail bytes of a UAS ID
uint64 timestamp # [us] Time since system start
uint64 unique_id # [-] Encoded identifier of the current most urgent traffic aircraft, selected in priority order: ICAO address, ADS-B callsign, then reduced UAS ID tail bytes
uint8 unique_id_encoding # [-] Namespace used to decode `unique_id`
bool has_action # [-] True if the configured DAA response for this most urgent conflict is stronger than Warn only
uint8 conflict_level # [-] Conflict level of the current most urgent active conflict
float32 aircraft_dist # [m] Approximate 3D range to the most urgent traffic aircraft (9999 when empty)

4
src/drivers/transponder/sagetech_mxs/CMakeLists.txt
View File

@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
# Copyright (c) 2022-2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -76,7 +76,7 @@ px4_add_module(
SagetechMXS.cpp
SagetechMXS.hpp
DEPENDS
adsb
geo
px4_work_queue
MODULE_CONFIG
module.yaml

35
src/drivers/transponder/sagetech_mxs/SagetechMXS.cpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -302,10 +302,14 @@ void SagetechMXS::Run()
if (!_mxs_ext_cfg.get()) {
// Auto configuration
auto_config_operating();
auto_config_installation();
auto_config_flightid();
_mxs_op_mode.set(sg_op_mode_t::modeStby);
_mxs_op_mode.commit();
if (_adsb_icao.get() >= 0) {
auto_config_installation();
auto_config_flightid();
_mxs_op_mode.set(sg_op_mode_t::modeStby);
_mxs_op_mode.commit();
}
send_targetreq_msg();
mxs_state.initialized = true;
}
@@ -695,13 +699,19 @@ void SagetechMXS::send_data_req(const sg_datatype_t dataReqType)
void SagetechMXS::send_install_msg()
{
const int32_t adsb_icao = _adsb_icao.get();
if (adsb_icao < 0) {
return;
}
// MXS must be in OFF mode to change ICAO or Registration
if (mxs_state.op.opMode != modeOff) {
// gcs().send_text(MAV_SEVERITY_WARNING, "ADSB Sagetech MXS: unable to send installation data while not in OFF mode.");
return;
}
mxs_state.inst.icao = _adsb_icao.get();
mxs_state.inst.icao = static_cast<uint32_t>(adsb_icao);
mxs_state.inst.emitter = convert_emitter_type_to_sg(_adsb_emit_type.get());
mxs_state.inst.size = (sg_size_t)_adsb_len_width.get();
mxs_state.inst.maxSpeed = (sg_airspeed_t)_adsb_max_speed.get();
@@ -723,8 +733,10 @@ void SagetechMXS::send_flight_id_msg()
void SagetechMXS::send_operating_msg()
{
mxs_state.op.opMode = (sg_op_mode_t)_mxs_op_mode.get();
// In auto-conf mode ADSB_ICAO_ID=-1 disables ADS-B Out. Keep the transponder
// off so a retained installation from an earlier setup cannot transmit.
const bool adsb_out_disabled = !_mxs_ext_cfg.get() && _adsb_icao.get() < 0;
mxs_state.op.opMode = adsb_out_disabled ? sg_op_mode_t::modeOff : (sg_op_mode_t)_mxs_op_mode.get();
if (check_valid_squawk(_adsb_squawk.get())) {
mxs_state.op.squawk = convert_base_to_decimal(BASE_OCTAL, _adsb_squawk.get());
@@ -1317,12 +1329,13 @@ void SagetechMXS::auto_config_operating()
void SagetechMXS::auto_config_installation()
{
if (mxs_state.ack.opMode != modeOff) {
PX4_ERR("MXS not put in OFF Mode before installation.");
const int32_t adsb_icao = _adsb_icao.get();
if (adsb_icao < 0) {
return;
}
mxs_state.inst.icao = (uint32_t) _adsb_icao.get();
mxs_state.inst.icao = static_cast<uint32_t>(adsb_icao);
snprintf(mxs_state.inst.reg, 8, "%-7s", "PX4TEST");
mxs_state.inst.com0 = sg_baud_t::baud230400;

2
src/drivers/transponder/sagetech_mxs/SagetechMXS.hpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2022-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions

514
src/lib/adsb/AdsbConflict.cpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2023-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,482 +31,88 @@
*
****************************************************************************/
#include "AdsbConflict.h"
#include "geo/geo.h"
#include <uORB/topics/transponder_report.h>
#include <cmath>
#include <float.h>
void AdsbConflict::detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now,
float vz_now)
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
bool AdsbConflict::valid_wgs84_coordinates(const double latitude, const double longitude)
{
return PX4_ISFINITE(latitude) && PX4_ISFINITE(longitude)
&& fabs(latitude) <= 90.0
&& fabs(longitude) <= 180.0;
}
float d_hor, d_vert;
get_distance_to_point_global_wgs84(lat_now, lon_now, alt_now,
_transponder_report.lat, _transponder_report.lon, _transponder_report.altitude, &d_hor, &d_vert);
bool AdsbConflict::calculate_daa_output(const daa_input_s &daa_input, detect_and_avoid_s &daa_output)
{
const transponder_report_s &transponder_report = daa_input.transponder_report;
const float xyz_traffic_speed = sqrtf(_transponder_report.hor_velocity * _transponder_report.hor_velocity +
_transponder_report.ver_velocity * _transponder_report.ver_velocity);
const matrix::Vector2d traffic_lat_lon{transponder_report.lat, transponder_report.lon};
const float hor_uav_speed = sqrtf(vx_now * vx_now + vy_now * vy_now);
const float xyz_uav_speed = sqrtf(hor_uav_speed * hor_uav_speed + vz_now * vz_now);
//assume always pointing at each other
const float relative_uav_traffic_speed = xyz_traffic_speed + xyz_uav_speed;
// Predict until the vehicle would have passed this system at its current speed
const float prediction_distance = d_hor + TRAFFIC_TO_UAV_DISTANCE_EXTENSION;
double end_lat, end_lon;
waypoint_from_heading_and_distance(_transponder_report.lat, _transponder_report.lon,
_transponder_report.heading, prediction_distance, &end_lat, &end_lon);
const bool cs_distance_conflict_threshold = (!get_distance_to_line(_crosstrack_error, lat_now,
lon_now, _transponder_report.lat,
_transponder_report.lon, end_lat,
end_lon))
&& (!_crosstrack_error.past_end)
&& (fabsf(_crosstrack_error.distance) < _conflict_detection_params.crosstrack_separation);
const bool _crosstrack_separation_check = (fabsf(alt_now - _transponder_report.altitude) <
_conflict_detection_params.vertical_separation);
bool collision_time_check = false;
const float d_xyz = sqrtf(d_hor * d_hor + d_vert * d_vert);
if (relative_uav_traffic_speed > FLT_EPSILON) {
const float time_to_collsion = d_xyz / relative_uav_traffic_speed;
collision_time_check = (time_to_collsion < _conflict_detection_params.collision_time_threshold);
if (!valid_wgs84_coordinates(daa_input.uav_lat_lon(0), daa_input.uav_lat_lon(1))
|| !valid_wgs84_coordinates(traffic_lat_lon(0), traffic_lat_lon(1))) {
PX4_DEBUG("DAA lib: invalid coordinates");
return false;
}
_conflict_detected = (cs_distance_conflict_threshold && _crosstrack_separation_check
&& collision_time_check);
}
int AdsbConflict::find_icao_address_in_conflict_list(uint32_t icao_address)
{
for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
if (_traffic_buffer.icao_address[i] == icao_address) {
return static_cast<int>(i);
}
if (!PX4_ISFINITE(daa_input.uav_alt) || !PX4_ISFINITE(transponder_report.altitude)) {
PX4_DEBUG("DAA lib: invalid altitude");
return false;
}
return -1;
}
void AdsbConflict::remove_icao_address_from_conflict_list(int traffic_index)
{
_traffic_buffer.icao_address.remove(traffic_index);
_traffic_buffer.timestamp.remove(traffic_index);
PX4_INFO("icao_address removed. Buffer Size: %d", (int)_traffic_buffer.timestamp.size());
}
void AdsbConflict::add_icao_address_from_conflict_list(uint32_t icao_address, hrt_abstime now)
{
_traffic_buffer.timestamp.push_back(now);
_traffic_buffer.icao_address.push_back(icao_address);
PX4_INFO("icao_address added. Buffer Size: %d", (int)_traffic_buffer.timestamp.size());
}
void AdsbConflict::get_traffic_state(hrt_abstime now)
{
const int traffic_index = find_icao_address_in_conflict_list(_transponder_report.icao_address);
const bool old_conflict = (traffic_index >= 0);
const bool new_traffic = (traffic_index < 0);
const bool _traffic_buffer_full = (_traffic_buffer.icao_address.size() >= NAVIGATOR_MAX_TRAFFIC);
bool old_conflict_warning_expired = false;
if (old_conflict && _conflict_detected) {
old_conflict_warning_expired = now > _traffic_buffer.timestamp[traffic_index] + CONFLICT_WARNING_TIMEOUT;
if (!(PX4_ISFINITE(transponder_report.hor_velocity) && transponder_report.hor_velocity >= 0.f
&& PX4_ISFINITE(transponder_report.ver_velocity))) {
PX4_DEBUG("DAA lib: invalid traffic velocity");
return false;
}
if (new_traffic && _conflict_detected && !_traffic_buffer_full) {
add_icao_address_from_conflict_list(_transponder_report.icao_address, now);
_traffic_state = TRAFFIC_STATE::ADD_CONFLICT;
if (!daa_input.uav_vel_ned.isAllFinite()) {
PX4_DEBUG("DAA lib: invalid ownship velocity");
return false;
}
} else if (new_traffic && _conflict_detected && _traffic_buffer_full) {
_traffic_state = TRAFFIC_STATE::BUFFER_FULL;
#if !defined(CONFIG_NAVIGATOR_ADSB_F3442) || !CONFIG_NAVIGATOR_ADSB_F3442
} else if (old_conflict && _conflict_detected
&& old_conflict_warning_expired) {
_traffic_buffer.timestamp[traffic_index] = now;
_traffic_state = TRAFFIC_STATE::REMIND_CONFLICT;
if (!PX4_ISFINITE(transponder_report.heading)) {
PX4_DEBUG("DAA lib: invalid traffic heading");
return false;
}
} else if (old_conflict && !_conflict_detected) {
remove_icao_address_from_conflict_list(traffic_index);
_traffic_state = TRAFFIC_STATE::REMOVE_OLD_CONFLICT;
#endif // !CONFIG_NAVIGATOR_ADSB_F3442
aircraft_state_s uav_state{};
uav_state.lat_lon = daa_input.uav_lat_lon;
uav_state.altitude = daa_input.uav_alt;
uav_state.velocity_ned = daa_input.uav_vel_ned;
aircraft_state_s traffic_state{};
traffic_state.lat_lon = traffic_lat_lon;
traffic_state.altitude = transponder_report.altitude;
traffic_state.heading = transponder_report.heading;
if (PX4_ISFINITE(transponder_report.heading)) {
traffic_state.velocity_ned = matrix::Vector3f(
cosf(transponder_report.heading) * transponder_report.hor_velocity,
sinf(transponder_report.heading) * transponder_report.hor_velocity,
-transponder_report.ver_velocity);
} else {
_traffic_state = TRAFFIC_STATE::NO_CONFLICT;
}
}
void AdsbConflict::remove_expired_conflicts()
{
for (uint8_t traffic_index = 0; traffic_index < _traffic_buffer.timestamp.size();) {
if (hrt_elapsed_time(&_traffic_buffer.timestamp[traffic_index]) > TRAFFIC_CONFLICT_LIFETIME) {
events::send<uint32_t>(events::ID("navigator_traffic_expired"), events::Log::Notice,
"Traffic Conflict {1} Expired and removed from buffer",
_traffic_buffer.icao_address[traffic_index]);
remove_icao_address_from_conflict_list(traffic_index);
} else {
traffic_index++;
}
}
}
bool AdsbConflict::handle_traffic_conflict()
{
const hrt_abstime now = hrt_absolute_time();
get_traffic_state(now);
bool take_action = false;
switch (_traffic_state) {
case TRAFFIC_STATE::ADD_CONFLICT:
case TRAFFIC_STATE::REMIND_CONFLICT: {
take_action = send_traffic_warning((int)math::degrees(_transponder_report.heading),
(int)fabsf(_crosstrack_error.distance), _transponder_report.flags,
_transponder_report.callsign,
_transponder_report.icao_address,
now);
}
break;
case TRAFFIC_STATE::REMOVE_OLD_CONFLICT: {
events::send<uint32_t>(events::ID("navigator_traffic_resolved"), events::Log::Notice,
"Traffic Conflict Resolved {1}!",
_transponder_report.icao_address);
_last_traffic_warning_time = now;
}
break;
case TRAFFIC_STATE::BUFFER_FULL: {
if ((_traffic_state_previous != TRAFFIC_STATE::BUFFER_FULL)
&& (hrt_elapsed_time(&_last_buffer_full_warning_time) > TRAFFIC_WARNING_TIMESTEP)) {
events::send(events::ID("buffer_full"), events::Log::Notice, "Too much traffic! Showing all messages from now on");
_last_buffer_full_warning_time = now;
}
//disable conflict warnings when buffer is full
}
break;
case TRAFFIC_STATE::NO_CONFLICT: {
}
break;
traffic_state.velocity_ned = matrix::Vector3f(transponder_report.hor_velocity, 0.f, -transponder_report.ver_velocity);
}
daa_stats_s daa_stats{};
daa_output.conflict_level = _daa.calculate_daa_stats(uav_state, traffic_state, daa_stats);
daa_output.aircraft_dist = daa_stats.aircraft_dist;
daa_output.aircraft_dist_hor = daa_stats.aircraft_dist_hor;
daa_output.aircraft_dist_vert = daa_stats.aircraft_dist_vert;
daa_output.expected_min_dist_time = daa_stats.expected_min_dist_time_sec;
_traffic_state_previous = _traffic_state;
return take_action;
return true;
}
void AdsbConflict::set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
int collision_time_threshold, uint8_t traffic_avoidance_mode)
bool AdsbConflict::try_updating_params()
{
_conflict_detection_params.crosstrack_separation = crosstrack_separation;
_conflict_detection_params.vertical_separation = vertical_separation;
_conflict_detection_params.collision_time_threshold = collision_time_threshold;
_conflict_detection_params.traffic_avoidance_mode = traffic_avoidance_mode;
}
bool AdsbConflict::send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address, hrt_abstime now)
{
switch (_conflict_detection_params.traffic_avoidance_mode) {
case 0: {
if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d",
tr_callsign,
traffic_seperation,
traffic_direction, (int)icao_address);
}
_last_traffic_warning_time = now;
break;
}
case 1: {
if (tr_flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
PX4_WARN("Traffic alert - UTM callsign %s! Separation Distance %d, Heading %d, ICAO Address %d",
tr_callsign,
traffic_seperation,
traffic_direction, (int)icao_address);
}
/* EVENT
* @description
* - ICAO Address: {1}
* - Traffic Separation Distance: {2m}
* - Heading: {3} degrees
*/
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic"), events::Log::Notice,
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}",
icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = now;
break;
}
case 2: {
/* EVENT
* @description
* - ICAO Address: {1}
* - Traffic Separation Distance: {2m}
* - Heading: {3} degrees
*/
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_rtl"), events::Log::Notice,
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, returning home",
icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = now;
return true;
break;
}
case 3: {
/* EVENT
* @description
* - ICAO Address: {1}
* - Traffic Separation Distance: {2m}
* - Heading: {3} degrees
*/
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_land"), events::Log::Notice,
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, landing",
icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = now;
return true;
break;
}
case 4: {
/* EVENT
* @description
* - ICAO Address: {1}
* - Traffic Separation Distance: {2m}
* - Heading: {3} degrees
*/
events::send<uint32_t, int32_t, int16_t>(events::ID("navigator_traffic_hold"), events::Log::Notice,
"Traffic alert - ICAO Address {1}! Separation Distance {2}, Heading {3}, holding position",
icao_address, traffic_seperation, traffic_direction);
_last_traffic_warning_time = now;
return true;
break;
}
}
return false;
}
void AdsbConflict::fake_traffic(const char *callsign, float distance, float direction, float traffic_heading,
float altitude_diff, float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav,
double lon_uav,
float &alt_uav)
{
double lat{0.0};
double lon{0.0};
waypoint_from_heading_and_distance(lat_uav, lon_uav, direction, distance, &lat,
&lon);
float alt = alt_uav + altitude_diff;
tr.timestamp = hrt_absolute_time();
tr.icao_address = icao_address;
tr.lat = lat; // Latitude, expressed as degrees
tr.lon = lon; // Longitude, expressed as degrees
tr.altitude_type = 0;
tr.altitude = alt;
tr.heading = traffic_heading; //-atan2(vel_e, vel_n); // Course over ground in radians
tr.hor_velocity = hor_velocity; //sqrtf(vel_e * vel_e + vel_n * vel_n); // The horizontal velocity in m/s
tr.ver_velocity = ver_velocity; //-vel_d; // The vertical velocity in m/s, positive is up
strncpy(&tr.callsign[0], callsign, sizeof(tr.callsign) - 1);
tr.callsign[sizeof(tr.callsign) - 1] = 0;
tr.emitter_type = emitter_type; // Type from ADSB_EMITTER_TYPE enum
tr.tslc = 2; // Time since last communication in seconds
tr.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS | transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
(transponder_report_s::ADSB_EMITTER_TYPE_UAV & emitter_type ? 0 :
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN); // Flags to indicate various statuses including valid data fields
tr.squawk = 6667;
#ifndef BOARD_HAS_NO_UUID
px4_guid_t px4_guid;
board_get_px4_guid(px4_guid);
memcpy(tr.uas_id, px4_guid, sizeof(px4_guid_t)); //simulate own GUID
#else
for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) {
tr.uas_id[i] = 0xe0 + i; //simulate GUID
}
#endif /* BOARD_HAS_NO_UUID */
orb_publish(ORB_ID(transponder_report), fake_traffic_report_publisher, &tr);
}
void AdsbConflict::run_fake_traffic(double &lat_uav, double &lon_uav,
float &alt_uav)
{
//Test with buffer size of 10
//first conflict
fake_traffic("LX001", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
alt_uav);
//spam
fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX002", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav);
//stop spamming
//new conflicts
fake_traffic("LX003", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX004", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX005", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 5, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX006", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 6, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX007", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 7, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX008", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 8, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX009", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 9, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX010", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 10, lat_uav, lon_uav,
alt_uav);
//buffer full
//buffer full conflicts
fake_traffic("LX011", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 11, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX012", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 12, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
alt_uav);
//end conflicts
fake_traffic("LX001", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 1, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX002", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 2, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX003", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 3, lat_uav, lon_uav,
alt_uav);
fake_traffic("LX004", 5000, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 4, lat_uav, lon_uav,
alt_uav);
//new conflicts with space in buffer
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
alt_uav);
//spam
fake_traffic("LX013", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 13, lat_uav, lon_uav,
alt_uav);
//new conflict
fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
alt_uav);
//spam
fake_traffic("LX014", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 14, lat_uav, lon_uav,
alt_uav);
//new conflict
fake_traffic("LX015", 5, 1.0f, 0.0f, 0.0f, 90000.0f, 90000.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, 15, lat_uav, lon_uav,
alt_uav);
for (size_t i = 0; i < _traffic_buffer.icao_address.size(); i++) {
PX4_INFO("%u ", static_cast<unsigned int>(_traffic_buffer.icao_address[i]));
}
return _daa.try_setting_params();
}

136
src/lib/adsb/AdsbConflict.h
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -31,129 +31,49 @@
*
****************************************************************************/
#pragma once
#include <stdbool.h>
#include <stdint.h>
// Provides CONFIG_NAVIGATOR_ADSB_F3442 for the standard selection.
#include <px4_boardconfig.h>
#include <lib/geo/geo.h>
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
#include "f3442_standard/DaaF3442.h"
#else
#include "crosstrack_standard/DaaCrosstrack.h"
#endif
#include <drivers/drv_hrt.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <matrix/math.hpp>
#include <uORB/topics/detect_and_avoid.h>
#include <uORB/topics/transponder_report.h>
#include <uORB/topics/vehicle_command.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/board_common.h>
#include <containers/Array.hpp>
using namespace time_literals;
static constexpr uint8_t NAVIGATOR_MAX_TRAFFIC{10};
static constexpr uint8_t UTM_CALLSIGN_LENGTH{9};
static constexpr uint64_t CONFLICT_WARNING_TIMEOUT{60_s};
static constexpr float TRAFFIC_TO_UAV_DISTANCE_EXTENSION{1000.0f};
static constexpr uint64_t TRAFFIC_WARNING_TIMESTEP{60_s}; //limits the max warning rate when traffic conflict buffer is full
static constexpr uint64_t TRAFFIC_CONFLICT_LIFETIME{120_s}; //limits the time a conflict can be in the buffer without being seen (as a conflict)
struct traffic_data_s {
double lat_traffic;
double lon_traffic;
float alt_traffic;
float heading_traffic;
float vxy_traffic;
float vz_traffic;
bool in_conflict;
struct daa_input_s {
matrix::Vector2d uav_lat_lon{};
float uav_alt{0.f};
matrix::Vector3f uav_vel_ned{};
transponder_report_s transponder_report{};
};
struct traffic_buffer_s {
px4::Array<uint32_t, NAVIGATOR_MAX_TRAFFIC> icao_address {};
px4::Array<hrt_abstime, NAVIGATOR_MAX_TRAFFIC> timestamp {};
};
struct conflict_detection_params_s {
float crosstrack_separation;
float vertical_separation;
int collision_time_threshold;
uint8_t traffic_avoidance_mode;
};
enum class TRAFFIC_STATE {
NO_CONFLICT = 0,
ADD_CONFLICT = 1,
REMIND_CONFLICT = 2,
REMOVE_OLD_CONFLICT = 3,
BUFFER_FULL = 4
};
class AdsbConflict
{
public:
AdsbConflict() = default;
~AdsbConflict() = default;
void detect_traffic_conflict(double lat_now, double lon_now, float alt_now, float vx_now, float vy_now, float vz_now);
/**
* @brief Validate ownship + transponder inputs and compute the DAA output.
* Returns false on non-finite inputs or when the built standard needs a heading the report does not provide.
*/
bool calculate_daa_output(const daa_input_s &daa_input, detect_and_avoid_s &daa_output);
int find_icao_address_in_conflict_list(uint32_t icao_address);
bool try_updating_params();
void remove_icao_address_from_conflict_list(int traffic_index);
void add_icao_address_from_conflict_list(uint32_t icao_address, hrt_abstime now);
void get_traffic_state(hrt_abstime now);
void set_conflict_detection_params(float crosstrack_separation, float vertical_separation,
int collision_time_threshold, uint8_t traffic_avoidance_mode);
bool send_traffic_warning(int traffic_direction, int traffic_seperation, uint16_t tr_flags,
char tr_callsign[UTM_CALLSIGN_LENGTH], uint32_t icao_address, hrt_abstime now);
transponder_report_s _transponder_report{};
bool handle_traffic_conflict();
void fake_traffic(const char *const callsign, float distance, float direction, float traffic_heading,
float altitude_diff,
float hor_velocity, float ver_velocity, int emitter_type, uint32_t icao_address, double lat_uav, double lon_uav,
float &alt_uav);
void run_fake_traffic(double &lat_uav, double &lon_uav, float &alt_uav);
void remove_expired_conflicts();
bool _conflict_detected{false};
TRAFFIC_STATE _traffic_state{TRAFFIC_STATE::NO_CONFLICT};
conflict_detection_params_s _conflict_detection_params{};
protected:
traffic_buffer_s _traffic_buffer;
static bool valid_wgs84_coordinates(const double latitude, const double longitude);
private:
crosstrack_error_s _crosstrack_error{};
transponder_report_s tr{};
orb_advert_t fake_traffic_report_publisher = orb_advertise(ORB_ID(transponder_report), &tr);
TRAFFIC_STATE _traffic_state_previous{TRAFFIC_STATE::NO_CONFLICT};
hrt_abstime _last_traffic_warning_time{0};
hrt_abstime _last_buffer_full_warning_time{0};
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
DaaF3442 _daa;
#else
DaaCrosstrack _daa;
#endif
};

462
src/lib/adsb/AdsbConflictTest.cpp
View File

@@ -1,314 +1,220 @@
#include <gtest/gtest.h>
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include "AdsbConflict.h"
#include "AdsbConflictTest.h"
#include <parameters/param.h>
class TestAdsbConflict : public AdsbConflict
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <limits>
struct RelativeTrafficScenario {
float distance_m;
float bearing_rad;
float altitude_offset_m;
float heading_rad;
float hor_speed_m_s;
float ver_speed_up_m_s;
};
class AdsbConflictTest : public ::testing::Test
{
public:
TestAdsbConflict() : AdsbConflict() {}
~TestAdsbConflict() = default;
protected:
const matrix::Vector2d uav_lat_lon_{32.617013, -96.490564};
const float uav_alt_{1000.f};
const matrix::Vector3f stationary_uav_vel_ned_{0.f, 0.f, 0.f};
void set_traffic_buffer(const traffic_buffer_s &traffic_buffer)
void SetUp() override
{
_traffic_buffer = traffic_buffer;
param_control_autosave(false);
param_reset_all();
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
set_f3442_params(20.f, 10.f, 100.f, 50.f, 30, 40);
#else
set_crosstrack_params(500.f, 500.f, 60);
#endif // CONFIG_NAVIGATOR_ADSB_F3442
}
void set_conflict(bool &conflict_detected)
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
void set_f3442_params(float nmac_radius, float nmac_height, float wc_radius,
float wc_height, int nmac_latency_s, int wc_latency_s)
{
_conflict_detected = conflict_detected;
param_set(param_handle(px4::params::DAA_LVL_CRIT_RAD), &nmac_radius);
param_set(param_handle(px4::params::DAA_LVL_CRIT_HGT), &nmac_height);
param_set(param_handle(px4::params::DAA_LVL_HIGH_RAD), &wc_radius);
param_set(param_handle(px4::params::DAA_LVL_HIGH_HGT), &wc_height);
param_set(param_handle(px4::params::DAA_LVL_MED_TIME), &nmac_latency_s);
param_set(param_handle(px4::params::DAA_LVL_LOW_TIME), &wc_latency_s);
}
#endif // CONFIG_NAVIGATOR_ADSB_F3442
void set_crosstrack_params(float horizontal_separation_m,
float vertical_separation_m, int collision_time_s)
{
param_set(param_handle(px4::params::NAV_TRAFF_A_HOR), &horizontal_separation_m);
param_set(param_handle(px4::params::NAV_TRAFF_A_VER), &vertical_separation_m);
param_set(param_handle(px4::params::NAV_TRAFF_COLL_T), &collision_time_s);
}
// Build a traffic report from a relative encounter against the fixed ownship state.
transponder_report_s create_relative_report(const RelativeTrafficScenario &scenario)
{
transponder_report_s report{};
report.timestamp = hrt_absolute_time();
waypoint_from_heading_and_distance(uav_lat_lon_(0), uav_lat_lon_(1),
scenario.bearing_rad, scenario.distance_m, &report.lat, &report.lon);
report.altitude = uav_alt_ + scenario.altitude_offset_m;
report.heading = scenario.heading_rad;
report.hor_velocity = scenario.hor_speed_m_s;
report.ver_velocity = scenario.ver_speed_up_m_s;
return report;
}
daa_input_s create_daa_input(const matrix::Vector2d &uav_lat_lon, const float uav_alt,
const matrix::Vector3f &uav_vel_ned, const transponder_report_s &report) const
{
daa_input_s daa_input{};
daa_input.uav_lat_lon = uav_lat_lon;
daa_input.uav_alt = uav_alt;
daa_input.uav_vel_ned = uav_vel_ned;
daa_input.transponder_report = report;
return daa_input;
}
};
TEST_F(AdsbConflictTest, detectTrafficConflict)
TEST_F(AdsbConflictTest, RejectsInvalidAircraftState)
{
int collision_time_threshold = 60;
AdsbConflict adsb_conflict;
const RelativeTrafficScenario valid_traffic{200.f, math::radians(90.f), 0.f, 0.f, 30.f, 0.f};
float crosstrack_separation = 500.0f;
float vertical_separation = 500.0f;
detect_and_avoid_s daa_output{};
transponder_report_s report = create_relative_report(valid_traffic);
const float nan = std::numeric_limits<float>::quiet_NaN();
const float inf = std::numeric_limits<float>::infinity();
double lat_now = 32.617013;
double lon_now = -96.490564;
float alt_now = 1000.0f;
// non-finite ownship coordinate
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(matrix::Vector2d(nan, uav_lat_lon_(1)), uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(matrix::Vector2d(uav_lat_lon_(0), nan), uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(matrix::Vector2d(90.1, uav_lat_lon_(1)), uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(matrix::Vector2d(uav_lat_lon_(0), -180.1), uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
float vx_now = 0.0f;
float vy_now = 0.0f;
float vz_now = 0.0f;
report.lat = nan;
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
report = create_relative_report(valid_traffic);
report.lon = 180.1;
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
uint32_t traffic_dataset_size = sizeof(traffic_dataset) / sizeof(traffic_dataset[0]);
report = create_relative_report(valid_traffic);
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, nan,
stationary_uav_vel_ned_, report), daa_output));
report.altitude = inf;
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
TestAdsbConflict adsb_conflict;
report = create_relative_report(valid_traffic);
report.hor_velocity = inf;
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
report.hor_velocity = -1.f;
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
adsb_conflict.set_conflict_detection_params(crosstrack_separation, vertical_separation, collision_time_threshold, 1);
for (uint32_t i = 0; i < traffic_dataset_size; i++) {
struct traffic_data_s traffic = traffic_dataset[i];
// GIVEN traffic dataset (which should result in conflict)
adsb_conflict._transponder_report.lat = traffic.lat_traffic;
adsb_conflict._transponder_report.lon = traffic.lon_traffic;
adsb_conflict._transponder_report.altitude = traffic.alt_traffic;
adsb_conflict._transponder_report.heading = traffic.heading_traffic;
adsb_conflict._transponder_report.hor_velocity = traffic.vxy_traffic;
adsb_conflict._transponder_report.ver_velocity = traffic.vz_traffic;
// WHEN detect traffic conflict is called
adsb_conflict.detect_traffic_conflict(lat_now, lon_now, alt_now, vx_now, vy_now, vz_now);
// THEN expect conflict to be detected
EXPECT_TRUE(adsb_conflict._conflict_detected == traffic.in_conflict);
}
report = create_relative_report(valid_traffic);
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
matrix::Vector3f(0.f, nan, 0.f), report), daa_output));
}
TEST_F(AdsbConflictTest, trafficAlerts)
#if !defined(CONFIG_NAVIGATOR_ADSB_F3442) || !CONFIG_NAVIGATOR_ADSB_F3442
TEST_F(AdsbConflictTest, CrosstrackRejectsNonFiniteTrafficHeading)
{
struct traffic_buffer_s used_buffer;
used_buffer.icao_address.push_back(2345);
used_buffer.icao_address.push_back(1234);
used_buffer.icao_address.push_back(1897);
used_buffer.icao_address.push_back(0567);
used_buffer.icao_address.push_back(8685);
used_buffer.icao_address.push_back(5000);
AdsbConflict adsb_conflict;
const RelativeTrafficScenario approaching_traffic{200.f, math::radians(90.f), 0.f, math::radians(270.f), 30.f, 0.f};
used_buffer.timestamp.push_back(3_s);
used_buffer.timestamp.push_back(800_s);
used_buffer.timestamp.push_back(100_s);
used_buffer.timestamp.push_back(20000_s);
used_buffer.timestamp.push_back(6000_s);
used_buffer.timestamp.push_back(6587_s);
detect_and_avoid_s daa_output{};
transponder_report_s report = create_relative_report(approaching_traffic);
const float nan = std::numeric_limits<float>::quiet_NaN();
struct traffic_buffer_s full_buffer;
full_buffer.icao_address.push_back(2345);
full_buffer.icao_address.push_back(1234);
full_buffer.icao_address.push_back(1897);
full_buffer.icao_address.push_back(0567);
full_buffer.icao_address.push_back(8685);
full_buffer.icao_address.push_back(5000);
full_buffer.icao_address.push_back(0000);
full_buffer.icao_address.push_back(2);
full_buffer.icao_address.push_back(589742397);
full_buffer.icao_address.push_back(99999);
report.heading = nan;
EXPECT_FALSE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
}
#endif // !CONFIG_NAVIGATOR_ADSB_F3442
full_buffer.timestamp.push_back(1_s);
full_buffer.timestamp.push_back(800_s);
full_buffer.timestamp.push_back(100_s);
full_buffer.timestamp.push_back(20000_s);
full_buffer.timestamp.push_back(6000_s);
full_buffer.timestamp.push_back(19000_s);
full_buffer.timestamp.push_back(5000_s);
full_buffer.timestamp.push_back(2_s);
full_buffer.timestamp.push_back(1000_s);
full_buffer.timestamp.push_back(58943_s);
// try_updating_params() validates against the built standard and rejects bad values.
TEST_F(AdsbConflictTest, UsesBuiltStandardForParamValidation)
{
AdsbConflict adsb_conflict;
constexpr float invalid_bound{-1.f};
struct traffic_buffer_s empty_buffer = {};
EXPECT_TRUE(adsb_conflict.try_updating_params());
TestAdsbConflict adsb_conflict;
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
set_f3442_params(invalid_bound, 10.f, 100.f, 50.f, 30, 40);
#else
set_crosstrack_params(invalid_bound, 500.f, 60);
#endif // CONFIG_NAVIGATOR_ADSB_F3442
// GIVEN used buffer
adsb_conflict.set_traffic_buffer(used_buffer);
// WHEN no conflict detected
bool conflict_detected = false;
hrt_abstime now = 0_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 00001;
adsb_conflict.get_traffic_state(now);
// THEN expect no conflict
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
// GIVEN conflict detected
conflict_detected = true;
now = 1_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 9876;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict to be added to buffer
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
// GIVEN empty buffer
adsb_conflict.set_traffic_buffer(empty_buffer);
// WHEN conflict detected
conflict_detected = true;
now = 0_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 9876;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict to be added to buffer
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
// GIVEN full buffer
adsb_conflict.set_traffic_buffer(full_buffer);
// WHEN conflict detected
conflict_detected = true;
now = 1_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 7777;
adsb_conflict.get_traffic_state(now);
// THEN expect buffer full
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::BUFFER_FULL);
// WHEN conflict set to false again
conflict_detected = false;
now = 2_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 7777;
adsb_conflict.get_traffic_state(now);
// THEN expect no conflict message
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
// WHEN existing conflict is set to false
conflict_detected = false;
now = 3_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict to be removed from buffer
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
// GIVEN used buffer
adsb_conflict.set_traffic_buffer(used_buffer);
// WHEN conflict is set to false again
conflict_detected = false;
now = 0_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict to be removed from buffer
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
EXPECT_FALSE(adsb_conflict.try_updating_params());
}
TEST_F(AdsbConflictTest, trafficReminder)
// The wrapper forwards the built standard's result (level + distances).
TEST_F(AdsbConflictTest, DelegatesToBuiltStandardAndForwardsOutput)
{
struct traffic_buffer_s used_buffer;
used_buffer.icao_address.push_back(2345);
used_buffer.icao_address.push_back(1234);
used_buffer.icao_address.push_back(1897);
used_buffer.icao_address.push_back(0567);
used_buffer.icao_address.push_back(8685);
used_buffer.icao_address.push_back(5000);
AdsbConflict adsb_conflict;
const RelativeTrafficScenario approaching_traffic{19.f, math::radians(90.f), 9.f, math::radians(270.f), 5.f, 0.f};
const transponder_report_s report = create_relative_report(approaching_traffic);
used_buffer.timestamp.push_back(3_s);
used_buffer.timestamp.push_back(80_s);
used_buffer.timestamp.push_back(10_s);
used_buffer.timestamp.push_back(1000_s);
used_buffer.timestamp.push_back(100_s);
used_buffer.timestamp.push_back(187_s);
detect_and_avoid_s daa_output{};
struct traffic_buffer_s full_buffer;
full_buffer.icao_address.push_back(2345);
full_buffer.icao_address.push_back(1234);
full_buffer.icao_address.push_back(1897);
full_buffer.icao_address.push_back(0567);
full_buffer.icao_address.push_back(8685);
full_buffer.icao_address.push_back(5000);
full_buffer.icao_address.push_back(0000);
full_buffer.icao_address.push_back(2);
full_buffer.icao_address.push_back(589742397);
full_buffer.icao_address.push_back(99999);
ASSERT_TRUE(adsb_conflict.try_updating_params());
ASSERT_TRUE(adsb_conflict.calculate_daa_output(create_daa_input(uav_lat_lon_, uav_alt_,
stationary_uav_vel_ned_, report), daa_output));
full_buffer.timestamp.push_back(1_s);
full_buffer.timestamp.push_back(80_s);
full_buffer.timestamp.push_back(10_s);
full_buffer.timestamp.push_back(1000_s);
full_buffer.timestamp.push_back(100_s);
full_buffer.timestamp.push_back(900_s);
full_buffer.timestamp.push_back(500_s);
full_buffer.timestamp.push_back(2_s);
full_buffer.timestamp.push_back(100_s);
full_buffer.timestamp.push_back(5843_s);
TestAdsbConflict adsb_conflict;
// GIVEN buffer with 8685 at t=100
adsb_conflict.set_traffic_buffer(used_buffer);
// WHEN conflict detected at t=200
bool conflict_detected = true;
hrt_abstime now = 200_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict reminder
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT);
// WHEN INSTEAD conflict is detected only 1s later
conflict_detected = true;
now = 201_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN do not sent conflict notification again
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
// GIVEN full buffer with 8685 at t=100
adsb_conflict.set_traffic_buffer(full_buffer);
// WHEN conflict detected at t=400
conflict_detected = true;
now = 400_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict reminder
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMIND_CONFLICT);
// WHEN INSTEAD conflict is detected only 1s later
conflict_detected = true;
now = 401_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN do not sent conflict notification again
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::NO_CONFLICT);
// WHEN conflict is set to false
conflict_detected = false;
now = 600_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 8685;
adsb_conflict.get_traffic_state(now);
// THEN expect conflict to be removed from buffer
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::REMOVE_OLD_CONFLICT);
// WHEN new conflict is detected
conflict_detected = true;
now = 700_s;
adsb_conflict.set_conflict(conflict_detected);
adsb_conflict._transponder_report.icao_address = 7777;
adsb_conflict.get_traffic_state(now);
// THEN expect new conflict to be added to buffer
printf("adsb_conflict._traffic_state %d \n", (int)adsb_conflict._traffic_state);
EXPECT_TRUE(adsb_conflict._traffic_state == TRAFFIC_STATE::ADD_CONFLICT);
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
EXPECT_EQ(daa_output.conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL);
EXPECT_NEAR(daa_output.aircraft_dist_hor, approaching_traffic.distance_m, 0.1f);
EXPECT_NEAR(daa_output.aircraft_dist_vert, approaching_traffic.altitude_offset_m, 0.1f);
#else
EXPECT_EQ(daa_output.conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH);
EXPECT_NEAR(daa_output.aircraft_dist_vert, approaching_traffic.altitude_offset_m, 0.1f);
EXPECT_LT(fabsf(daa_output.aircraft_dist_hor), 500.f);
#endif // CONFIG_NAVIGATOR_ADSB_F3442
}

31
src/lib/adsb/CMakeLists.txt
View File

@@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2022 PX4 Development Team. All rights reserved.
# Copyright (c) 2022-2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
@@ -31,9 +31,34 @@
#
############################################################################
px4_add_library(adsb AdsbConflict.cpp)
set(adsb_srcs
AdsbConflict.cpp
ConflictTracker.cpp
DaaActionPolicy.cpp
DaaEncodedId.cpp
)
if(CONFIG_NAVIGATOR_ADSB_F3442)
list(APPEND adsb_srcs f3442_standard/DaaF3442.cpp)
else()
list(APPEND adsb_srcs crosstrack_standard/DaaCrosstrack.cpp)
endif()
px4_add_library(adsb ${adsb_srcs})
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/parameters.yaml)
target_link_libraries(adsb PUBLIC geo)
px4_add_functional_gtest(SRC AdsbConflictTest.cpp LINKLIBS adsb)
if(CONFIG_NAVIGATOR_ADSB)
px4_add_functional_gtest(SRC AdsbConflictTest.cpp LINKLIBS adsb)
px4_add_functional_gtest(SRC ConflictTrackerTest.cpp LINKLIBS adsb)
px4_add_unit_gtest(SRC DaaActionPolicyTest.cpp LINKLIBS adsb)
px4_add_unit_gtest(SRC DaaEncodedIdTest.cpp LINKLIBS adsb)
if(CONFIG_NAVIGATOR_ADSB_F3442)
px4_add_functional_gtest(SRC f3442_standard/DaaF3442Test.cpp LINKLIBS adsb)
else()
px4_add_functional_gtest(SRC crosstrack_standard/DaaCrosstrackTest.cpp LINKLIBS adsb)
endif()
endif()

283
src/lib/adsb/ConflictTracker.cpp Normal file
View File

@@ -0,0 +1,283 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ConflictTracker.cpp
*
* Fixed-size buffer of active DAA conflicts.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "ConflictTracker.h"
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
namespace
{
void append_change(conflict_tracker_changes_s &changes, const conflict_tracker_change_s &change)
{
if (!changes.push_back(change)) {
PX4_ERR("DAA: conflict changes overflow");
}
}
void record_ignored(conflict_tracker_changes_s &changes, const conflict_info_s &conflict, const IgnoreTrafficCause cause)
{
conflict_tracker_change_s change{};
change.type = ConflictTrackerChangeType::kReportIgnored;
change.conflict = conflict;
change.ignore_cause = cause;
append_change(changes, change);
}
void record_removed(conflict_tracker_changes_s &changes, const conflict_info_s &conflict, const RemoveBufferCause cause)
{
conflict_tracker_change_s change{};
change.type = ConflictTrackerChangeType::kConflictRemoved;
change.conflict = conflict;
change.remove_cause = cause;
append_change(changes, change);
}
} // namespace
bool ConflictTracker::apply_conflict(const conflict_info_s &conflict, conflict_tracker_changes_s &changes)
{
if (conflict.encoded_id.id == 0
|| conflict.conflict_level > detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL
|| !PX4_ISFINITE(conflict.aircraft_dist)
|| conflict.aircraft_dist < 0.f) {
PX4_ERR("DAA: invalid conflict");
return false;
}
const int conflict_idx = find_conflict_idx(conflict.encoded_id);
if (conflict_idx < 0) {
if (conflict.conflict_level == detect_and_avoid_s::DAA_CONFLICT_LVL_NONE) {
return false;
}
return add_conflict(conflict, changes);
}
return update_conflict(conflict, conflict_idx, changes);
}
bool ConflictTracker::add_conflict(const conflict_info_s &conflict, conflict_tracker_changes_s &changes)
{
if (_buffer.size() < _buffer.max_size()) {
_buffer.push_back(conflict);
conflict_tracker_change_s change{};
change.type = ConflictTrackerChangeType::kConflictAdded;
change.conflict = conflict;
append_change(changes, change);
return true;
}
const int least_urgent_conflict_idx = find_conflict_idx_by_priority(ConflictPriority::kLeastUrgent);
if (!is_conflict_more_important(conflict, _buffer[least_urgent_conflict_idx])) {
PX4_DEBUG("DAA: new conflict does not outrank tracked conflicts, ignoring.");
record_ignored(changes, conflict, IgnoreTrafficCause::kBufferFull);
return false;
}
const conflict_info_s removed_conflict = _buffer[least_urgent_conflict_idx];
_buffer[least_urgent_conflict_idx] = conflict;
record_removed(changes, removed_conflict, RemoveBufferCause::kBufferFull);
conflict_tracker_change_s change{};
change.type = ConflictTrackerChangeType::kConflictAdded;
change.conflict = conflict;
append_change(changes, change);
return true;
}
bool ConflictTracker::update_conflict(const conflict_info_s &conflict, const int conflict_idx,
conflict_tracker_changes_s &changes)
{
const uint8_t previous_conflict_level = _buffer[conflict_idx].conflict_level;
if (conflict.conflict_level == detect_and_avoid_s::DAA_CONFLICT_LVL_NONE) {
PX4_DEBUG("DAA: Conflict avoided");
_buffer.remove(conflict_idx);
} else {
_buffer[conflict_idx] = conflict;
}
if (conflict.conflict_level != previous_conflict_level) {
conflict_tracker_change_s change{};
change.type = ConflictTrackerChangeType::kConflictLevelChanged;
change.conflict = conflict;
change.previous_level = previous_conflict_level;
const int most_urgent_idx = find_conflict_idx_by_priority(ConflictPriority::kMostUrgent);
change.conflict_is_most_urgent = (_most_urgent.encoded_id == conflict.encoded_id)
|| (is_valid_idx(most_urgent_idx)
&& _buffer[most_urgent_idx].encoded_id == conflict.encoded_id);
append_change(changes, change);
}
return true;
}
bool ConflictTracker::remove_stale_conflicts(const hrt_abstime now, const hrt_abstime timeout_us,
conflict_tracker_changes_s &changes)
{
int nb_conflicts_removed = 0;
// Iterate backwards because Array::remove() shifts later entries left.
for (int idx = (static_cast<int>(_buffer.size()) - 1); idx >= 0; --idx) {
const hrt_abstime latest_update = _buffer[idx].latest_update_timestamp;
if (latest_update == 0 || latest_update > now || (now - latest_update) > timeout_us) {
const conflict_info_s removed_conflict = _buffer[idx];
_buffer.remove(idx);
record_removed(changes, removed_conflict, RemoveBufferCause::kStaleConflict);
nb_conflicts_removed++;
}
}
if (nb_conflicts_removed > 0) {
PX4_DEBUG("DAA: removed %d stale conflicts from buffer", nb_conflicts_removed);
}
return nb_conflicts_removed > 0;
}
void ConflictTracker::clear()
{
_buffer.clear();
reset_most_urgent();
}
void ConflictTracker::reset_most_urgent()
{
static constexpr float kNoConflictDistance{9999.f};
PX4_DEBUG("DAA: reset most urgent buffer to null.");
_most_urgent = {};
_most_urgent.aircraft_dist = kNoConflictDistance;
}
void ConflictTracker::refresh_most_urgent()
{
if (_buffer.empty()) {
PX4_DEBUG("DAA: refresh_most_urgent empty buffer.");
reset_most_urgent();
return;
}
_most_urgent = find_most_urgent();
}
conflict_info_s ConflictTracker::find_most_urgent() const
{
const int most_urgent_conflict_idx = find_conflict_idx_by_priority(ConflictPriority::kMostUrgent);
if (!is_valid_idx(most_urgent_conflict_idx)) {
return {};
}
return _buffer[most_urgent_conflict_idx];
}
conflict_info_s ConflictTracker::get_conflict(const DaaEncodedId &encoded_id) const
{
const int conflict_idx = find_conflict_idx(encoded_id);
if (!is_valid_idx(conflict_idx)) {
return {};
}
return _buffer[conflict_idx];
}
int ConflictTracker::find_conflict_idx_by_priority(const ConflictPriority priority) const
{
const int buff_size = static_cast<int>(_buffer.size());
if (buff_size < 1) {
return -1;
}
int best_idx = 0;
for (int i = 1; i < buff_size; ++i) {
const bool is_better = (priority == ConflictPriority::kMostUrgent)
? is_conflict_more_important(_buffer[i], _buffer[best_idx])
: is_conflict_less_important(_buffer[i], _buffer[best_idx]);
if (is_better) {
best_idx = i;
}
}
return best_idx;
}
int ConflictTracker::find_conflict_idx(const DaaEncodedId &encoded_id) const
{
for (size_t i = 0; i < _buffer.size(); i++) {
if (_buffer[i].encoded_id == encoded_id) {
return static_cast<int>(i);
}
}
return -1;
}
bool ConflictTracker::is_conflict_less_important(const conflict_info_s &new_conflict,
const conflict_info_s &base_conflict)
{
if (new_conflict.conflict_level != base_conflict.conflict_level) {
return new_conflict.conflict_level < base_conflict.conflict_level;
}
return new_conflict.aircraft_dist > base_conflict.aircraft_dist;
}
bool ConflictTracker::is_conflict_more_important(const conflict_info_s &new_conflict,
const conflict_info_s &base_conflict)
{
if (new_conflict.conflict_level != base_conflict.conflict_level) {
return new_conflict.conflict_level > base_conflict.conflict_level;
}
return new_conflict.aircraft_dist < base_conflict.aircraft_dist;
}

174
src/lib/adsb/ConflictTracker.h Normal file
View File

@@ -0,0 +1,174 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ConflictTracker.h
*
* Fixed-size buffer of active DAA conflicts.
*
* Buffer changes that may require a user-facing message are reported as
* conflict_tracker_change_s entries so the caller can apply its own
* notification policy.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include <containers/Array.hpp>
#include <drivers/drv_hrt.h>
#include <uORB/topics/detect_and_avoid.h>
#include <uORB/topics/transponder_report.h>
#include "DaaEncodedId.h"
static constexpr uint8_t kDaaMaxTraffic{5};
struct conflict_info_s {
DaaEncodedId encoded_id{};
hrt_abstime latest_update_timestamp{0};
uint8_t conflict_level{detect_and_avoid_s::DAA_CONFLICT_LVL_NONE};
float aircraft_dist{0.f}; // Distance to aircraft = sqrtf(dist_hor^2 + dist_vert^2)
};
enum class RemoveBufferCause : uint8_t {
kStaleConflict = 0,
kBufferFull = 1
};
enum class IgnoreTrafficCause : uint8_t {
kBufferFull = 0
};
enum class ConflictTrackerChangeType : uint8_t {
kConflictAdded = 0,
kConflictLevelChanged = 1,
kConflictRemoved = 2,
kReportIgnored = 3
};
struct conflict_tracker_change_s {
ConflictTrackerChangeType type{ConflictTrackerChangeType::kConflictAdded};
conflict_info_s conflict{}; // the affected entry (for kConflictRemoved: the removed entry)
uint8_t previous_level{detect_and_avoid_s::DAA_CONFLICT_LVL_NONE}; // kConflictLevelChanged only
RemoveBufferCause remove_cause{RemoveBufferCause::kStaleConflict}; // kConflictRemoved only
IgnoreTrafficCause ignore_cause{IgnoreTrafficCause::kBufferFull}; // kReportIgnored only
// Required because the caller processes the full Uorb queue before emitting warnings
bool conflict_is_most_urgent{false};
};
// A stale sweep can remove the full buffer and each queued report can replace one entry.
static constexpr uint16_t kMaxConflictChangesPerCycle{kDaaMaxTraffic + 2 * transponder_report_s::ORB_QUEUE_LENGTH};
using conflict_tracker_changes_s = px4::Array<conflict_tracker_change_s, kMaxConflictChangesPerCycle>;
class ConflictTracker
{
public:
ConflictTracker() { reset_most_urgent(); }
~ConflictTracker() = default;
/**
* @brief Apply one evaluated report to the tracked set.
*
* Inserts new conflicts (evicting the least urgent entry when the buffer is full),
* overwrites known conflicts, and drops known conflicts that report
* DAA_CONFLICT_LVL_NONE.
*
* Emitted changes are appended to @p changes. Returns true if the tracked set changed.
*/
bool apply_conflict(const conflict_info_s &conflict, conflict_tracker_changes_s &changes);
/**
* @brief Drop entries whose last update is older than @p timeout_us at time @p now.
*
* Emits one kConflictRemoved (kStaleConflict) change per dropped entry.
* Returns true if at least one entry was removed.
*/
bool remove_stale_conflicts(const hrt_abstime now, const hrt_abstime timeout_us, conflict_tracker_changes_s &changes);
// Drop all conflicts and reset the most-urgent cache.
void clear();
// Recompute the cached most-urgent conflict from the buffer.
void refresh_most_urgent();
// Cached value; only refresh_most_urgent() and clear() update it.
const conflict_info_s &most_urgent() const { return _most_urgent; }
// Highest-priority conflict in the buffer, or id=0 when empty.
conflict_info_s find_most_urgent() const;
bool contains(const DaaEncodedId &encoded_id) const { return find_conflict_idx(encoded_id) >= 0; }
// Tracked entry for encoded_id, or id=0 when not tracked.
conflict_info_s get_conflict(const DaaEncodedId &encoded_id) const;
size_t size() const { return _buffer.size(); }
bool empty() const { return _buffer.empty(); }
// Priority rules: higher conflict level first, ties broken by aircraft distance.
// Public so the priority policy can be unit tested directly.
static bool is_conflict_more_important(const conflict_info_s &new_conflict, const conflict_info_s &base_conflict);
static bool is_conflict_less_important(const conflict_info_s &new_conflict, const conflict_info_s &base_conflict);
private:
enum class ConflictPriority : uint8_t {
kMostUrgent,
kLeastUrgent
};
/** @brief Insert a new conflict, replacing the least urgent entry when the buffer is full. */
bool add_conflict(const conflict_info_s &conflict, conflict_tracker_changes_s &changes);
/**
* @brief Overwrite a tracked entry from a new report.
*
* If the level changed to DAA_CONFLICT_LVL_NONE, the conflict is removed form the buffer.
*
* Emits kConflictLevelChanged when the level differs from the stored entry.
*/
bool update_conflict(const conflict_info_s &conflict, const int conflict_idx, conflict_tracker_changes_s &changes);
/** @brief Return the index of the most or least urgent entry, or -1 if the buffer is empty. */
int find_conflict_idx_by_priority(const ConflictPriority priority) const;
/** @brief Return the buffer index of @p encoded_id, or -1 if not tracked. */
int find_conflict_idx(const DaaEncodedId &encoded_id) const;
void reset_most_urgent();
bool is_valid_idx(const int idx) const { return idx >= 0 && idx < static_cast<int>(_buffer.size()); }
px4::Array<conflict_info_s, kDaaMaxTraffic> _buffer{};
conflict_info_s _most_urgent{};
};

367
src/lib/adsb/ConflictTrackerTest.cpp Normal file
View File

@@ -0,0 +1,367 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ConflictTrackerTest.cpp
* @brief Unit tests for the ConflictTracker conflict buffer.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include <gtest/gtest.h>
#include <limits>
#include <lib/adsb/ConflictTracker.h>
namespace
{
conflict_info_s make_conflict(const uint64_t icao_id, const uint8_t conflict_level, const float aircraft_dist,
const hrt_abstime timestamp = 1000000)
{
conflict_info_s conflict{};
conflict.encoded_id.id = icao_id;
conflict.encoded_id.encoding = detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO;
conflict.conflict_level = conflict_level;
conflict.aircraft_dist = aircraft_dist;
conflict.latest_update_timestamp = timestamp;
return conflict;
}
} // namespace
// Accepted conflict enters the buffer and emits kConflictAdded; most_urgent updates only on refresh.
TEST(ConflictTrackerTest, AddsNewConflictAndRefreshesMostUrgent)
{
ConflictTracker tracker;
EXPECT_TRUE(tracker.empty());
EXPECT_EQ(tracker.most_urgent().conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_EQ(tracker.most_urgent().encoded_id.id, 0u);
const conflict_info_s conflict = make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM, 800.f);
conflict_tracker_changes_s changes{};
EXPECT_TRUE(tracker.apply_conflict(conflict, changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_EQ(changes[0].type, ConflictTrackerChangeType::kConflictAdded);
EXPECT_EQ(changes[0].conflict.encoded_id, conflict.encoded_id);
EXPECT_EQ(tracker.size(), 1u);
EXPECT_TRUE(tracker.contains(conflict.encoded_id));
const conflict_info_s tracked = tracker.get_conflict(conflict.encoded_id);
ASSERT_NE(tracked.encoded_id.id, 0u);
EXPECT_EQ(tracked.conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM);
// most_urgent stays empty until refresh
EXPECT_EQ(tracker.most_urgent().encoded_id.id, 0u);
tracker.refresh_most_urgent();
EXPECT_EQ(tracker.most_urgent().encoded_id, conflict.encoded_id);
}
// NONE-level traffic that isn't tracked never takes a buffer slot.
TEST(ConflictTrackerTest, IgnoresUntrackedTrafficWithoutConflict)
{
ConflictTracker tracker;
const conflict_info_s no_conflict = make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE, 5000.f);
conflict_tracker_changes_s changes{};
EXPECT_FALSE(tracker.apply_conflict(no_conflict, changes));
EXPECT_EQ(changes.size(), 0u);
EXPECT_TRUE(tracker.empty());
}
TEST(ConflictTrackerTest, RejectsInvalidConflictData)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
EXPECT_FALSE(tracker.apply_conflict(
make_conflict(0, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH, 100.f), changes));
EXPECT_FALSE(tracker.apply_conflict(
make_conflict(1, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL + 1, 100.f), changes));
EXPECT_FALSE(tracker.apply_conflict(
make_conflict(1, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH,
std::numeric_limits<float>::quiet_NaN()), changes));
EXPECT_TRUE(tracker.empty());
EXPECT_TRUE(changes.empty());
}
// kConflictLevelChanged is emitted only when the level differs; same-level just overwrites.
TEST(ConflictTrackerTest, EmitsLevelChangedOnlyWhenLevelDiffers)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 2000.f, 1000000), changes);
// same level: overwrite, no change emitted
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 1900.f, 2000000),
changes));
EXPECT_EQ(changes.size(), 0u);
const conflict_info_s tracked = tracker.get_conflict(make_conflict(0x6E9F7B, 0, 0.f).encoded_id);
ASSERT_NE(tracked.encoded_id.id, 0u);
EXPECT_EQ(tracked.latest_update_timestamp, 2000000u);
EXPECT_FLOAT_EQ(tracked.aircraft_dist, 1900.f);
// escalation: one level-changed change carrying the previous level
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH, 600.f, 3000000),
changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_EQ(changes[0].type, ConflictTrackerChangeType::kConflictLevelChanged);
EXPECT_EQ(changes[0].conflict.conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH);
EXPECT_EQ(changes[0].previous_level, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW);
// de-escalation to a still-active level
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM, 900.f,
4000000), changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_EQ(changes[0].previous_level, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH);
EXPECT_EQ(tracker.size(), 1u);
}
// A resolved conflict leaves the buffer and is reported as a level change to NONE, not a removal,
// so the caller can announce "solved" instead of a drop warning.
TEST(ConflictTrackerTest, ResolvedConflictLeavesBufferAsLevelChange)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH, 600.f), changes);
tracker.refresh_most_urgent();
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE, 5000.f),
changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_EQ(changes[0].type, ConflictTrackerChangeType::kConflictLevelChanged);
EXPECT_EQ(changes[0].conflict.conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_EQ(changes[0].previous_level, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH);
EXPECT_TRUE(tracker.empty());
// refresh resets the cache to no-conflict
tracker.refresh_most_urgent();
EXPECT_EQ(tracker.most_urgent().conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_EQ(tracker.most_urgent().encoded_id.id, 0u);
}
// conflict_is_most_urgent must be set per change, resolved from both the cache and the live buffer
// (the caller defers secondary warnings until the whole transponder queue is processed).
TEST(ConflictTrackerTest, LevelChangeCapturesMostUrgent)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
// critical (most urgent) plus a secondary low conflict
tracker.apply_conflict(make_conflict(0xA, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 100.f), changes);
tracker.apply_conflict(make_conflict(0xB, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 2000.f), changes);
tracker.refresh_most_urgent();
// secondary escalates but stays below the most urgent: not flagged
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0xB, detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM, 1500.f),
changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_FALSE(changes[0].conflict_is_most_urgent);
// most urgent changes level: flagged via the cache
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0xA, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH, 400.f), changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_TRUE(changes[0].conflict_is_most_urgent);
// a new conflict overtakes as most urgent while the cache still points at 0xA: flagged via buffer
changes = {};
tracker.apply_conflict(make_conflict(0xC, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 3000.f), changes);
changes = {};
EXPECT_TRUE(tracker.apply_conflict(make_conflict(0xC, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 50.f),
changes));
ASSERT_EQ(changes.size(), 1u);
EXPECT_TRUE(changes[0].conflict_is_most_urgent);
}
// A more urgent conflict displaces the least urgent when the buffer is full, reporting the removal
// before the insertion.
TEST(ConflictTrackerTest, EvictsLeastUrgentWhenFull)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
// fill so that 0x100 is least urgent (lowest level, largest distance)
for (uint64_t i = 0; i < kDaaMaxTraffic; ++i) {
changes = {};
ASSERT_TRUE(tracker.apply_conflict(make_conflict(0x100 + i, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW + (i > 0),
2000.f - static_cast<float>(i) * 100.f), changes));
}
ASSERT_EQ(tracker.size(), static_cast<size_t>(kDaaMaxTraffic));
const conflict_info_s critical = make_conflict(0xBEEF, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 100.f);
changes = {};
EXPECT_TRUE(tracker.apply_conflict(critical, changes));
// removal first, then insertion
ASSERT_EQ(changes.size(), 2u);
EXPECT_EQ(changes[0].type, ConflictTrackerChangeType::kConflictRemoved);
EXPECT_EQ(changes[0].remove_cause, RemoveBufferCause::kBufferFull);
EXPECT_EQ(changes[0].conflict.encoded_id.id, 0x100u);
EXPECT_EQ(changes[1].type, ConflictTrackerChangeType::kConflictAdded);
EXPECT_EQ(changes[1].conflict.encoded_id.id, 0xBEEFu);
EXPECT_EQ(tracker.size(), static_cast<size_t>(kDaaMaxTraffic));
EXPECT_FALSE(tracker.contains(make_conflict(0x100, 0, 0.f).encoded_id));
tracker.refresh_most_urgent();
EXPECT_EQ(tracker.most_urgent().encoded_id.id, 0xBEEFu);
}
// A full buffer rejects weaker traffic and keeps its entries.
TEST(ConflictTrackerTest, RejectsLessImportantWhenFull)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
for (uint64_t i = 0; i < kDaaMaxTraffic; ++i) {
changes = {};
ASSERT_TRUE(tracker.apply_conflict(make_conflict(0x200 + i, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH,
600.f + static_cast<float>(i)), changes));
}
changes = {};
EXPECT_FALSE(tracker.apply_conflict(make_conflict(0xBEEF, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 2000.f), changes));
// reported ignored (buffer full), nothing displaced
ASSERT_EQ(changes.size(), 1u);
EXPECT_EQ(changes[0].type, ConflictTrackerChangeType::kReportIgnored);
EXPECT_EQ(changes[0].ignore_cause, IgnoreTrafficCause::kBufferFull);
EXPECT_EQ(changes[0].conflict.encoded_id.id, 0xBEEFu);
EXPECT_EQ(tracker.size(), static_cast<size_t>(kDaaMaxTraffic));
EXPECT_FALSE(tracker.contains(make_conflict(0xBEEF, 0, 0.f).encoded_id));
}
// Priority: level dominates, distance only breaks ties.
TEST(ConflictTrackerTest, PriorityIsLevelFirstThenDistance)
{
const conflict_info_s far_critical = make_conflict(1, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 3000.f);
const conflict_info_s close_low = make_conflict(2, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 10.f);
const conflict_info_s close_critical = make_conflict(3, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 100.f);
const conflict_info_s same_as_close_critical =
make_conflict(4, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 100.f);
// level dominates distance
EXPECT_TRUE(ConflictTracker::is_conflict_more_important(far_critical, close_low));
EXPECT_TRUE(ConflictTracker::is_conflict_less_important(close_low, far_critical));
// same level: distance breaks the tie
EXPECT_TRUE(ConflictTracker::is_conflict_more_important(close_critical, far_critical));
EXPECT_TRUE(ConflictTracker::is_conflict_less_important(far_critical, close_critical));
EXPECT_FALSE(ConflictTracker::is_conflict_more_important(close_critical, same_as_close_critical));
EXPECT_FALSE(ConflictTracker::is_conflict_less_important(close_critical, same_as_close_critical));
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
// insert out of priority order; closest critical wins
tracker.apply_conflict(close_low, changes);
tracker.apply_conflict(far_critical, changes);
tracker.apply_conflict(close_critical, changes);
const conflict_info_s most_urgent = tracker.find_most_urgent();
ASSERT_NE(most_urgent.encoded_id.id, 0u);
EXPECT_EQ(most_urgent.encoded_id.id, 3u);
}
// Stale sweep drops timed-out and never-stamped entries, keeps fresh ones, and reports each drop.
TEST(ConflictTrackerTest, RemovesOnlyStaleConflicts)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
static constexpr hrt_abstime kTimeout{10000000}; // 10 s
static constexpr hrt_abstime kNow{20000000}; // 20 s
tracker.apply_conflict(make_conflict(1, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 2000.f, kNow - 1000000), changes);
tracker.apply_conflict(make_conflict(2, detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH, 600.f, kNow - kTimeout - 1),
changes);
tracker.apply_conflict(make_conflict(3, detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM, 900.f, 0), changes);
tracker.apply_conflict(make_conflict(4, detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, 1200.f, kNow + 1), changes);
changes = {};
EXPECT_TRUE(tracker.remove_stale_conflicts(kNow, kTimeout, changes));
// Timed-out, missing, and future timestamps are invalid; the fresh entry survives.
ASSERT_EQ(changes.size(), 3u);
for (size_t i = 0; i < changes.size(); ++i) {
EXPECT_EQ(changes[i].type, ConflictTrackerChangeType::kConflictRemoved);
EXPECT_EQ(changes[i].remove_cause, RemoveBufferCause::kStaleConflict);
}
EXPECT_EQ(tracker.size(), 1u);
EXPECT_TRUE(tracker.contains(make_conflict(1, 0, 0.f).encoded_id));
// second sweep removes nothing
changes = {};
EXPECT_FALSE(tracker.remove_stale_conflicts(kNow, kTimeout, changes));
EXPECT_EQ(changes.size(), 0u);
}
// clear() empties the buffer and resets the most-urgent cache.
TEST(ConflictTrackerTest, ClearResetsBufferAndMostUrgent)
{
ConflictTracker tracker;
conflict_tracker_changes_s changes{};
tracker.apply_conflict(make_conflict(0x6E9F7B, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, 100.f), changes);
tracker.refresh_most_urgent();
EXPECT_EQ(tracker.most_urgent().conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL);
tracker.clear();
EXPECT_TRUE(tracker.empty());
EXPECT_EQ(tracker.most_urgent().conflict_level, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_EQ(tracker.most_urgent().encoded_id.id, 0u);
EXPECT_EQ(tracker.most_urgent().encoded_id.encoding, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO);
}

261
src/lib/adsb/DaaActionPolicy.cpp Normal file
View File

@@ -0,0 +1,261 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaActionPolicy.cpp
*
* DAA action policy.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "DaaActionPolicy.h"
#include <px4_platform_common/log.h>
#include <uORB/topics/detect_and_avoid.h>
#include <uORB/topics/vehicle_status.h>
daa_action_decision_s DaaActionPolicy::decide(const uint8_t conflict_level, const uint8_t prev_conflict_level,
const uint8_t nav_state, const bool landed, const bool armed,
const DaaAction previous_action, const daa_action_params_s &params)
{
daa_action_decision_s decision{};
const DaaAction requested_action = action_from_conflict_level(conflict_level, params);
if (landed) {
if (requested_action <= DaaAction::kWarnOnly) {
return decision;
}
decision.warn_on_ground = true;
decision.ground_warning_cause = armed ? NotifyLandedActCause::kConflictAndArmed
: NotifyLandedActCause::kConflictAndDisarmed;
return decision;
}
if (conflict_level == prev_conflict_level) {
return decision;
}
const bool conflict_escalated = conflict_level > prev_conflict_level;
if (conflict_escalated && action_escalates_above_nav_state(requested_action, nav_state)) {
decision.action_command = requested_action;
decision.announce_action = previous_action != requested_action
|| prev_conflict_level == detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
return decision;
}
if (!conflict_escalated) {
PX4_DEBUG("DAA: De-escalation, prev act %d, nav state %d, ", static_cast<int>(previous_action), nav_state);
}
return decision;
}
DaaAction DaaActionPolicy::action_from_conflict_level(const uint8_t conflict_level,
const daa_action_params_s &params)
{
if (conflict_level < detect_and_avoid_s::DAA_CONFLICT_LVL_LOW
|| conflict_level > detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL) {
return DaaAction::kDisabled;
}
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
switch (conflict_level) {
case detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL: {
DaaAction action = action_param_to_daa_action(params.lvl_crit_act);
if (action != DaaAction::kDisabled) {
return action;
}
action = action_param_to_daa_action(params.lvl_high_act);
if (action != DaaAction::kDisabled) {
return action;
}
action = action_param_to_daa_action(params.lvl_med_act);
if (action != DaaAction::kDisabled) {
return action;
}
return action_param_to_daa_action(params.lvl_low_act);
}
case detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH: {
const DaaAction action = action_param_to_daa_action(params.lvl_high_act);
return action != DaaAction::kDisabled ? action : action_param_to_daa_action(params.lvl_low_act);
}
case detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM: {
const DaaAction action = action_param_to_daa_action(params.lvl_med_act);
return action != DaaAction::kDisabled ? action : action_param_to_daa_action(params.lvl_low_act);
}
case detect_and_avoid_s::DAA_CONFLICT_LVL_LOW: {
return action_param_to_daa_action(params.lvl_low_act);
}
}
return DaaAction::kDisabled;
#else
return action_param_to_daa_action(params.traff_avoid);
#endif // CONFIG_NAVIGATOR_ADSB_F3442
}
bool DaaActionPolicy::action_escalates_above_nav_state(const DaaAction requested_action, const uint8_t nav_state)
{
if (requested_action <= DaaAction::kWarnOnly) {
PX4_DEBUG("DAA: Escalation to Warn, no action published");
return false;
}
const DaaAction current_nav_state_action = nav_state_to_equivalent_daa_action(nav_state);
// Only publish command if requested command is more critical than current navigator state.
if (requested_action <= current_nav_state_action) {
PX4_DEBUG("DAA: Requested action: %d less critical than current nav state: %d, no action published",
(int)requested_action,
(int)nav_state);
return false;
}
return true;
}
DaaAction DaaActionPolicy::nav_state_to_equivalent_daa_action(const uint8_t nav_state)
{
switch (nav_state) {
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
case vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE: {
return DaaAction::kDisabled;
}
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: {
return DaaAction::kPositionHoldMode;
}
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: {
return DaaAction::kReturnMode;
}
case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
case vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND:
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
case vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE:
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
case vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW:
case vehicle_status_s::NAVIGATION_STATE_ACRO:
case vehicle_status_s::NAVIGATION_STATE_STAB: {
return DaaAction::kLandMode;
}
case vehicle_status_s::NAVIGATION_STATE_TERMINATION: {
return DaaAction::kTerminate;
}
// OFFBOARD special handling, responsibility is given to offboard
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
return DaaAction::kMaxActionValue;
}
default:
break;
}
// Unknown states are treated as highest priority so DAA will not override them.
return DaaAction::kMaxActionValue;
}
DaaAction DaaActionPolicy::action_param_to_daa_action(const int32_t action_param)
{
switch (action_param) {
case 0:
return DaaAction::kDisabled;
case 1:
return DaaAction::kWarnOnly;
case 2:
return DaaAction::kReturnMode;
case 3:
return DaaAction::kLandMode;
case 4:
return DaaAction::kPositionHoldMode;
case 5:
return DaaAction::kTerminate;
default:
return DaaAction::kDisabled;
}
}
uint8_t DaaActionPolicy::daa_action_to_action_param(const DaaAction action)
{
// Inverse of action_param_to_daa_action. Operator messages report actions with the same
// numbering as the NAV_TRAFF_AVOID / DAA_LVL_*_ACT parameters, not the internal severity ladder.
switch (action) {
case DaaAction::kDisabled:
return 0;
case DaaAction::kWarnOnly:
return 1;
case DaaAction::kReturnMode:
return 2;
case DaaAction::kLandMode:
return 3;
case DaaAction::kPositionHoldMode:
return 4;
case DaaAction::kTerminate:
return 5;
default:
return 0;
}
}

121
src/lib/adsb/DaaActionPolicy.h Normal file
View File

@@ -0,0 +1,121 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaActionPolicy.h
*
* Maps conflict levels to actions and decides the
* automated response for the most urgent conflict escalation.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include <stdint.h>
#include <px4_platform_common/defines.h> // Provides CONFIG_NAVIGATOR_ADSB_F3442
// Internal action priority order used for escalation comparisons.
// User-facing parameter values are translated separately so DAA_LVL_*_ACT can
// share the NAV_TRAFF_AVOID convention without changing the priority ladder.
enum class DaaAction : uint8_t {
kDisabled = 0,
kWarnOnly = 1,
kPositionHoldMode = 2,
kReturnMode = 3,
kLandMode = 4,
kTerminate = 5,
kMaxActionValue = 6
};
enum class NotifyLandedActCause : uint8_t {
kConflictAndArmed = 0,
kConflictAndDisarmed = 1
};
// Action parameter values (NAV_TRAFF_AVOID / DAA_LVL_*_ACT convention), read by the caller.
struct daa_action_params_s {
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
int32_t lvl_low_act {0};
int32_t lvl_med_act{0};
int32_t lvl_high_act{0};
int32_t lvl_crit_act{0};
#else
int32_t traff_avoid {0};
#endif // CONFIG_NAVIGATOR_ADSB_F3442
};
// A command is only requested in the air; a ground warning is only requested when landed.
struct daa_action_decision_s {
DaaAction action_command{DaaAction::kDisabled};
bool announce_action{false};
bool warn_on_ground{false};
NotifyLandedActCause ground_warning_cause{NotifyLandedActCause::kConflictAndDisarmed};
};
class DaaActionPolicy
{
public:
/**
* @brief Decide the automated response to a most urgent conflict level escalation.
*
* When landed, action-requiring conflicts request a ground warning instead of a command.
* The caller is responsible for rate-limiting repeated warnings.
*
* In the air, commands are requested only on level escalation and only when stronger
* than the current navigator state. Unchanged levels and de-escalations do not act.
*/
static daa_action_decision_s decide(const uint8_t conflict_level, const uint8_t prev_conflict_level,
const uint8_t nav_state, const bool landed, const bool armed,
const DaaAction previous_action, const daa_action_params_s &params);
/**
* @brief Map a conflict level to a DAA action.
*
* For F3442, a disabled level falls back only to zones guaranteed to contain
* the reported zone.
*/
static DaaAction action_from_conflict_level(const uint8_t conflict_level, const daa_action_params_s &params);
// True if requested_action is more urgent than the mode equivalent of nav_state.
static bool action_escalates_above_nav_state(const DaaAction requested_action, const uint8_t nav_state);
// DAA action equivalent to a navigation state (used to gate escalation).
static DaaAction nav_state_to_equivalent_daa_action(const uint8_t nav_state);
// Convert a user-facing action param value into the internal DaaAction enum.
static DaaAction action_param_to_daa_action(const int32_t action_param);
// Inverse of action_param_to_daa_action.
static uint8_t daa_action_to_action_param(const DaaAction action);
};

332
src/lib/adsb/DaaActionPolicyTest.cpp Normal file
View File

@@ -0,0 +1,332 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaActionPolicyTest.cpp
* @brief Unit tests for the DAA action policy.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include <gtest/gtest.h>
#include <lib/adsb/DaaActionPolicy.h>
#include <uORB/topics/detect_and_avoid.h>
#include <uORB/topics/vehicle_status.h>
namespace
{
// Same action parameter value for every conflict level, so the decide() tests
// behave identically in F3442 and Crosstrack builds.
daa_action_params_s params_for_all_levels(const int32_t action_param)
{
daa_action_params_s params{};
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
params.lvl_low_act = action_param;
params.lvl_med_act = action_param;
params.lvl_high_act = action_param;
params.lvl_crit_act = action_param;
#else
params.traff_avoid = action_param;
#endif // CONFIG_NAVIGATOR_ADSB_F3442
return params;
}
// Navigation state groups by the weakest DAA action allowed to escalate above them.
const uint8_t kStatesEnableHold[] {
vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION,
vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF,
vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET,
vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF,
vehicle_status_s::NAVIGATION_STATE_GUIDED_COURSE,
};
const uint8_t kStatesEnableRtl[] {
vehicle_status_s::NAVIGATION_STATE_ORBIT,
vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER,
};
const uint8_t kStatesEnableLand[] {
vehicle_status_s::NAVIGATION_STATE_AUTO_RTL,
};
const uint8_t kStatesEnableTermination[] {
vehicle_status_s::NAVIGATION_STATE_AUTO_LAND,
vehicle_status_s::NAVIGATION_STATE_DESCEND,
vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND,
vehicle_status_s::NAVIGATION_STATE_MANUAL,
vehicle_status_s::NAVIGATION_STATE_ALTCTL,
vehicle_status_s::NAVIGATION_STATE_ALTITUDE_CRUISE,
vehicle_status_s::NAVIGATION_STATE_POSCTL,
vehicle_status_s::NAVIGATION_STATE_POSITION_SLOW,
vehicle_status_s::NAVIGATION_STATE_ACRO,
vehicle_status_s::NAVIGATION_STATE_STAB,
};
const uint8_t kStatesNeverEnable[] {
vehicle_status_s::NAVIGATION_STATE_TERMINATION,
vehicle_status_s::NAVIGATION_STATE_OFFBOARD,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL1,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL2,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL3,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL4,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL5,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL6,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL7,
vehicle_status_s::NAVIGATION_STATE_EXTERNAL8,
};
struct escalation_case_s {
DaaAction action{DaaAction::kDisabled};
bool hold_allowed{false};
bool rtl_allowed{false};
bool land_allowed{false};
bool terminate_allowed{false};
};
void check_escalation_matrix(const escalation_case_s &test_case)
{
for (const uint8_t nav_state : kStatesEnableHold) {
EXPECT_EQ(DaaActionPolicy::action_escalates_above_nav_state(test_case.action, nav_state),
test_case.hold_allowed) << "action " << (int)test_case.action << " nav_state " << (int)nav_state;
}
for (const uint8_t nav_state : kStatesEnableRtl) {
EXPECT_EQ(DaaActionPolicy::action_escalates_above_nav_state(test_case.action, nav_state),
test_case.rtl_allowed) << "action " << (int)test_case.action << " nav_state " << (int)nav_state;
}
for (const uint8_t nav_state : kStatesEnableLand) {
EXPECT_EQ(DaaActionPolicy::action_escalates_above_nav_state(test_case.action, nav_state),
test_case.land_allowed) << "action " << (int)test_case.action << " nav_state " << (int)nav_state;
}
for (const uint8_t nav_state : kStatesEnableTermination) {
EXPECT_EQ(DaaActionPolicy::action_escalates_above_nav_state(test_case.action, nav_state),
test_case.terminate_allowed) << "action " << (int)test_case.action << " nav_state " << (int)nav_state;
}
for (const uint8_t nav_state : kStatesNeverEnable) {
EXPECT_FALSE(DaaActionPolicy::action_escalates_above_nav_state(test_case.action, nav_state))
<< "action " << (int)test_case.action << " nav_state " << (int)nav_state;
}
}
void expect_no_response(const daa_action_decision_s &decision)
{
EXPECT_EQ(decision.action_command, DaaAction::kDisabled);
EXPECT_FALSE(decision.announce_action);
EXPECT_FALSE(decision.warn_on_ground);
}
void expect_ground_warning(const daa_action_decision_s &decision, const NotifyLandedActCause expected_cause)
{
EXPECT_EQ(decision.action_command, DaaAction::kDisabled);
EXPECT_FALSE(decision.announce_action);
EXPECT_TRUE(decision.warn_on_ground);
EXPECT_EQ(decision.ground_warning_cause, expected_cause);
}
void expect_command(const daa_action_decision_s &decision, const DaaAction expected_action,
const bool expected_announcement)
{
EXPECT_EQ(decision.action_command, expected_action);
EXPECT_EQ(decision.announce_action, expected_announcement);
EXPECT_FALSE(decision.warn_on_ground);
}
} // namespace
// Param numbering and the internal action ladder map both ways; out-of-range maps to Disabled.
TEST(DaaActionPolicyTest, ActionParamMappingUsesParameterNumbering)
{
struct action_param_case_s {
int32_t param;
DaaAction action;
};
const action_param_case_s cases[] {
{0, DaaAction::kDisabled},
{1, DaaAction::kWarnOnly},
{2, DaaAction::kReturnMode},
{3, DaaAction::kLandMode},
{4, DaaAction::kPositionHoldMode},
{5, DaaAction::kTerminate},
};
for (const action_param_case_s &test_case : cases) {
EXPECT_EQ(DaaActionPolicy::action_param_to_daa_action(test_case.param), test_case.action)
<< "param " << test_case.param;
EXPECT_EQ(DaaActionPolicy::daa_action_to_action_param(test_case.action), test_case.param)
<< "action " << static_cast<int>(test_case.action);
}
// out-of-range -> Disabled
EXPECT_EQ(DaaActionPolicy::action_param_to_daa_action(-1), DaaAction::kDisabled);
EXPECT_EQ(DaaActionPolicy::action_param_to_daa_action(6), DaaAction::kDisabled);
EXPECT_EQ(DaaActionPolicy::daa_action_to_action_param(DaaAction::kMaxActionValue), 0);
}
// An action escalates only when stronger than the current navigator state (protects manual/offboard).
TEST(DaaActionPolicyTest, ActionEscalatesOnlyAboveNavState)
{
check_escalation_matrix({DaaAction::kDisabled, false, false, false, false});
check_escalation_matrix({DaaAction::kWarnOnly, false, false, false, false});
check_escalation_matrix({DaaAction::kPositionHoldMode, true, false, false, false});
check_escalation_matrix({DaaAction::kReturnMode, true, true, false, false});
check_escalation_matrix({DaaAction::kLandMode, true, true, true, false});
check_escalation_matrix({DaaAction::kTerminate, true, true, true, true});
}
// NONE and out-of-range levels never request an action, even with actions configured everywhere.
TEST(DaaActionPolicyTest, NoActionForInvalidLevels)
{
const daa_action_params_s params = params_for_all_levels(5); // Terminate everywhere
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_NONE, params),
DaaAction::kDisabled);
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL + 1, params),
DaaAction::kDisabled);
}
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
TEST(DaaActionPolicyTest, F3442FallbackUsesGuaranteedBreachedZones)
{
daa_action_params_s params{};
params.lvl_low_act = 1; // Warn only
params.lvl_med_act = 4; // Hold
params.lvl_high_act = 0; // Disabled
params.lvl_crit_act = 5; // Terminate
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_LOW, params),
DaaAction::kWarnOnly);
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM, params),
DaaAction::kPositionHoldMode);
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH, params),
DaaAction::kWarnOnly);
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, params),
DaaAction::kTerminate);
// CRITICAL guarantees that HIGH, MEDIUM, and LOW are also breached.
params.lvl_crit_act = 0;
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, params),
DaaAction::kPositionHoldMode);
// all levels disabled -> fallback chain ends disabled
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL,
params_for_all_levels(0)), DaaAction::kDisabled);
}
#else
// Crosstrack maps every level through the single NAV_TRAFF_AVOID action.
TEST(DaaActionPolicyTest, CrosstrackSingleActionMapping)
{
const daa_action_params_s params = params_for_all_levels(3); // Land
for (uint8_t level = detect_and_avoid_s::DAA_CONFLICT_LVL_LOW;
level <= detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL; ++level) {
EXPECT_EQ(DaaActionPolicy::action_from_conflict_level(level, params), DaaAction::kLandMode);
}
}
#endif // CONFIG_NAVIGATOR_ADSB_F3442
// Unchanged level: no command or warning.
TEST(DaaActionPolicyTest, DecideNoResponseWithoutLevelChange)
{
const daa_action_decision_s decision = DaaActionPolicy::decide(
detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL, detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL,
vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, false, true,
DaaAction::kDisabled, params_for_all_levels(5));
expect_no_response(decision);
}
TEST(DaaActionPolicyTest, DecideWarnsOnGroundInsteadOfActing)
{
const uint8_t critical = detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL;
const uint8_t none = detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
const uint8_t nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
// armed -> takeoff warning
daa_action_decision_s decision = DaaActionPolicy::decide(critical, none, nav_state, true, true,
DaaAction::kDisabled, params_for_all_levels(5));
expect_ground_warning(decision, NotifyLandedActCause::kConflictAndArmed);
// disarmed -> arming warning
decision = DaaActionPolicy::decide(critical, none, nav_state, true, false,
DaaAction::kDisabled, params_for_all_levels(5));
expect_ground_warning(decision, NotifyLandedActCause::kConflictAndDisarmed);
// Repeated requests allow the caller to emit rate-limited ground warnings.
decision = DaaActionPolicy::decide(critical, critical, nav_state, true, false,
DaaAction::kDisabled, params_for_all_levels(5));
expect_ground_warning(decision, NotifyLandedActCause::kConflictAndDisarmed);
// warn-only config -> nothing
decision = DaaActionPolicy::decide(critical, none, nav_state, true, true,
DaaAction::kDisabled, params_for_all_levels(1));
expect_no_response(decision);
}
TEST(DaaActionPolicyTest, DecideRequestsCommandOnAirborneEscalationOnly)
{
const uint8_t critical = detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL;
const uint8_t low = detect_and_avoid_s::DAA_CONFLICT_LVL_LOW;
const daa_action_params_s land_params = params_for_all_levels(3); // Land
// escalation -> Land command, announced
daa_action_decision_s decision = DaaActionPolicy::decide(critical, low,
vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, false, true,
DaaAction::kDisabled, land_params);
expect_command(decision, DaaAction::kLandMode, true);
// A later level escalation that maps to the same action is not re-announced.
decision = DaaActionPolicy::decide(critical, low, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION,
false, true, DaaAction::kLandMode, land_params);
expect_command(decision, DaaAction::kLandMode, false);
// The first escalation after all conflicts cleared starts a new action episode.
decision = DaaActionPolicy::decide(critical, detect_and_avoid_s::DAA_CONFLICT_LVL_NONE,
vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION,
false, true, DaaAction::kLandMode, land_params);
expect_command(decision, DaaAction::kLandMode, true);
// already in a stronger mode -> nothing
decision = DaaActionPolicy::decide(critical, low, vehicle_status_s::NAVIGATION_STATE_TERMINATION,
false, true, DaaAction::kDisabled, land_params);
expect_no_response(decision);
// de-escalation -> nothing
decision = DaaActionPolicy::decide(low, critical, vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION,
false, true, DaaAction::kLandMode, land_params);
expect_no_response(decision);
}

336
src/lib/adsb/DaaEncodedId.cpp Normal file
View File

@@ -0,0 +1,336 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaEncodedId.cpp
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "DaaEncodedId.h"
#include <cinttypes>
#include <cstdio>
#include <cstring>
#include <px4_platform_common/log.h>
DaaEncodedId DaaEncodedId::from_report(const transponder_report_s &report)
{
if (report.icao_address > 0 && report.icao_address <= kMaxIcaoAddress) {
PX4_DEBUG("DAA: Unique ID encoding: ICAO.");
return {
static_cast<uint64_t>(report.icao_address),
detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO
};
}
if (report.icao_address > kMaxIcaoAddress) {
PX4_DEBUG("DAA: invalid ICAO address");
}
if (report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) {
PX4_DEBUG("DAA: Unique ID encoding: Callsign.");
const uint64_t callsign_id = callsign_to_uint64(report.callsign);
if (callsign_id != 0) {
return {
callsign_id,
detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN
};
}
// Fall back to the UAS-ID encoding below.
PX4_DEBUG("DAA: Failed to convert callsign to uint64.");
}
bool uas_id_valid = false;
for (int i = 0; i < kUasIdByteLength; ++i) {
if (report.uas_id[i] != 0) {
uas_id_valid = true;
break;
}
}
if (uas_id_valid) {
PX4_DEBUG("DAA: Unique ID encoding: UAS id.");
const uint64_t uas_id = last_uas_id_bytes_to_uint64(report.uas_id);
if (uas_id != 0) {
return {
uas_id,
detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID
};
}
PX4_DEBUG("DAA: Failed to convert uas_id to uint64.");
}
return {};
}
DaaEncodedId DaaEncodedId::identify_traffic_report(const transponder_report_s &report,
const daa_ownship_ids_s &ownship_ids)
{
const DaaEncodedId encoded_id = from_report(report);
if (encoded_id.id == 0) {
PX4_DEBUG("DAA: No valid unique ID, skipping report");
return {};
}
if (is_self_detection(encoded_id, ownship_ids)) {
PX4_DEBUG("DAA: Self detection, skipping report.");
return {};
}
return encoded_id;
}
bool DaaEncodedId::is_self_detection(const DaaEncodedId &encoded_id, const daa_ownship_ids_s &ownship_ids)
{
switch (encoded_id.encoding) {
case detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO: {
if (ownship_ids.icao >= 0
&& static_cast<uint32_t>(encoded_id.id) == static_cast<uint32_t>(ownship_ids.icao)) {
PX4_DEBUG("DAA: Received own main ICAO.");
return true;
}
if (ownship_ids.icao_2 >= 0
&& static_cast<uint32_t>(encoded_id.id) == static_cast<uint32_t>(ownship_ids.icao_2)) {
PX4_DEBUG("DAA: Received own secondary ICAO.");
return true;
}
break;
}
case detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN: {
if (encoded_id.id == ownship_ids.callsign) {
PX4_DEBUG("DAA: Received own Callsign.");
return true;
}
break;
}
case detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID: {
if (ownship_ids.uas_id_valid && encoded_id.id == ownship_ids.uas_id) {
PX4_DEBUG("DAA: Received own UAS ID.");
return true;
}
break;
}
default:
break;
}
return false;
}
void DaaEncodedId::to_string(char *buffer, size_t buffer_size) const
{
if (buffer == nullptr || buffer_size == 0) { return; }
memset(buffer, 0, buffer_size);
switch (encoding) {
case detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO: {
convert_icao_uint32_to_hex_str(id, buffer, buffer_size);
break;
}
case detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN: {
char callsign[kCallsignLength] {};
convert_uint64_callsign_to_str(id, callsign);
// Keep malformed/custom publishers from injecting control
// characters into MAVLink status text.
for (char &character : callsign) {
const uint8_t byte = static_cast<uint8_t>(character);
if (byte == 0) {
break;
}
if (byte < 0x20 || byte > 0x7e) {
character = '?';
}
}
snprintf(buffer, buffer_size, "%s", callsign);
break;
}
case detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID: {
char uas_id[kUtmGuidMsgLength] {};
convert_uas_id_uint64_to_str(id, uas_id);
snprintf(buffer, buffer_size, "%s", uas_id);
break;
}
default:
snprintf(buffer, buffer_size, "Unknown ID.");
break;
}
}
uint64_t DaaEncodedId::callsign_to_uint64(const char callsign[kCallsignLength])
{
if (callsign == nullptr) {
return 0;
}
bool null_terminated = false;
for (size_t i = 0; i < kCallsignLength; ++i) {
if (callsign[i] == '\0') {
null_terminated = true;
break;
}
}
if (!null_terminated) {
return 0;
}
uint64_t result = 0;
for (int i = 0; i < kIdEncodingNbBytes && callsign[i] != '\0'; ++i) {
result |= (static_cast<uint64_t>(static_cast<uint8_t>(callsign[i])) << (i * 8));
}
return result;
}
uint64_t DaaEncodedId::callsign_params_to_uint64(const int32_t callsign_part1, const int32_t callsign_part2)
{
const uint32_t parts[] {
static_cast<uint32_t>(callsign_part1),
static_cast<uint32_t>(callsign_part2)
};
char callsign[kCallsignLength] {};
for (size_t part_idx = 0; part_idx < 2; ++part_idx) {
for (size_t char_idx = 0; char_idx < sizeof(uint32_t); ++char_idx) {
const unsigned shift = static_cast<unsigned>((sizeof(uint32_t) - char_idx - 1) * 8);
callsign[part_idx * sizeof(uint32_t) + char_idx] = static_cast<char>((parts[part_idx] >> shift) & 0xffu);
}
}
return callsign_to_uint64(callsign);
}
void DaaEncodedId::convert_uint64_callsign_to_str(uint64_t value, char callsign[kCallsignLength])
{
if (callsign == nullptr) {
return;
}
memset(callsign, 0, kCallsignLength);
const int max_chars = kCallsignLength - 1;
if (value == 0) {
memset(callsign, '0', max_chars);
} else {
for (int i = 0; i < max_chars; ++i) {
char c = (value >> (i * 8)) & 0xFF;
callsign[i] = c;
if (c == 0) {
break;
}
}
}
callsign[max_chars] = '\0';
}
uint64_t DaaEncodedId::last_uas_id_bytes_to_uint64(const uint8_t uas_id[kUasIdByteLength])
{
if (uas_id == nullptr) {
return 0;
}
uint64_t uas_id_int = 0;
// Pack the last kIdEncodingNbBytes bytes into the key.
for (int i = 0; i < kIdEncodingNbBytes; ++i) {
uas_id_int |= static_cast<uint64_t>(uas_id[kUasIdByteLength - kIdEncodingNbBytes + i]) << (i * 8);
}
return uas_id_int;
}
void DaaEncodedId::convert_uas_id_uint64_to_str(const uint64_t uas_id_int, char uas_id_char_arr[kUtmGuidMsgLength])
{
if (uas_id_char_arr == nullptr) {
return;
}
static constexpr char kHexDigits[] = "0123456789abcdef";
if (uas_id_int == 0) {
memset(uas_id_char_arr, '0', kUtmGuidMsgLength - 1);
} else {
// Render the low reduced_uas_id_length bytes as hex.
const int reduced_uas_id_length = (kUtmGuidMsgLength - 1) / 2;
for (int i = 0; i < reduced_uas_id_length; ++i) {
const uint8_t byte = static_cast<uint8_t>((uas_id_int >>(i * 8)) & 0xFF);
uas_id_char_arr[i * 2] = kHexDigits[byte >> 4];
uas_id_char_arr[i * 2 + 1] = kHexDigits[byte & 0x0F];
}
}
uas_id_char_arr[kUtmGuidMsgLength - 1] = '\0';
}
void DaaEncodedId::convert_icao_uint32_to_hex_str(uint64_t value, char *buffer, size_t buffer_size)
{
if (buffer == nullptr || buffer_size < kIcaoLength) {
return;
}
const uint32_t icao_address = static_cast<uint32_t>(value & 0xFFFFFFu);
snprintf(buffer, buffer_size, "%06" PRIX32, icao_address);
}

135
src/lib/adsb/DaaEncodedId.h Normal file
View File

@@ -0,0 +1,135 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaEncodedId.h
* @brief Traffic report identification helpers and encoded traffic IDs.
*
* A transponder report can identify an aircraft three different ways: an ICAO
* address, an ADS-B callsign, or a UAS / UTM GUID. The DAA traffic buffer keys
* conflicts on a single 64-bit value, so each of those representations is packed
* into a uint64_t and the encoding that produced it.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include <cstddef>
#include <cstdint>
#include <uORB/topics/detect_and_avoid.h> // UNIQUE_ID_ENCODING_*
#include <uORB/topics/transponder_report.h> // from_report()
// Number of trailing identifier bytes packed into the 64-bit key. Max 8 (fits a uint64_t).
static constexpr uint8_t kIdEncodingNbBytes{8};
// Chars used to display a UAS id: 10 hex + null terminator. Max: 17 = 16 + null.
static constexpr uint8_t kUtmGuidMsgLength{11};
// ADS-B callsign string length: 8 chars + null terminator.
static constexpr uint8_t kCallsignLength{9};
// ICAO address string length: 6 hex chars + null terminator.
static constexpr uint8_t kIcaoLength{7};
static constexpr uint32_t kMaxIcaoAddress{0xFFFFFFu};
// Length of a UAS ID (UTM GUID) byte array.
// Similar to PX4_GUID_BYTE_LENGTH and transponder_report_s::uas_id;
static constexpr uint8_t kUasIdByteLength{18};
// The ownship identifiers used to reject self-detections.
struct daa_ownship_ids_s {
int32_t icao{-1}; // ADSB_ICAO_ID convention: negative = unset
int32_t icao_2{-1}; // ADSB_ICAO_ID_2 convention: negative = unset
uint64_t callsign{0}; // packed with DaaEncodedId::callsign_to_uint64
uint64_t uas_id{0}; // packed with DaaEncodedId::last_uas_id_bytes_to_uint64
bool uas_id_valid{false}; // false when the board UUID is unavailable
};
/**
* @brief A traffic identifier reduced to a 64-bit key plus its source encoding.
*
* @c encoding is one of detect_and_avoid_s::UNIQUE_ID_ENCODING_* and tells the
* codecs and to_string() how to interpret @c id.
*/
struct DaaEncodedId {
uint64_t id{0};
uint8_t encoding{detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO};
bool operator==(const DaaEncodedId &other) const { return encoding == other.encoding && id == other.id; }
bool operator!=(const DaaEncodedId &other) const { return !(*this == other); }
/**
* @brief Pick the best available identifier from a transponder report.
*
* Priority is a valid 24-bit ICAO address, ADS-B callsign, then UAS-ID.
* Returns the default value when the report has no usable identifier.
*/
static DaaEncodedId from_report(const transponder_report_s &report);
/**
* @brief Identify a transponder traffic report.
*
* Returns the extracted identifier, or id=0 (the default-constructed value) when the report
* carries no usable identifier or matches ownship (self-detection).
*/
static DaaEncodedId identify_traffic_report(const transponder_report_s &report, const daa_ownship_ids_s &ownship_ids);
// True if the report's identifier matches ownship (ICAO, callsign or UAS-ID).
static bool is_self_detection(const DaaEncodedId &encoded_id, const daa_ownship_ids_s &ownship_ids);
// Render to a null-terminated string per its encoding.
void to_string(char *buffer, size_t buffer_size) const;
// Pack a callsign into a 64-bit key. Returns 0 if the input is not null-terminated.
static uint64_t callsign_to_uint64(const char callsign[kCallsignLength]);
/**
* @brief Convert ADSB_CALLSIGN_1/2 parameter values to the packed callsign key.
*
* Each parameter stores four characters in display order (most-significant byte
* first), while callsign_to_uint64() stores the first character in the least-significant
* byte. This conversion keeps ownship parameter IDs in the same representation as
* callsigns received in transponder_report.
*/
static uint64_t callsign_params_to_uint64(int32_t callsign_part1, int32_t callsign_part2);
// Inverse of callsign_to_uint64.
static void convert_uint64_callsign_to_str(uint64_t value, char callsign[kCallsignLength]);
// Pack the trailing bytes of a UAS-ID (UTM GUID) into a 64-bit key.
static uint64_t last_uas_id_bytes_to_uint64(const uint8_t uas_id[kUasIdByteLength]);
// Render a packed UAS-ID key as a reduced lowercase hex string.
static void convert_uas_id_uint64_to_str(uint64_t uas_id_int, char uas_id_char_arr[kUtmGuidMsgLength]);
// Print the low 24 bits of an ICAO address as a 6-digit uppercase hex string.
static void convert_icao_uint32_to_hex_str(uint64_t value, char *buffer, size_t buffer_size);
};

354
src/lib/adsb/DaaEncodedIdTest.cpp Normal file
View File

@@ -0,0 +1,354 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaEncodedIdTest.cpp
* @brief Unit tests for the DaaEncodedId.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include <gtest/gtest.h>
#include <cstring>
#include <lib/adsb/DaaEncodedId.h>
namespace
{
void set_report_callsign(transponder_report_s &report, const char *callsign)
{
strncpy(report.callsign, callsign, sizeof(report.callsign) - 1);
report.callsign[sizeof(report.callsign) - 1] = '\0';
}
} // namespace
// Callsign packs to a 64-bit key and back; the round-trip re-encodes identically, unterminated rejected.
TEST(DaaEncodedIdTest, CallsignRoundTrip)
{
const char *callsigns[] {
"A",
"PX4TEST1",
"ABCD1234",
"ZZ999999",
};
for (const char *callsign : callsigns) {
const uint64_t key = DaaEncodedId::callsign_to_uint64(callsign);
char recovered[kCallsignLength];
DaaEncodedId::convert_uint64_callsign_to_str(key, recovered);
EXPECT_EQ(key, DaaEncodedId::callsign_to_uint64(recovered));
}
// unterminated input -> 0
char callsign_non_null[kCallsignLength];
memset(callsign_non_null, 'A', sizeof(callsign_non_null));
EXPECT_EQ(DaaEncodedId::callsign_to_uint64(callsign_non_null), 0u);
}
// High-bit callsign bytes stay in their own byte slot; no sign extension across the key.
TEST(DaaEncodedIdTest, CallsignPackingDoesNotSignExtend)
{
const char callsign[kCallsignLength] {static_cast<char>(0x80), static_cast<char>(0xFF), '\0'};
EXPECT_EQ(DaaEncodedId::callsign_to_uint64(callsign), static_cast<uint64_t>(0xFF80u));
}
TEST(DaaEncodedIdTest, CallsignParamsUseDocumentedCharacterOrder)
{
// Documented parameter values: "PX4 " = 0x50583420, "TEST" = 0x54455354.
const uint64_t encoded = DaaEncodedId::callsign_params_to_uint64(0x50583420, 0x54455354);
EXPECT_EQ(encoded, 0x5453455420345850ULL);
char decoded[kCallsignLength] {};
DaaEncodedId::convert_uint64_callsign_to_str(encoded, decoded);
EXPECT_STREQ(decoded, "PX4 TEST");
// An embedded terminator ends a shorter callsign just as it does in a traffic report.
EXPECT_EQ(DaaEncodedId::callsign_params_to_uint64(0x50583400, 0x54455354),
DaaEncodedId::callsign_to_uint64("PX4"));
EXPECT_EQ(DaaEncodedId::callsign_params_to_uint64(0, 0), 0u);
}
// Decoding a short callsign clears every byte after the terminator (MAVLink copies the whole field).
TEST(DaaEncodedIdTest, CallsignDecodeClearsUnusedBytes)
{
const char callsign[kCallsignLength] {'A', 'B', '\0'};
const uint64_t key = DaaEncodedId::callsign_to_uint64(callsign);
char recovered[kCallsignLength];
memset(recovered, 0x7F, sizeof(recovered));
DaaEncodedId::convert_uint64_callsign_to_str(key, recovered);
EXPECT_STREQ(recovered, "AB");
for (int i = 2; i < kCallsignLength; ++i) {
EXPECT_EQ(recovered[i], '\0');
}
}
// ICAO formats as fixed-width uppercase hex of the low 24 bits; a too-small buffer is left untouched.
TEST(DaaEncodedIdTest, FormatsIcaoAsFixedWidthHex)
{
char icao_buffer[kIcaoLength] {};
// zero-padded + uppercased
DaaEncodedId::convert_icao_uint32_to_hex_str(0xABCu, icao_buffer, sizeof(icao_buffer));
EXPECT_STREQ(icao_buffer, "000ABC");
// only the low 24 bits
DaaEncodedId::convert_icao_uint32_to_hex_str(0x12ABCDEFu, icao_buffer, sizeof(icao_buffer));
EXPECT_STREQ(icao_buffer, "ABCDEF");
// too-small buffer untouched
char small_buffer[kIcaoLength - 1];
memset(small_buffer, 'X', sizeof(small_buffer));
DaaEncodedId::convert_icao_uint32_to_hex_str(0xABCu, small_buffer, sizeof(small_buffer));
EXPECT_EQ(small_buffer[0], 'X');
}
// Packed UAS-ID key preserves the exact GUID tail bytes (little-endian); all-zero packs to 0.
TEST(DaaEncodedIdTest, UasIdPackingPreservesTailBytes)
{
uint8_t uas_id[kUasIdByteLength];
for (int i = 0; i < 4; ++i) {
for (int b = 0; b < kUasIdByteLength; ++b) {
uas_id[b] = static_cast<uint8_t>(0x10 * i + b);
}
const uint64_t key = DaaEncodedId::last_uas_id_bytes_to_uint64(uas_id);
// key byte k == GUID tail byte k
for (int k = 0; k < kIdEncodingNbBytes; ++k) {
const uint8_t key_byte = static_cast<uint8_t>((key >>(k * 8)) & 0xFF);
EXPECT_EQ(uas_id[kUasIdByteLength - kIdEncodingNbBytes + k], key_byte);
}
}
uint8_t zero_uas_id[kUasIdByteLength] {};
EXPECT_EQ(DaaEncodedId::last_uas_id_bytes_to_uint64(zero_uas_id), 0u);
}
// to_string() renders each encoding, plus an explicit placeholder for an unknown one.
TEST(DaaEncodedIdTest, ToStringRendersEachEncoding)
{
char buffer[kUtmGuidMsgLength];
// ICAO -> fixed-width uppercase hex of the low 24 bits.
DaaEncodedId{0xABCu, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO}.to_string(buffer, sizeof(buffer));
EXPECT_STREQ(buffer, "000ABC");
// Callsign -> the decoded callsign string.
const uint64_t callsign_key = DaaEncodedId::callsign_to_uint64("AB");
DaaEncodedId{callsign_key, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN}.to_string(buffer, sizeof(buffer));
EXPECT_STREQ(buffer, "AB");
const char callsign_with_controls[kCallsignLength] {'A', '\n', static_cast<char>(0x7f), 'B', '\0'};
DaaEncodedId{DaaEncodedId::callsign_to_uint64(callsign_with_controls),
detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN}.to_string(buffer, sizeof(buffer));
EXPECT_STREQ(buffer, "A??B");
// UAS ID -> reduced lowercase hex of the packed tail bytes (little-endian).
DaaEncodedId{0x0102030405u, detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID}.to_string(buffer, sizeof(buffer));
EXPECT_STREQ(buffer, "0504030201");
// Unknown encoding -> explicit placeholder.
char unknown_buffer[32];
DaaEncodedId{42u, 0xFF}.to_string(unknown_buffer, sizeof(unknown_buffer));
EXPECT_STREQ(unknown_buffer, "Unknown ID.");
}
TEST(DaaEncodedIdTest, ToStringRespectsBufferSize)
{
struct guarded_buffer_s {
char output[4];
char guard;
};
guarded_buffer_s callsign_buffer{{}, 'X'};
const uint64_t callsign_key = DaaEncodedId::callsign_to_uint64("ABCDEFGH");
DaaEncodedId{callsign_key, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN}.to_string(
callsign_buffer.output, sizeof(callsign_buffer.output));
EXPECT_STREQ(callsign_buffer.output, "ABC");
EXPECT_EQ(callsign_buffer.guard, 'X');
guarded_buffer_s uas_id_buffer{{}, 'X'};
DaaEncodedId{0x0102030405u, detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID}.to_string(
uas_id_buffer.output, sizeof(uas_id_buffer.output));
EXPECT_STREQ(uas_id_buffer.output, "050");
EXPECT_EQ(uas_id_buffer.guard, 'X');
char one_byte_buffer{'X'};
DaaEncodedId{callsign_key, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN}.to_string(&one_byte_buffer, 1);
EXPECT_EQ(one_byte_buffer, '\0');
}
// Equality keys on both id and encoding.
TEST(DaaEncodedIdTest, EqualityComparesIdAndEncoding)
{
const DaaEncodedId a{42u, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO};
// Same id and encoding: equal.
EXPECT_TRUE(a == (DaaEncodedId{42u, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO}));
EXPECT_FALSE(a != (DaaEncodedId{42u, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO}));
// Different id: not equal.
EXPECT_FALSE(a == (DaaEncodedId{43u, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO}));
// Same numeric id, different encoding: not equal (keeps colliding values in separate buffer slots).
EXPECT_FALSE(a == (DaaEncodedId{42u, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN}));
EXPECT_TRUE(a != (DaaEncodedId{42u, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN}));
}
// from_report() selects ICAO > callsign > UAS-ID, or id=0 when nothing usable is present.
TEST(DaaEncodedIdTest, FromReportSelectsEncodingByPriority)
{
// ICAO present: selected, and it wins even when a valid callsign is also present.
transponder_report_s report{};
report.icao_address = 0x4F74EE;
report.flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN;
set_report_callsign(report, "ABC");
DaaEncodedId id = DaaEncodedId::from_report(report);
EXPECT_EQ(id.encoding, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO);
EXPECT_EQ(id.id, 0x4F74EEu);
// No ICAO, valid callsign flag: callsign selected.
report.icao_address = 0;
id = DaaEncodedId::from_report(report);
EXPECT_EQ(id.encoding, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN);
EXPECT_EQ(id.id, DaaEncodedId::callsign_to_uint64("ABC"));
// An out-of-range ICAO address is ignored in favor of the next valid identifier.
report.icao_address = kMaxIcaoAddress + 1u;
id = DaaEncodedId::from_report(report);
EXPECT_EQ(id.encoding, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN);
EXPECT_EQ(id.id, DaaEncodedId::callsign_to_uint64("ABC"));
// No ICAO, no callsign flag, but a non-zero UAS ID: UAS-ID selected.
transponder_report_s uas_report{};
uas_report.uas_id[kUasIdByteLength - 1] = 0xAB;
id = DaaEncodedId::from_report(uas_report);
EXPECT_EQ(id.encoding, detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID);
EXPECT_EQ(id.id, DaaEncodedId::last_uas_id_bytes_to_uint64(uas_report.uas_id));
// Nothing usable: default id.
transponder_report_s empty_report{};
EXPECT_EQ(DaaEncodedId::from_report(empty_report).id, 0u);
}
// ICAO self-detection matches the primary or secondary ownship ICAO; unset (negative) never matches.
TEST(DaaEncodedIdTest, SelfDetectionIcao)
{
const DaaEncodedId traffic_id{0x123456, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO};
daa_ownship_ids_s ownship_ids{};
EXPECT_FALSE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids.icao = 0x123456;
EXPECT_TRUE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids = {};
ownship_ids.icao_2 = 0x123456;
EXPECT_TRUE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids.icao = 0x654321;
ownship_ids.icao_2 = 0x654322;
EXPECT_FALSE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
}
// Callsign self-detection requires an exact packed match.
TEST(DaaEncodedIdTest, SelfDetectionCallsign)
{
const char callsign[kCallsignLength] = "TST1234";
const uint64_t packed_callsign = DaaEncodedId::callsign_to_uint64(callsign);
ASSERT_NE(packed_callsign, 0u);
const DaaEncodedId traffic_id{packed_callsign, detect_and_avoid_s::UNIQUE_ID_ENCODING_ADSB_CALLSIGN};
daa_ownship_ids_s ownship_ids{};
EXPECT_FALSE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids.callsign = packed_callsign;
EXPECT_TRUE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids.callsign = packed_callsign ^ 1u;
EXPECT_FALSE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
}
// UAS-ID self-detection only when the board UUID is valid and the packed key matches.
TEST(DaaEncodedIdTest, SelfDetectionUasId)
{
uint8_t uas_id_bytes[kUasIdByteLength];
for (int i = 0; i < kUasIdByteLength; ++i) {
uas_id_bytes[i] = 0xE0 + i;
}
const uint64_t packed_uas_id = DaaEncodedId::last_uas_id_bytes_to_uint64(uas_id_bytes);
ASSERT_NE(packed_uas_id, 0u);
const DaaEncodedId traffic_id{packed_uas_id, detect_and_avoid_s::UNIQUE_ID_ENCODING_UAS_ID};
daa_ownship_ids_s ownship_ids{};
ownship_ids.uas_id = packed_uas_id;
EXPECT_FALSE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids.uas_id_valid = true;
EXPECT_TRUE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
ownship_ids.uas_id = packed_uas_id ^ 1u;
EXPECT_FALSE(DaaEncodedId::is_self_detection(traffic_id, ownship_ids));
}
// identify_traffic_report() rejects reports with no identity and ownship reports, decodes the rest.
TEST(DaaEncodedIdTest, IdentifyTrafficReport)
{
daa_ownship_ids_s ownship_ids{};
transponder_report_s report{};
EXPECT_EQ(DaaEncodedId::identify_traffic_report(report, ownship_ids).id, 0u);
report.icao_address = 0xABCDEF;
const DaaEncodedId encoded_id = DaaEncodedId::identify_traffic_report(report, ownship_ids);
EXPECT_EQ(encoded_id.id, 0xABCDEFu);
EXPECT_EQ(encoded_id.encoding, detect_and_avoid_s::UNIQUE_ID_ENCODING_ICAO);
ownship_ids.icao = 0xABCDEF;
EXPECT_EQ(DaaEncodedId::identify_traffic_report(report, ownship_ids).id, 0u);
}

70
src/lib/adsb/DaaHelper.h Normal file
View File

@@ -0,0 +1,70 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaHelper.h
* @brief Shared helpers and state structs for DAA standards
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include <cstdint>
#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>
struct aircraft_state_s {
matrix::Vector2d lat_lon{};
float altitude{0.f};
matrix::Vector3f velocity_ned{};
float heading{0.f};
};
struct daa_stats_s {
float aircraft_dist{0.f};
float aircraft_dist_hor{0.f};
float aircraft_dist_vert{0.f};
float expected_min_dist_time_sec{0.f};
};
inline matrix::Vector2f calculate_horizontal_vertical_speed_magnitudes(const aircraft_state_s &aircraft_state)
{
return matrix::Vector2f(aircraft_state.velocity_ned.xy().norm(), fabsf(aircraft_state.velocity_ned(2)));
}
// Worst-case closing speed (UAV and traffic flying straight at each other).
inline float calculate_relative_uav_traffic_speed(const aircraft_state_s &uav_state, const aircraft_state_s &traffic_state)
{
return traffic_state.velocity_ned.norm() + uav_state.velocity_ned.norm();
}

137
src/lib/adsb/crosstrack_standard/DaaCrosstrack.cpp Normal file
View File

@@ -0,0 +1,137 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Crosstrack.cpp
*
* Helper class to do detect and avoid based on crosstrack distance
*
*/
#include "DaaCrosstrack.h"
#include <float.h>
#include <lib/geo/geo.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
#include <uORB/topics/detect_and_avoid.h>
DaaCrosstrack::DaaCrosstrack() :
ModuleParams(nullptr)
{
}
bool DaaCrosstrack::try_setting_params()
{
updateParams();
const float crosstrack_sep = _param_nav_traff_a_hor.get();
const float vertical_sep = _param_nav_traff_a_ver.get();
const int32_t collision_time = _param_nav_traff_coll_t.get();
const bool crosstrack_ok = PX4_ISFINITE(crosstrack_sep) && crosstrack_sep > 0.f;
const bool vertical_ok = PX4_ISFINITE(vertical_sep) && vertical_sep > 0.f;
const bool collision_time_ok = collision_time > 0;
if (!(crosstrack_ok && vertical_ok && collision_time_ok)) {
PX4_ERR("DAA: invalid crosstrack parameters");
return false;
}
_crosstrack_separation_m = crosstrack_sep;
_vertical_separation_m = vertical_sep;
_collision_time_threshold_s = collision_time;
return true;
}
uint8_t DaaCrosstrack::calculate_daa_stats(const aircraft_state_s &uav_state, const aircraft_state_s &traffic_state,
daa_stats_s &daa_stats) const
{
if (!PX4_ISFINITE(traffic_state.heading)) {
PX4_DEBUG("CRT lib: invalid traffic heading");
daa_stats = {};
return detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
}
float d_hor{0.f};
float d_vert{0.f};
get_distance_to_point_global_wgs84(uav_state.lat_lon(0), uav_state.lat_lon(1), uav_state.altitude,
traffic_state.lat_lon(0), traffic_state.lat_lon(1), traffic_state.altitude, &d_hor, &d_vert);
const float relative_uav_traffic_speed = calculate_relative_uav_traffic_speed(uav_state, traffic_state);
// Predict until the vehicle would have passed this system at its current speed
const float prediction_distance = d_hor + kTrafficToUavDistanceExtension;
double end_lat{0.0};
double end_lon{0.0};
waypoint_from_heading_and_distance(traffic_state.lat_lon(0), traffic_state.lat_lon(1),
traffic_state.heading, prediction_distance, &end_lat, &end_lon);
crosstrack_error_s crosstrack_error{};
const bool line_distance_valid = !get_distance_to_line(crosstrack_error, uav_state.lat_lon(0),
uav_state.lat_lon(1), traffic_state.lat_lon(0),
traffic_state.lat_lon(1), end_lat,
end_lon);
const bool cs_distance_conflict_threshold = line_distance_valid
&& (!crosstrack_error.past_end)
&& (fabsf(crosstrack_error.distance) < _crosstrack_separation_m);
const float vertical_separation = fabsf(uav_state.altitude - traffic_state.altitude);
const bool vertical_separation_conflict = vertical_separation < _vertical_separation_m;
bool collision_time_check = false;
const float d_xyz = hypotf(d_hor, d_vert);
float time_to_collision = 0.f;
if (relative_uav_traffic_speed > FLT_EPSILON) {
time_to_collision = d_xyz / relative_uav_traffic_speed;
collision_time_check = time_to_collision < _collision_time_threshold_s;
}
daa_stats.aircraft_dist = d_xyz;
daa_stats.aircraft_dist_hor = line_distance_valid ? crosstrack_error.distance : d_hor;
daa_stats.aircraft_dist_vert = vertical_separation;
daa_stats.expected_min_dist_time_sec = time_to_collision;
const bool conflict_detected = cs_distance_conflict_threshold && vertical_separation_conflict && collision_time_check;
if (!conflict_detected) {
return detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
}
// Crosstrack mode has one conflict level, mapped to HIGH for compatibility.
return detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH;
}

80
src/lib/adsb/crosstrack_standard/DaaCrosstrack.h Normal file
View File

@@ -0,0 +1,80 @@
/***************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaCrosstrack.h
*
* Helper class to do detect and avoid based on crosstrack distance.
*
*/
#pragma once
#include "../DaaHelper.h"
#include <cstdint>
#include <px4_platform_common/module_params.h>
static constexpr float kTrafficToUavDistanceExtension{1000.0f};
/**
* @brief Crosstrack-distance DAA standard.
*
* Projects the traffic forward along its heading and declares a single
* high-priority conflict when the ownship is within the configured crosstrack,
* vertical separation, and time-to-collision thresholds. Useful for
* transponders that have a reliable heading but no standardised conflict zones.
*/
class DaaCrosstrack : public ModuleParams
{
public:
DaaCrosstrack();
// Crosstrack-based conflict level for one traffic target.
uint8_t calculate_daa_stats(const aircraft_state_s &uav_state, const aircraft_state_s &traffic_state,
daa_stats_s &daa_stats) const;
// Refresh the crosstrack thresholds from parameters. False on invalid values.
bool try_setting_params();
private:
float _crosstrack_separation_m{500.f};
float _vertical_separation_m{500.f};
int _collision_time_threshold_s{60};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::NAV_TRAFF_A_VER>) _param_nav_traff_a_ver,
(ParamFloat<px4::params::NAV_TRAFF_A_HOR>) _param_nav_traff_a_hor,
(ParamInt<px4::params::NAV_TRAFF_COLL_T>) _param_nav_traff_coll_t
)
};

200
src/lib/adsb/crosstrack_standard/DaaCrosstrackTest.cpp Normal file
View File

@@ -0,0 +1,200 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <gtest/gtest.h>
#include "DaaCrosstrackTestData.h"
#include "DaaCrosstrack.h"
#include <parameters/param.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <limits>
#include <uORB/topics/detect_and_avoid.h>
namespace
{
const matrix::Vector2d kUavLatLon{32.617013, -96.490564};
constexpr float kUavAlt{1000.f};
void set_crosstrack_params(float horizontal_separation_m, float vertical_separation_m, int collision_time_s)
{
param_set(param_handle(px4::params::NAV_TRAFF_A_HOR), &horizontal_separation_m);
param_set(param_handle(px4::params::NAV_TRAFF_A_VER), &vertical_separation_m);
param_set(param_handle(px4::params::NAV_TRAFF_COLL_T), &collision_time_s);
}
matrix::Vector3f velocity_ned_from_track(float heading_rad, float horizontal_speed_m_s, float vertical_speed_up_m_s)
{
return matrix::Vector3f(cosf(heading_rad) * horizontal_speed_m_s, sinf(heading_rad) * horizontal_speed_m_s, -vertical_speed_up_m_s);
}
aircraft_state_s create_aircraft_state(const matrix::Vector2d &reference_lat_lon, float reference_altitude,
float distance_m, float bearing_rad, float altitude_offset_m, float heading_rad,
const matrix::Vector3f &velocity_ned)
{
double lat{0.0};
double lon{0.0};
waypoint_from_heading_and_distance(reference_lat_lon(0), reference_lat_lon(1), bearing_rad, distance_m, &lat, &lon);
return aircraft_state_s{
matrix::Vector2d(lat, lon),
reference_altitude + altitude_offset_m,
velocity_ned,
heading_rad
};
}
aircraft_state_s create_aircraft_state(const traffic_data_s &traffic)
{
return aircraft_state_s{
matrix::Vector2d(traffic.lat_traffic, traffic.lon_traffic),
traffic.alt_traffic,
velocity_ned_from_track(traffic.heading_traffic, traffic.vxy_traffic, traffic.vz_traffic),
traffic.heading_traffic
};
}
} // namespace
class DaaCrosstrackTest : public ::testing::Test
{
protected:
DaaCrosstrack daa;
void SetUp() override
{
param_control_autosave(false);
param_reset_all();
set_crosstrack_params(500.f, 500.f, 60);
}
};
TEST_F(DaaCrosstrackTest, RejectsInvalidParams)
{
ASSERT_TRUE(daa.try_setting_params());
const float nan = std::numeric_limits<float>::quiet_NaN();
const float inf = std::numeric_limits<float>::infinity();
for (const float bad : {nan, inf, 0.f}) {
set_crosstrack_params(bad, 500.f, 60);
EXPECT_FALSE(daa.try_setting_params()) << "horizontal separation = " << bad;
}
for (const float bad : {nan, inf, 0.f}) {
set_crosstrack_params(500.f, bad, 60);
EXPECT_FALSE(daa.try_setting_params()) << "vertical separation = " << bad;
}
for (const int bad : {0, -1}) {
set_crosstrack_params(500.f, 500.f, bad);
EXPECT_FALSE(daa.try_setting_params()) << "collision time = " << bad;
}
// valid again
set_crosstrack_params(500.f, 500.f, 60);
EXPECT_TRUE(daa.try_setting_params());
}
// A NaN traffic heading (no predicted path line) returns no conflict with cleared stats.
TEST_F(DaaCrosstrackTest, InvalidTrafficHeadingReturnsNoConflict)
{
ASSERT_TRUE(daa.try_setting_params());
const aircraft_state_s uav_state{
kUavLatLon,
kUavAlt,
matrix::Vector3f(0.f, 0.f, 0.f),
0.f
};
aircraft_state_s traffic_state = create_aircraft_state(kUavLatLon, kUavAlt, 200.f, math::radians(90.f), 0.f,
std::numeric_limits<float>::quiet_NaN(), matrix::Vector3f(0.f, -30.f, 0.f));
daa_stats_s daa_stats{};
EXPECT_EQ(daa.calculate_daa_stats(uav_state, traffic_state, daa_stats), detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_FLOAT_EQ(daa_stats.aircraft_dist, 0.f);
EXPECT_FLOAT_EQ(daa_stats.aircraft_dist_hor, 0.f);
EXPECT_FLOAT_EQ(daa_stats.aircraft_dist_vert, 0.f);
EXPECT_FLOAT_EQ(daa_stats.expected_min_dist_time_sec, 0.f);
}
// A head-on approaching encounter is flagged HIGH with bounded crosstrack, vertical and timing stats.
TEST_F(DaaCrosstrackTest, DetectsApproachingConflictAndReportsExpectedStats)
{
ASSERT_TRUE(daa.try_setting_params());
const aircraft_state_s uav_state{
kUavLatLon,
kUavAlt,
matrix::Vector3f(0.f, 0.f, 0.f),
0.f
};
const aircraft_state_s traffic_state = create_aircraft_state(kUavLatLon, kUavAlt, 200.f, math::radians(90.f), 0.f,
math::radians(270.f), matrix::Vector3f(0.f, -30.f, 0.f));
daa_stats_s daa_stats{};
EXPECT_EQ(daa.calculate_daa_stats(uav_state, traffic_state, daa_stats), detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH);
EXPECT_NEAR(daa_stats.aircraft_dist, 200.f, 0.1f);
EXPECT_LT(fabsf(daa_stats.aircraft_dist_hor), 500.f);
EXPECT_NEAR(daa_stats.aircraft_dist_vert, 0.f, 0.1f);
EXPECT_NEAR(daa_stats.expected_min_dist_time_sec, 200.f / 30.f, 0.1f);
}
// Regression: the current solver reproduces the legacy conflict decisions for every frozen sample
// (dataset from adsb/test_adsb_helper.ipynb).
TEST_F(DaaCrosstrackTest, MatchesLegacyNotebookDataset)
{
ASSERT_TRUE(daa.try_setting_params());
const aircraft_state_s uav_state{
kUavLatLon,
kUavAlt,
matrix::Vector3f(0.f, 0.f, 0.f),
0.f
};
for (uint32_t i = 0; i < sizeof(kTrafficDataset) / sizeof(kTrafficDataset[0]); ++i) {
const traffic_data_s &traffic = kTrafficDataset[i];
SCOPED_TRACE(testing::Message() << "kTrafficDataset[" << i << "]");
daa_stats_s daa_stats{};
const uint8_t conflict_level = daa.calculate_daa_stats(uav_state, create_aircraft_state(traffic), daa_stats);
const uint8_t expected_conflict_level = traffic.in_conflict ? detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH :
detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
EXPECT_EQ(conflict_level, expected_conflict_level);
}
}

51
src/lib/adsb/AdsbConflictTest.h → src/lib/adsb/crosstrack_standard/DaaCrosstrackTestData.h
View File

@@ -1,17 +1,50 @@
/***************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
class AdsbConflictTest : public ::testing::Test
{
public:
AdsbConflictTest() = default;
virtual ~AdsbConflictTest() = default;
struct traffic_data_s {
double lat_traffic;
double lon_traffic;
float alt_traffic;
float heading_traffic;
float vxy_traffic;
float vz_traffic;
bool in_conflict;
};
// traffic dataset generated using notebook in adsb/test_adsb_helper.ipynb
traffic_data_s traffic_dataset[1940] = {
static constexpr traffic_data_s kTrafficDataset[] = {
{3.270694516059187151e+01, -9.638379328402535862e+01, 4.470000000000000000e+02, -3.141592653589793116e+00, 2.042375980563040798e+02, 2.042375980563040798e+02, 0},
{3.270694516059187151e+01, -9.638379328402535862e+01, 7.900000000000000000e+02, -3.109129529502698386e+00, 2.500275609807847104e+03, 2.500275609807847104e+03, 0},

176
src/lib/adsb/f3442_standard/DaaF3442.cpp Normal file
View File

@@ -0,0 +1,176 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file AdsbF3442.cpp
*
* Helper class to do detect and avoid based on the ASTM F3442 standard
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "DaaF3442.h"
#include <float.h>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/log.h>
#include <uORB/topics/detect_and_avoid.h>
DaaF3442::DaaF3442() :
ModuleParams(nullptr)
{
}
bool DaaF3442::try_setting_params()
{
updateParams();
const matrix::Vector2f nmac_bounds{_param_daa_lvl_critical_rad.get(), _param_daa_lvl_critical_hgt.get()};
const matrix::Vector2f wc_bounds{_param_daa_lvl_high_rad.get(), _param_daa_lvl_high_hgt.get()};
const int32_t nmac_latency = _param_daa_lvl_medium_time.get();
const int32_t wc_latency = _param_daa_lvl_low_time.get();
const bool nmac_ok = nmac_bounds.isAllFinite() && nmac_bounds.min() > 0.f;
const bool wc_ok = wc_bounds.isAllFinite() && wc_bounds.min() > 0.f;
const bool ordering_ok = nmac_ok && wc_ok && (wc_bounds - nmac_bounds).min() >= 0.f;
const bool latencies_ok = nmac_latency >= 0 && wc_latency >= nmac_latency;
if (!(nmac_ok && wc_ok && ordering_ok && latencies_ok)) {
PX4_ERR("DAA: invalid F3442 parameters");
return false;
}
_nmac_bounds_m = nmac_bounds;
_wc_bounds_m = wc_bounds;
_aug_nmac_latency_s = static_cast<float>(nmac_latency);
_aug_wc_latency_s = static_cast<float>(wc_latency);
return true;
}
bool DaaF3442::is_in_bounds(const matrix::Vector2f &distance, const matrix::Vector2f &bounds)
{
if (!bounds.isAllFinite() || bounds.min() < 0.f) {
PX4_DEBUG("F34 bounds nan/neg");
return false;
}
if (!distance.isAllFinite() || distance.min() < 0.f) {
PX4_DEBUG("F34 dist nan/neg");
return false;
}
return (distance - bounds).max() <= 0.f;
}
void DaaF3442::calculate_aircraft_conflict_volume(const matrix::Vector2f &base_bounds,
const matrix::Vector2f &vel,
const float latency, matrix::Vector2f &augmented_bounds)
{
// Symmetric (abs) to cover sudden course changes.
augmented_bounds = base_bounds + vel.abs() * fabsf(latency);
}
void DaaF3442::calculate_augmented_boundaries(const matrix::Vector2f &base_bounds,
const matrix::Vector2f &uav_vel_hor_vert,
const matrix::Vector2f &traffic_vel, const float latency, matrix::Vector2f &augmented_bounds)
{
matrix::Vector2f uav_bounds{};
calculate_aircraft_conflict_volume(base_bounds, uav_vel_hor_vert, latency, uav_bounds);
matrix::Vector2f traffic_bounds{};
calculate_aircraft_conflict_volume(base_bounds, traffic_vel, latency, traffic_bounds);
augmented_bounds = uav_bounds + traffic_bounds;
}
uint8_t DaaF3442::calculate_conflict_level(const matrix::Vector2f &distance,
const matrix::Vector2f &uav_vel_hor_vert, const matrix::Vector2f &traffic_vel) const
{
// Severity is imposed by evaluation order; HIGH and MEDIUM may overlap without containment.
// Multiply by two because both aircraft have their respective NMAC bounds
if (is_in_bounds(distance, 2.f * _nmac_bounds_m)) {
PX4_DEBUG("F3442: NMAC breach");
return detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL;
}
// Multiply by two because both aircraft have their respective WC bounds
if (is_in_bounds(distance, 2.f * _wc_bounds_m)) {
PX4_DEBUG("F3442: WC breach");
return detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH;
}
matrix::Vector2f aug_nmac_bounds{};
calculate_augmented_boundaries(_nmac_bounds_m, uav_vel_hor_vert, traffic_vel, _aug_nmac_latency_s, aug_nmac_bounds);
if (is_in_bounds(distance, aug_nmac_bounds)) {
PX4_DEBUG("F3442: AUG_NMAC breach");
return detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM;
}
matrix::Vector2f aug_wc_bounds{};
calculate_augmented_boundaries(_wc_bounds_m, uav_vel_hor_vert, traffic_vel, _aug_wc_latency_s, aug_wc_bounds);
if (is_in_bounds(distance, aug_wc_bounds)) {
PX4_DEBUG("F3442: AUG_WC breach");
return detect_and_avoid_s::DAA_CONFLICT_LVL_LOW;
}
return detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
}
uint8_t DaaF3442::calculate_daa_stats(const aircraft_state_s &uav_state, const aircraft_state_s &traffic_state,
daa_stats_s &daa_stats) const
{
float horizontal_dist{0.f};
float vertical_dist{0.f};
get_distance_to_point_global_wgs84(uav_state.lat_lon(0), uav_state.lat_lon(1), uav_state.altitude,
traffic_state.lat_lon(0), traffic_state.lat_lon(1), traffic_state.altitude, &horizontal_dist, &vertical_dist);
const float aircraft_dist = hypotf(horizontal_dist, vertical_dist);
daa_stats.aircraft_dist = aircraft_dist;
daa_stats.aircraft_dist_hor = horizontal_dist;
daa_stats.aircraft_dist_vert = fabsf(vertical_dist);
const float relative_uav_traffic_speed = calculate_relative_uav_traffic_speed(uav_state, traffic_state);
daa_stats.expected_min_dist_time_sec = relative_uav_traffic_speed > FLT_EPSILON ?
aircraft_dist / relative_uav_traffic_speed : 0.f;
const matrix::Vector2f distance(fabsf(horizontal_dist), fabsf(vertical_dist));
const matrix::Vector2f uav_vel_hor_vert = calculate_horizontal_vertical_speed_magnitudes(uav_state);
const matrix::Vector2f traffic_vel_hor_vert = calculate_horizontal_vertical_speed_magnitudes(traffic_state);
return calculate_conflict_level(distance, uav_vel_hor_vert, traffic_vel_hor_vert);
}

107
src/lib/adsb/f3442_standard/DaaF3442.h Normal file
View File

@@ -0,0 +1,107 @@
/***************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid.h
*
* Helper class to do detect and avoid based on F3442 standard
*
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include "../DaaHelper.h"
#include <px4_platform_common/module_params.h>
/**
* @brief ASTM F3442 conflict evaluation.
*
* Evaluates NMAC (CRITICAL), Well-Clear (HIGH), and two augmented zones grown
* by aircraft speed and a latency budget (MEDIUM and LOW). The first breached
* zone in severity order defines the reported conflict level.
*/
class DaaF3442 : public ModuleParams
{
public:
DaaF3442();
// Distance, time-to-min-distance and the F3442 conflict level for one target.
uint8_t calculate_daa_stats(const aircraft_state_s &uav_state, const aircraft_state_s &traffic_state,
daa_stats_s &daa_stats) const;
// True if every component of distance is within the symmetric bounds box.
static bool is_in_bounds(const matrix::Vector2f &distance, const matrix::Vector2f &bounds);
/**
* @brief Expand a base bounds box by |vel| * |latency|.
*
* Result is symmetric so it covers a sudden course change regardless of @p vel sign.
*/
static void calculate_aircraft_conflict_volume(const matrix::Vector2f &base_bounds, const matrix::Vector2f &vel,
const float latency, matrix::Vector2f &output_bounds);
// Sum of the ownship and traffic augmented volumes for a base zone and latency.
static void calculate_augmented_boundaries(const matrix::Vector2f &base_bounds, const matrix::Vector2f &uav_vel_hor_vert,
const matrix::Vector2f &traffic_vel, const float latency, matrix::Vector2f &augmented_bounds);
// Evaluate the four zones in severity order and return the first breach.
uint8_t calculate_conflict_level(const matrix::Vector2f &distance, const matrix::Vector2f &uav_vel_hor_vert,
const matrix::Vector2f &traffic_vel) const;
/**
* @brief Refresh F3442 zone radii and latency from parameters.
*
* Rejects non-positive or non-finite bounds and settings that break the
* guaranteed NMAC-in-WC and augmented-MEDIUM-in-LOW containment.
*/
bool try_setting_params();
private:
// Per-aircraft half-zones (radius, height). They match the parameter defaults and the ASTM
// F3442 zones once doubled in calculate_conflict_level(): NMAC = 2 x (153 m, 31 m),
// Well-Clear = 2 x (610 m, 77 m). Overwritten by try_setting_params() on activation.
matrix::Vector2f _nmac_bounds_m{77.f, 16.f};
matrix::Vector2f _wc_bounds_m{305.f, 39.f};
float _aug_nmac_latency_s{33.f};
float _aug_wc_latency_s{33.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::DAA_LVL_CRIT_RAD>) _param_daa_lvl_critical_rad,
(ParamFloat<px4::params::DAA_LVL_CRIT_HGT>) _param_daa_lvl_critical_hgt,
(ParamFloat<px4::params::DAA_LVL_HIGH_RAD>) _param_daa_lvl_high_rad,
(ParamFloat<px4::params::DAA_LVL_HIGH_HGT>) _param_daa_lvl_high_hgt,
(ParamInt<px4::params::DAA_LVL_MED_TIME>) _param_daa_lvl_medium_time,
(ParamInt<px4::params::DAA_LVL_LOW_TIME>) _param_daa_lvl_low_time
)
};

328
src/lib/adsb/f3442_standard/DaaF3442Test.cpp Normal file
View File

@@ -0,0 +1,328 @@
/****************************************************************************
*
* Copyright (C) 2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file DaaF3442Test.cpp
*
* Test file for the F3442 DAA standard
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include <gtest/gtest.h>
#include <parameters/param.h>
#include <array>
#include <lib/geo/geo.h>
#include <lib/mathlib/mathlib.h>
#include <limits>
#include <uORB/topics/detect_and_avoid.h>
#include "DaaF3442.h"
class DaaF3442Test : public ::testing::Test
{
protected:
DaaF3442 daa;
void SetUp() override
{
param_control_autosave(false);
param_reset_all();
}
void set_f3442_params(float nmac_radius, float nmac_height, float wc_radius, float wc_height,
int nmac_latency_s, int wc_latency_s)
{
param_set(param_handle(px4::params::DAA_LVL_CRIT_RAD), &nmac_radius);
param_set(param_handle(px4::params::DAA_LVL_CRIT_HGT), &nmac_height);
param_set(param_handle(px4::params::DAA_LVL_HIGH_RAD), &wc_radius);
param_set(param_handle(px4::params::DAA_LVL_HIGH_HGT), &wc_height);
param_set(param_handle(px4::params::DAA_LVL_MED_TIME), &nmac_latency_s);
param_set(param_handle(px4::params::DAA_LVL_LOW_TIME), &wc_latency_s);
}
};
TEST_F(DaaF3442Test, RejectsInvalidParams)
{
struct ParamCase {
float nmac_radius;
float nmac_height;
float wc_radius;
float wc_height;
int nmac_latency_s;
int wc_latency_s;
bool expected_result;
};
constexpr float valid_nmac_radius_m = 10.f;
constexpr float valid_nmac_height_m = 10.f;
constexpr float valid_wc_radius_m = 20.f;
constexpr float valid_wc_height_m = 20.f;
constexpr int valid_nmac_latency_s = 5;
constexpr int valid_wc_latency_s = 5;
const float nan = std::numeric_limits<float>::quiet_NaN();
const float inf = std::numeric_limits<float>::infinity();
const std::array<ParamCase, 13> cases{{
{valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, true},
{0.f, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{-valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, -valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, -valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, -valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{nan, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, inf, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, nan, valid_wc_height_m, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, inf, valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, -valid_nmac_latency_s, valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s, -valid_wc_latency_s, false},
{valid_nmac_radius_m, valid_nmac_height_m, valid_wc_radius_m, valid_wc_height_m, valid_nmac_latency_s + 1, valid_wc_latency_s, false},
}};
for (size_t i = 0; i < cases.size(); ++i) {
const ParamCase &test_case = cases[i];
SCOPED_TRACE(i);
set_f3442_params(test_case.nmac_radius, test_case.nmac_height, test_case.wc_radius, test_case.wc_height,
test_case.nmac_latency_s, test_case.wc_latency_s);
EXPECT_EQ(daa.try_setting_params(), test_case.expected_result);
}
}
// Bounds must nest: an outer zone no larger than the inner one is rejected.
TEST_F(DaaF3442Test, RejectsNonHierarchicalBounds)
{
struct HierarchyCase {
float nmac_radius;
float nmac_height;
float wc_radius;
float wc_height;
};
constexpr int nmac_latency_s = 30;
constexpr int wc_latency_s = 30;
const std::array<HierarchyCase, 3> cases{{
{20.f, 10.f, 19.f, 10.f},
{20.f, 10.f, 20.f, 9.f},
{200.f, 100.f, 100.f, 50.f},
}};
for (size_t i = 0; i < cases.size(); ++i) {
const HierarchyCase &test_case = cases[i];
SCOPED_TRACE(i);
set_f3442_params(test_case.nmac_radius, test_case.nmac_height, test_case.wc_radius, test_case.wc_height,
nmac_latency_s, wc_latency_s);
EXPECT_FALSE(daa.try_setting_params());
}
}
// is_in_bounds() is inclusive and fails on negative or non-finite inputs.
TEST_F(DaaF3442Test, IsInBoundsRejectsInvalidInputs)
{
const matrix::Vector2f bounds(10.f, 10.f);
const matrix::Vector2f within_bounds(5.f, 5.f);
const matrix::Vector2f on_boundary(10.f, 10.f);
const matrix::Vector2f outside_both_bounds(15.f, 15.f);
const matrix::Vector2f outside_horizontal_bounds(15.f, 4.f);
const matrix::Vector2f outside_vertical_bounds(4.f, 15.f);
const matrix::Vector2f negative_bounds(-10.f, 10.f);
const matrix::Vector2f negative_distance(-5.f, 5.f);
const matrix::Vector2f non_finite_distance(5.f, std::numeric_limits<float>::quiet_NaN());
EXPECT_TRUE(daa.is_in_bounds(within_bounds, bounds));
EXPECT_TRUE(daa.is_in_bounds(on_boundary, bounds));
EXPECT_FALSE(daa.is_in_bounds(outside_both_bounds, bounds));
EXPECT_FALSE(daa.is_in_bounds(outside_horizontal_bounds, bounds));
EXPECT_FALSE(daa.is_in_bounds(outside_vertical_bounds, bounds));
EXPECT_FALSE(daa.is_in_bounds(within_bounds, negative_bounds));
EXPECT_FALSE(daa.is_in_bounds(negative_distance, bounds));
EXPECT_FALSE(daa.is_in_bounds(non_finite_distance, bounds));
}
// Volume augmentation uses |velocity| and |latency|, so signs never shrink it.
TEST_F(DaaF3442Test, CalculateAircraftConflictVolumeUsesAbsoluteInputs)
{
const matrix::Vector2f base_bounds(10.f, 20.f);
const matrix::Vector2f velocity(-2.f, 4.f);
const float latency_s = -5.f;
const float expected_horizontal_bounds_m = base_bounds(0) + fabsf(velocity(0)) * fabsf(latency_s);
const float expected_vertical_bounds_m = base_bounds(1) + fabsf(velocity(1)) * fabsf(latency_s);
matrix::Vector2f augmented_bounds{};
daa.calculate_aircraft_conflict_volume(base_bounds, velocity, latency_s, augmented_bounds);
EXPECT_FLOAT_EQ(augmented_bounds(0), expected_horizontal_bounds_m);
EXPECT_FLOAT_EQ(augmented_bounds(1), expected_vertical_bounds_m);
}
// Augmented boundary sums both aircraft's volumes (motion from each side).
TEST_F(DaaF3442Test, CalculateAugmentedBoundariesAddsBothAircraftVolumes)
{
const matrix::Vector2f base_bounds(10.f, 20.f);
const matrix::Vector2f uav_velocity(2.f, 4.f);
const matrix::Vector2f traffic_velocity(8.f, 10.f);
const float latency_s = 3.f;
const float expected_horizontal_bounds_m = 2.f * base_bounds(0)
+ (fabsf(uav_velocity(0)) + fabsf(traffic_velocity(0))) * latency_s;
const float expected_vertical_bounds_m = 2.f * base_bounds(1)
+ (fabsf(uav_velocity(1)) + fabsf(traffic_velocity(1))) * latency_s;
matrix::Vector2f augmented_bounds{};
daa.calculate_augmented_boundaries(base_bounds, uav_velocity, traffic_velocity, latency_s, augmented_bounds);
EXPECT_FLOAT_EQ(augmented_bounds(0), expected_horizontal_bounds_m);
EXPECT_FLOAT_EQ(augmented_bounds(1), expected_vertical_bounds_m);
}
// calculate_conflict_level() returns the strictest breached zone; a single-axis miss is NONE.
TEST_F(DaaF3442Test, CalculateConflictLevelRespectsPriorityOrder)
{
constexpr float nmac_radius_m = 20.f;
constexpr float nmac_height_m = 10.f;
constexpr float wc_radius_m = 100.f;
constexpr float wc_height_m = 50.f;
constexpr int nmac_latency_s = 30;
constexpr int wc_latency_s = 40;
set_f3442_params(nmac_radius_m, nmac_height_m, wc_radius_m, wc_height_m, nmac_latency_s, wc_latency_s);
ASSERT_TRUE(daa.try_setting_params());
const matrix::Vector2f uav_velocity(10.f, 2.f);
const matrix::Vector2f traffic_velocity(5.f, 4.f);
const matrix::Vector2f nmac_bounds(2.f * nmac_radius_m, 2.f * nmac_height_m);
const matrix::Vector2f wc_bounds(2.f * wc_radius_m, 2.f * wc_height_m);
const matrix::Vector2f aug_nmac_bounds(
2.f * nmac_radius_m + (fabsf(uav_velocity(0)) + fabsf(traffic_velocity(0))) * nmac_latency_s,
2.f * nmac_height_m + (fabsf(uav_velocity(1)) + fabsf(traffic_velocity(1))) * nmac_latency_s);
const matrix::Vector2f aug_wc_bounds(
2.f * wc_radius_m + (fabsf(uav_velocity(0)) + fabsf(traffic_velocity(0))) * wc_latency_s,
2.f * wc_height_m + (fabsf(uav_velocity(1)) + fabsf(traffic_velocity(1))) * wc_latency_s);
constexpr float inside_margin_m = 1.f;
const matrix::Vector2f critical_distance(nmac_bounds(0) - inside_margin_m, nmac_bounds(1) - inside_margin_m);
const matrix::Vector2f high_distance(wc_bounds(0) - inside_margin_m, wc_bounds(1) - inside_margin_m);
const matrix::Vector2f medium_distance(aug_nmac_bounds(0) - inside_margin_m, aug_nmac_bounds(1) - inside_margin_m);
const matrix::Vector2f low_distance(aug_wc_bounds(0) - inside_margin_m, aug_wc_bounds(1) - inside_margin_m);
const matrix::Vector2f outside_low_distance(aug_wc_bounds(0) + inside_margin_m, aug_wc_bounds(1) + inside_margin_m);
const matrix::Vector2f outside_low_vertical_only(low_distance(0), aug_wc_bounds(1) + inside_margin_m);
const matrix::Vector2f outside_low_horizontal_only(aug_wc_bounds(0) + inside_margin_m, low_distance(1));
// same velocity pair against progressively weaker breaches
EXPECT_EQ(daa.calculate_conflict_level(critical_distance, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL);
EXPECT_EQ(daa.calculate_conflict_level(high_distance, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH);
EXPECT_EQ(daa.calculate_conflict_level(medium_distance, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM);
EXPECT_EQ(daa.calculate_conflict_level(low_distance, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_LOW);
EXPECT_EQ(daa.calculate_conflict_level(outside_low_distance, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_EQ(daa.calculate_conflict_level(outside_low_vertical_only, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_EQ(daa.calculate_conflict_level(outside_low_horizontal_only, uav_velocity, traffic_velocity),
detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
}
// calculate_daa_stats() reports measured geometry and severity for a near-collision and a clear pass.
TEST_F(DaaF3442Test, CalculateDaaStatsReportsExpectedGeometryAndLevel)
{
constexpr float nmac_radius_m = 20.f;
constexpr float nmac_height_m = 10.f;
constexpr float wc_radius_m = 100.f;
constexpr float wc_height_m = 50.f;
constexpr int nmac_latency_s = 30;
constexpr int wc_latency_s = 40;
set_f3442_params(nmac_radius_m, nmac_height_m, wc_radius_m, wc_height_m, nmac_latency_s, wc_latency_s);
ASSERT_TRUE(daa.try_setting_params());
const matrix::Vector2d uav_lat_lon(46.52342, 6.524234);
constexpr float uav_altitude_m = 400.f;
const matrix::Vector3f uav_velocity_ned_m_s(10.f, 0.f, -2.f);
constexpr float uav_heading_rad = 0.f;
const aircraft_state_s uav_state{
uav_lat_lon,
uav_altitude_m,
uav_velocity_ned_m_s,
uav_heading_rad
};
aircraft_state_s traffic_state{};
const matrix::Vector3f traffic_velocity_ned_m_s(-5.f, 0.f, -4.f);
const float traffic_heading_rad = math::radians(180.f);
const float east_bearing_rad = math::radians(90.f);
constexpr float expectation_tolerance = 0.1f;
double traffic_lat{0.0};
double traffic_lon{0.0};
traffic_state.velocity_ned = traffic_velocity_ned_m_s;
traffic_state.heading = traffic_heading_rad;
auto place_traffic_due_east = [&](float horizontal_distance_m, float traffic_altitude_m) {
waypoint_from_heading_and_distance(uav_state.lat_lon(0), uav_state.lat_lon(1), east_bearing_rad, horizontal_distance_m,
&traffic_lat, &traffic_lon);
traffic_state.lat_lon = matrix::Vector2d(traffic_lat, traffic_lon);
traffic_state.altitude = traffic_altitude_m;
};
const float uav_speed_m_s = uav_velocity_ned_m_s.norm();
const float traffic_speed_m_s = traffic_velocity_ned_m_s.norm();
const float expected_relative_speed_m_s = uav_speed_m_s + traffic_speed_m_s;
constexpr float near_horizontal_distance_m = 39.f;
constexpr float near_vertical_separation_m = 9.f;
const float near_traffic_altitude_m = uav_altitude_m + near_vertical_separation_m;
const float near_aircraft_distance_m = hypotf(near_horizontal_distance_m, near_vertical_separation_m);
place_traffic_due_east(near_horizontal_distance_m, near_traffic_altitude_m);
// near-conflict geometry
daa_stats_s daa_stats{};
EXPECT_EQ(daa.calculate_daa_stats(uav_state, traffic_state, daa_stats), detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL);
EXPECT_NEAR(daa_stats.aircraft_dist, near_aircraft_distance_m, expectation_tolerance);
EXPECT_NEAR(daa_stats.aircraft_dist_hor, near_horizontal_distance_m, expectation_tolerance);
EXPECT_NEAR(daa_stats.aircraft_dist_vert, near_vertical_separation_m, expectation_tolerance);
EXPECT_NEAR(daa_stats.expected_min_dist_time_sec, near_aircraft_distance_m / expected_relative_speed_m_s,
expectation_tolerance);
// traffic well outside every protection volume
constexpr float far_horizontal_distance_m = 900.f;
constexpr float far_vertical_separation_m = 400.f;
const float far_traffic_altitude_m = uav_altitude_m + far_vertical_separation_m;
const float far_aircraft_distance_m = hypotf(far_horizontal_distance_m, far_vertical_separation_m);
place_traffic_due_east(far_horizontal_distance_m, far_traffic_altitude_m);
EXPECT_EQ(daa.calculate_daa_stats(uav_state, traffic_state, daa_stats), detect_and_avoid_s::DAA_CONFLICT_LVL_NONE);
EXPECT_NEAR(daa_stats.aircraft_dist, far_aircraft_distance_m, expectation_tolerance);
EXPECT_NEAR(daa_stats.aircraft_dist_hor, far_horizontal_distance_m, expectation_tolerance);
EXPECT_NEAR(daa_stats.aircraft_dist_vert, far_vertical_separation_m, expectation_tolerance);
EXPECT_NEAR(daa_stats.expected_min_dist_time_sec, far_aircraft_distance_m / expected_relative_speed_m_s,
expectation_tolerance);
}

15
src/lib/adsb/parameters.yaml
View File

@@ -30,10 +30,19 @@ parameters:
reboot_required: true
ADSB_ICAO_ID:
description:
short: ADSB-Out ICAO configuration
long: Defines the ICAO ID of the vehicle
short: ADSB-Out ICAO ID
long: Vehicle ICAO ID. Use -1 to disable.
type: int32
default: 1194684
default: -1
reboot_required: true
min: -1
max: 16777215
ADSB_ICAO_ID_2:
description:
short: Secondary ownship ICAO ID
long: Optional second ownship ICAO ID used to filter self-detections. Use -1 to disable.
type: int32
default: -1
reboot_required: true
min: -1
max: 16777215

9
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
View File

@@ -31,7 +31,7 @@
#
############################################################################
px4_add_library(health_and_arming_checks
set(health_and_arming_checks_srcs
Common.cpp
HealthAndArmingChecks.cpp
@@ -74,6 +74,13 @@ px4_add_library(health_and_arming_checks
checks/externalChecks.cpp
)
if(CONFIG_NAVIGATOR_ADSB)
list(APPEND health_and_arming_checks_srcs checks/daaCheck.cpp)
endif()
px4_add_library(health_and_arming_checks ${health_and_arming_checks_srcs})
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/esc_check_params.yaml)
add_dependencies(health_and_arming_checks mode_util)
target_link_libraries(health_and_arming_checks PRIVATE geo hysteresis)

4
src/modules/commander/HealthAndArmingChecks/Common.hpp
View File

@@ -335,6 +335,10 @@ private:
FRIEND_TEST(ReporterTest, arming_checks_mode_category2);
FRIEND_TEST(ReporterTest, reporting);
FRIEND_TEST(ReporterTest, reporting_multiple);
#if CONFIG_NAVIGATOR_ADSB
FRIEND_TEST(ReporterTest, daaCheckIgnoresStaleConflict);
FRIEND_TEST(ReporterTest, daaCheckIgnoresConflictWhileArmed);
#endif // CONFIG_NAVIGATOR_ADSB
/**
* Reset current results.

9
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
View File

@@ -46,6 +46,9 @@
#include "checks/baroCheck.hpp"
#include "checks/companionComputerCheck.hpp"
#include "checks/cpuResourceCheck.hpp"
#if CONFIG_NAVIGATOR_ADSB
#include "checks/daaCheck.hpp"
#endif // CONFIG_NAVIGATOR_ADSB
#include "checks/distanceSensorChecks.hpp"
#include "checks/opticalFlowCheck.hpp"
#include "checks/escCheck.hpp"
@@ -138,6 +141,9 @@ private:
BaroChecks _baro_checks;
CompanionComputerChecks _companion_computer_checks;
CpuResourceChecks _cpu_resource_checks;
#if CONFIG_NAVIGATOR_ADSB
DaaChecks _daa_checks;
#endif // CONFIG_NAVIGATOR_ADSB
DistanceSensorChecks _distance_sensor_checks;
OpticalFlowCheck _optical_flow_check;
EscChecks _esc_checks;
@@ -182,6 +188,9 @@ private:
&_baro_checks,
&_companion_computer_checks,
&_cpu_resource_checks,
#if CONFIG_NAVIGATOR_ADSB
&_daa_checks,
#endif // CONFIG_NAVIGATOR_ADSB
&_distance_sensor_checks,
&_optical_flow_check,
&_esc_checks,

122
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp
View File

@@ -34,6 +34,12 @@
#include <gtest/gtest.h>
#include "Common.hpp"
#if CONFIG_NAVIGATOR_ADSB
#include "checks/daaCheck.hpp"
#include <parameters/param.h>
#include <uORB/Publication.hpp>
#include <uORB/topics/detect_and_avoid_most_urgent.h>
#endif // CONFIG_NAVIGATOR_ADSB
#include <uORB/topics/event.h>
#include <uORB/Subscription.hpp>
@@ -285,3 +291,119 @@ TEST_F(ReporterTest, reporting_multiple)
}
}
}
#if CONFIG_NAVIGATOR_ADSB
TEST_F(ReporterTest, daaCheckIgnoresStaleConflict)
{
param_control_autosave(false);
param_reset_all();
const int32_t daa_enabled = 1;
const int32_t daa_timeout_s = 1;
param_set(param_handle(px4::params::DAA_EN), &daa_enabled);
param_set(param_handle(px4::params::DAA_TRAFF_TOUT), &daa_timeout_s);
uORB::Publication<detect_and_avoid_most_urgent_s> daa_pub{ORB_ID(detect_and_avoid_most_urgent)};
DaaChecks daa_checks;
vehicle_status_s status{};
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
Context context{status};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
uORB::Subscription event_sub{ORB_ID(event)};
event_s event{};
while (event_sub.update(&event)) {}
detect_and_avoid_most_urgent_s daa_status{};
daa_status.timestamp = hrt_absolute_time();
daa_status.has_action = true;
daa_pub.publish(daa_status);
reporter.reset();
daa_checks.checkAndReport(context, reporter);
reporter.finalize();
reporter.report(false);
EXPECT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
bool saw_daa_conflict_event = false;
while (event_sub.update(&event)) {
if (event.id == events::ID("check_daa_conflict")) {
saw_daa_conflict_event = true;
}
}
EXPECT_TRUE(saw_daa_conflict_event);
const hrt_abstime now = hrt_absolute_time();
daa_status.timestamp = now > 2_s ? now - 2_s : 0;
daa_pub.publish(daa_status);
reporter.reset();
daa_checks.checkAndReport(context, reporter);
reporter.finalize();
reporter.report(false);
EXPECT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
saw_daa_conflict_event = false;
while (event_sub.update(&event)) {
if (event.id == events::ID("check_daa_conflict")) {
saw_daa_conflict_event = true;
}
}
EXPECT_FALSE(saw_daa_conflict_event);
}
TEST_F(ReporterTest, daaCheckIgnoresConflictWhileArmed)
{
param_control_autosave(false);
param_reset_all();
const int32_t daa_enabled = 1;
const int32_t daa_timeout_s = 1;
param_set(param_handle(px4::params::DAA_EN), &daa_enabled);
param_set(param_handle(px4::params::DAA_TRAFF_TOUT), &daa_timeout_s);
uORB::Publication<detect_and_avoid_most_urgent_s> daa_pub{ORB_ID(detect_and_avoid_most_urgent)};
DaaChecks daa_checks;
vehicle_status_s status{};
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
status.arming_state = vehicle_status_s::ARMING_STATE_ARMED;
Context context{status};
failsafe_flags_s failsafe_flags{};
Report reporter{failsafe_flags, 0_s};
uORB::Subscription event_sub{ORB_ID(event)};
event_s event{};
while (event_sub.update(&event)) {}
detect_and_avoid_most_urgent_s daa_status{};
daa_status.timestamp = hrt_absolute_time();
daa_status.has_action = true;
daa_pub.publish(daa_status);
reporter.reset();
daa_checks.checkAndReport(context, reporter);
reporter.finalize();
reporter.report(false);
EXPECT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION));
bool saw_daa_conflict_event = false;
while (event_sub.update(&event)) {
if (event.id == events::ID("check_daa_conflict")) {
saw_daa_conflict_event = true;
}
}
EXPECT_FALSE(saw_daa_conflict_event);
}
#endif // CONFIG_NAVIGATOR_ADSB

74
src/modules/commander/HealthAndArmingChecks/checks/daaCheck.cpp Normal file
View File

@@ -0,0 +1,74 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file daaCheck.cpp
*
* Prevent arming while an active traffic conflict requires an automated DAA action.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "daaCheck.hpp"
void DaaChecks::checkAndReport(const Context &context, Report &reporter)
{
if (_param_daa_en.get() <= 0) {
return;
}
if (context.isArmed()) {
return;
}
detect_and_avoid_most_urgent_s daa_status{};
const hrt_abstime stale_timeout = static_cast<hrt_abstime>(_param_daa_traff_tout.get()) * 1_s;
if (!_detect_and_avoid_most_urgent_sub.copy(&daa_status)
|| daa_status.timestamp == 0
|| hrt_elapsed_time(&daa_status.timestamp) > stale_timeout
|| !daa_status.has_action) {
return;
}
/* EVENT
* @description
* Resolve the reported detect-and-avoid conflict before arming.
*/
reporter.armingCheckFailure(NavModes::All, health_component_t::traffic_avoidance,
events::ID("check_daa_conflict"),
events::Log::Error, "Air conflict detected");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "In conflict with automated DAA action\t");
}
}

56
src/modules/commander/HealthAndArmingChecks/checks/daaCheck.hpp Normal file
View File

@@ -0,0 +1,56 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/detect_and_avoid_most_urgent.h>
class DaaChecks : public HealthAndArmingCheckBase
{
public:
DaaChecks() = default;
~DaaChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
uORB::Subscription _detect_and_avoid_most_urgent_sub{ORB_ID(detect_and_avoid_most_urgent)};
DEFINE_PARAMETERS(
(ParamInt<px4::params::DAA_EN>) _param_daa_en,
(ParamInt<px4::params::DAA_TRAFF_TOUT>) _param_daa_traff_tout
)
};

4
src/modules/logger/logged_topics.cpp
View File

@@ -58,6 +58,10 @@ void LoggedTopics::add_default_topics()
add_topic("commander_state");
add_topic("config_overrides");
add_topic("cpuload");
#if defined(CONFIG_NAVIGATOR_ADSB)
add_topic("detect_and_avoid");
add_topic("detect_and_avoid_most_urgent");
#endif // CONFIG_NAVIGATOR_ADSB
add_topic("distance_sensor_mode_change_request");
add_topic("device_information", 900);
add_topic_multi("dronecan_node_status", 250);

1
src/modules/mavlink/CMakeLists.txt
View File

@@ -128,7 +128,6 @@ px4_add_module(
module.yaml
mavlink_params.yaml
DEPENDS
adsb
airspeed
component_general_json # for checksums.h
dataman_client

14
src/modules/mavlink/streams/UAVIONIX_ADSB_OUT_CFG.hpp
View File

@@ -124,15 +124,25 @@ private:
bool send() override
{
// Required update for static message is 0.1 [Hz]
const int32_t adsb_icao = _adsb_icao.get();
const bool adsb_out_enabled = adsb_icao >= 0;
// ADSB_ICAO_ID controls transmission only. Keep the receiver enabled so
// uAvionix traffic reports remain available to ADS-B In and DAA.
uint8_t rf_select{UAVIONIX_ADSB_OUT_RF_SELECT_RX_ENABLED};
if (adsb_out_enabled) {
rf_select |= UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED;
}
mavlink_uavionix_adsb_out_cfg_t cfg_msg = {
.ICAO = static_cast<uint32_t>(_adsb_icao.get()),
.ICAO = adsb_out_enabled ? static_cast<uint32_t>(adsb_icao) : 0,
.stallSpeed = _stall_speed,
.callsign = {'\0'},
.emitterType = static_cast<uint8_t>(_adsb_emit_type.get()),
.aircraftSize = static_cast<uint8_t>(_adsb_len_width.get()),
.gpsOffsetLat = static_cast<uint8_t>(_adsb_gps_offset_lat.get()),
.gpsOffsetLon = static_cast<uint8_t>(_adsb_gps_offset_lon.get()),
.rfSelect = UAVIONIX_ADSB_OUT_RF_SELECT_TX_ENABLED
.rfSelect = rf_select
};
static_assert(sizeof(cfg_msg.callsign) == sizeof(_callsign), "Size mismatch");

9
src/modules/navigator/CMakeLists.txt
View File

@@ -34,6 +34,10 @@
add_subdirectory(MissionFeasibility)
add_subdirectory(RTLPlanner)
if(CONFIG_NAVIGATOR_ADSB)
add_subdirectory(DetectAndAvoid)
endif()
if(BUILD_TESTING)
add_subdirectory(test)
endif()
@@ -60,7 +64,6 @@ set(NAVIGATOR_SOURCES
set(NAVIGATOR_DEPENDS
dataman_client
geo
adsb
motion_planning
mission_feasibility_checker
rtl_time_estimator)
@@ -77,6 +80,10 @@ if(CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF)
vtol_takeoff.cpp)
endif()
if(CONFIG_NAVIGATOR_ADSB)
list(APPEND NAVIGATOR_DEPENDS adsb detect_and_avoid)
endif()
px4_add_module(
MODULE modules__navigator
MAIN navigator

58
src/modules/navigator/DetectAndAvoid/CMakeLists.txt Normal file
View File

@@ -0,0 +1,58 @@
############################################################################
#
# Copyright (c) 2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
set(detect_and_avoid_srcs
ConflictNotifier.cpp
ConflictNotifier.h
detect_and_avoid.cpp
detect_and_avoid.h
)
if(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
list(APPEND detect_and_avoid_srcs detect_and_avoid_fake_traffic.cpp)
endif()
px4_add_library(detect_and_avoid ${detect_and_avoid_srcs})
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/detect_and_avoid_params.yaml)
if(CONFIG_NAVIGATOR_ADSB_F3442)
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/detect_and_avoid_f3442_params.yaml)
endif()
target_link_libraries(detect_and_avoid PUBLIC adsb)
# The functional tests use the fake-traffic injector.
if(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
add_subdirectory(test)
endif()

492
src/modules/navigator/DetectAndAvoid/ConflictNotifier.cpp Normal file
View File

@@ -0,0 +1,492 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ConflictNotifier.cpp
*
* All DAA operator messaging (MAVLink STATUSTEXT and PX4 events) in one place.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "ConflictNotifier.h"
#include <cmath>
#include <cstdint>
#include <cstdio>
#include <lib/mathlib/mathlib.h>
#include <systemlib/err.h>
#include <systemlib/mavlink_log.h>
using namespace time_literals;
uint32_t ConflictNotifier::distance_meters_for_log(const float distance)
{
if (!PX4_ISFINITE(distance)) {
return UINT32_MAX;
}
const double absolute_distance = fabs(static_cast<double>(distance));
return absolute_distance >= static_cast<double>(UINT32_MAX)
? UINT32_MAX
: static_cast<uint32_t>(absolute_distance);
}
void ConflictNotifier::reset()
{
_time_last_status_notif = 0;
_time_last_traffic_ignored = 0;
_time_last_landed_warning = 0;
}
void ConflictNotifier::report_cycle(const conflict_tracker_changes_s &changes, const ConflictTracker &tracker,
const cycle_context_s &context)
{
// Defer new-conflict messages until the final tracker state for this cycle is known
// so a conflict added and removed in the same cycle is never announced
static new_conflicts_pending_notif_s new_conflicts_pending_notif{};
new_conflicts_pending_notif.clear();
for (size_t i = 0; i < changes.size(); ++i) {
const conflict_tracker_change_s &change = changes[i];
switch (change.type) {
case ConflictTrackerChangeType::kConflictAdded:
if (level_requires_warning(context.warning_levels_mask, change.conflict.conflict_level)
&& !pending_new_conflict_notification_exists(change.conflict.encoded_id, new_conflicts_pending_notif)
&& !new_conflicts_pending_notif.push_back(change.conflict.encoded_id)) {
PX4_ERR("DAA: pending notifications overflow");
}
break;
case ConflictTrackerChangeType::kConflictLevelChanged:
if (!pending_new_conflict_notification_exists(change.conflict.encoded_id, new_conflicts_pending_notif)) {
maybe_notify_secondary_level_change(change, context.warning_levels_mask);
}
break;
case ConflictTrackerChangeType::kConflictRemoved:
// Do not notify traffic removed if the traffic was never visible to the user
if (level_requires_warning(context.warning_levels_mask, change.conflict.conflict_level)
&& !pending_new_conflict_notification_exists(change.conflict.encoded_id, new_conflicts_pending_notif)) {
notify_traffic_removed(change.conflict, change.remove_cause);
}
break;
case ConflictTrackerChangeType::kReportIgnored:
maybe_notify_ignored_traffic(change.conflict, change.ignore_cause, context.warning_levels_mask);
break;
}
}
const conflict_info_s &most_urgent_conflict = tracker.most_urgent();
const bool main_conflict_pending_notif =
pending_new_conflict_notification_exists(most_urgent_conflict.encoded_id, new_conflicts_pending_notif);
// Necessary to avoid a double notification when a new conflict is also the main conflict
if (main_conflict_pending_notif) {
notify_conflict_level(most_urgent_conflict, context.prev_most_urgent_level, ConflictNotifyKind::kMostUrgentNew);
_time_last_status_notif = hrt_absolute_time();
}
for (const DaaEncodedId &new_conflict_id : new_conflicts_pending_notif) {
if (main_conflict_pending_notif && new_conflict_id == most_urgent_conflict.encoded_id) {
continue;
}
// No need to notify new conflicts that are not in the buffer anymore
const conflict_info_s new_conflict = tracker.get_conflict(new_conflict_id);
if (new_conflict.encoded_id.id == 0) {
continue;
}
notify_new_conflict(new_conflict);
}
if (main_conflict_pending_notif) {
return;
}
if (must_notify(most_urgent_conflict.conflict_level, _time_last_status_notif, context.status_notif_interval,
context.prev_most_urgent_level, context.warning_levels_mask)) {
notify_conflict_level(most_urgent_conflict, context.prev_most_urgent_level, ConflictNotifyKind::kMostUrgent);
_time_last_status_notif = hrt_absolute_time();
}
}
bool ConflictNotifier::must_notify(const uint8_t current_conflict_level, const hrt_abstime time_last_notified,
const hrt_abstime interval, const uint8_t previous_conflict_level,
const uint8_t warning_levels_mask)
{
const bool current_lvl_requires_warning = level_requires_warning(warning_levels_mask, current_conflict_level);
const bool time_to_notify = (time_last_notified == 0)
|| (interval > 0 && (hrt_absolute_time() - time_last_notified) > interval);
// Force notification if conflict level changed and at least one of the levels requires notifications.
const bool prev_action_requires_warning = previous_conflict_level != 0
&& level_requires_warning(warning_levels_mask, previous_conflict_level);
const bool force_notification = (previous_conflict_level != current_conflict_level) &&
(prev_action_requires_warning || current_lvl_requires_warning);
return force_notification || (current_lvl_requires_warning && time_to_notify);
}
void ConflictNotifier::maybe_notify_secondary_level_change(const conflict_tracker_change_s &change,
const uint8_t warning_levels_mask)
{
const bool level_change_requires_notification = level_requires_warning(warning_levels_mask, change.previous_level)
|| level_requires_warning(warning_levels_mask, change.conflict.conflict_level);
// Known traffic, only notify if level changed. If most urgent, no need to notify, will be done in DAA status.
if (!level_change_requires_notification || change.conflict_is_most_urgent) {
return;
}
notify_conflict_level(change.conflict, change.previous_level, ConflictNotifyKind::kSecondary);
}
void ConflictNotifier::maybe_notify_ignored_traffic(const conflict_info_s &conflict, const IgnoreTrafficCause cause,
const uint8_t warning_levels_mask)
{
static constexpr uint64_t kIgnoredTrafficNotifTime{2_s};
if (must_notify(conflict.conflict_level, _time_last_traffic_ignored, kIgnoredTrafficNotifTime,
conflict.conflict_level, warning_levels_mask)) {
notify_traffic_ignored(conflict, cause);
}
}
void ConflictNotifier::maybe_notify_action_on_ground(const NotifyLandedActCause cause, const uint8_t conflict_level,
const cycle_context_s &context)
{
if (must_notify(conflict_level, _time_last_landed_warning, context.status_notif_interval,
conflict_level, context.warning_levels_mask)) {
notify_action_on_ground(cause);
_time_last_landed_warning = hrt_absolute_time();
}
}
void ConflictNotifier::notify_traffic_ignored(const conflict_info_s &conflict_info, const IgnoreTrafficCause cause)
{
char encoded_id_str[kUtmGuidMsgLength];
conflict_info.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
_time_last_traffic_ignored = hrt_absolute_time();
mavlink_log_info(&_mavlink_log_pub, "DAA %s ignored (%u) lvl %u.\t",
encoded_id_str,
static_cast<unsigned>(cause),
static_cast<unsigned>(conflict_info.conflict_level));
/* EVENT
* @description
* - ID: {1}
* - ID encoding: {2} (0:ICAO, 1:callsign, 2:UAS ID)
* - cause: {3} (0:buffer full)
* - conflict level: {4}
*/
events::send<uint64_t, uint8_t, uint8_t, uint8_t>(events::ID("navigator_traffic_ignore"), events::Log::Warning,
"DAA: ignored",
conflict_info.encoded_id.id, conflict_info.encoded_id.encoding,
static_cast<uint8_t>(cause), conflict_info.conflict_level);
}
void ConflictNotifier::notify_traffic_removed(const conflict_info_s &conflict_info, const RemoveBufferCause cause)
{
const uint32_t time_since_last_comm = static_cast<uint32_t>((hrt_absolute_time() -
conflict_info.latest_update_timestamp) / 1_s);
char encoded_id_str[kUtmGuidMsgLength];
conflict_info.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
mavlink_log_warning(&_mavlink_log_pub, "DAA %s out (%u) lvl %u (%us).\t",
encoded_id_str,
static_cast<unsigned>(cause),
static_cast<unsigned>(conflict_info.conflict_level),
static_cast<unsigned>(time_since_last_comm));
/* EVENT
* @description
* - ID: {1}
* - ID encoding: {2} (0:ICAO, 1:callsign, 2:UAS ID)
* - cause: {3} (0:stale, 1:buffer full)
* - conflict level: {4}
* - last seen: {5} s ago
*/
events::send<uint64_t, uint8_t, uint8_t, uint8_t, uint32_t>(events::ID("navigator_traffic_remove"),
events::Log::Warning,
"DAA: removed",
conflict_info.encoded_id.id, conflict_info.encoded_id.encoding,
static_cast<uint8_t>(cause), conflict_info.conflict_level, time_since_last_comm);
}
bool ConflictNotifier::mavlink_log_conflict_by_level(const uint8_t conflict_level, const char *message,
events::Log &log_level)
{
switch (conflict_level) {
case detect_and_avoid_s::DAA_CONFLICT_LVL_NONE:
return false;
case detect_and_avoid_s::DAA_CONFLICT_LVL_LOW:
case detect_and_avoid_s::DAA_CONFLICT_LVL_MEDIUM:
log_level = events::Log::Warning;
break;
case detect_and_avoid_s::DAA_CONFLICT_LVL_HIGH:
log_level = events::Log::Critical;
break;
case detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL:
log_level = events::Log::Emergency;
break;
default:
return false;
}
mavlink_log_warning(&_mavlink_log_pub, "%s.\t", message);
return true;
}
void ConflictNotifier::notify_conflict_level(const conflict_info_s &conflict_info,
const uint8_t previous_conflict_level, const ConflictNotifyKind kind)
{
const uint8_t conflict_level = conflict_info.conflict_level;
if (kind == ConflictNotifyKind::kSecondary && conflict_level == previous_conflict_level) {
return;
}
const uint32_t aircraft_dist = distance_meters_for_log(conflict_info.aircraft_dist);
char encoded_id_str[kUtmGuidMsgLength];
conflict_info.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
const char *const prefix = (kind == ConflictNotifyKind::kSecondary) ? "DAA SEC:"
: (kind == ConflictNotifyKind::kMostUrgentNew) ? "DAA New and Main:" : "DAA Main:";
events::Log log_level = events::Log::Info;
if (conflict_level >= previous_conflict_level) {
const bool escalation = conflict_level > previous_conflict_level;
char message_buffer[kMaxLogMessageSize];
snprintf(message_buffer, sizeof(message_buffer), escalation ? "%s %s lvl UP %u. %u m" : "%s %s lvl %u. %u m",
prefix, encoded_id_str, static_cast<unsigned>(conflict_level), static_cast<unsigned>(aircraft_dist));
if (escalation || kind == ConflictNotifyKind::kMostUrgentNew) {
if (!mavlink_log_conflict_by_level(conflict_level, message_buffer, log_level)) {
return;
}
} else {
mavlink_log_info(&_mavlink_log_pub, "%s.\t", message_buffer);
}
} else if (conflict_level == detect_and_avoid_s::DAA_CONFLICT_LVL_NONE) {
if (kind == ConflictNotifyKind::kSecondary) {
mavlink_log_info(&_mavlink_log_pub, "%s %s solved. %u m.\t", prefix, encoded_id_str,
static_cast<unsigned>(aircraft_dist));
} else {
mavlink_log_info(&_mavlink_log_pub, "DAA all conflicts solved.\t");
}
} else {
mavlink_log_info(&_mavlink_log_pub, "%s %s lvl DOWN %u. %u m.\t", prefix, encoded_id_str,
static_cast<unsigned>(conflict_level), static_cast<unsigned>(aircraft_dist));
}
// A single structured event covers every transition: the current and previous levels make the
// direction (escalation, reduction, solved) explicit, and the kind distinguishes the most-urgent
// conflict from secondary traffic without a separate event ID per case.
/* EVENT
* @description
* - ID: {1}
* - ID encoding: {2} (0:ICAO, 1:callsign, 2:UAS ID)
* - conflict level: {3}
* - previous level: {4}
* - distance: {5m}
* - kind: {6} (0:main, 1:new+main, 2:secondary)
*/
events::send<uint64_t, uint8_t, uint8_t, uint8_t, uint32_t, uint8_t>(events::ID("navigator_traffic_conflict_update"),
log_level, "DAA conflict update",
conflict_info.encoded_id.id, conflict_info.encoded_id.encoding,
conflict_level, previous_conflict_level, aircraft_dist,
static_cast<uint8_t>(kind));
}
void ConflictNotifier::notify_new_conflict(const conflict_info_s &conflict_info)
{
const uint32_t aircraft_dist = distance_meters_for_log(conflict_info.aircraft_dist);
const uint8_t conflict_level = conflict_info.conflict_level;
char encoded_id_str[kUtmGuidMsgLength];
conflict_info.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
char message_buffer[kMaxLogMessageSize];
snprintf(message_buffer, sizeof(message_buffer), "DAA New %s lvl %u. %u m",
encoded_id_str, static_cast<unsigned>(conflict_level), static_cast<unsigned>(aircraft_dist));
events::Log log_level = events::Log::Warning;
if (mavlink_log_conflict_by_level(conflict_level, message_buffer, log_level)) {
/* EVENT
* @description
* - ID: {1}
* - ID encoding: {2} (0:ICAO, 1:callsign, 2:UAS ID)
* - conflict level: {3}
* - distance: {4m}
*/
events::send<uint64_t, uint8_t, uint8_t, uint32_t>(events::ID("navigator_new_traffic"),
log_level,
"New traffic",
conflict_info.encoded_id.id, conflict_info.encoded_id.encoding,
conflict_info.conflict_level, aircraft_dist);
}
}
void ConflictNotifier::notify_action_on_ground(const NotifyLandedActCause cause)
{
const char *blocked_action = nullptr;
events::Log log_level{events::Log::Warning};
switch (cause) {
case NotifyLandedActCause::kConflictAndArmed:
blocked_action = "takeoff";
log_level = events::Log::Critical;
break;
case NotifyLandedActCause::kConflictAndDisarmed:
blocked_action = "arm";
log_level = events::Log::Warning;
break;
default:
PX4_DEBUG("DAA: invalid landed cause");
return;
}
char message_buffer[kMaxLogMessageSize];
snprintf(message_buffer, sizeof(message_buffer), "DAA do not %s until air conflict solved!", blocked_action);
if (cause == NotifyLandedActCause::kConflictAndArmed) {
mavlink_log_critical(&_mavlink_log_pub, "%s.\t", message_buffer);
} else {
mavlink_log_warning(&_mavlink_log_pub, "%s.\t", message_buffer);
}
events::send(events::ID("navigator_traffic_ground_conflict"), log_level,
"DAA: resolve air-traffic conflict before flight");
}
void ConflictNotifier::notify_new_action(const conflict_info_s &conflict_info, const DaaAction action)
{
const uint32_t aircraft_dist = distance_meters_for_log(conflict_info.aircraft_dist);
const uint8_t conflict_level = conflict_info.conflict_level;
const char *action_name = nullptr;
events::Log log_level = events::Log::Warning;
switch (action) {
case DaaAction::kPositionHoldMode:
action_name = "Hold";
break;
case DaaAction::kReturnMode:
action_name = "Return";
log_level = events::Log::Critical;
break;
case DaaAction::kLandMode:
action_name = "Land";
log_level = events::Log::Critical;
break;
case DaaAction::kTerminate:
action_name = "Terminate";
log_level = events::Log::Emergency;
break;
case DaaAction::kDisabled:
case DaaAction::kWarnOnly:
default:
return;
}
char encoded_id_str[kUtmGuidMsgLength];
conflict_info.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
char message_buffer[kMaxLogMessageSize];
snprintf(message_buffer, sizeof(message_buffer), "DAA %s: %s! lvl %u. %u m",
encoded_id_str, action_name, static_cast<unsigned>(conflict_level), static_cast<unsigned>(aircraft_dist));
if (action == DaaAction::kTerminate) {
mavlink_log_emergency(&_mavlink_log_pub, "%s.\t", message_buffer);
} else {
mavlink_log_warning(&_mavlink_log_pub, "%s.\t", message_buffer);
}
/* EVENT
* @description
* - ID: {1}
* - ID encoding: {2} (0:ICAO, 1:callsign, 2:UAS ID)
* - action: {3} (2:return, 3:land, 4:hold, 5:terminate)
* - conflict level: {4}
* - distance: {5m}
*/
events::send<uint64_t, uint8_t, uint8_t, uint8_t, uint32_t>(events::ID("navigator_traffic_action"), log_level,
"DAA automated action",
conflict_info.encoded_id.id, conflict_info.encoded_id.encoding,
DaaActionPolicy::daa_action_to_action_param(action), conflict_level, aircraft_dist);
}
bool ConflictNotifier::pending_new_conflict_notification_exists(const DaaEncodedId &target_encoded_id,
const new_conflicts_pending_notif_s &new_conflicts_pending_notif)
{
for (size_t i = 0; i < new_conflicts_pending_notif.size(); ++i) {
if (new_conflicts_pending_notif[i] == target_encoded_id) {
return true;
}
}
return false;
}

149
src/modules/navigator/DetectAndAvoid/ConflictNotifier.h Normal file
View File

@@ -0,0 +1,149 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file ConflictNotifier.h
*
* All DAA operator messaging (MAVLink STATUSTEXT and PX4 events) in one place.
*
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include <drivers/drv_hrt.h>
#include <lib/adsb/ConflictTracker.h>
#include <lib/adsb/DaaActionPolicy.h>
#include <px4_platform_common/events.h>
#include <uORB/uORB.h>
#include <uORB/topics/transponder_report.h>
class ConflictNotifier
{
public:
ConflictNotifier() = default;
~ConflictNotifier() = default;
struct cycle_context_s {
uint8_t prev_most_urgent_level{0};
uint8_t warning_levels_mask{0};
hrt_abstime status_notif_interval{0};
};
/**
* @brief Decide and send all of this cycle's conflict messages.
*
* Processes the tracker changes in the order they were recorded, then sends the deferred "new conflict"
* warnings and the most-urgent status.
*
* Conflicts that were removed but have a "new" warning
* is still pending from the same cycle are not sent because they were never user-visible.
*
* Call exactly once per cycle; the periodic most-urgent status relies on it.
*/
void report_cycle(const conflict_tracker_changes_s &changes, const ConflictTracker &tracker,
const cycle_context_s &context);
void notify_new_action(const conflict_info_s &conflict_info, const DaaAction action);
void maybe_notify_action_on_ground(const NotifyLandedActCause cause, const uint8_t conflict_level,
const cycle_context_s &context);
void reset();
private:
// Selects the message prefix and structured event severity used when a tracked conflict's level changes.
// The most-urgent conflict drives DAA actions and is reported with
// level-based event severity; secondary conflicts stay informational.
enum class ConflictNotifyKind : uint8_t {
kMostUrgent = 0, // already tracked most urgent conflict ("DAA Main:")
kMostUrgentNew = 1, // most urgent conflict that appeared this cycle ("DAA New and Main:")
kSecondary = 2 // any other tracked conflict ("DAA SEC:")
};
using new_conflicts_pending_notif_s = px4::Array<DaaEncodedId, transponder_report_s::ORB_QUEUE_LENGTH>;
static bool must_notify(const uint8_t current_conflict_level, const hrt_abstime time_last_notified,
const hrt_abstime interval, const uint8_t previous_conflict_level,
const uint8_t warning_levels_mask);
// Announce a tracked conflict's level change, unless it was (about to become) the most urgent at
// recording time, which the DAA status reports instead.
void maybe_notify_secondary_level_change(const conflict_tracker_change_s &change,
const uint8_t warning_levels_mask);
void maybe_notify_ignored_traffic(const conflict_info_s &conflict, const IgnoreTrafficCause cause,
const uint8_t warning_levels_mask);
void notify_traffic_ignored(const conflict_info_s &conflict_info, const IgnoreTrafficCause cause);
void notify_traffic_removed(const conflict_info_s &conflict_info, const RemoveBufferCause cause);
void notify_new_conflict(const conflict_info_s &conflict_info);
/**
* @brief Notify the operator about a tracked conflict whose level changed (or a periodic
* update of the most-urgent conflict).
*
* Handles escalation, de-escalation and the "solved" transition for both the most urgent
* ("main") and secondary conflicts. @p kind selects the message prefix and whether the
* severity follows the conflict level.
*
* Secondary conflicts emit nothing when the level is unchanged.
* The most urgent conflict emits a periodic status when the level is unchanged.
*
*/
void notify_conflict_level(const conflict_info_s &conflict_info, const uint8_t previous_conflict_level,
const ConflictNotifyKind kind);
void notify_action_on_ground(const NotifyLandedActCause cause);
bool mavlink_log_conflict_by_level(const uint8_t conflict_level, const char *message, events::Log &log_level);
// Distance in meters for operator messages, non-finite or out-of-range maps to UINT32_MAX.
static uint32_t distance_meters_for_log(const float distance);
static constexpr size_t kMaxLogMessageSize{128};
static bool level_requires_warning(const uint8_t warning_levels_mask, const uint8_t conflict_level)
{
return conflict_level < 8 && (warning_levels_mask & (1u << conflict_level));
}
static bool pending_new_conflict_notification_exists(const DaaEncodedId &target_encoded_id,
const new_conflicts_pending_notif_s &new_conflicts_pending_notif);
orb_advert_t _mavlink_log_pub{nullptr};
/* Timers to avoid spamming */
hrt_abstime _time_last_status_notif{0};
hrt_abstime _time_last_traffic_ignored{0};
hrt_abstime _time_last_landed_warning{0};
};

644
src/modules/navigator/DetectAndAvoid/detect_and_avoid.cpp Normal file
View File

@@ -0,0 +1,644 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid.cpp
*
* Helper class to do detect and avoid traffic
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "../navigator.h"
#include <cinttypes>
#include <commander/px4_custom_mode.h>
#include <lib/adsb/DaaEncodedId.h>
#include <lib/mathlib/mathlib.h>
#include <px4_platform_common/board_common.h>
#include <px4_platform_common/log.h>
#include <uORB/topics/vehicle_command.h>
using namespace time_literals;
static_assert(kUasIdByteLength == PX4_GUID_BYTE_LENGTH, "UAS ID size must match the platform GUID size");
static_assert(detect_and_avoid_s::ORB_QUEUE_LENGTH >= transponder_report_s::ORB_QUEUE_LENGTH,
"DAA output queue must hold one full traffic-input burst");
// Static so the buffer lives in .bss instead of Navigator's allocation.
conflict_tracker_changes_s DetectAndAvoid::_cycle_changes;
DetectAndAvoid::DetectAndAvoid(Navigator *navigator) :
MissionBlock(navigator, 0),
ModuleParams(navigator)
{
_detect_and_avoid_pub.advertise();
_detect_and_avoid_most_urgent_pub.advertise();
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
_fake_traffic_pub.advertise();
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
}
void DetectAndAvoid::reset()
{
_is_activated = false;
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
stop_fake_traffic();
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
clear_conflicts();
}
void DetectAndAvoid::clear_conflicts()
{
_conflict_tracker.clear();
_conflict_notifier.reset();
_most_urgent_conflict_changed = true;
_previous_action = DaaAction::kDisabled;
_prev_most_urgent_conflict_level = detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
_time_last_buffer_clean = 0;
}
void DetectAndAvoid::on_inactivation()
{
PX4_DEBUG("DAA inactivation");
reset();
publish_most_urgent_conflict_if_changed();
}
void DetectAndAvoid::on_activation()
{
reset();
update_activation_status();
publish_most_urgent_conflict_if_changed();
}
void DetectAndAvoid::update_activation_status()
{
ModuleParams::updateParams();
refresh_ownship_ids();
if (!_param_daa_en.get()) {
if (_is_activated) {
PX4_DEBUG("DAA: module disabled");
reset();
}
return;
}
if (!_adsb_conflict_detector.try_updating_params()) {
if (_is_activated) {
reset();
}
PX4_ERR("DAA invalid params. Traffic warnings and actions disabled");
return;
}
if (!_is_activated) {
PX4_DEBUG("DAA: init ok");
_is_activated = true;
}
}
void DetectAndAvoid::on_active()
{
if (_parameter_update_sub.updated()) {
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
update_activation_status();
}
if (_is_activated) {
process_traffic();
}
publish_most_urgent_conflict_if_changed();
}
void DetectAndAvoid::process_traffic()
{
daa_input_s daa_input{};
if (!gather_ownship_input(daa_input)) {
if (!_conflict_tracker.empty()
|| _conflict_tracker.most_urgent().conflict_level != detect_and_avoid_s::DAA_CONFLICT_LVL_NONE) {
PX4_DEBUG("DAA: uav pose stale, clearing conflicts");
clear_conflicts();
}
return;
}
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
process_fake_traffic();
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
_cycle_changes.clear();
const hrt_abstime now = hrt_absolute_time();
if ((_time_last_buffer_clean == 0) || ((now - _time_last_buffer_clean) > kOwnshipPositionTimeout)) {
_time_last_buffer_clean = now;
if (_conflict_tracker.remove_stale_conflicts(now,
static_cast<hrt_abstime>(_param_daa_traff_tout.get()) * 1_s, _cycle_changes)) {
update_most_urgent_conflict();
}
}
if (process_transponder_queue(daa_input)) {
update_most_urgent_conflict();
}
_conflict_notifier.report_cycle(_cycle_changes, _conflict_tracker, notifier_cycle_context());
if (_most_urgent_conflict_changed) {
evaluate_and_publish_action();
_prev_most_urgent_conflict_level = _conflict_tracker.most_urgent().conflict_level;
if (_prev_most_urgent_conflict_level == detect_and_avoid_s::DAA_CONFLICT_LVL_NONE) {
_previous_action = DaaAction::kDisabled;
}
}
}
ConflictNotifier::cycle_context_s DetectAndAvoid::notifier_cycle_context() const
{
ConflictNotifier::cycle_context_s context{};
context.prev_most_urgent_level = _prev_most_urgent_conflict_level;
for (uint8_t level = detect_and_avoid_s::DAA_CONFLICT_LVL_LOW;
level <= detect_and_avoid_s::DAA_CONFLICT_LVL_CRITICAL; ++level) {
if (get_action_from_conflict_level(level) >= DaaAction::kWarnOnly) {
context.warning_levels_mask |= static_cast<uint8_t>(1u << level);
}
}
context.status_notif_interval = static_cast<hrt_abstime>(math::max<int32_t>(0, _param_daa_notif_state.get())) * 1_s;
return context;
}
bool DetectAndAvoid::transponder_data_valid(const transponder_report_s &report, const hrt_abstime now,
const hrt_abstime timeout_us)
{
if (!AdsbConflict::valid_wgs84_coordinates(report.lat, report.lon)) {
PX4_DEBUG("DAA: transponder data rejected, invalid lat/lon.");
return false;
}
if (!PX4_ISFINITE(report.altitude)) {
PX4_DEBUG("DAA: transponder data rejected, invalid altitude.");
return false;
}
uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
#if !defined(CONFIG_NAVIGATOR_ADSB_F3442) || !CONFIG_NAVIGATOR_ADSB_F3442
required_flags |= transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
#endif // !CONFIG_NAVIGATOR_ADSB_F3442
if ((report.flags & required_flags) != required_flags) {
PX4_DEBUG("DAA: transponder data rejected, missing flags.");
return false;
}
#if !defined(CONFIG_NAVIGATOR_ADSB_F3442) || !CONFIG_NAVIGATOR_ADSB_F3442
if (!PX4_ISFINITE(report.heading)) {
PX4_DEBUG("DAA: transponder data rejected, invalid heading.");
return false;
}
#endif // !CONFIG_NAVIGATOR_ADSB_F3442
if (report.timestamp == 0 || report.timestamp > now) {
PX4_DEBUG("DAA: transponder data rejected, invalid timestamp.");
return false;
}
const hrt_abstime local_age = now - report.timestamp;
const hrt_abstime source_age = static_cast<hrt_abstime>(report.tslc) * 1_s;
if (local_age > timeout_us || source_age > (timeout_us - local_age)) {
PX4_DEBUG("DAA: transponder data rejected, too old.");
return false;
}
return true;
}
bool DetectAndAvoid::process_transponder_queue(daa_input_s &daa_input)
{
bool buffer_updated = false;
transponder_report_s transponder_report{};
const hrt_abstime traffic_timeout_us = static_cast<hrt_abstime>(_param_daa_traff_tout.get()) * 1_s;
for (uint8_t processed_reports = 0; processed_reports < transponder_report_s::ORB_QUEUE_LENGTH; ++processed_reports) {
if (!_traffic_sub.update(&transponder_report)) {
break;
}
#if defined(DEBUG_BUILD)
debug_print_transponder_report(transponder_report);
#endif
if (!transponder_data_valid(transponder_report, hrt_absolute_time(), traffic_timeout_us)) {
PX4_DEBUG("DAA: transponder data not valid.");
continue;
}
const bool report_processed = process_transponder_report(daa_input, transponder_report);
buffer_updated = buffer_updated || report_processed;
}
return buffer_updated;
}
bool DetectAndAvoid::process_transponder_report(daa_input_s &daa_input,
const transponder_report_s &transponder_report)
{
const DaaEncodedId encoded_id = DaaEncodedId::identify_traffic_report(transponder_report, _ownship_ids);
if (encoded_id.id == 0) {
return false;
}
daa_input.transponder_report = transponder_report;
if (!(daa_input.transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY)) {
daa_input.transponder_report.ver_velocity = 0.f;
daa_input.transponder_report.hor_velocity = 0.f;
}
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
if (_param_daa_en_dflt_vel.get()) {
const float velocity_sign = (daa_input.transponder_report.ver_velocity >= 0.f) ? 1.f : -1.f;
daa_input.transponder_report.ver_velocity = velocity_sign * _param_daa_dflt_vel.get();
}
#endif // CONFIG_NAVIGATOR_ADSB_F3442
detect_and_avoid_s daa_output{};
if (!_adsb_conflict_detector.calculate_daa_output(daa_input, daa_output)) {
PX4_DEBUG("DAA: Failed to calculate DAA output, skipping report.");
return false;
}
daa_output.unique_id = encoded_id.id;
daa_output.unique_id_encoding = encoded_id.encoding;
daa_output.timestamp = transponder_report.timestamp;
_detect_and_avoid_pub.publish(daa_output);
if (daa_output.conflict_level == detect_and_avoid_s::DAA_CONFLICT_LVL_NONE
&& !_conflict_tracker.contains(encoded_id)) {
return false;
}
conflict_info_s current_conflict{};
current_conflict.encoded_id = encoded_id;
current_conflict.latest_update_timestamp = transponder_report.timestamp;
current_conflict.conflict_level = daa_output.conflict_level;
current_conflict.aircraft_dist = daa_output.aircraft_dist;
#if defined(DEBUG_BUILD)
PX4_DEBUG("DAA: transponder data processed, conflict detected.");
debug_print_conflict_info(current_conflict);
#endif
return _conflict_tracker.apply_conflict(current_conflict, _cycle_changes);
}
#if !defined(CONSTRAINED_FLASH) && !defined(__PX4_NUTTX)
void DetectAndAvoid::print_status() const
{
PX4_INFO("DAA: buf %u/%u, lvl %u, dist %.1fm",
static_cast<unsigned>(_conflict_tracker.size()),
static_cast<unsigned>(kDaaMaxTraffic),
static_cast<unsigned>(_conflict_tracker.most_urgent().conflict_level),
static_cast<double>(_conflict_tracker.most_urgent().aircraft_dist));
}
#endif // !CONSTRAINED_FLASH && !__PX4_NUTTX
void DetectAndAvoid::publish_most_urgent_conflict_if_changed()
{
if (!_most_urgent_conflict_changed) {
return;
}
_most_urgent_conflict_changed = false;
const conflict_info_s &most_urgent_conflict = _conflict_tracker.most_urgent();
detect_and_avoid_most_urgent_s daa_status{};
daa_status.timestamp = hrt_absolute_time();
daa_status.unique_id = most_urgent_conflict.encoded_id.id;
daa_status.unique_id_encoding = most_urgent_conflict.encoded_id.encoding;
daa_status.has_action = get_action_from_conflict_level(most_urgent_conflict.conflict_level) > DaaAction::kWarnOnly;
daa_status.conflict_level = most_urgent_conflict.conflict_level;
daa_status.aircraft_dist = most_urgent_conflict.aircraft_dist;
_detect_and_avoid_most_urgent_pub.publish(daa_status);
}
void DetectAndAvoid::update_most_urgent_conflict()
{
// Every call follows a buffer mutation, so the published status must be
// refreshed even if the most urgent conflict itself is unchanged.
_most_urgent_conflict_changed = true;
#if defined(DEBUG_BUILD)
debug_print_buffer_status();
#endif
_conflict_tracker.refresh_most_urgent();
}
void DetectAndAvoid::evaluate_and_publish_action()
{
const conflict_info_s &most_urgent_conflict = _conflict_tracker.most_urgent();
const daa_action_decision_s decision = DaaActionPolicy::decide(
most_urgent_conflict.conflict_level, _prev_most_urgent_conflict_level,
_navigator->get_vstatus()->nav_state,
_navigator->get_land_detected()->landed,
_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED,
_previous_action, action_params());
if (decision.warn_on_ground) {
_conflict_notifier.maybe_notify_action_on_ground(decision.ground_warning_cause,
most_urgent_conflict.conflict_level, notifier_cycle_context());
}
if (decision.action_command != DaaAction::kDisabled) {
PX4_DEBUG("DAA: publish action %d", (int)decision.action_command);
publish_action_command(decision.action_command);
if (decision.announce_action) {
_conflict_notifier.notify_new_action(most_urgent_conflict, decision.action_command);
}
_previous_action = decision.action_command;
}
}
void DetectAndAvoid::refresh_ownship_ids()
{
_ownship_ids = {};
_ownship_ids.icao = _vehicle_adsb_icao.get();
_ownship_ids.icao_2 = _vehicle_adsb_icao_2.get();
_ownship_ids.callsign = DaaEncodedId::callsign_params_to_uint64(_vehicle_adsb_callsign_1.get(),
_vehicle_adsb_callsign_2.get());
#ifndef BOARD_HAS_NO_UUID
px4_guid_t px4_guid {};
if (board_get_px4_guid(px4_guid) == PX4_GUID_BYTE_LENGTH) {
_ownship_ids.uas_id = DaaEncodedId::last_uas_id_bytes_to_uint64(px4_guid);
_ownship_ids.uas_id_valid = true;
} else {
PX4_DEBUG("DAA: Failed to get own UAS ID.");
}
#endif // BOARD_HAS_NO_UUID
}
bool DetectAndAvoid::gather_ownship_input(daa_input_s &daa_input) const
{
const vehicle_global_position_s &global_position = *_navigator->get_global_position();
if (!global_position.lat_lon_valid || !global_position.alt_valid
|| !AdsbConflict::valid_wgs84_coordinates(global_position.lat, global_position.lon)
|| !PX4_ISFINITE(global_position.alt)) {
PX4_DEBUG("DAA: invalid global pose");
return false;
}
if (global_position.timestamp == 0 || hrt_elapsed_time(&global_position.timestamp) > kOwnshipPositionTimeout) {
PX4_DEBUG("DAA: stale global pose");
return false;
}
const vehicle_local_position_s &local_position = *_navigator->get_local_position();
if (local_position.timestamp == 0 || hrt_elapsed_time(&local_position.timestamp) > kOwnshipPositionTimeout) {
PX4_DEBUG("DAA: stale local position");
return false;
}
daa_input.uav_lat_lon = matrix::Vector2d(global_position.lat, global_position.lon);
daa_input.uav_alt = global_position.alt;
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
// F3442's static alert volumes remain usable when a velocity group is unavailable.
daa_input.uav_vel_ned(0) = local_position.v_xy_valid && PX4_ISFINITE(local_position.vx) ? local_position.vx : 0.f;
daa_input.uav_vel_ned(1) = local_position.v_xy_valid && PX4_ISFINITE(local_position.vy) ? local_position.vy : 0.f;
daa_input.uav_vel_ned(2) = local_position.v_z_valid && PX4_ISFINITE(local_position.vz) ? local_position.vz : 0.f;
#else
if (!local_position.v_xy_valid || !local_position.v_z_valid
|| !PX4_ISFINITE(local_position.vx) || !PX4_ISFINITE(local_position.vy)
|| !PX4_ISFINITE(local_position.vz)) {
PX4_DEBUG("DAA: invalid local velocity");
return false;
}
daa_input.uav_vel_ned = matrix::Vector3f(local_position.vx, local_position.vy, local_position.vz);
#endif // CONFIG_NAVIGATOR_ADSB_F3442
return true;
}
DaaAction DetectAndAvoid::get_action_from_conflict_level(const uint8_t conflict_level) const
{
return DaaActionPolicy::action_from_conflict_level(conflict_level, action_params());
}
daa_action_params_s DetectAndAvoid::action_params() const
{
daa_action_params_s params{};
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
params.lvl_low_act = _param_daa_lvl_low_act.get();
params.lvl_med_act = _param_daa_lvl_med_act.get();
params.lvl_high_act = _param_daa_lvl_high_act.get();
params.lvl_crit_act = _param_daa_lvl_crit_act.get();
#else
params.traff_avoid = _param_nav_traff_avoid.get();
#endif // CONFIG_NAVIGATOR_ADSB_F3442
return params;
}
void DetectAndAvoid::publish_action_command(const DaaAction requested_action)
{
vehicle_command_s vcmd = {};
switch (requested_action) {
case DaaAction::kDisabled:
case DaaAction::kWarnOnly: {
return;
}
case DaaAction::kPositionHoldMode: {
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE;
vcmd.param1 = 1;
vcmd.param2 = PX4_CUSTOM_MAIN_MODE_AUTO;
vcmd.param3 = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
break;
}
case DaaAction::kReturnMode: {
_navigator->set_rtl_return_alt_min(true);
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
break;
}
case DaaAction::kLandMode: {
vcmd.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
break;
}
case DaaAction::kTerminate: {
vcmd.param1 = 1;
vcmd.command = vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION;
break;
}
default:
break;
}
_navigator->publish_vehicle_command(vcmd);
}
#if defined(DEBUG_BUILD)
void DetectAndAvoid::debug_print_buffer_status()
{
const conflict_info_s &most_urgent_conflict = _conflict_tracker.most_urgent();
char encoded_id_str[kUtmGuidMsgLength];
most_urgent_conflict.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
const int time_since_last_comm = static_cast<int>((hrt_absolute_time() -
most_urgent_conflict.latest_update_timestamp) / 1_s);
const uint16_t aircraft_dist = static_cast<uint16_t>(fabsf(most_urgent_conflict.aircraft_dist));
const int buff_size = static_cast<int>(_conflict_tracker.size());
PX4_DEBUG("Buffer status: %d conflict(s), max lvl %d, prev max lvl %d,",
buff_size,
most_urgent_conflict.conflict_level,
_prev_most_urgent_conflict_level);
PX4_DEBUG("Max conflict: Unique ID %" PRIu64 ", ID str %s, lvl %d, distance %d, last comm %d sec \n",
most_urgent_conflict.encoded_id.id,
encoded_id_str,
most_urgent_conflict.conflict_level,
aircraft_dist,
time_since_last_comm);
}
void DetectAndAvoid::debug_print_conflict_info(const conflict_info_s &conflict)
{
char encoded_id_str[kUtmGuidMsgLength];
conflict.encoded_id.to_string(encoded_id_str, sizeof(encoded_id_str));
const int time_since_last_comm = static_cast<int>((hrt_absolute_time() - conflict.latest_update_timestamp) / 1_s);
const uint16_t aircraft_dist = static_cast<uint16_t>(fabsf(conflict.aircraft_dist));
PX4_DEBUG("ID: uint %" PRIu64 ", ID str %s, lvl %d, distance %d, last comm %d sec \n",
conflict.encoded_id.id,
encoded_id_str,
conflict.conflict_level,
aircraft_dist,
time_since_last_comm);
}
void DetectAndAvoid::debug_print_transponder_report(const transponder_report_s &transponder_report)
{
const int traffic_direction = math::degrees(transponder_report.heading) + 180;
// Unique ID conversions
uint64_t uas_id_int = DaaEncodedId::last_uas_id_bytes_to_uint64(transponder_report.uas_id);
char uas_id_char[kUtmGuidMsgLength];
DaaEncodedId::convert_uas_id_uint64_to_str(uas_id_int, uas_id_char);
uint64_t callsign_int = DaaEncodedId::callsign_to_uint64(transponder_report.callsign);
char callsign[kCallsignLength];
DaaEncodedId::convert_uint64_callsign_to_str(callsign_int, callsign);
uint64_t icao_address = static_cast<uint64_t>(transponder_report.icao_address);
char icao_str[kIcaoLength];
DaaEncodedId::convert_icao_uint32_to_hex_str(icao_address, icao_str, sizeof(icao_str));
PX4_DEBUG("ADSB_IN: ICAO uint %" PRIu64 ", ICAO str %s",
icao_address,
icao_str);
PX4_DEBUG("ADSB_IN: Callsign uint %" PRIu64 ", Callsign str %s",
callsign_int,
callsign);
PX4_DEBUG("ADSB_IN: UAS_ID uint %" PRIu64 ", UAS_ID str %s",
uas_id_int,
uas_id_char);
// Log which flags are enabled in one line using printf
PX4_DEBUG("ADSB_IN: Flags missing: %s%s%s%s%s%s%s",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS) ? "" : "coord ",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE) ? "" : "alt ",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING) ? "" : "hdg ",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY) ? "" : "vel ",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN) ? "" : "callsign ",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK) ? "" : "squawk ",
(transponder_report.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE) ? "" : "Retranslate ");
PX4_DEBUG("ADSB_IN: lat %.2f, lon %.2f, alt %.2f, hdg %.d, vel hor %.1f, vel vert %.1f \n",
(double)transponder_report.lat,
(double)transponder_report.lon,
(double)transponder_report.altitude,
traffic_direction,
(double)transponder_report.hor_velocity,
(double)transponder_report.ver_velocity);
}
#endif // DEBUG_BUILD

253
src/modules/navigator/DetectAndAvoid/detect_and_avoid.h Normal file
View File

@@ -0,0 +1,253 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid.h
*
* Helper class to do detect and avoid traffic
*
* @author Jonas Perolini <jonspero@me.com>
*/
#pragma once
#include "../mission_block.h"
#include "ConflictNotifier.h"
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
#include <pthread.h>
#endif
#include <drivers/drv_hrt.h>
#include <lib/adsb/AdsbConflict.h>
#include <lib/adsb/ConflictTracker.h>
#include <lib/adsb/DaaEncodedId.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/time.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/detect_and_avoid.h>
#include <uORB/topics/detect_and_avoid_most_urgent.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/transponder_report.h>
using namespace time_literals;
class DetectAndAvoid : public MissionBlock, public ModuleParams
{
public:
DetectAndAvoid(Navigator *navigator);
~DetectAndAvoid() override = default;
void on_activation() override;
void on_active() override;
void on_inactivation() override;
void reset();
bool is_activated() const { return _is_activated; }
#if !defined(CONSTRAINED_FLASH) && !defined(__PX4_NUTTX)
void print_status() const;
#endif // !CONSTRAINED_FLASH && !__PX4_NUTTX
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
static constexpr uint16_t kFakeTrafficDefaultFlags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN;
// Fake traffic scripts used for manual DAA validation from the navigator shell.
enum class FakeTraffMode : uint8_t {
kUniqueIds = 0,
kEscalation = 1,
kSpamSame = 2,
kSpamNew = 3,
kFlags = 4,
kQueueFill = 5
};
struct SyntheticTrafficReport {
uint32_t icao_address{0};
const char *callsign{""};
float distance{0.f};
float direction{0.f};
float traffic_heading{0.f};
float altitude_diff{10.f};
float hor_velocity{100.f};
float ver_velocity{10.f};
int emitter_type{transponder_report_s::ADSB_EMITTER_TYPE_LIGHT};
double lat_uav{45.35324098};
double lon_uav{6.446453};
float alt_uav{300.f};
uint16_t flags{kFakeTrafficDefaultFlags};
};
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
DaaAction get_action_from_conflict_level(const uint8_t conflict_level) const;
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
/**
* @brief Arm a fake-traffic script from the navigator shell for manual DAA testing.
*
* Uses the ownship position so the fake targets stay anchored to a fixed origin.
*/
void run_fake_traffic(FakeTraffMode mode, double lat_uav, double lon_uav, float alt_uav);
void stop_fake_traffic();
void fake_traffic(const SyntheticTrafficReport &report);
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
conflict_info_s get_most_urgent_conflict() const { return _conflict_tracker.most_urgent(); }
private:
static constexpr hrt_abstime kOwnshipPositionTimeout{2_s};
// Change records collected by the conflict tracker over one detection cycle.
// Static so the ~1.8 KB buffer lives in .bss (AXI_SRAM on FMU targets).
static conflict_tracker_changes_s _cycle_changes;
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
struct fake_traffic_origin_s {
double lat{0.0};
double lon{0.0};
float alt{0.f};
};
struct fake_traffic_state_s {
bool active{false};
FakeTraffMode mode{FakeTraffMode::kUniqueIds};
uint8_t next_step_idx{0};
hrt_abstime next_publish_at{0};
fake_traffic_origin_s origin{};
void clear()
{
active = false;
mode = FakeTraffMode::kUniqueIds;
next_step_idx = 0;
next_publish_at = 0;
origin = {};
}
};
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
uORB::Publication<detect_and_avoid_s> _detect_and_avoid_pub{ORB_ID(detect_and_avoid)};
uORB::Publication<detect_and_avoid_most_urgent_s> _detect_and_avoid_most_urgent_pub{ORB_ID(detect_and_avoid_most_urgent)};
void publish_most_urgent_conflict_if_changed();
void clear_conflicts();
void update_activation_status();
void process_traffic();
bool _is_activated{false};
ConflictTracker _conflict_tracker{};
ConflictNotifier _conflict_notifier{};
ConflictNotifier::cycle_context_s notifier_cycle_context() const;
void update_most_urgent_conflict();
bool _most_urgent_conflict_changed{false};
uint8_t _prev_most_urgent_conflict_level = detect_and_avoid_s::DAA_CONFLICT_LVL_NONE;
void evaluate_and_publish_action();
void publish_action_command(const DaaAction requested_action);
daa_action_params_s action_params() const;
DaaAction _previous_action = DaaAction::kDisabled;
#if defined(CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC)
uORB::Publication<transponder_report_s> _fake_traffic_pub {ORB_ID(transponder_report)};
// The shell command and the navigator update loop can touch fake traffic state
// from different threads, so protect the pending script with a mutex.
pthread_mutex_t _fake_traffic_mutex = PTHREAD_MUTEX_INITIALIZER;
fake_traffic_state_s _fake_traffic_state{};
void process_fake_traffic();
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
uORB::Subscription _traffic_sub {ORB_ID(transponder_report)};
AdsbConflict _adsb_conflict_detector;
daa_ownship_ids_s _ownship_ids{};
void refresh_ownship_ids();
bool gather_ownship_input(daa_input_s &daa_input) const;
static bool transponder_data_valid(const transponder_report_s &report, hrt_abstime now, hrt_abstime timeout_us);
/**
* @brief Process queued transponder reports and update the conflict buffer.
*
* @p daa_input carries the ownship state (filled once per cycle); the traffic half is
* overwritten in place for each processed report, avoiding a copy for each report.
*/
bool process_transponder_queue(daa_input_s &daa_input);
bool process_transponder_report(daa_input_s &daa_input, const transponder_report_s &transponder_report);
hrt_abstime _time_last_buffer_clean{0};
/* Debug functions */
#if defined(DEBUG_BUILD)
void debug_print_transponder_report(const transponder_report_s &transponder_report);
void debug_print_conflict_info(const conflict_info_s &conflict);
void debug_print_buffer_status();
#endif
/* Parameters */
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
DEFINE_PARAMETERS(
(ParamInt<px4::params::DAA_EN>) _param_daa_en,
(ParamInt<px4::params::DAA_NOTIF_STATE>) _param_daa_notif_state,
(ParamInt<px4::params::DAA_TRAFF_TOUT>) _param_daa_traff_tout,
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
(ParamFloat<px4::params::DAA_DFLT_VEL>) _param_daa_dflt_vel,
(ParamInt<px4::params::DAA_EN_DFLT_VEL>) _param_daa_en_dflt_vel,
(ParamInt<px4::params::DAA_LVL_LOW_ACT>) _param_daa_lvl_low_act,
(ParamInt<px4::params::DAA_LVL_MED_ACT>) _param_daa_lvl_med_act,
(ParamInt<px4::params::DAA_LVL_HIGH_ACT>) _param_daa_lvl_high_act,
(ParamInt<px4::params::DAA_LVL_CRIT_ACT>) _param_daa_lvl_crit_act,
#else
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid,
#endif // CONFIG_NAVIGATOR_ADSB_F3442
(ParamInt<px4::params::ADSB_ICAO_ID>) _vehicle_adsb_icao,
(ParamInt<px4::params::ADSB_ICAO_ID_2>) _vehicle_adsb_icao_2,
(ParamInt<px4::params::ADSB_CALLSIGN_1>) _vehicle_adsb_callsign_1,
(ParamInt<px4::params::ADSB_CALLSIGN_2>) _vehicle_adsb_callsign_2
)
};

138
src/modules/navigator/DetectAndAvoid/detect_and_avoid_f3442_params.yaml Normal file
View File

@@ -0,0 +1,138 @@
module_name: detect_and_avoid_f3442
parameters:
- group: Detect and Avoid F3442 Standard
definitions:
DAA_DFLT_VEL:
description:
short: Default vertical speed of other aircraft
long: Used when DAA_EN_DFLT_VEL is enabled. Specific to the F3442 DAA standard.
type: float
default: 10.0
unit: m/s
min: 0
DAA_EN_DFLT_VEL:
description:
short: Ignore reported vertical speed of other aircraft
long: Use DAA_DFLT_VEL instead of reported traffic vertical speed. Specific to the F3442 DAA standard.
type: boolean
default: 1
DAA_LVL_LOW_ACT:
description:
short: Low conflict action
long: |-
Action requested when the low conflict level is reached.
Runtime changes apply on later conflict level transitions.
Specific to the F3442 DAA standard.
type: enum
values:
0: Disabled
1: Warn only
2: Return mode
3: Land mode
4: Position Hold mode
5: Terminate
default: 1
min: 0
max: 5
DAA_LVL_MED_ACT:
description:
short: Medium conflict action
long: |-
Action requested when the medium conflict level is reached.
Runtime changes apply on later conflict level transitions.
Specific to the F3442 DAA standard.
type: enum
values:
0: Disabled
1: Warn only
2: Return mode
3: Land mode
4: Position Hold mode
5: Terminate
default: 1
min: 0
max: 5
DAA_LVL_HIGH_ACT:
description:
short: High conflict action
long: |-
Action requested when the high conflict level is reached.
Runtime changes apply on later conflict level transitions.
Specific to the F3442 DAA standard.
type: enum
values:
0: Disabled
1: Warn only
2: Return mode
3: Land mode
4: Position Hold mode
5: Terminate
default: 1
min: 0
max: 5
DAA_LVL_CRIT_ACT:
description:
short: Critical conflict action
long: |-
Action requested when the critical conflict level is reached.
Runtime changes apply on later conflict level transitions.
Specific to the F3442 DAA standard.
type: enum
values:
0: Disabled
1: Warn only
2: Return mode
3: Land mode
4: Position Hold mode
5: Terminate
default: 1
min: 0
max: 5
DAA_LVL_CRIT_RAD:
description:
short: Critical conflict zone cylinder radius (NMAC)
long: Per-aircraft half-zone. Specific to the F3442 DAA standard.
type: float
default: 77.0
unit: m
min: 10
DAA_LVL_CRIT_HGT:
description:
short: Critical conflict zone cylinder height (NMAC)
long: Per-aircraft half-zone. Specific to the F3442 DAA standard.
type: float
default: 16.0
unit: m
min: 10
DAA_LVL_HIGH_RAD:
description:
short: High conflict zone cylinder radius (WC)
long: Per-aircraft half-zone. Specific to the F3442 DAA standard.
type: float
default: 305.0
unit: m
min: 10
DAA_LVL_HIGH_HGT:
description:
short: High conflict zone cylinder height (WC)
long: Per-aircraft half-zone. Specific to the F3442 DAA standard.
type: float
default: 39.0
unit: m
min: 10
DAA_LVL_MED_TIME:
description:
short: Medium conflict latency time
long: Expands NMAC bounds by aircraft speed times this value. Specific to the F3442 DAA standard.
type: int32
default: 33
unit: s
min: 0
DAA_LVL_LOW_TIME:
description:
short: Low conflict latency time
long: Expands well-clear bounds by aircraft speed times this value. Specific to the F3442 DAA standard.
type: int32
default: 33
unit: s
min: 0

484
src/modules/navigator/DetectAndAvoid/detect_and_avoid_fake_traffic.cpp Normal file
View File

@@ -0,0 +1,484 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid_fake_traffic.cpp
*
* Generate synthetic traffic for manual Detect and Avoid testing
*
* @author Jonas Perolini <jonspero@me.com>
*/
#include "detect_and_avoid.h"
#include <containers/LockGuard.hpp>
#include <cstring>
#include <lib/geo/geo.h>
using namespace time_literals;
namespace
{
// Each fake-traffic mode is a deterministic script. `run_fake_traffic()` only
// stores the selected mode and an ownship position snapshot; `process_fake_traffic()`
// later publishes the scripted reports that are due from the navigator update loop.
static constexpr uint16_t kDefaultFakeTrafficFlags = DetectAndAvoid::kFakeTrafficDefaultFlags;
static constexpr uint16_t kCallsignNotValidFlags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
static constexpr uint16_t kVelocityNotValidFlags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN;
// Publish one final far-away sample to clear the same traffic entry cleanly.
static constexpr float kFakeTrafficResolveDistance = 5000.f;
struct fake_traffic_script_step_s {
uint32_t icao_address;
const char *callsign;
float distance;
float direction;
float traffic_heading;
float altitude_diff;
float hor_velocity;
float ver_velocity;
int emitter_type;
uint16_t flags;
hrt_abstime delay_to_next;
};
// Three isolated sequences that exercise ICAO, callsign, and UAS-ID fallback.
static constexpr fake_traffic_script_step_s kUniqueIdScript[] {
{
7249787, "6E9F7B", 1500.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
7249787, "6E9F7B", 800.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
7249787, "6E9F7B", 200.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
7249787, "6E9F7B", kFakeTrafficResolveDistance, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
0, "LX00777A", 1500.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
0, "LX00777A", 800.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
0, "LX00777A", 200.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
0, "LX00777A", kFakeTrafficResolveDistance, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 2_s
},
{
0, "NOCALL", 1500.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kCallsignNotValidFlags, 2_s
},
{
0, "NOCALL", 800.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kCallsignNotValidFlags, 2_s
},
{
0, "NOCALL", 200.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kCallsignNotValidFlags, 2_s
},
{
0, "NOCALL", kFakeTrafficResolveDistance, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kCallsignNotValidFlags, 2_s
},
};
// Deterministic ranges chosen to fill, evict, and ignore entries in the traffic buffer.
static constexpr fake_traffic_script_step_s kSpamNewScript[] {
{
3673751, "", 1200.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // 380E97 LOW
{
6813217, "", 900.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // 67F621 LOW
{
11329143, "", 700.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // ACDE77 MEDIUM
{
4584054, "", 500.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // 45F276 HIGH
{
3447055, "", 100.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // 34990F CRITICAL
{
15426008, "", 130.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // EB61D8 CRITICAL
{
10611378, "", 550.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // A1EAB2 HIGH
{
12971298, "", 1100.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // C5ED22 LOW -> ignored
{
11728980, "", 750.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // B2F854 MEDIUM -> ignored
{
14771352, "", 140.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, 1_s
}, // E16498 CRITICAL
};
static constexpr fake_traffic_script_step_s kFlagsScript[] {
{
0, "L07NOVEL", 100.f, 1.0f, 0.0f, 0.0f, 100.0f, 10.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kVelocityNotValidFlags, 0
}
};
template <size_t N>
bool get_script_step(const fake_traffic_script_step_s(&script)[N], const uint8_t step_idx,
fake_traffic_script_step_s &step)
{
if (step_idx >= N) {
return false;
}
step = script[step_idx];
return true;
}
bool get_queue_fill_step(const uint8_t report_idx, fake_traffic_script_step_s &step)
{
constexpr uint8_t batch_count = 3;
constexpr uint8_t reports_per_batch = transponder_report_s::ORB_QUEUE_LENGTH;
constexpr uint8_t total_report_count = batch_count * reports_per_batch;
constexpr uint8_t replace_count = (reports_per_batch < (kDaaMaxTraffic - 1))
? reports_per_batch : (kDaaMaxTraffic - 1);
constexpr hrt_abstime batch_delay = 2_s;
constexpr uint32_t main_icao = 0xDDF0A1u;
constexpr uint32_t step_2_icao_base = 0x510000u;
constexpr uint32_t step_3_icao_base = 0x520000u;
constexpr float main_distance = 100.f;
constexpr float step_2_first_distance = 800.f;
constexpr float step_2_distance_delta = 10.f;
constexpr float step_3_distance_delta = 20.f;
constexpr float step_2_closest_distance = step_2_first_distance - ((reports_per_batch - 1u) * step_2_distance_delta);
constexpr float step_3_first_distance = step_2_closest_distance - (replace_count * step_3_distance_delta);
if (report_idx >= total_report_count) {
return false;
}
// `queue_fill` is a three-batch scenario. Each batch publishes exactly
// `reports_per_batch` reports, but only one batch is allowed per navigator
// cycle so the queue drain and the scripted step count stay distinct.
const uint8_t batch_idx = report_idx / reports_per_batch;
const uint8_t report_idx_in_batch = report_idx % reports_per_batch;
const bool end_of_batch = (report_idx_in_batch + 1u) == reports_per_batch;
const hrt_abstime delay_to_next = (end_of_batch && (batch_idx + 1u) < batch_count) ? batch_delay : 0;
switch (batch_idx) {
case 0:
step = {
main_icao, "DDF0A1", main_distance, 0.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, delay_to_next
};
return true;
case 1:
step = {
static_cast<uint32_t>(step_2_icao_base + report_idx_in_batch + 1u), "",
step_2_first_distance - (report_idx_in_batch * step_2_distance_delta),
0.1f * static_cast<float>(report_idx_in_batch), 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, delay_to_next
};
return true;
case 2:
step = {
static_cast<uint32_t>(step_3_icao_base + report_idx_in_batch + 1u), "",
step_3_first_distance + (report_idx_in_batch * step_3_distance_delta),
0.1f * static_cast<float>(report_idx_in_batch), 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, delay_to_next
};
return true;
default:
return false;
}
}
const char *fake_traffic_mode_to_string(const DetectAndAvoid::FakeTraffMode mode)
{
switch (mode) {
case DetectAndAvoid::FakeTraffMode::kUniqueIds:
return "unique_ids";
case DetectAndAvoid::FakeTraffMode::kEscalation:
return "escalation";
case DetectAndAvoid::FakeTraffMode::kSpamSame:
return "spam_same";
case DetectAndAvoid::FakeTraffMode::kSpamNew:
return "spam_new";
case DetectAndAvoid::FakeTraffMode::kFlags:
return "flags";
case DetectAndAvoid::FakeTraffMode::kQueueFill:
return "queue_fill";
default:
return "";
}
}
bool get_fake_traffic_step(const DetectAndAvoid::FakeTraffMode mode, const uint8_t step_idx,
fake_traffic_script_step_s &step)
{
switch (mode) {
case DetectAndAvoid::FakeTraffMode::kUniqueIds:
return get_script_step(kUniqueIdScript, step_idx, step);
case DetectAndAvoid::FakeTraffMode::kSpamNew:
return get_script_step(kSpamNewScript, step_idx, step);
case DetectAndAvoid::FakeTraffMode::kFlags:
return get_script_step(kFlagsScript, step_idx, step);
case DetectAndAvoid::FakeTraffMode::kEscalation: {
static constexpr uint8_t kStepCount = 30;
static constexpr hrt_abstime kDelayEsc = 2_s;
if (step_idx >= kStepCount) {
return false;
}
step = {
10436515, "9F3FA3", 3000.f - (100.f * step_idx), 1.0f, 0.0f, 0.0f, 50.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, kDelayEsc
};
return true;
}
case DetectAndAvoid::FakeTraffMode::kSpamSame: {
static constexpr uint8_t kStepCount = 40;
static constexpr hrt_abstime kDelaySpam = 100_ms;
if (step_idx >= kStepCount) {
return false;
}
step = {
6420348, "61F77C", 200.f, 1.0f, 0.0f, 0.0f, 20.0f, 0.0f,
transponder_report_s::ADSB_EMITTER_TYPE_LIGHT, kDefaultFakeTrafficFlags, kDelaySpam
};
return true;
}
case DetectAndAvoid::FakeTraffMode::kQueueFill:
return get_queue_fill_step(step_idx, step);
default:
return false;
}
}
} // namespace
void DetectAndAvoid::run_fake_traffic(const FakeTraffMode mode, const double lat_uav, const double lon_uav,
const float alt_uav)
{
const char *const mode_name = fake_traffic_mode_to_string(mode);
if (mode_name[0] == '\0') {
PX4_ERR("DAA: unknown fake traffic mode %u", static_cast<unsigned>(mode));
return;
}
{
const LockGuard lg{_fake_traffic_mutex};
_fake_traffic_state.clear();
_fake_traffic_state.active = true;
_fake_traffic_state.mode = mode;
_fake_traffic_state.origin.lat = lat_uav;
_fake_traffic_state.origin.lon = lon_uav;
_fake_traffic_state.origin.alt = alt_uav;
}
PX4_WARN("DAA: fake traffic can trigger configured actions, including terminate");
PX4_INFO("DAA: fake traffic '%s' scheduled", mode_name);
}
void DetectAndAvoid::stop_fake_traffic()
{
const LockGuard lg{_fake_traffic_mutex};
_fake_traffic_state.clear();
}
void DetectAndAvoid::process_fake_traffic()
{
const hrt_abstime now = hrt_absolute_time();
uint8_t max_reports_per_cycle = 1;
{
const LockGuard lg{_fake_traffic_mutex};
if (!_fake_traffic_state.active) {
return;
}
// `queue_fill` is the only burst mode. All other modes publish at most one
// report per navigator update, even if a future script adds zero-delay steps.
max_reports_per_cycle = (_fake_traffic_state.mode == DetectAndAvoid::FakeTraffMode::kQueueFill) ?
transponder_report_s::ORB_QUEUE_LENGTH : 1u;
}
for (uint8_t published_reports = 0; published_reports < max_reports_per_cycle; ++published_reports) {
fake_traffic_script_step_s step{};
fake_traffic_origin_s origin{};
{
const LockGuard lg{_fake_traffic_mutex};
if (!_fake_traffic_state.active) {
return;
}
if ((_fake_traffic_state.next_publish_at != 0) && (now < _fake_traffic_state.next_publish_at)) {
return;
}
if (!get_fake_traffic_step(_fake_traffic_state.mode, _fake_traffic_state.next_step_idx, step)) {
_fake_traffic_state.clear();
return;
}
origin = _fake_traffic_state.origin;
++_fake_traffic_state.next_step_idx;
_fake_traffic_state.next_publish_at = now + step.delay_to_next;
}
SyntheticTrafficReport report{};
report.icao_address = step.icao_address;
report.callsign = step.callsign;
report.distance = step.distance;
report.direction = step.direction;
report.traffic_heading = step.traffic_heading;
report.altitude_diff = step.altitude_diff;
report.hor_velocity = step.hor_velocity;
report.ver_velocity = step.ver_velocity;
report.emitter_type = step.emitter_type;
report.lat_uav = origin.lat;
report.lon_uav = origin.lon;
report.alt_uav = origin.alt;
report.flags = step.flags;
fake_traffic(report);
if (step.delay_to_next != 0) {
return;
}
}
}
void DetectAndAvoid::fake_traffic(const SyntheticTrafficReport &report)
{
double lat{0.0};
double lon{0.0};
waypoint_from_heading_and_distance(report.lat_uav, report.lon_uav, report.direction, report.distance, &lat,
&lon);
float alt = report.alt_uav + report.altitude_diff;
transponder_report_s tr{};
tr.timestamp = hrt_absolute_time();
tr.icao_address = report.icao_address;
tr.lat = lat;
tr.lon = lon;
tr.altitude_type = 0;
tr.altitude = alt;
tr.heading = report.traffic_heading;
tr.hor_velocity = report.hor_velocity;
tr.ver_velocity = report.ver_velocity;
strncpy(&tr.callsign[0], report.callsign != nullptr ? report.callsign : "", sizeof(tr.callsign) - 1);
tr.callsign[sizeof(tr.callsign) - 1] = 0;
tr.emitter_type = report.emitter_type;
tr.tslc = 2; // seconds since last communication
tr.flags = report.flags;
tr.squawk = 6667;
#ifndef BOARD_HAS_NO_UUID
px4_guid_t px4_guid {};
if (board_get_px4_guid(px4_guid) == PX4_GUID_BYTE_LENGTH) {
memcpy(tr.uas_id, px4_guid, sizeof(px4_guid_t));
tr.uas_id[PX4_GUID_BYTE_LENGTH - 1] ^= 0x01;
} else {
for (int i = 0; i < PX4_GUID_BYTE_LENGTH; ++i) {
tr.uas_id[i] = 0xe0 + i;
}
}
#else
for (int i = 0; i < PX4_GUID_BYTE_LENGTH ; i++) {
tr.uas_id[i] = 0xe0 + i;
}
#endif /* BOARD_HAS_NO_UUID */
PX4_DEBUG("Publish fake traffic");
_fake_traffic_pub.publish(tr);
}

27
src/modules/navigator/DetectAndAvoid/detect_and_avoid_params.yaml Normal file
View File

@@ -0,0 +1,27 @@
module_name: detect_and_avoid
parameters:
- group: Detect and Avoid
definitions:
DAA_EN:
description:
short: Enable DAA
type: boolean
default: 1
DAA_NOTIF_STATE:
description:
short: DAA status notification period
long: Time between notifications for the most urgent conflict. Set to 0 to disable the periodic status notification.
type: int32
default: 20
unit: s
min: 0
max: 600
DAA_TRAFF_TOUT:
description:
short: DAA stale conflict timeout
long: Conflict age before removal.
type: int32
default: 20
unit: s
min: 1
max: 120

47
src/modules/navigator/DetectAndAvoid/test/CMakeLists.txt Normal file
View File

@@ -0,0 +1,47 @@
############################################################################
#
# Copyright (c) 2026 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# Let the test sources include the module headers (detect_and_avoid.h) and the
# parent navigator headers (navigator.h, mission.h) without relative paths.
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/..)
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/../..)
# Common tests run for whichever conflict standard is built.
px4_add_functional_gtest(SRC detect_and_avoid_test.cpp LINKLIBS detect_and_avoid modules__navigator modules__dataman)
# Standard-specific tests follow the standard selected at build time.
if(CONFIG_NAVIGATOR_ADSB_F3442)
px4_add_functional_gtest(SRC detect_and_avoid_f3442_test.cpp LINKLIBS detect_and_avoid modules__navigator modules__dataman)
else()
px4_add_functional_gtest(SRC detect_and_avoid_crosstrack_test.cpp LINKLIBS detect_and_avoid modules__navigator modules__dataman)
endif()

189
src/modules/navigator/DetectAndAvoid/test/detect_and_avoid_crosstrack_test.cpp Normal file
View File

@@ -0,0 +1,189 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid_crosstrack_test.cpp
* @brief Tests specific to the crosstrack DAA standard.
*
* This file is only built when CONFIG_NAVIGATOR_ADSB_F3442 is disabled; it tests the
* single-threshold crosstrack behavior and the traffic-heading input requirement
* that only applies to the crosstrack standard.
*
* @author Jonas Perolini <jonspero@me.com>
*
*/
#include "detect_and_avoid_test_common.h"
// Activation fails closed when a crosstrack gate parameter is invalid.
TEST_F(DetectAndAvoidTest, ActivationFailsWithInvalidCrosstrackParams)
{
EXPECT_TRUE(navigator->get_detect_and_avoid()->is_activated());
const float negative_value = -10.f;
param_set(param_handle(px4::params::NAV_TRAFF_A_HOR), &negative_value);
navigator->get_detect_and_avoid()->on_activation();
EXPECT_FALSE(navigator->get_detect_and_avoid()->is_activated());
param_reset_all();
}
// A runtime NAV_TRAFF_AVOID change doesn't re-evaluate the current buffer, but the next conflict
// uses the refreshed action.
TEST_F(DetectAndAvoidTest, RuntimeCrosstrackTerminateUpdateAppliesOnNextConflict)
{
const int warn_only = action_param_value(DaaAction::kWarnOnly);
const int terminate_action = action_param_value(DaaAction::kTerminate);
param_set(param_handle(px4::params::NAV_TRAFF_AVOID), &warn_only);
recreate_navigator();
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{0.f, 0.f, 0.f};
const float conflict_distance = 200.f;
const float resolve_distance = 5000.f;
const float traffic_heading = 3.f * M_PI_2_F;
const float traffic_direction = M_PI_2_F;
const float hor_velocity = 30.f;
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
drain_detect_and_avoid_most_urgent_topic();
while (_vehicle_command_sub.update()) {}
const auto publish_crosstrack_sample = [&](const float distance) {
publish_traffic_and_check([&]() {
navigator->get_detect_and_avoid()->fake_traffic(
fake_traffic_report(14545057, "DDF0A1", distance)
.with_direction(traffic_direction)
.with_heading(traffic_heading)
.with_altitude_diff(0.f)
.with_velocity(hor_velocity, 0.f)
.from_ownship(lat_uav, lon_uav, alt_uav));
});
};
publish_crosstrack_sample(conflict_distance);
ASSERT_TRUE(_detect_and_avoid_most_urgent_sub.update());
EXPECT_EQ(_detect_and_avoid_most_urgent_sub.get().conflict_level, kDaaConflictLvlHigh);
EXPECT_FALSE(_detect_and_avoid_most_urgent_sub.get().has_action);
EXPECT_FALSE(_vehicle_command_sub.update());
param_set(param_handle(px4::params::NAV_TRAFF_AVOID), &terminate_action);
publish_parameter_update();
drain_detect_and_avoid_most_urgent_topic();
while (_vehicle_command_sub.update()) {}
navigator->check_traffic();
EXPECT_FALSE(_detect_and_avoid_most_urgent_sub.update());
EXPECT_FALSE(_vehicle_command_sub.update());
publish_crosstrack_sample(resolve_distance);
ASSERT_TRUE(_detect_and_avoid_most_urgent_sub.update());
EXPECT_EQ(_detect_and_avoid_most_urgent_sub.get().conflict_level, kDaaConflictLvlNone);
EXPECT_FALSE(_detect_and_avoid_most_urgent_sub.get().has_action);
publish_crosstrack_sample(conflict_distance);
ASSERT_TRUE(_detect_and_avoid_most_urgent_sub.update());
EXPECT_EQ(_detect_and_avoid_most_urgent_sub.get().conflict_level, kDaaConflictLvlHigh);
EXPECT_TRUE(_detect_and_avoid_most_urgent_sub.get().has_action);
ASSERT_TRUE(_vehicle_command_sub.update());
EXPECT_EQ(_vehicle_command_sub.get().command, vehicle_command_s::VEHICLE_CMD_DO_FLIGHTTERMINATION);
}
// Crosstrack mode fails closed on a NaN/Inf traffic heading even with VALID_HEADING set.
TEST_F(DetectAndAvoidTest, CrosstrackRejectsNonFiniteTrafficHeading)
{
recreate_navigator();
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
for (const float heading : {std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::infinity()}) {
drain_detect_and_avoid_topic();
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav,
alt_uav, 30.f, 0.f, flags);
tr.heading = heading;
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
conflict_info_s conflict = navigator->get_detect_and_avoid()->get_most_urgent_conflict();
EXPECT_EQ(conflict.conflict_level, kDaaConflictLvlNone);
}
}
// Crosstrack prediction requires every ownship velocity
TEST_F(DetectAndAvoidTest, CrosstrackRejectsInvalidOwnshipVelocityFlags)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
publish_global_pos(lat_uav, lon_uav, alt_uav);
publish_land_status(false);
publish_vehicle_status(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, vehicle_status_s::ARMING_STATE_ARMED);
publish_local_pos_vel(uav_vel, hrt_absolute_time(), 0.f, false, true);
sync_navigator_topics();
drain_detect_and_avoid_topic();
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav,
alt_uav, 0.f, 0.f, flags);
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
}

1578
src/modules/navigator/DetectAndAvoid/test/detect_and_avoid_f3442_test.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

343
src/modules/navigator/DetectAndAvoid/test/detect_and_avoid_test.cpp Normal file
View File

@@ -0,0 +1,343 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid_test.cpp
* @brief DetectAndAvoid functional tests common to all DAA standards.
*
* @author Jonas Perolini <jonspero@me.com>
*
*/
#include "detect_and_avoid_test_common.h"
// Activation succeeds with default params and is refused when DAA_EN is cleared.
TEST_F(DetectAndAvoidTest, OnActivation)
{
EXPECT_TRUE(navigator->get_detect_and_avoid()->is_activated());
navigator->get_detect_and_avoid()->on_inactivation();
EXPECT_FALSE(navigator->get_detect_and_avoid()->is_activated());
// disable via param before activation
const int daa_en = 0;
param_t param_daa_en = param_handle(px4::params::DAA_EN);
param_set(param_daa_en, &daa_en);
navigator->get_detect_and_avoid()->on_activation();
EXPECT_FALSE(navigator->get_detect_and_avoid()->is_activated());
param_reset_all();
}
// Primary and secondary ownship ICAO default to -1 (disabled), so unrelated traffic is not suppressed.
TEST_F(DetectAndAvoidTest, PrimaryAndSecondaryIcaoDefaultsDisabled)
{
int32_t primary_icao = 0;
ASSERT_EQ(param_get(param_handle(px4::params::ADSB_ICAO_ID), &primary_icao), 0);
EXPECT_EQ(primary_icao, -1);
int32_t secondary_icao = 0;
ASSERT_EQ(param_get(param_handle(px4::params::ADSB_ICAO_ID_2), &secondary_icao), 0);
EXPECT_EQ(secondary_icao, -1);
}
// stop_fake_traffic() cancels a queued script before it publishes any synthetic traffic.
TEST_F(DetectAndAvoidTest, StopFakeTrafficCancelsPendingScript)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
navigator->run_fake_traffic(DetectAndAvoid::FakeTraffMode::kQueueFill);
navigator->stop_fake_traffic();
drain_mavlink_logs();
navigator->check_traffic();
conflict_info_s conflict = navigator->get_detect_and_avoid()->get_most_urgent_conflict();
EXPECT_EQ(conflict.conflict_level, kDaaConflictLvlNone);
const std::vector<std::string> logs = drain_mavlink_logs();
EXPECT_EQ(count_logs_with_prefix(logs, "DAA New and Main:"), 0u);
EXPECT_EQ(count_logs_with_prefix(logs, "DAA New "), 0u);
}
// A NaN traffic altitude is rejected before any DAA math; nothing is published or buffered.
TEST_F(DetectAndAvoidTest, RejectsNonFiniteTrafficAltitude)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
drain_detect_and_avoid_topic();
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav,
std::numeric_limits<float>::quiet_NaN(), 0.f, 0.f, flags);
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
conflict_info_s conflict = navigator->get_detect_and_avoid()->get_most_urgent_conflict();
EXPECT_EQ(conflict.conflict_level, kDaaConflictLvlNone);
}
// A report missing a required validity flag (here VALID_ALTITUDE) is dropped.
TEST_F(DetectAndAvoidTest, RejectsTrafficMissingRequiredFlags)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
drain_detect_and_avoid_topic();
// Same geometry as ownship (an immediate conflict if processed), but VALID_ALTITUDE is absent.
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS;
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav, alt_uav, 30.f, 0.f, flags);
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
}
// Finite coordinates are still unusable when the estimator marks either global position group invalid.
TEST_F(DetectAndAvoidTest, RejectsInvalidOwnshipGlobalPositionFlags)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
publish_land_status(false);
publish_vehicle_status(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, vehicle_status_s::ARMING_STATE_ARMED);
publish_local_pos_vel(uav_vel);
publish_global_pos(lat_uav, lon_uav, alt_uav, hrt_absolute_time(), false, true);
sync_navigator_topics();
drain_detect_and_avoid_topic();
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav,
alt_uav, 0.f, 0.f, flags);
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
// Drain the queued report after switching to an altitude-invalid pose as well.
publish_global_pos(lat_uav, lon_uav, alt_uav, hrt_absolute_time(), true, false);
sync_navigator_topics();
navigator->check_traffic();
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
}
// A report older than DAA_TRAFF_TOUT is dropped.
TEST_F(DetectAndAvoidTest, RejectsStaleTrafficReport)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
// Shortest allowed timeout so a modest backdate is unambiguously stale.
const int32_t traffic_timeout_s = 1;
param_set(param_handle(px4::params::DAA_TRAFF_TOUT), &traffic_timeout_s);
reload_daa_parameters();
drain_detect_and_avoid_topic();
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav, alt_uav, 30.f, 0.f, flags);
// Backdate well beyond the 1 s timeout. The harness has been running long enough (Navigator
// construction waits on dataman) that hrt_absolute_time() is comfortably past the offset.
const hrt_abstime backdate = 5_s;
const hrt_abstime now = hrt_absolute_time();
tr.timestamp = (now > backdate) ? (now - backdate) : 1;
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
param_reset_all();
}
// Source age (tslc) participates in input freshness even when the local publication is new.
TEST_F(DetectAndAvoidTest, RejectsSourceStaleTrafficReport)
{
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
const int32_t traffic_timeout_s = 1;
param_set(param_handle(px4::params::DAA_TRAFF_TOUT), &traffic_timeout_s);
reload_daa_parameters();
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
drain_detect_and_avoid_topic();
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
transponder_report_s tr = create_transponder_report(14545057, "DDF0A1", lat_uav, lon_uav,
alt_uav, 0.f, 0.f, flags);
tr.tslc = 2;
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
}
// Ownship reports (ICAO, callsign or UAS ID) are filtered out; foreign traffic with the same
// geometry creates a conflict. Identity comes from parameters and the board GUID.
TEST_F(DetectAndAvoidTest, SelfDetection)
{
EXPECT_TRUE(navigator->get_detect_and_avoid()->is_activated());
const double lat_uav = 46.52342;
const double lon_uav = 6.524234;
const float alt_uav = 400.f;
const matrix::Vector3f uav_vel{5.f, 10.f, 2.f};
const uint16_t flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY;
const auto refresh_uav_pose = [&]() {
set_default_uav_state(lat_uav, lon_uav, alt_uav, uav_vel);
sync_navigator_topics();
};
const auto expect_report_filtered = [&](const transponder_report_s & tr) {
refresh_uav_pose();
drain_detect_and_avoid_topic();
publish_transponder_report_and_check(tr);
EXPECT_FALSE(_detect_and_avoid_sub.update());
EXPECT_EQ(navigator->get_detect_and_avoid()->get_most_urgent_conflict().conflict_level, kDaaConflictLvlNone);
};
const auto expect_report_in_conflict = [&](const transponder_report_s & tr, const uint64_t expected_id) {
refresh_uav_pose();
drain_detect_and_avoid_topic();
publish_transponder_report_and_check(tr);
EXPECT_TRUE(_detect_and_avoid_sub.update());
const conflict_info_s conflict = navigator->get_detect_and_avoid()->get_most_urgent_conflict();
EXPECT_GT(conflict.conflict_level, kDaaConflictLvlNone);
EXPECT_EQ(conflict.encoded_id.id, expected_id);
};
// Each parameter stores four callsign characters in display order (most-significant byte first).
char callsign[kCallsignLength] = "PX4 TEST";
const int32_t own_adsb_callsign1 = 0x50583420; // "PX4 "
const int32_t own_adsb_callsign2 = 0x54455354; // "TEST"
param_set(param_handle(px4::params::ADSB_CALLSIGN_1), &own_adsb_callsign1);
param_set(param_handle(px4::params::ADSB_CALLSIGN_2), &own_adsb_callsign2);
reload_daa_parameters();
// ICAO zeroed so the callsign identifies it: ownship callsign dropped, foreign callsign conflicts
transponder_report_s tr = create_transponder_report(0, callsign, lat_uav, lon_uav, alt_uav, 30.f, 0.f,
flags | transponder_report_s::PX4_ADSB_FLAGS_VALID_CALLSIGN);
expect_report_filtered(tr);
char foreign_callsign[kCallsignLength];
memcpy(foreign_callsign, callsign, sizeof(foreign_callsign));
foreign_callsign[0] = (callsign[0] == 'A') ? 'B' : 'A';
set_report_callsign(tr, foreign_callsign);
expect_report_in_conflict(tr, DaaEncodedId::callsign_to_uint64(foreign_callsign));
// primary and secondary ICAO as ownship identity: both dropped, foreign ICAO conflicts
const uint32_t own_icao = 6593425;
const uint32_t own_icao_2 = 3318901;
param_set(param_handle(px4::params::ADSB_ICAO_ID), &own_icao);
param_set(param_handle(px4::params::ADSB_ICAO_ID_2), &own_icao_2);
reload_daa_parameters(); // also clears the previous foreign conflict
expect_report_filtered(create_transponder_report(own_icao, "DDF0A1", lat_uav, lon_uav, alt_uav, 30.f, 0.f, flags));
expect_report_filtered(create_transponder_report(own_icao_2, "DDF0A1", lat_uav, lon_uav, alt_uav, 30.f, 0.f, flags));
const uint32_t foreign_icao = 14545057;
expect_report_in_conflict(create_transponder_report(foreign_icao, "DDF0A1", lat_uav, lon_uav, alt_uav, 30.f, 0.f,
flags), foreign_icao);
// UAS-ID only (no ICAO, no valid callsign)
reload_daa_parameters(); // clear the previous foreign conflict
#ifndef BOARD_HAS_NO_UUID
px4_guid_t px4_guid {};
ASSERT_EQ(board_get_px4_guid(px4_guid), PX4_GUID_BYTE_LENGTH);
// boards with a UUID drop traffic broadcasting the local UAS ID
transponder_report_s tr_own_uas = create_transponder_report(0, "", lat_uav, lon_uav, alt_uav, 30.f, 0.f, flags);
memcpy(tr_own_uas.uas_id, px4_guid, sizeof(px4_guid));
expect_report_filtered(tr_own_uas);
#endif // !BOARD_HAS_NO_UUID
// foreign UAS ID conflicts (also on boards without a UUID)
transponder_report_s tr_foreign_uas = create_transponder_report(0, "", lat_uav, lon_uav, alt_uav, 30.f, 0.f, flags);
for (int i = 0; i < PX4_GUID_BYTE_LENGTH; ++i) {
tr_foreign_uas.uas_id[i] = 0xe0 + i;
}
expect_report_in_conflict(tr_foreign_uas, DaaEncodedId::last_uas_id_bytes_to_uint64(tr_foreign_uas.uas_id));
}

609
src/modules/navigator/DetectAndAvoid/test/detect_and_avoid_test_common.h Normal file
View File

@@ -0,0 +1,609 @@
/****************************************************************************
*
* Copyright (c) 2026 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file detect_and_avoid_test_common.h
* @brief Shared fixture and helpers for the DetectAndAvoid functional tests.
*
* @author Jonas Perolini <jonspero@me.com>
*
*/
#pragma once
#include <gtest/gtest.h>
#include <cstring>
#include <limits>
#include <memory>
#include <string>
#include <vector>
#include "detect_and_avoid.h"
#include "navigator.h"
#include "mission.h"
#include <px4_platform_common/px4_work_queue/WorkQueueManager.hpp>
#include <uORB/topics/mavlink_log.h>
using namespace time_literals;
extern "C" __EXPORT int dataman_main(int argc, char *argv[]);
static constexpr uint8_t kDaaConflictLvlNone = 0;
static constexpr uint8_t kDaaConflictLvlLow = 1;
static constexpr uint8_t kDaaConflictLvlMedium = 2;
static constexpr uint8_t kDaaConflictLvlHigh = 3;
static constexpr uint8_t kDaaConflictLvlCritical = 4;
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
/* F3442 default volumes. Set simple bounds */
static constexpr float kDfltDaaLvlCriticalRad = 150.f;
static constexpr float kDfltDaaLvlCriticalHgt = 30.f;
static constexpr float kDfltDaaLvlHighRad = 600.f;
static constexpr float kDfltDaaLvlHighHgt = 80.f;
static constexpr int kDfltDaaLvlMediumTime = 30;
static constexpr int kDfltDaaLvlLowTime = 30;
static constexpr float kDfltDaaDfltVel = 5.f;
static constexpr int kDfltDaaEnDfltVel = 1;
#else
/* Crosstrack default gates. These match the NAV_TRAFF_* parameter defaults. */
static constexpr float kDfltNavTraffHorSep = 500.f;
static constexpr float kDfltNavTraffVerSep = 500.f;
static constexpr int kDfltNavTraffCollTime = 60;
#endif // CONFIG_NAVIGATOR_ADSB_F3442
static constexpr int action_param_value(const DaaAction action)
{
switch (action) {
case DaaAction::kDisabled:
return 0;
case DaaAction::kWarnOnly:
return 1;
case DaaAction::kReturnMode:
return 2;
case DaaAction::kLandMode:
return 3;
case DaaAction::kPositionHoldMode:
return 4;
case DaaAction::kTerminate:
return 5;
default:
return 0;
}
}
/* Default drone position. Not important as traffic is defined relatively to uav */
static constexpr float kDfltLatUav = 150.f;
static constexpr float kDfltLonUav = 30.f;
static constexpr float kDfltAltUav = 600.f;
static constexpr hrt_abstime kOrbWaitTimeoutUs = 200000;
static constexpr useconds_t kOrbWaitPollIntervalUs = 1000;
// Start dataman once for the lifetime of the test binary. Each Navigator constructs
// multiple DatamanClient instances and each one waits up to 1s for a client ID, so
// restarting dataman between test suites adds ~4 s of timeout per parameterized case.
// Same pattern as in src/modules/navigator/test/test_mission_base.cpp.
class NavigatorDatamanRuntime
{
public:
NavigatorDatamanRuntime()
{
param_control_autosave(false);
px4::WorkQueueManagerStart();
char name[] = "dataman";
char start[] = "start";
char ram[] = "-r";
char *argv[] = {name, start, ram};
dataman_main(3, argv);
}
~NavigatorDatamanRuntime()
{
param_control_autosave(true);
char name[] = "dataman";
char stop[] = "stop";
char *argv[] = {name, stop};
dataman_main(2, argv);
px4::WorkQueueManagerStop();
}
};
static inline NavigatorDatamanRuntime &navigatorDatamanRuntime()
{
static NavigatorDatamanRuntime runtime{};
return runtime;
}
// To run: make tests TESTFILTER=detect_and_avoid
class DetectAndAvoidTest : public ::testing::Test
{
protected:
void SetUp() override
{
navigatorDatamanRuntime();
param_reset_all();
set_DFLT_daa_params();
navigator = std::make_unique<Navigator>();
}
void TearDown() override
{
navigator.reset();
transponder_report_s tr{};
_traffic_pub.publish(tr);
}
public:
uORB::SubscriptionData<detect_and_avoid_s> _detect_and_avoid_sub{ORB_ID(detect_and_avoid)};
uORB::SubscriptionData<detect_and_avoid_most_urgent_s> _detect_and_avoid_most_urgent_sub{ORB_ID(detect_and_avoid_most_urgent)};
uORB::SubscriptionData<vehicle_command_s> _vehicle_command_sub{ORB_ID(vehicle_command)};
uORB::SubscriptionData<transponder_report_s> _traffic_sub{ORB_ID(transponder_report)};
uORB::Subscription _mavlink_log_sub{ORB_ID(mavlink_log)};
uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)};
uORB::Subscription _vehicle_status_state_sub{ORB_ID(vehicle_status)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
uORB::Publication<transponder_report_s> _traffic_pub{ORB_ID(transponder_report)};
uORB::Publication<vehicle_global_position_s> _global_pos_pub{ORB_ID(vehicle_global_position)};
uORB::Publication<vehicle_status_s> _vehicle_status_pub{ORB_ID(vehicle_status)};
uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub{ORB_ID(vehicle_local_position)};
uORB::Publication<vehicle_land_detected_s> _vehicle_land_detected_pub{ORB_ID(vehicle_land_detected)};
uORB::Publication<parameter_update_s> _parameter_update_pub{ORB_ID(parameter_update)};
std::unique_ptr<Navigator> navigator{};
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
struct breach_distances_s {
float nmac_hor;
float nmac_vert;
float wc_hor;
float wc_vert;
float aug_nmac_hor;
float aug_nmac_vert;
float aug_wc_hor;
float aug_wc_vert;
};
breach_distances_s get_DFLT_breach_distances(const matrix::Vector3f uav_vel, const float traffic_hor_vel)
{
breach_distances_s breach_distance{};
const float uav_hor_vel = uav_vel.xy().norm();
/* NMAC */
breach_distance.nmac_hor = 2 * kDfltDaaLvlCriticalRad;
breach_distance.nmac_vert = 2 * kDfltDaaLvlCriticalHgt;
/* WC */
breach_distance.wc_hor = 2 * kDfltDaaLvlHighRad;
breach_distance.wc_vert = 2 * kDfltDaaLvlHighHgt;
/* Aug NMAC */
breach_distance.aug_nmac_hor = 2 * kDfltDaaLvlCriticalRad + kDfltDaaLvlMediumTime * (fabsf(
traffic_hor_vel) + fabsf(uav_hor_vel));
breach_distance.aug_nmac_vert = 2 * kDfltDaaLvlCriticalHgt + kDfltDaaLvlMediumTime * (fabsf(
kDfltDaaDfltVel) + fabsf(uav_vel(2)));
/* Aug WC */
breach_distance.aug_wc_hor = 2 * kDfltDaaLvlHighRad + kDfltDaaLvlLowTime * (fabsf(
traffic_hor_vel) + fabsf(uav_hor_vel));
breach_distance.aug_wc_vert = 2 * kDfltDaaLvlHighHgt + kDfltDaaLvlLowTime * (fabsf(
kDfltDaaDfltVel) + fabsf(uav_vel(2)));
return breach_distance;
}
#endif // CONFIG_NAVIGATOR_ADSB_F3442
void publish_local_pos_vel(const matrix::Vector3f local_pos_vel, const hrt_abstime timestamp = hrt_absolute_time(),
const float heading = 0.f, const bool v_xy_valid = true, const bool v_z_valid = true)
{
vehicle_local_position_s l_pos{};
l_pos.timestamp_sample = timestamp;
l_pos.timestamp = timestamp;
l_pos.v_xy_valid = v_xy_valid;
l_pos.v_z_valid = v_z_valid;
l_pos.vx = local_pos_vel(0);
l_pos.vy = local_pos_vel(1);
l_pos.vz = local_pos_vel(2);
l_pos.heading = heading;
_vehicle_local_position_pub.publish(l_pos);
}
void publish_global_pos(const double lat, const double lon, const double alt,
const hrt_abstime timestamp = hrt_absolute_time(), const bool lat_lon_valid = true,
const bool alt_valid = true)
{
vehicle_global_position_s g_pos{};
g_pos.timestamp_sample = timestamp;
g_pos.lat_lon_valid = lat_lon_valid;
g_pos.alt_valid = alt_valid;
g_pos.lat = lat;
g_pos.lon = lon;
g_pos.alt = alt;
g_pos.timestamp = timestamp;
_global_pos_pub.publish(g_pos);
}
void publish_vehicle_status(const uint8_t nav_state, const uint8_t arming_state)
{
vehicle_status_s v_status{};
v_status.timestamp = hrt_absolute_time();
v_status.nav_state = nav_state;
v_status.arming_state = arming_state;
_vehicle_status_pub.publish(v_status);
}
void publish_land_status(const bool has_landed)
{
vehicle_land_detected_s vehicle_land{};
vehicle_land.timestamp = hrt_absolute_time();
vehicle_land.landed = has_landed;
_vehicle_land_detected_pub.publish(vehicle_land);
}
void publish_parameter_update()
{
parameter_update_s update{};
update.timestamp = hrt_absolute_time();
_parameter_update_pub.publish(update);
}
// Force the DAA to reload its cached parameters immediately.
// on_activation() calls ModuleParams::updateParams()
void reload_daa_parameters()
{
navigator->get_detect_and_avoid()->on_activation();
}
conflict_info_s expect_most_urgent_conflict(uint8_t expected_level, uint64_t expected_id)
{
const conflict_info_s conflict = navigator->get_detect_and_avoid()->get_most_urgent_conflict();
SCOPED_TRACE(testing::Message() << "most-urgent conflict: expected level " << static_cast<int>(expected_level)
<< ", id " << expected_id);
EXPECT_EQ(conflict.conflict_level, expected_level);
EXPECT_EQ(conflict.encoded_id.id, expected_id);
return conflict;
}
struct TestSyntheticTrafficReport : public DetectAndAvoid::SyntheticTrafficReport {
TestSyntheticTrafficReport(uint32_t icao, const char *report_callsign, float relative_distance)
{
icao_address = icao;
callsign = report_callsign;
distance = relative_distance;
}
TestSyntheticTrafficReport with_direction(float relative_direction) const
{
TestSyntheticTrafficReport report{*this};
report.direction = relative_direction;
return report;
}
TestSyntheticTrafficReport with_heading(float heading) const
{
TestSyntheticTrafficReport report{*this};
report.traffic_heading = heading;
return report;
}
TestSyntheticTrafficReport with_altitude_diff(float relative_altitude) const
{
TestSyntheticTrafficReport report{*this};
report.altitude_diff = relative_altitude;
return report;
}
TestSyntheticTrafficReport with_velocity(float horizontal_velocity, float vertical_velocity) const
{
TestSyntheticTrafficReport report{*this};
report.hor_velocity = horizontal_velocity;
report.ver_velocity = vertical_velocity;
return report;
}
TestSyntheticTrafficReport from_ownship(double lat, double lon, float alt) const
{
TestSyntheticTrafficReport report{*this};
report.lat_uav = lat;
report.lon_uav = lon;
report.alt_uav = alt;
return report;
}
};
TestSyntheticTrafficReport fake_traffic_report(uint32_t icao_address, const char *callsign, float distance)
{
return TestSyntheticTrafficReport{icao_address, callsign, distance};
}
// Copy a callsign into a transponder report, always leaving the field null-terminated.
// Centralizes the strncpy + terminator pattern so individual tests cannot get the bounds wrong.
void set_report_callsign(transponder_report_s &tr, const char *callsign)
{
strncpy(tr.callsign, callsign, sizeof(tr.callsign) - 1);
tr.callsign[sizeof(tr.callsign) - 1] = '\0';
}
transponder_report_s create_transponder_report(const uint32_t icao_address, const char *callsign, const double lat,
const double lon, const float altitude, const float hor_velocity, const float ver_velocity,
const uint16_t flags)
{
transponder_report_s tr{};
tr.timestamp = hrt_absolute_time();
tr.icao_address = icao_address;
tr.lat = lat;
tr.lon = lon;
tr.altitude = altitude;
tr.heading = 0.f;
tr.hor_velocity = hor_velocity;
tr.ver_velocity = ver_velocity;
tr.altitude_type = 0;
tr.emitter_type = 1;
tr.tslc = 1;
tr.flags = flags;
set_report_callsign(tr, callsign);
return tr;
}
void publish_transponder_report(const transponder_report_s &tr)
{
_traffic_pub.publish(tr);
}
template<typename Predicate>
void wait_until(const Predicate &predicate, const hrt_abstime timeout_us = kOrbWaitTimeoutUs)
{
const hrt_abstime start = hrt_absolute_time();
bool condition_met = predicate();
while (!condition_met && hrt_elapsed_time(&start) < timeout_us) {
px4_usleep(kOrbWaitPollIntervalUs);
condition_met = predicate();
}
ASSERT_TRUE(condition_met);
}
void wait_for_topic_update(uORB::Subscription &subscription, const hrt_abstime timeout_us = kOrbWaitTimeoutUs)
{
wait_until([&subscription]() {
return subscription.updated();
}, timeout_us);
}
void drain_transponder_report_topic()
{
while (_traffic_sub.update()) {}
}
template<typename Publisher>
void publish_traffic_and_check(const Publisher &publisher)
{
drain_transponder_report_topic();
publisher();
wait_for_topic_update(_traffic_sub);
navigator->check_traffic();
}
void publish_transponder_report_and_check(const transponder_report_s &tr)
{
publish_traffic_and_check([&]() {
publish_transponder_report(tr);
});
}
// Install valid default parameters for the standard built into this firmware so the
// module activates cleanly. The standard-specific tests still override individual
// parameters to exercise their own boundaries.
void set_DFLT_daa_params()
{
#if defined(CONFIG_NAVIGATOR_ADSB_F3442) && CONFIG_NAVIGATOR_ADSB_F3442
// NMAC: (150, 30), WC: (600, 80), Aug-WC: 30s, Aug-NMAC: 30s
param_set(param_handle(px4::params::DAA_LVL_CRIT_RAD), &kDfltDaaLvlCriticalRad);
param_set(param_handle(px4::params::DAA_LVL_CRIT_HGT), &kDfltDaaLvlCriticalHgt);
param_set(param_handle(px4::params::DAA_LVL_HIGH_RAD), &kDfltDaaLvlHighRad);
param_set(param_handle(px4::params::DAA_LVL_HIGH_HGT), &kDfltDaaLvlHighHgt);
param_set(param_handle(px4::params::DAA_LVL_MED_TIME), &kDfltDaaLvlMediumTime);
param_set(param_handle(px4::params::DAA_LVL_LOW_TIME), &kDfltDaaLvlLowTime);
param_set(param_handle(px4::params::DAA_EN_DFLT_VEL), &kDfltDaaEnDfltVel);
param_set(param_handle(px4::params::DAA_DFLT_VEL), &kDfltDaaDfltVel);
#else
param_set(param_handle(px4::params::NAV_TRAFF_A_HOR), &kDfltNavTraffHorSep);
param_set(param_handle(px4::params::NAV_TRAFF_A_VER), &kDfltNavTraffVerSep);
param_set(param_handle(px4::params::NAV_TRAFF_COLL_T), &kDfltNavTraffCollTime);
#endif // CONFIG_NAVIGATOR_ADSB_F3442
}
void set_default_uav_state(const double lat_uav, const double lon_uav, const float alt_uav,
const matrix::Vector3f uav_vel)
{
/* Navigation state */
const bool has_landed = false;
const uint8_t arming_state = vehicle_status_s::ARMING_STATE_ARMED;
const uint8_t nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
/* Publish UAV state */
publish_global_pos(lat_uav, lon_uav, alt_uav);
publish_land_status(has_landed);
publish_vehicle_status(nav_state, arming_state);
publish_local_pos_vel(uav_vel);
}
void generate_random_callsign(char *callsign, size_t length = 8)
{
const char *chars = "ABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789";
const size_t chars_length = 36; // Length of the chars array.
for (size_t i = 0; i < length; ++i) {
const size_t index = rand() % chars_length;
callsign[i] = chars[index];
}
callsign[length] = '\0'; // Null-terminate the string.
}
void generate_random_UAS_ID(uint8_t uas_id[PX4_GUID_BYTE_LENGTH])
{
for (int i = 0; i < PX4_GUID_BYTE_LENGTH; i++) {
uas_id[i] = rand() % 256;
}
}
void check_highest_conflict(const uint8_t conflict_level)
{
const bool action_required = navigator->get_detect_and_avoid()->get_action_from_conflict_level(conflict_level) > DaaAction::kWarnOnly;
if (_detect_and_avoid_most_urgent_sub.updated()) {
ASSERT_TRUE(_detect_and_avoid_most_urgent_sub.update());
}
const detect_and_avoid_most_urgent_s &daa_status = _detect_and_avoid_most_urgent_sub.get();
ASSERT_EQ(daa_status.conflict_level, conflict_level);
ASSERT_EQ(daa_status.has_action, action_required);
}
void expect_empty_most_urgent_status(const detect_and_avoid_most_urgent_s &status)
{
EXPECT_EQ(status.conflict_level, kDaaConflictLvlNone);
EXPECT_FALSE(status.has_action);
EXPECT_EQ(status.unique_id, 0ULL);
EXPECT_EQ(status.unique_id_encoding, detect_and_avoid_most_urgent_s::UNIQUE_ID_ENCODING_ICAO);
EXPECT_FLOAT_EQ(status.aircraft_dist, 9999.f);
EXPECT_GT(status.timestamp, 0u);
}
std::vector<std::string> drain_mavlink_logs()
{
std::vector<std::string> logs;
mavlink_log_s mavlink_log{};
while (_mavlink_log_sub.update(&mavlink_log)) {
logs.emplace_back(mavlink_log.text);
}
return logs;
}
size_t count_logs_with_prefix(const std::vector<std::string> &logs, const char *prefix)
{
size_t count = 0;
for (const std::string &log : logs) {
if (log.rfind(prefix, 0) == 0) {
++count;
}
}
return count;
}
bool any_log_contains(const std::vector<std::string> &logs, const char *needle)
{
for (const std::string &log : logs) {
if (log.find(needle) != std::string::npos) {
return true;
}
}
return false;
}
void drain_detect_and_avoid_topic()
{
while (_detect_and_avoid_sub.update()) {}
}
void drain_detect_and_avoid_most_urgent_topic()
{
while (_detect_and_avoid_most_urgent_sub.update()) {}
}
void recreate_navigator()
{
navigator.reset();
navigator = std::make_unique<Navigator>();
}
void sync_navigator_topics(Navigator *navigator_instance = nullptr)
{
Navigator *target = navigator_instance ? navigator_instance : navigator.get();
if (_global_pos_sub.updated()) {
vehicle_global_position_s global_position{};
if (_global_pos_sub.copy(&global_position)) {
*target->get_global_position() = global_position;
}
}
if (_vehicle_land_detected_sub.updated()) {
vehicle_land_detected_s land_detected{};
if (_vehicle_land_detected_sub.copy(&land_detected)) {
*target->get_land_detected() = land_detected;
}
}
vehicle_local_position_s local_position{};
if (_vehicle_local_position_sub.copy(&local_position)) {
*target->get_local_position() = local_position;
}
vehicle_status_s vehicle_status{};
if (_vehicle_status_state_sub.copy(&vehicle_status)) {
*target->get_vstatus() = vehicle_status;
}
}
};

22
src/modules/navigator/Kconfig
View File

@@ -25,8 +25,18 @@ menuconfig NAVIGATOR_ADSB
default y
depends on MODULES_NAVIGATOR
---help---
Add support for acting on ADSB transponder_report or ADSB_VEHICLE MAVLink messages.
Actions are warnings, Loiter, Land and RTL without climb.
Evaluate cooperative traffic reports and request configured warnings or
navigation actions. The crosstrack conflict model is built by default;
enabling NAVIGATOR_ADSB_F3442 selects the F3442-derived model instead.
menuconfig NAVIGATOR_ADSB_F3442
bool "Use F3442-derived traffic conflict evaluation"
default n
depends on NAVIGATOR_ADSB
---help---
Build the conflict model based on selected ASTM F3442 alert concepts
instead of the crosstrack model. This option alone does not establish
compliance.
menuconfig NAVIGATOR_GEOFENCE_AVOIDANCE
bool "Include geofence avoidance path planning for RTL"
@@ -42,3 +52,11 @@ config NAVIGATOR_GEOFENCE_MAX_NODES
---help---
Max number of geofence vertices which the RTL planner can use.
Replanning time (on geofence / safety margin change) is cubic, RAM usage quadratic in this number.
menuconfig NAVIGATOR_ADSB_FAKE_TRAFFIC
bool "Include fake-traffic injection for DAA testing"
default n
depends on NAVIGATOR_ADSB
---help---
Adds the `navigator fake_traffic` shell command and the scripted scenarios
used to validate the Detect and Avoid pipeline without a real transponder.

24
src/modules/navigator/navigator.h
View File

@@ -50,6 +50,9 @@
#include "navigator_mode.h"
#include "rtl.h"
#include "takeoff.h"
#if CONFIG_NAVIGATOR_ADSB
#include "DetectAndAvoid/detect_and_avoid.h"
#endif // CONFIG_NAVIGATOR_ADSB
#if CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
#include "vtol_takeoff.h"
#endif //CONFIG_MODE_NAVIGATOR_VTOL_TAKEOFF
@@ -63,9 +66,6 @@
#include <uORB/SubscriptionMultiArray.hpp>
#include <uORB/topics/telemetry_status.h>
#if CONFIG_NAVIGATOR_ADSB
#include <lib/adsb/AdsbConflict.h>
#endif // CONFIG_NAVIGATOR_ADSB
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/events.h>
#include <px4_platform_common/module.h>
@@ -157,9 +157,11 @@ public:
* Check nearby traffic for potential collisions
*/
void check_traffic();
void take_traffic_conflict_action();
void run_fake_traffic();
#endif // CONFIG_NAVIGATOR_ADSB
#if CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
void run_fake_traffic(DetectAndAvoid::FakeTraffMode mode = DetectAndAvoid::FakeTraffMode::kUniqueIds);
void stop_fake_traffic();
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
/**
* Setters
@@ -180,9 +182,13 @@ public:
uint8_t get_takeoff_state() { return _takeoff_status_sub.get().takeoff_state; }
vehicle_local_position_s *get_local_position() { return &_local_pos; }
vehicle_status_s *get_vstatus() { return &_vstatus; }
void set_rtl_return_alt_min(bool enable) { _rtl.set_return_alt_min(enable); }
PrecLand *get_precland() { return &_precland; } /**< allow others, e.g. Mission, to use the precision land block */
Course *get_course() { return &_course; }
#if CONFIG_NAVIGATOR_ADSB
DetectAndAvoid *get_detect_and_avoid() { return &_detect_and_avoid; }
#endif // CONFIG_NAVIGATOR_ADSB
const PositionYawSetpoint &get_last_pos_with_gcs_heartbeat() const { return _last_pos_with_gcs_heartbeat; }
@@ -346,7 +352,6 @@ private:
uORB::Subscription _home_pos_sub{ORB_ID(home_position)}; /**< home position subscription */
uORB::Subscription _land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */
uORB::Subscription _pos_ctrl_landing_status_sub{ORB_ID(position_controller_landing_status)}; /**< position controller landing status subscription */
uORB::Subscription _traffic_sub{ORB_ID(transponder_report)}; /**< traffic subscription */
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; /**< vehicle commands (onboard and offboard) */
uORB::Publication<geofence_result_s> _geofence_result_pub{ORB_ID(geofence_result)};
@@ -410,8 +415,7 @@ private:
RTL _rtl; /**< class that handles RTL */
Course _course; /**< class that handles course */
#if CONFIG_NAVIGATOR_ADSB
AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */
traffic_buffer_s _traffic_buffer{};
DetectAndAvoid _detect_and_avoid;
#endif // CONFIG_NAVIGATOR_ADSB
NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */
@@ -466,10 +470,6 @@ private:
_param_nav_fw_altl_rad, /**< acceptance rad for fixedwing alt before landing*/
(ParamFloat<px4::params::NAV_MC_ALT_RAD>) _param_nav_mc_alt_rad, /**< acceptance rad for multicopter alt */
(ParamInt<px4::params::NAV_FORCE_VT>) _param_nav_force_vt, /**< acceptance radius for multicopter alt */
(ParamInt<px4::params::NAV_TRAFF_AVOID>) _param_nav_traff_avoid, /**< avoiding other aircraft is enabled */
(ParamFloat<px4::params::NAV_TRAFF_A_HOR>) _param_nav_traff_a_hor_ct, /**< avoidance Distance Crosstrack*/
(ParamFloat<px4::params::NAV_TRAFF_A_VER>) _param_nav_traff_a_ver, /**< avoidance Distance Vertical*/
(ParamInt<px4::params::NAV_TRAFF_COLL_T>) _param_nav_traff_collision_time,
(ParamFloat<px4::params::NAV_MIN_LTR_ALT>) _param_min_ltr_alt, /**< minimum altitude in Loiter mode*/
(ParamFloat<px4::params::NAV_MIN_GND_DIST>)
_param_nav_min_gnd_dist, /**< minimum distance to ground (Mission and RTL)*/

188
src/modules/navigator/navigator_main.cpp
View File

@@ -67,6 +67,69 @@ namespace navigator
Navigator *g_navigator;
}
#if CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
namespace
{
int print_fake_traffic_usage(const char *reason = nullptr)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PX4_INFO_RAW("Usage:\n");
PX4_INFO_RAW(" navigator fake_traffic [mode]\n");
PX4_INFO_RAW(" navigator fake_traffic stop\n");
PX4_INFO_RAW(" navigator fake_traffic help\n");
PX4_INFO_RAW("\n");
PX4_INFO_RAW("Modes:\n");
PX4_INFO_RAW(" unique_ids identifier fallback checks\n");
PX4_INFO_RAW(" escalation one target getting closer\n");
PX4_INFO_RAW(" spam_same repeated updates for one target\n");
PX4_INFO_RAW(" spam_new many new targets\n");
PX4_INFO_RAW(" flags invalid velocity-flag case\n");
PX4_INFO_RAW(" queue_fill 3 batches of ORB_QUEUE_LENGTH\n");
PX4_INFO_RAW("\n");
PX4_INFO_RAW("Warning: fake traffic runs through the normal DAA pipeline and can trigger configured actions.\n");
return 0;
}
bool parse_fake_traffic_mode(const char *mode_name, DetectAndAvoid::FakeTraffMode &mode)
{
if (!strcmp(mode_name, "unique_ids")) {
mode = DetectAndAvoid::FakeTraffMode::kUniqueIds;
return true;
}
if (!strcmp(mode_name, "escalation")) {
mode = DetectAndAvoid::FakeTraffMode::kEscalation;
return true;
}
if (!strcmp(mode_name, "spam_same")) {
mode = DetectAndAvoid::FakeTraffMode::kSpamSame;
return true;
}
if (!strcmp(mode_name, "spam_new")) {
mode = DetectAndAvoid::FakeTraffMode::kSpamNew;
return true;
}
if (!strcmp(mode_name, "flags")) {
mode = DetectAndAvoid::FakeTraffMode::kFlags;
return true;
}
if (!strcmp(mode_name, "queue_fill")) {
mode = DetectAndAvoid::FakeTraffMode::kQueueFill;
return true;
}
return false;
}
} // namespace
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
Navigator::Navigator() :
ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -81,6 +144,9 @@ Navigator::Navigator() :
_precland(this),
_rtl(this),
_course(this)
#if CONFIG_NAVIGATOR_ADSB
, _detect_and_avoid(this)
#endif // CONFIG_NAVIGATOR_ADSB
{
/* Create a list of our possible navigation types */
_navigation_mode_array[0] = &_mission;
@@ -115,6 +181,10 @@ Navigator::Navigator() :
_distance_sensor_mode_change_request_pub.get().request_on_off = distance_sensor_mode_change_request_s::REQUEST_OFF;
_distance_sensor_mode_change_request_pub.update();
#if CONFIG_NAVIGATOR_ADSB
_detect_and_avoid.on_activation();
#endif // CONFIG_NAVIGATOR_ADSB
reset_triplets();
}
@@ -124,6 +194,9 @@ Navigator::~Navigator()
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_mission_sub);
orb_unsubscribe(_vehicle_status_sub);
#if CONFIG_NAVIGATOR_ADSB
_detect_and_avoid.on_inactivation();
#endif // CONFIG_NAVIGATOR_ADSB
}
void Navigator::params_update()
@@ -143,11 +216,6 @@ void Navigator::params_update()
}
_mission.set_command_timeout(_param_mis_command_tout.get());
#if CONFIG_NAVIGATOR_ADSB
_adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(),
_param_nav_traff_a_ver.get(),
_param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get());
#endif // CONFIG_NAVIGATOR_ADSB
}
void Navigator::run()
@@ -1191,6 +1259,9 @@ int Navigator::print_status()
PX4_INFO("Running");
_geofence.printStatus();
#if CONFIG_NAVIGATOR_ADSB && !defined(CONSTRAINED_FLASH) && !defined(__PX4_NUTTX)
_detect_and_avoid.print_status();
#endif // CONFIG_NAVIGATOR_ADSB && !CONSTRAINED_FLASH && !__PX4_NUTTX
return 0;
}
@@ -1314,65 +1385,23 @@ void Navigator::load_fence_from_file(const char *filename)
_geofence.loadFromFile(filename);
}
#if CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
void Navigator::run_fake_traffic(DetectAndAvoid::FakeTraffMode mode)
{
_detect_and_avoid.run_fake_traffic(mode, get_global_position()->lat, get_global_position()->lon,
get_global_position()->alt);
}
void Navigator::stop_fake_traffic()
{
_detect_and_avoid.stop_fake_traffic();
}
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
#if CONFIG_NAVIGATOR_ADSB
void Navigator::take_traffic_conflict_action()
{
vehicle_command_s vehicle_command{};
switch (_adsb_conflict._conflict_detection_params.traffic_avoidance_mode) {
case 2: {
_rtl.set_return_alt_min(true);
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_RETURN_TO_LAUNCH;
publish_vehicle_command(vehicle_command);
break;
}
case 3: {
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LAND;
publish_vehicle_command(vehicle_command);
break;
}
case 4: {
vehicle_command.command = vehicle_command_s::VEHICLE_CMD_NAV_LOITER_UNLIM;
publish_vehicle_command(vehicle_command);
break;
}
}
}
void Navigator::run_fake_traffic()
{
_adsb_conflict.run_fake_traffic(get_global_position()->lat, get_global_position()->lon,
get_global_position()->alt);
}
void Navigator::check_traffic()
{
if (_traffic_sub.updated()) {
_traffic_sub.copy(&_adsb_conflict._transponder_report);
uint16_t required_flags = transponder_report_s::PX4_ADSB_FLAGS_VALID_COORDS |
transponder_report_s::PX4_ADSB_FLAGS_VALID_HEADING |
transponder_report_s::PX4_ADSB_FLAGS_VALID_VELOCITY | transponder_report_s::PX4_ADSB_FLAGS_VALID_ALTITUDE;
if ((_adsb_conflict._transponder_report.flags & required_flags) == required_flags) {
_adsb_conflict.detect_traffic_conflict(get_global_position()->lat, get_global_position()->lon,
get_global_position()->alt, _local_pos.vx, _local_pos.vy, _local_pos.vz);
if (_adsb_conflict.handle_traffic_conflict()) {
take_traffic_conflict_action();
}
}
}
_adsb_conflict.remove_expired_conflicts();
_detect_and_avoid.on_active();
}
#endif // CONFIG_NAVIGATOR_ADSB
@@ -1416,17 +1445,42 @@ int Navigator::custom_command(int argc, char *argv[])
if (!strcmp(argv[0], "fencefile")) {
get_instance<Navigator>(desc)->load_fence_from_file(GEOFENCE_FILENAME);
return 0;
}
#if CONFIG_NAVIGATOR_ADSB
#if CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
} else if (!strcmp(argv[0], "fake_traffic")) {
if (!strcmp(argv[0], "fake_traffic")) {
DetectAndAvoid::FakeTraffMode mode = DetectAndAvoid::FakeTraffMode::kUniqueIds;
get_instance<Navigator>(desc)->run_fake_traffic();
if (argc > 1) {
const char *const argument = argv[1];
if (!strcmp(argument, "help")) {
return print_fake_traffic_usage();
}
if (!strcmp(argument, "stop")) {
if (argc > 2) {
return print_fake_traffic_usage("fake_traffic stop takes no extra arguments");
}
get_instance<Navigator>(desc)->stop_fake_traffic();
PX4_INFO("DAA: fake traffic stopped");
return 0;
}
if (!parse_fake_traffic_mode(argument, mode)) {
return print_fake_traffic_usage("unknown fake_traffic mode");
}
}
get_instance<Navigator>(desc)->run_fake_traffic(mode);
return 0;
#endif // CONFIG_NAVIGATOR_ADSB
}
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
return print_usage("unknown command");
}
@@ -1796,7 +1850,9 @@ controller.
PRINT_MODULE_USAGE_NAME("navigator", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_COMMAND_DESCR("fencefile", "load a geofence file from SD card, stored at etc/geofence.txt");
PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "publishes 24 fake transponder_report_s uORB messages");
#if CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
PRINT_MODULE_USAGE_COMMAND_DESCR("fake_traffic", "run synthetic DAA traffic; use 'navigator fake_traffic help'");
#endif // CONFIG_NAVIGATOR_ADSB_FAKE_TRAFFIC
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;

21
src/modules/navigator/navigator_params.yaml
View File

@@ -66,10 +66,10 @@ parameters:
increment: 0.5
NAV_TRAFF_AVOID:
description:
short: Set traffic avoidance mode
short: Set traffic avoidance action
long: |-
Enabling this will allow the system to respond
to transponder data from e.g. ADSB transponders
Action requested for a crosstrack traffic conflict.
Runtime changes apply on later conflict level transitions.
type: enum
values:
0: Disabled
@@ -77,18 +77,21 @@ parameters:
2: Return mode
3: Land mode
4: Position Hold mode
5: Terminate
default: 1
min: 0
max: 5
NAV_TRAFF_A_HOR:
description:
short: Set NAV TRAFFIC AVOID horizontal distance
long: Defines a crosstrack horizontal distance
short: Crosstrack conflict distance threshold
long: Maximum absolute distance from the projected traffic track.
type: float
default: 500
unit: m
min: 500
NAV_TRAFF_A_VER:
description:
short: Set NAV TRAFFIC AVOID vertical distance
short: Crosstrack vertical separation threshold
type: float
default: 500
unit: m
@@ -96,10 +99,10 @@ parameters:
max: 500
NAV_TRAFF_COLL_T:
description:
short: Estimated time until collision
short: Crosstrack collision time threshold
long: |-
Minimum acceptable time until collsion.
Assumes constant speed over 3d distance.
A conflict is raised when the time estimate is below this threshold.
The estimate is the current 3D separation divided by the sum of the ownship and traffic speed magnitudes.
type: int32
default: 60
unit: s

4
test/CMakeLists.txt
View File

@@ -17,11 +17,13 @@ endif()
add_custom_target(test_results
# antlr4_tests_NOT_BUILT gets added by fuzztest
COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test ${TESTFILTERARG} ${TESTFILTER} --exclude-regex "antlr4_tests_NOT_BUILT"
COMMAND GTEST_COLOR=1 ${CMAKE_CTEST_COMMAND} --output-on-failure -T Test
${TESTFILTERARG} ${TESTFILTER} --exclude-regex "antlr4_tests_NOT_BUILT"
DEPENDS
px4
examples__dyn_hello
USES_TERMINAL
VERBATIM
COMMENT "Running tests"
WORKING_DIRECTORY ${PX4_BINARY_DIR})
set_target_properties(test_results PROPERTIES EXCLUDE_FROM_ALL TRUE)