From d2011e99b2017c9e22e5f3d42d4cd981ecd9324d Mon Sep 17 00:00:00 2001
From: Eric Katzfey <53063038+katzfey@users.noreply.github.com>
Date: Wed, 31 May 2023 22:52:52 -0700
Subject: [PATCH] commander: add Open Drone ID arming check (#21652)
---
src/lib/events/enums.json | 4 +
.../HealthAndArmingChecks/CMakeLists.txt | 1 +
.../HealthAndArmingChecks.hpp | 5 +-
.../checks/openDroneIDCheck.cpp | 86 +++++++++++++++++++
.../checks/openDroneIDCheck.hpp | 50 +++++++++++
src/modules/commander/commander_params.c | 14 +++
6 files changed, 159 insertions(+), 1 deletion(-)
create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp
create mode 100644 src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp
diff --git a/src/lib/events/enums.json b/src/lib/events/enums.json
index ff8ed06511..da5c2ade56 100644
--- a/src/lib/events/enums.json
+++ b/src/lib/events/enums.json
@@ -240,6 +240,10 @@
"536870912": {
"name": "gyro",
"description": "Gyroscope"
+ },
+ "1073741824": {
+ "name": "open_drone_id",
+ "description": "Open Drone ID system"
}
}
},
diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
index fb5bcc8025..89dbf9fe19 100644
--- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
+++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
@@ -61,6 +61,7 @@ px4_add_library(health_and_arming_checks
checks/rcAndDataLinkCheck.cpp
checks/vtolCheck.cpp
checks/offboardCheck.cpp
+ checks/openDroneIDCheck.cpp
)
add_dependencies(health_and_arming_checks mode_util)
diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
index c094c65ebb..ac5ffbe567 100644
--- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
+++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
@@ -67,6 +67,7 @@
#include "checks/rcAndDataLinkCheck.hpp"
#include "checks/vtolCheck.hpp"
#include "checks/offboardCheck.hpp"
+#include "checks/openDroneIDCheck.hpp"
class HealthAndArmingChecks : public ModuleParams
{
@@ -126,6 +127,7 @@ private:
ManualControlChecks _manual_control_checks;
HomePositionChecks _home_position_checks;
ModeChecks _mode_checks;
+ OpenDroneIDChecks _open_drone_id_checks;
ParachuteChecks _parachute_checks;
PowerChecks _power_checks;
RcCalibrationChecks _rc_calibration_checks;
@@ -140,7 +142,7 @@ private:
VtolChecks _vtol_checks;
OffboardChecks _offboard_checks;
- HealthAndArmingCheckBase *_checks[30] = {
+ HealthAndArmingCheckBase *_checks[31] = {
&_accelerometer_checks,
&_airspeed_checks,
&_baro_checks,
@@ -157,6 +159,7 @@ private:
&_mission_checks,
&_offboard_checks, // must be after _estimator_checks
&_mode_checks, // must be after _estimator_checks, _home_position_checks, _mission_checks, _offboard_checks
+ &_open_drone_id_checks,
&_parachute_checks,
&_power_checks,
&_rc_calibration_checks,
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp
new file mode 100644
index 0000000000..bb4847a2c5
--- /dev/null
+++ b/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2023 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 "openDroneIDCheck.hpp"
+
+
+void OpenDroneIDChecks::checkAndReport(const Context &context, Report &reporter)
+{
+ // Check to see if the check has been disabled
+ if (!_param_com_arm_odid.get()) {
+ return;
+ }
+
+ NavModes affected_modes{NavModes::None};
+
+ if (_param_com_arm_odid.get() == 2) {
+ // disallow arming without the Open Drone ID system
+ affected_modes = NavModes::All;
+ }
+
+ if (!context.status().open_drone_id_system_present) {
+ /* EVENT
+ * @description
+ * Open Drone ID system failed to report. Make sure it is setup and installed properly.
+ *
+ *
+ * This check can be configured via COM_ARM_ODID parameter.
+ *
+ */
+ reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+ events::ID("check_open_drone_id_missing"),
+ events::Log::Error, "Open Drone ID system missing");
+
+ if (reporter.mavlink_log_pub()) {
+ mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system missing");
+ }
+
+ } else if (!context.status().open_drone_id_system_healthy) {
+ /* EVENT
+ * @description
+ * Open Drone ID system reported being unhealthy.
+ *
+ *
+ * This check can be configured via COM_ARM_ODID parameter.
+ *
+ */
+ reporter.armingCheckFailure(affected_modes, health_component_t::open_drone_id,
+ events::ID("check_open_drone_id_unhealthy"),
+ events::Log::Error, "Open Drone ID system not ready");
+
+ if (reporter.mavlink_log_pub()) {
+ mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Open Drone ID system not ready");
+ }
+
+ }
+}
diff --git a/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp
new file mode 100644
index 0000000000..ba9da50663
--- /dev/null
+++ b/src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2023 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 OpenDroneIDChecks : public HealthAndArmingCheckBase
+{
+public:
+ OpenDroneIDChecks() = default;
+ ~OpenDroneIDChecks() = default;
+
+ void checkAndReport(const Context &context, Report &reporter) override;
+
+private:
+ DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
+ (ParamInt) _param_com_arm_odid
+ )
+};
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 784a953f2d..2dace1f8c6 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -1003,6 +1003,20 @@ PARAM_DEFINE_INT32(COM_ARM_SDCARD, 1);
*/
PARAM_DEFINE_INT32(COM_ARM_HFLT_CHK, 1);
+/**
+ * Enable Drone ID system detection and health check
+ *
+ * This check detects if the Open Drone ID system 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 Open Drone ID system presence
+ */
+PARAM_DEFINE_INT32(COM_ARM_ODID, 0);
+
/**
* Enforced delay between arming and further navigation
*