dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: add Open Drone ID arming check (#21652)

This commit is contained in:
Eric Katzfey 2023-05-31 22:52:52 -07:00 committed by GitHub
parent 4b5e14aeec
commit d2011e99b2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 159 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/events/enums.json
View File

@ -240,6 +240,10 @@
"536870912": {
"name": "gyro",
"description": "Gyroscope"
},
"1073741824": {
"name": "open_drone_id",
"description": "Open Drone ID system"
}
}
},

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

@ -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)

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

@ -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,

86
src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.cpp Normal file
View File

@ -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.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_ODID</param> parameter.
* </profile>
*/
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.
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_ODID</param> parameter.
* </profile>
*/
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");
}
}
}

50
src/modules/commander/HealthAndArmingChecks/checks/openDroneIDCheck.hpp Normal file
View File

@ -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<px4::params::COM_ARM_ODID>) _param_com_arm_odid
)
};

14
src/modules/commander/commander_params.c
View File

@ -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
*