From 4c1078c699ba5a1f1d0d61d3687cc37649246bf0 Mon Sep 17 00:00:00 2001 From: gguidone Date: Fri, 10 Apr 2026 10:59:14 +0200 Subject: [PATCH] test(failure_detector): add unit tests for altitude loss detection --- .../commander/failure_detector/CMakeLists.txt | 4 + .../FailureDetectorAltitudeLossTest.cpp | 162 ++++++++++++++++++ 2 files changed, 166 insertions(+) create mode 100644 src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp diff --git a/src/modules/commander/failure_detector/CMakeLists.txt b/src/modules/commander/failure_detector/CMakeLists.txt index 91503f37b4..9e391573b9 100644 --- a/src/modules/commander/failure_detector/CMakeLists.txt +++ b/src/modules/commander/failure_detector/CMakeLists.txt @@ -36,3 +36,7 @@ px4_add_library(failure_detector FailureInjector.cpp ) set_property(GLOBAL APPEND PROPERTY PX4_MODULE_CONFIG_FILES ${CMAKE_CURRENT_SOURCE_DIR}/failure_detector_params.yaml) + +px4_add_functional_gtest(SRC FailureDetectorAltitudeLossTest.cpp + LINKLIBS failure_detector hysteresis +) diff --git a/src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp b/src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp new file mode 100644 index 0000000000..aebd2025e4 --- /dev/null +++ b/src/modules/commander/failure_detector/FailureDetectorAltitudeLossTest.cpp @@ -0,0 +1,162 @@ +/**************************************************************************** + * + * 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 + +#include "FailureDetector.hpp" + +#include +#include +#include + +// to run: make tests TESTFILTER=FailureDetectorAltitudeLossTest + +using namespace time_literals; + +class FailureDetectorAltitudeLossTest : public ::testing::Test +{ +public: + void SetUp() override + { + param_control_autosave(false); + + // FD_ALT_LOSS = 5m, FD_ALT_LOSS_T = 0s + param_t param = param_handle(px4::params::FD_ALT_LOSS); + float threshold = 5.f; + param_set(param, &threshold); + + param = param_handle(px4::params::FD_ALT_LOSS_T); + float ttri = 0.f; + param_set(param, &ttri); + } + + // Publish position and setpoint, then run the detector. Returns the alt flag state. + bool update(float lpos_z, float lpos_sp_z, float delta_z = 0.f, uint8_t z_reset_counter = 0) + { + vehicle_local_position_s lpos{}; + lpos.timestamp = hrt_absolute_time(); + lpos.z = lpos_z; + lpos.z_valid = true; + lpos.delta_z = delta_z; + lpos.z_reset_counter = z_reset_counter; + _lpos_pub.publish(lpos); + + vehicle_local_position_setpoint_s lpos_sp{}; + lpos_sp.timestamp = hrt_absolute_time(); + lpos_sp.z = lpos_sp_z; + _lpos_sp_pub.publish(lpos_sp); + + vehicle_status_s status{}; + status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + vehicle_control_mode_s control_mode{}; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + + _fd.update(status, control_mode); + + return _fd.getStatus().flags.alt; + } + +private: + FailureDetector _fd{nullptr}; + + uORB::Publication _lpos_pub{ORB_ID(vehicle_local_position)}; + uORB::Publication _lpos_sp_pub{ORB_ID(vehicle_local_position_setpoint)}; +}; + +TEST_F(FailureDetectorAltitudeLossTest, NoTriggerWhenDisabled) +{ + param_t param = param_handle(px4::params::FD_ALT_LOSS); + float threshold = 0.f; + param_set(param, &threshold); + + EXPECT_FALSE(update(-90.f, -100.f)); +} + +TEST_F(FailureDetectorAltitudeLossTest, NoTriggerAboveSetpoint) +{ + EXPECT_FALSE(update(-105.f, -100.f)); +} + +TEST_F(FailureDetectorAltitudeLossTest, NoTriggerWithinThreshold) +{ + // 3m below setpoint, threshold is 5m + EXPECT_FALSE(update(-97.f, -100.f)); +} + +TEST_F(FailureDetectorAltitudeLossTest, TriggerAfterSustainedDrop) +{ + // Ratchet initialises at -97; vehicle sinks to -91 (6m drop exceeds 5m threshold) + EXPECT_FALSE(update(-97.f, -100.f)); // 0m drop + EXPECT_FALSE(update(-95.f, -100.f)); // 2m drop + EXPECT_FALSE(update(-93.f, -100.f)); // 4m drop + EXPECT_TRUE(update(-91.f, -100.f)); // 6m drop, triggers +} + +TEST_F(FailureDetectorAltitudeLossTest, RatchetHoldsOnPartialRecovery) +{ + // Ratchet tracks the best recovery point and is not reset by a partial climb + EXPECT_FALSE(update(-96.f, -100.f)); // ratchet = -96, drop = 0m + EXPECT_FALSE(update(-94.f, -100.f)); // ratchet = -96, drop = 2m + EXPECT_FALSE(update(-96.f, -100.f)); // partial recovery, ratchet stays at -96, drop = 0m + EXPECT_FALSE(update(-95.f, -100.f)); // ratchet = -96, drop = 1m + EXPECT_TRUE(update(-90.f, -100.f)); // ratchet = -96, drop = 6m, triggers +} + +TEST_F(FailureDetectorAltitudeLossTest, ResetWhenBackAboveSetpoint) +{ + // After triggering, climbing above setpoint clears the flag and resets the ratchet + EXPECT_FALSE(update(-97.f, -100.f)); + EXPECT_TRUE(update(-91.f, -100.f)); + + EXPECT_FALSE(update(-101.f, -100.f)); // above setpoint, resets + + EXPECT_FALSE(update(-97.f, -100.f)); // ratchet reinitialises at -97, drop = 0m + EXPECT_FALSE(update(-95.f, -100.f)); // drop = 2m, no trigger +} + +TEST_F(FailureDetectorAltitudeLossTest, NoTriggerOnEkfZReset) +{ + // EKF resets z by 6m downward; reference shifts with it so the drop stays 0m. + EXPECT_FALSE(update(-97.f, -100.f)); // ratchet = -97, drop = 0m + EXPECT_FALSE(update(-91.f, -100.f, 6.f, 1)); // EKF reset: ref shifts to -91, drop = 0m +} + +TEST_F(FailureDetectorAltitudeLossTest, NoTriggerOnSetpointJump) +{ + // Setpoint jumps 10m higher; vehicle position unchanged so ratchet stays at -97, drop = 0m. + EXPECT_FALSE(update(-97.f, -100.f)); // ratchet = -97, drop = 0m + EXPECT_FALSE(update(-97.f, -110.f)); // setpoint jumps, vehicle 13m below sp, but drop = 0m + EXPECT_FALSE(update(-97.f, -110.f)); // vehicle holds position, no trigger +}