From f23363f88cd8712abc24772e73ef49bbcef58c89 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 2 Dec 2020 16:47:10 +0100 Subject: [PATCH] HealthAndArmingChecks: add unit tests for Report --- .../HealthAndArmingChecks/CMakeLists.txt | 3 + .../HealthAndArmingChecks/Common.cpp | 2 +- .../HealthAndArmingChecks/Common.hpp | 16 +- .../HealthAndArmingChecksTest.cpp | 285 ++++++++++++++++++ 4 files changed, 303 insertions(+), 3 deletions(-) create mode 100644 src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp diff --git a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt index 98bdfe7dd4..474cf13e74 100644 --- a/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt +++ b/src/modules/commander/HealthAndArmingChecks/CMakeLists.txt @@ -36,3 +36,6 @@ px4_add_library(health_and_arming_checks HealthAndArmingChecks.cpp ) +px4_add_functional_gtest(SRC HealthAndArmingChecksTest.cpp + LINKLIBS health_and_arming_checks mode_util + ) diff --git a/src/modules/commander/HealthAndArmingChecks/Common.cpp b/src/modules/commander/HealthAndArmingChecks/Common.cpp index 74e607120c..b10a377913 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.cpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.cpp @@ -177,7 +177,7 @@ bool Report::report(bool is_armed, bool force) const hrt_abstime now = hrt_absolute_time(); const bool has_difference = _had_unreported_difference || _results[0] != _results[1]; - if (now - _last_report < min_reporting_interval && !force) { + if (now - _last_report < _min_reporting_interval && !force) { if (has_difference) { _had_unreported_difference = true; } diff --git a/src/modules/commander/HealthAndArmingChecks/Common.hpp b/src/modules/commander/HealthAndArmingChecks/Common.hpp index 3a73ea554b..702e8f5f4d 100644 --- a/src/modules/commander/HealthAndArmingChecks/Common.hpp +++ b/src/modules/commander/HealthAndArmingChecks/Common.hpp @@ -50,6 +50,10 @@ #define CONSOLE_PRINT_ARMING_CHECK_EVENT(log_level, id, str) #endif +#ifndef FRIEND_TEST // for gtest +#define FRIEND_TEST(a, b) +#endif + using namespace time_literals; class HealthAndArmingChecks; @@ -185,7 +189,8 @@ public: } }; - Report(vehicle_status_flags_s &status_flags) : _status_flags(status_flags) { } + Report(vehicle_status_flags_s &status_flags, hrt_abstime min_reporting_interval = 2_s) + : _min_reporting_interval(min_reporting_interval), _status_flags(status_flags) { } ~Report() = default; vehicle_status_flags_s &failsafeFlags() { return _status_flags; } @@ -292,6 +297,13 @@ private: NavModes getModeGroup(uint8_t nav_state) const; friend class HealthAndArmingChecks; + FRIEND_TEST(ReporterTest, basic_no_checks); + FRIEND_TEST(ReporterTest, basic_fail_all_modes); + FRIEND_TEST(ReporterTest, arming_checks_mode_category); + FRIEND_TEST(ReporterTest, arming_checks_mode_category2); + FRIEND_TEST(ReporterTest, reporting); + FRIEND_TEST(ReporterTest, reporting_multiple); + /** * Reset current results. * The calling order needs to be: @@ -305,7 +317,7 @@ private: bool report(bool is_armed, bool force); - static constexpr hrt_abstime min_reporting_interval{2_s}; + const hrt_abstime _min_reporting_interval; /// event buffer: stores current events + arguments. /// Since the amount of extra arguments varies, 4 bytes is used here as estimate diff --git a/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp new file mode 100644 index 0000000000..375a2bf7ea --- /dev/null +++ b/src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecksTest.cpp @@ -0,0 +1,285 @@ +/**************************************************************************** + * + * Copyright (c) 2020 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 "Common.hpp" +#include +#include + +#include + +using namespace time_literals; + +// to run: make tests TESTFILTER=HealthAndArmingChecks + + +class ReporterTest : public ::testing::Test + +{ +public: + + void SetUp() override + { + // ensure topic exists, otherwise we might lose first queued events + orb_advertise_queue(ORB_ID(event), nullptr, event_s::ORB_QUEUE_LENGTH); + } + +}; + + +TEST_F(ReporterTest, basic_no_checks) +{ + vehicle_status_flags_s status_flags{}; + Report reporter{status_flags, 0_s}; + ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); + + reporter.reset(); + reporter.finalize(); + reporter.report(false, false); + + ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); + ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, 0xff); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().error, 0); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().warning, 0); + + ASSERT_EQ((uint64_t)reporter.healthResults().is_present, 0); + ASSERT_EQ((uint64_t)reporter.healthResults().error, 0); + ASSERT_EQ((uint64_t)reporter.healthResults().warning, 0); +} + +TEST_F(ReporterTest, basic_fail_all_modes) +{ + vehicle_status_flags_s status_flags{}; + Report reporter{status_flags, 0_s}; + + // ensure arming is always denied with a NavModes::All failure + for (uint8_t nav_state = 0; nav_state < vehicle_status_s::NAVIGATION_STATE_MAX; ++nav_state) { + reporter.reset(); + reporter.armingCheckFailure(NavModes::All, health_component_t::manual_control_input, + events::ID("arming_test_basic_fail_all_modes_fail1"), events::Log::Info, ""); + reporter.finalize(); + reporter.report(false, false); + + ASSERT_FALSE(reporter.canArm(nav_state)); + ASSERT_TRUE(reporter.canRun(nav_state)); + } +} + +TEST_F(ReporterTest, arming_checks_mode_category) +{ + vehicle_status_flags_s status_flags{}; + Report reporter{status_flags, 0_s}; + + // arming must still be possible for non-relevant failures + reporter.reset(); + reporter.armingCheckFailure(NavModes::PositionControl | NavModes::Stabilized, health_component_t::manual_control_input, + events::ID("arming_test_arming_checks_mode_category_fail1"), events::Log::Warning, ""); + reporter.clearCanRunBits(NavModes::PositionControl | NavModes::Stabilized); + reporter.healthFailure(NavModes::PositionControl, health_component_t::local_position_estimate, + events::ID("arming_test_arming_checks_mode_category_fail2"), events::Log::Info, ""); + reporter.setIsPresent(health_component_t::battery); + reporter.finalize(); + reporter.report(false, false); + + ASSERT_TRUE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); + ASSERT_TRUE(reporter.canRun(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); + ASSERT_FALSE(reporter.canRun(vehicle_status_s::NAVIGATION_STATE_POSCTL)); + + ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, (uint8_t)~(NavModes::PositionControl | NavModes::Stabilized)); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().error, 0); + ASSERT_EQ(reporter.armingCheckResults().warning, events::px4::enums::health_component_t::manual_control_input); + + ASSERT_EQ(reporter.healthResults().is_present, events::px4::enums::health_component_t::battery); + ASSERT_EQ((uint64_t)reporter.healthResults().error, 0); + ASSERT_EQ((uint64_t)reporter.healthResults().warning, 0); +} + +TEST_F(ReporterTest, arming_checks_mode_category2) +{ + vehicle_status_flags_s status_flags{}; + Report reporter{status_flags, 0_s}; + + // A matching mode category must deny arming + reporter.reset(); + reporter.healthFailure(NavModes::Mission, health_component_t::manual_control_input, + events::ID("arming_test_arming_checks_mode_category2_fail1"), events::Log::Warning, ""); + reporter.finalize(); + reporter.report(false, false); + + ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION)); + + ASSERT_EQ((uint8_t)reporter.armingCheckResults().can_arm, (uint8_t)~(NavModes::Mission)); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().error, 0); + ASSERT_EQ((uint64_t)reporter.armingCheckResults().warning, 0); + + ASSERT_EQ((uint64_t)reporter.healthResults().is_present, 0); + ASSERT_EQ((uint64_t)reporter.healthResults().error, 0); + ASSERT_EQ(reporter.healthResults().warning, events::px4::enums::health_component_t::manual_control_input); +} + +TEST_F(ReporterTest, reporting) +{ + vehicle_status_flags_s status_flags{}; + Report reporter{status_flags, 0_s}; + + uORB::Subscription event_sub{ORB_ID(event)}; + event_sub.subscribe(); + event_s event; + + while (event_sub.update(&event)); // clear all updates + + for (int j = 0; j < 2; ++j) { // test with and without additional report arguments + const bool with_arg = j == 0; + + for (int i = 0; i < 3; ++i) { // repeat same report: we expect reporting only the first time + reporter.reset(); + + if (with_arg) { + reporter.armingCheckFailure(NavModes::All, health_component_t::manual_control_input, + events::ID("arming_test_reporting_fail1"), events::Log::Warning, "", 4938); + + } else { + reporter.armingCheckFailure(NavModes::PositionControl, health_component_t::manual_control_input, + events::ID("arming_test_reporting_fail2"), events::Log::Warning, ""); + } + + reporter.finalize(); + reporter.report(false, false); + ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); + + if (i == 0) { + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("commander_arming_check_summary")); + ASSERT_TRUE(event_sub.update(&event)); + + if (with_arg) { + ASSERT_EQ(event.id, events::ID("arming_test_reporting_fail1")); + + } else { + ASSERT_EQ(event.id, events::ID("arming_test_reporting_fail2")); + } + + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("commander_health_summary")); + + } else { + ASSERT_FALSE(event_sub.updated()); + } + } + } + + // now the same for health + for (int j = 0; j < 2; ++j) { + const bool with_arg = j == 0; + + for (int i = 0; i < 3; ++i) { + reporter.reset(); + + if (with_arg) { + reporter.healthFailure(NavModes::All, health_component_t::manual_control_input, + events::ID("arming_test_reporting_fail3"), events::Log::Warning, "", 4938); + + } else { + reporter.healthFailure(NavModes::PositionControl, health_component_t::manual_control_input, + events::ID("arming_test_reporting_fail4"), events::Log::Warning, ""); + } + + reporter.finalize(); + reporter.report(false, false); + ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); + + if (i == 0) { + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("commander_arming_check_summary")); + ASSERT_TRUE(event_sub.update(&event)); + + if (with_arg) { + ASSERT_EQ(event.id, events::ID("arming_test_reporting_fail3")); + + } else { + ASSERT_EQ(event.id, events::ID("arming_test_reporting_fail4")); + } + + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("commander_health_summary")); + + } else { + ASSERT_FALSE(event_sub.updated()); + } + } + } + +} + +TEST_F(ReporterTest, reporting_multiple) +{ + vehicle_status_flags_s status_flags{}; + Report reporter{status_flags, 0_s}; + + uORB::Subscription event_sub{ORB_ID(event)}; + event_sub.subscribe(); + event_s event; + + while (event_sub.update(&event)); // clear all updates + + for (int i = 0; i < 3; ++i) { + reporter.reset(); + reporter.armingCheckFailure(NavModes::All, health_component_t::manual_control_input, + events::ID("arming_test_reporting_multiple_fail1"), events::Log::Warning, "", 4938); + reporter.armingCheckFailure(NavModes::All, health_component_t::manual_control_input, + events::ID("arming_test_reporting_multiple_fail2"), events::Log::Warning, "", 123.f); + reporter.armingCheckFailure(NavModes::All, health_component_t::manual_control_input, + events::ID("arming_test_reporting_multiple_fail3"), events::Log::Warning, "", 55); + reporter.finalize(); + reporter.report(false, false); + ASSERT_FALSE(reporter.canArm(vehicle_status_s::NAVIGATION_STATE_POSCTL)); + + if (i == 0) { + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("commander_arming_check_summary")); + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("arming_test_reporting_multiple_fail1")); + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("arming_test_reporting_multiple_fail2")); + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("arming_test_reporting_multiple_fail3")); + ASSERT_TRUE(event_sub.update(&event)); + ASSERT_EQ(event.id, events::ID("commander_health_summary")); + + } else { + ASSERT_FALSE(event_sub.updated()); + } + } +} +