diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c index d396039f3f..644e849d9d 100644 --- a/src/lib/systemlib/system_params.c +++ b/src/lib/systemlib/system_params.c @@ -240,6 +240,18 @@ PARAM_DEFINE_INT32(SYS_HAS_NUM_ASPD, 0); */ PARAM_DEFINE_INT32(SYS_HAS_NUM_DIST, 0); +/** + * Number of optical flow sensors required to be available + * + * The preflight check will fail if fewer than this number of optical flow sensors with valid data are present. + * + * @group System + * @min 0 + * @max 1 + */ + +PARAM_DEFINE_INT32(SYS_HAS_NUM_OF, 0); + /** * Enable factory calibration mode * diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 7265e042b4..d62d577ca4 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -42,6 +42,7 @@ px4_add_library(health_and_arming_checks checks/batteryCheck.cpp checks/cpuResourceCheck.cpp checks/distanceSensorChecks.cpp + checks/opticalFlowCheck.cpp checks/escCheck.cpp checks/estimatorCheck.cpp checks/failureDetectorCheck.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp index d04b8d088e..d4f7136f1d 100644 --- a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp @@ -46,6 +46,7 @@ #include "checks/baroCheck.hpp" #include "checks/cpuResourceCheck.hpp" #include "checks/distanceSensorChecks.hpp" +#include "checks/opticalFlowCheck.hpp" #include "checks/escCheck.hpp" #include "checks/estimatorCheck.hpp" #include "checks/failureDetectorCheck.hpp" @@ -130,6 +131,7 @@ private: BaroChecks _baro_checks; CpuResourceChecks _cpu_resource_checks; DistanceSensorChecks _distance_sensor_checks; + OpticalFlowCheck _optical_flow_check; EscChecks _esc_checks; EstimatorChecks _estimator_checks; FailureDetectorChecks _failure_detector_checks; @@ -169,6 +171,7 @@ private: &_baro_checks, &_cpu_resource_checks, &_distance_sensor_checks, + &_optical_flow_check, &_esc_checks, &_estimator_checks, &_failure_detector_checks, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp new file mode 100644 index 0000000000..54e7d2c78a --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.cpp @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "opticalFlowCheck.hpp" + +void OpticalFlowCheck::checkAndReport(const Context &context, Report &reporter) +{ + if (!_param_sys_has_num_of.get()) { + return; + } + + const bool exists = _vehicle_optical_flow_sub.advertised(); + bool valid = false; + + if (exists) { + vehicle_optical_flow_s flow_sens; + valid = _vehicle_optical_flow_sub.copy(&flow_sens) && (hrt_elapsed_time(&flow_sens.timestamp) < 1_s); + reporter.setIsPresent(health_component_t::optical_flow); + + if (!valid) { + /* EVENT + */ + reporter.healthFailure(NavModes::All, health_component_t::optical_flow, + events::ID("check_optical_flow_sensor_invalid"), + events::Log::Error, "No valid data from optical flow sensor"); + } + + } else { + /* EVENT + * @description + * + * This check can be configured via SYS_HAS_NUM_OF parameter. + * + */ + reporter.healthFailure(NavModes::All, health_component_t::optical_flow, + events::ID("check_optical_sensor_missing"), + events::Log::Error, "Optical flow sensor missing"); + } +} diff --git a/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp new file mode 100644 index 0000000000..4eaa47f1f4 --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/checks/opticalFlowCheck.hpp @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 +#include + +class OpticalFlowCheck : public HealthAndArmingCheckBase +{ +public: + OpticalFlowCheck() = default; + ~OpticalFlowCheck() = default; + + void checkAndReport(const Context &context, Report &reporter) override; + +private: + uORB::Subscription _vehicle_optical_flow_sub{ORB_ID::vehicle_optical_flow}; + + DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, + (ParamInt) _param_sys_has_num_of + ) +};