From 05de941399c6937fc5bbc7a1acc660900608ea4c Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Mon, 16 Feb 2026 11:10:32 -0800 Subject: [PATCH] Add Traffic Avoidance System (ADSB/FLARM) Arming Check (#26390) * Add traffic avoidance system checks * Update msg/px4_msgs_old/msg/VehicleStatusV1.msg Co-authored-by: Hamish Willee * docs: add arming check for traffic avoidance systems in ADS-B/FLARM documentation * fix formating * trafficAvoidanceCheck: switch case for configuration options --------- Co-authored-by: Hamish Willee Co-authored-by: Matthias Grob --- docs/en/peripherals/adsb_flarm.md | 23 ++++- msg/TelemetryStatus.msg | 1 + msg/px4_msgs_old/msg/VehicleStatusV1.msg | 98 +++++++++---------- .../translation_vehicle_status_v2.h | 8 +- msg/versioned/VehicleStatus.msg | 2 + src/lib/events/enums.json | 4 + src/modules/commander/Commander.cpp | 26 +++++ src/modules/commander/Commander.hpp | 2 + .../HealthAndArmingChecks/CMakeLists.txt | 1 + .../HealthAndArmingChecks.hpp | 3 + .../checks/trafficAvoidanceCheck.cpp | 71 ++++++++++++++ .../checks/trafficAvoidanceCheck.hpp | 50 ++++++++++ src/modules/commander/commander_params.c | 15 +++ src/modules/mavlink/mavlink_receiver.cpp | 5 + src/modules/mavlink/mavlink_receiver.h | 1 + 15 files changed, 256 insertions(+), 54 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.cpp create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.hpp diff --git a/docs/en/peripherals/adsb_flarm.md b/docs/en/peripherals/adsb_flarm.md index 802f197e82..f8773d3555 100644 --- a/docs/en/peripherals/adsb_flarm.md +++ b/docs/en/peripherals/adsb_flarm.md @@ -73,11 +73,28 @@ Configure the action when there is a potential collision using the parameter bel | Parameter | Description | | ----------------------------------------------------------------------------------------------------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| [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. | -| [NAV_TRAFF_A_HOR](../advanced_config/parameter_reference.md#NAV_TRAFF_A_HOR) | Horizonal radius of cylinder around the vehicle that defines its airspace (i.e. the airspace in the ground plane). | -| [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)). | +| [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. | +| [NAV_TRAFF_A_HOR](../advanced_config/parameter_reference.md#NAV_TRAFF_A_HOR) | Horizonal radius of cylinder around the vehicle that defines its airspace (i.e. the airspace in the ground plane). | +| [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)). | | [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). | +### Arming Check + +PX4 can be configured to check for the presence of a traffic avoidance system (ADSB or FLARM transponder) before arming. +This ensures that a traffic avoidance system is connected and functioning before flight. + +The check is configured using the [COM_ARM_TRAFF](../advanced_config/parameter_reference.md#COM_ARM_TRAFF) parameter: + +| Value | Description | +| ----- | --------------------------------------------------------------------------------------------------------------------------- | +| 0 | Disabled (default). No check is performed. | +| 1 | Warning only. A warning is issued if no traffic avoidance system is detected, but arming is allowed. | +| 2 | Enforce for all modes. Arming is denied if no traffic avoidance system is detected, regardless of flight mode. | +| 3 | Enforce for mission modes only. Arming is denied if no traffic avoidance system is detected and a mission mode is planned. | + +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 ### ADSB/FLARM diff --git a/msg/TelemetryStatus.msg b/msg/TelemetryStatus.msg index b2b3920a36..1f5b027f4d 100644 --- a/msg/TelemetryStatus.msg +++ b/msg/TelemetryStatus.msg @@ -43,6 +43,7 @@ bool heartbeat_type_gcs # MAV_TYPE_GCS bool heartbeat_type_onboard_controller # MAV_TYPE_ONBOARD_CONTROLLER bool heartbeat_type_gimbal # MAV_TYPE_GIMBAL bool heartbeat_type_adsb # MAV_TYPE_ADSB +bool heartbeat_type_flarm # MAV_TYPE_FLARM bool heartbeat_type_camera # MAV_TYPE_CAMERA bool heartbeat_type_parachute # MAV_TYPE_PARACHUTE bool heartbeat_type_open_drone_id # MAV_TYPE_ODID diff --git a/msg/px4_msgs_old/msg/VehicleStatusV1.msg b/msg/px4_msgs_old/msg/VehicleStatusV1.msg index 185076a5d6..4724a09a22 100644 --- a/msg/px4_msgs_old/msg/VehicleStatusV1.msg +++ b/msg/px4_msgs_old/msg/VehicleStatusV1.msg @@ -2,14 +2,14 @@ uint32 MESSAGE_VERSION = 1 -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # [us] Time since system start -uint64 armed_time # Arming timestamp (microseconds) -uint64 takeoff_time # Takeoff timestamp (microseconds) +uint64 armed_time # [us] Arming timestamp +uint64 takeoff_time # [us] Takeoff timestamp uint8 arming_state uint8 ARMING_STATE_DISARMED = 1 -uint8 ARMING_STATE_ARMED = 2 +uint8 ARMING_STATE_ARMED = 2 uint8 latest_arming_reason uint8 latest_disarming_reason @@ -24,34 +24,34 @@ uint8 ARM_DISARM_REASON_KILL_SWITCH = 8 uint8 ARM_DISARM_REASON_RC_BUTTON = 13 uint8 ARM_DISARM_REASON_FAILSAFE = 14 -uint64 nav_state_timestamp # time when current nav_state activated +uint64 nav_state_timestamp # Time when current nav_state activated -uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) +uint8 nav_state_user_intention # Mode that the user selected (might be different from nav_state in a failsafe situation) -uint8 nav_state # Currently active mode -uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode -uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode -uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode -uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode -uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode -uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode +uint8 nav_state # [@enum NAVIGATION_STATE] Currently active mode +uint8 NAVIGATION_STATE_MANUAL = 0 # Manual mode +uint8 NAVIGATION_STATE_ALTCTL = 1 # Altitude control mode +uint8 NAVIGATION_STATE_POSCTL = 2 # Position control mode +uint8 NAVIGATION_STATE_AUTO_MISSION = 3 # Auto mission mode +uint8 NAVIGATION_STATE_AUTO_LOITER = 4 # Auto loiter mode +uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_POSITION_SLOW = 6 uint8 NAVIGATION_STATE_FREE5 = 7 -uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode +uint8 NAVIGATION_STATE_ALTITUDE_CRUISE = 8 # Altitude with Cruise mode uint8 NAVIGATION_STATE_FREE3 = 9 -uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode +uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode uint8 NAVIGATION_STATE_FREE2 = 11 -uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) -uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode +uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) +uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 -uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode +uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode uint8 NAVIGATION_STATE_FREE1 = 16 -uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff -uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land -uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow -uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target -uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle -uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter +uint8 NAVIGATION_STATE_AUTO_TAKEOFF = 17 # Takeoff +uint8 NAVIGATION_STATE_AUTO_LAND = 18 # Land +uint8 NAVIGATION_STATE_AUTO_FOLLOW_TARGET = 19 # Auto Follow +uint8 NAVIGATION_STATE_AUTO_PRECLAND = 20 # Precision land with landing target +uint8 NAVIGATION_STATE_ORBIT = 21 # Orbit in a circle +uint8 NAVIGATION_STATE_AUTO_VTOL_TAKEOFF = 22 # Takeoff, transition, establish loiter uint8 NAVIGATION_STATE_EXTERNAL1 = 23 uint8 NAVIGATION_STATE_EXTERNAL2 = 24 uint8 NAVIGATION_STATE_EXTERNAL3 = 25 @@ -62,29 +62,29 @@ uint8 NAVIGATION_STATE_EXTERNAL7 = 29 uint8 NAVIGATION_STATE_EXTERNAL8 = 30 uint8 NAVIGATION_STATE_MAX = 31 -uint8 executor_in_charge # Current mode executor in charge (0=Autopilot) +uint8 executor_in_charge # [-] Current mode executor in charge (0=Autopilot) -uint32 valid_nav_states_mask # Bitmask for all valid nav_state values -uint32 can_set_nav_states_mask # Bitmask for all modes that a user can select +uint32 valid_nav_states_mask # [-] Bitmask for all valid nav_state values +uint32 can_set_nav_states_mask # [-] Bitmask for all modes that a user can select # Bitmask of detected failures -uint16 failure_detector_status +uint16 failure_detector_status # [@enum FAILURE] uint16 FAILURE_NONE = 0 -uint16 FAILURE_ROLL = 1 # (1 << 0) -uint16 FAILURE_PITCH = 2 # (1 << 1) -uint16 FAILURE_ALT = 4 # (1 << 2) -uint16 FAILURE_EXT = 8 # (1 << 3) -uint16 FAILURE_ARM_ESC = 16 # (1 << 4) -uint16 FAILURE_BATTERY = 32 # (1 << 5) -uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) -uint16 FAILURE_MOTOR = 128 # (1 << 7) +uint16 FAILURE_ROLL = 1 # (1 << 0) +uint16 FAILURE_PITCH = 2 # (1 << 1) +uint16 FAILURE_ALT = 4 # (1 << 2) +uint16 FAILURE_EXT = 8 # (1 << 3) +uint16 FAILURE_ARM_ESC = 16 # (1 << 4) +uint16 FAILURE_BATTERY = 32 # (1 << 5) +uint16 FAILURE_IMBALANCED_PROP = 64 # (1 << 6) +uint16 FAILURE_MOTOR = 128 # (1 << 7) -uint8 hil_state +uint8 hil_state # [enum HIL_STATE] uint8 HIL_STATE_OFF = 0 uint8 HIL_STATE_ON = 1 # Current vehicle locomotion method. A vehicle can have different methods (e.g. VTOL transitions from RW to FW method) -uint8 vehicle_type +uint8 vehicle_type # [@enum VEHICLE_TYPE] uint8 VEHICLE_TYPE_UNSPECIFIED = 0 uint8 VEHICLE_TYPE_ROTARY_WING = 1 uint8 VEHICLE_TYPE_FIXED_WING = 2 @@ -96,29 +96,29 @@ uint8 FAILSAFE_DEFER_STATE_WOULD_FAILSAFE = 2 # Failsafes deferred, but would tr bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) bool failsafe_and_user_took_over # true if system is in failsafe state but the user took over control -uint8 failsafe_defer_state # one of FAILSAFE_DEFER_STATE_* +uint8 failsafe_defer_state # [@enum FAILSAFE_DEFER_STATE] # Link loss -bool gcs_connection_lost # datalink to GCS lost -uint8 gcs_connection_lost_counter # counts unique GCS connection lost events +bool gcs_connection_lost # datalink to GCS lost +uint8 gcs_connection_lost_counter # counts unique GCS connection lost events bool high_latency_data_link_lost # Set to true if the high latency data link (eg. RockBlock Iridium 9603 telemetry module) is lost # VTOL flags -bool is_vtol # True if the system is VTOL capable -bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW -bool in_transition_mode # True if VTOL is doing a transition +bool is_vtol # True if the system is VTOL capable +bool is_vtol_tailsitter # True if the system performs a 90° pitch down rotation during transition from MC to FW +bool in_transition_mode # True if VTOL is doing a transition bool in_transition_to_fw # True if VTOL is doing a transition from MC to FW # MAVLink identification -uint8 system_type # system type, contains mavlink MAV_TYPE -uint8 system_id # system id, contains MAVLink's system ID field +uint8 system_type # system type, contains mavlink MAV_TYPE +uint8 system_id # system id, contains MAVLink's system ID field uint8 component_id # subsystem / component id, contains MAVLink's component ID field bool safety_button_available # Set to true if a safety button is connected bool safety_off # Set to true if safety is off -bool power_input_valid # set if input power is valid -bool usb_connected # set to true (never cleared) once telemetry received from usb link +bool power_input_valid # Set if input power is valid +bool usb_connected # Set to true (never cleared) once telemetry received from usb link bool open_drone_id_system_present bool open_drone_id_system_healthy @@ -129,4 +129,4 @@ bool parachute_system_healthy bool rc_calibration_in_progress bool calibration_enabled -bool pre_flight_checks_pass # true if all checks necessary to arm pass +bool pre_flight_checks_pass # true if all checks necessary to arm pass diff --git a/msg/translation_node/translations/translation_vehicle_status_v2.h b/msg/translation_node/translations/translation_vehicle_status_v2.h index 926ade4da3..da4b315566 100644 --- a/msg/translation_node/translations/translation_vehicle_status_v2.h +++ b/msg/translation_node/translations/translation_vehicle_status_v2.h @@ -1,5 +1,5 @@ /**************************************************************************** - * Copyright (c) 2025 PX4 Development Team. + * Copyright (c) 2026 PX4 Development Team. * SPDX-License-Identifier: BSD-3-Clause ****************************************************************************/ #pragma once @@ -19,6 +19,7 @@ public: static constexpr const char* kTopic = "fmu/out/vehicle_status"; static void fromOlder(const MessageOlder &msg_older, MessageNewer &msg_newer) { + // Set msg_newer from msg_older msg_newer.timestamp = msg_older.timestamp; msg_newer.armed_time = msg_older.armed_time; msg_newer.takeoff_time = msg_older.takeoff_time; @@ -56,13 +57,14 @@ public: msg_newer.open_drone_id_system_healthy = msg_older.open_drone_id_system_healthy; msg_newer.parachute_system_present = msg_older.parachute_system_present; msg_newer.parachute_system_healthy = msg_older.parachute_system_healthy; + msg_newer.traffic_avoidance_system_present = false; // New field, default to false msg_newer.rc_calibration_in_progress = msg_older.rc_calibration_in_progress; msg_newer.calibration_enabled = msg_older.calibration_enabled; msg_newer.pre_flight_checks_pass = msg_older.pre_flight_checks_pass; } static void toOlder(const MessageNewer &msg_newer, MessageOlder &msg_older) { - // copy everything except the new executor_nav_state + // Set msg_older from msg_newer msg_older.timestamp = msg_newer.timestamp; msg_older.armed_time = msg_newer.armed_time; msg_older.takeoff_time = msg_newer.takeoff_time; @@ -73,6 +75,7 @@ public: msg_older.nav_state_user_intention = msg_newer.nav_state_user_intention; msg_older.nav_state = msg_newer.nav_state; msg_older.executor_in_charge = msg_newer.executor_in_charge; + // nav_state_display dropped (not in V1) msg_older.valid_nav_states_mask = msg_newer.valid_nav_states_mask; msg_older.can_set_nav_states_mask = msg_newer.can_set_nav_states_mask; msg_older.failure_detector_status = msg_newer.failure_detector_status; @@ -99,6 +102,7 @@ public: msg_older.open_drone_id_system_healthy = msg_newer.open_drone_id_system_healthy; msg_older.parachute_system_present = msg_newer.parachute_system_present; msg_older.parachute_system_healthy = msg_newer.parachute_system_healthy; + // traffic_avoidance_system_present is dropped (not in V1) msg_older.rc_calibration_in_progress = msg_newer.rc_calibration_in_progress; msg_older.calibration_enabled = msg_newer.calibration_enabled; msg_older.pre_flight_checks_pass = msg_newer.pre_flight_checks_pass; diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index 41daa384b5..3420ada11c 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -127,6 +127,8 @@ bool open_drone_id_system_healthy bool parachute_system_present bool parachute_system_healthy +bool traffic_avoidance_system_present + bool rc_calibration_in_progress bool calibration_enabled diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json index acd73c1447..20b7d841e1 100644 --- a/src/lib/events/enums.json +++ b/src/lib/events/enums.json @@ -276,6 +276,10 @@ "1073741824": { "name": "open_drone_id", "description": "Open Drone ID system" + }, + "2147483648": { + "name": "traffic_avoidance", + "description": "Traffic Avoidance (ADSB/FLARM)" } } }, diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index a6175e9c67..0d11fa47c0 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2880,6 +2880,22 @@ void Commander::dataLinkCheck() _vehicle_status.open_drone_id_system_present = true; _vehicle_status.open_drone_id_system_healthy = healthy; } + + // Traffic avoidance system (ADSB or FLARM) + if (telemetry.heartbeat_type_adsb || telemetry.heartbeat_type_flarm) { + if (_traffic_avoidance_system_lost) { // recovered + _traffic_avoidance_system_lost = false; + + if (_datalink_last_heartbeat_traffic_avoidance_system != 0) { + mavlink_log_info(&_mavlink_log_pub, "Traffic avoidance system regained\t"); + events::send(events::ID("commander_traffic_avoidance_regained"), events::Log::Info, + "Traffic avoidance system regained"); + } + } + + _datalink_last_heartbeat_traffic_avoidance_system = telemetry.timestamp; + _vehicle_status.traffic_avoidance_system_present = true; + } } } @@ -2931,6 +2947,16 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = true; _status_changed = true; } + + // Traffic avoidance system (ADSB/FLARM) + if ((hrt_elapsed_time(&_datalink_last_heartbeat_traffic_avoidance_system) > 3_s) + && !_traffic_avoidance_system_lost) { + mavlink_log_critical(&_mavlink_log_pub, "Traffic avoidance system lost"); + events::send(events::ID("commander_traffic_avoidance_lost"), events::Log::Critical, "Traffic avoidance system lost"); + _vehicle_status.traffic_avoidance_system_present = false; + _traffic_avoidance_system_lost = true; + _status_changed = true; + } } void Commander::battery_status_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e4f0fef7ca..748fa9f858 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -243,6 +243,7 @@ private: hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; + hrt_abstime _datalink_last_heartbeat_traffic_avoidance_system{0}; hrt_abstime _last_print_mode_reject_time{0}; ///< To remember when last notification was sent @@ -268,6 +269,7 @@ private: bool _open_drone_id_system_lost{true}; bool _onboard_controller_lost{false}; bool _parachute_system_lost{true}; + bool _traffic_avoidance_system_lost{true}; bool _last_overload{false}; bool _mode_switch_mapped{false}; diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index d62d577ca4..b170d1d2ca 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -65,6 +65,7 @@ px4_add_library(health_and_arming_checks checks/rcCalibrationCheck.cpp checks/sdcardCheck.cpp checks/systemCheck.cpp + checks/trafficAvoidanceCheck.cpp checks/vtolCheck.cpp checks/windCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index d4f7136f1d..1986e55e15 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -72,6 +72,7 @@ #include "checks/vtolCheck.hpp" #include "checks/offboardCheck.hpp" #include "checks/openDroneIDCheck.hpp" +#include "checks/trafficAvoidanceCheck.hpp" #include "checks/externalChecks.hpp" class HealthAndArmingChecks : public ModuleParams @@ -157,6 +158,7 @@ private: RcAndDataLinkChecks _rc_and_data_link_checks; VtolChecks _vtol_checks; OffboardChecks _offboard_checks; + TrafficAvoidanceChecks _traffic_avoidance_checks; #ifndef CONSTRAINED_FLASH ExternalChecks _external_checks; #endif @@ -197,5 +199,6 @@ private: &_flight_time_checks, &_rc_and_data_link_checks, &_vtol_checks, + &_traffic_avoidance_checks, }; }; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.cpp new file mode 100644 index 0000000000..baba01fd18 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * 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 "trafficAvoidanceCheck.hpp" + + +void TrafficAvoidanceChecks::checkAndReport(const Context &context, Report &reporter) +{ + NavModes affected_modes{NavModes::None}; // COM_ARM_TRAFF 1 - warning only, arming allowed, affected_modes stays None + + switch (_param_com_arm_traff.get()) { + case 0: + return; // Check disabled + + case 2: + affected_modes = NavModes::All; // Disallow arming for all modes + break; + + case 3: + affected_modes = NavModes::Mission; // Disallow arming for mission + break; + } + + if (!context.status().traffic_avoidance_system_present) { + /* EVENT + * @description + * Traffic avoidance system (ADSB/FLARM) failed to report. Make sure it is setup and connected properly. + * + * + * This check can be configured via COM_ARM_TRAFF parameter. + * + */ + reporter.armingCheckFailure(affected_modes, health_component_t::traffic_avoidance, + events::ID("check_traffic_avoidance_missing"), + events::Log::Error, "Traffic avoidance system missing"); + + if (reporter.mavlink_log_pub()) { + mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Traffic avoidance system missing"); + } + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.hpp new file mode 100644 index 0000000000..f34faba6fc --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/trafficAvoidanceCheck.hpp @@ -0,0 +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 + +#include "../Common.hpp" + +class TrafficAvoidanceChecks : public HealthAndArmingCheckBase +{ +public: + TrafficAvoidanceChecks() = default; + ~TrafficAvoidanceChecks() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_com_arm_traff + ) +}; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index eb3c2109e4..ed762fd40c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -836,6 +836,21 @@ PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1); */ PARAM_DEFINE_INT32(COM_ARM_ODID, 0); +/** + * Enable Traffic Avoidance system detection check + * + * This check detects if a traffic avoidance system (ADSB/FLARM transponder) + * is missing. Depending on the value of the parameter, the check can be + * disabled, warn only, or deny arming. + * + * @group Commander + * @value 0 Disabled + * @value 1 Warning only + * @value 2 Enforce for all modes + * @value 3 Enforce for mission modes only + */ +PARAM_DEFINE_INT32(COM_ARM_TRAFF, 0); + /** * Enforced delay between arming and further navigation * diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d34074cba2..41eaf4a355 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2177,6 +2177,10 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) _heartbeat_type_adsb = now; break; + case MAV_TYPE_FLARM: + _heartbeat_type_flarm = now; + break; + case MAV_TYPE_CAMERA: _heartbeat_type_camera = now; camera_status.timestamp = now; @@ -2925,6 +2929,7 @@ void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force) tstatus.heartbeat_type_onboard_controller = (t <= TIMEOUT + _heartbeat_type_onboard_controller); tstatus.heartbeat_type_gimbal = (t <= TIMEOUT + _heartbeat_type_gimbal); tstatus.heartbeat_type_adsb = (t <= TIMEOUT + _heartbeat_type_adsb); + tstatus.heartbeat_type_flarm = (t <= TIMEOUT + _heartbeat_type_flarm); tstatus.heartbeat_type_camera = (t <= TIMEOUT + _heartbeat_type_camera); tstatus.heartbeat_type_parachute = (t <= TIMEOUT + _heartbeat_type_parachute); tstatus.heartbeat_type_open_drone_id = (t <= TIMEOUT + _heartbeat_type_open_drone_id); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 5023c7a7df..50a38a8c1a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -391,6 +391,7 @@ private: hrt_abstime _heartbeat_type_onboard_controller{0}; hrt_abstime _heartbeat_type_gimbal{0}; hrt_abstime _heartbeat_type_adsb{0}; + hrt_abstime _heartbeat_type_flarm{0}; hrt_abstime _heartbeat_type_camera{0}; hrt_abstime _heartbeat_type_parachute{0}; hrt_abstime _heartbeat_type_open_drone_id{0};