mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-20 11:23:06 +08:00
hysteresis: move out of systemlib, move to gtest
This moves the hysteresis test out of the systemlib and makes it its own small library. Since it still depends on hrt_absolute_time this does not link yet. My attempt to get all link dependencies together failed.
This commit is contained in:
@@ -15,7 +15,6 @@ set(tests
|
||||
file2
|
||||
float
|
||||
hrt
|
||||
hysteresis
|
||||
int
|
||||
IntrusiveQueue
|
||||
List
|
||||
@@ -40,7 +39,6 @@ set(tests
|
||||
|
||||
if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
|
||||
list(REMOVE_ITEM tests
|
||||
hysteresis
|
||||
mixer
|
||||
uorb
|
||||
)
|
||||
@@ -48,7 +46,6 @@ endif()
|
||||
|
||||
if (CMAKE_SYSTEM_NAME STREQUAL "CYGWIN")
|
||||
list(REMOVE_ITEM tests
|
||||
hysteresis # Intermittent timing fails.
|
||||
uorb
|
||||
)
|
||||
endif()
|
||||
|
||||
@@ -45,6 +45,7 @@ add_subdirectory(conversion)
|
||||
add_subdirectory(drivers)
|
||||
add_subdirectory(ecl)
|
||||
add_subdirectory(FlightTasks)
|
||||
add_subdirectory(hysteresis)
|
||||
add_subdirectory(landing_slope)
|
||||
add_subdirectory(led)
|
||||
add_subdirectory(mathlib)
|
||||
|
||||
@@ -50,4 +50,4 @@ px4_add_library(cdev
|
||||
|
||||
if(PX4_TESTING)
|
||||
add_subdirectory(test)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
@@ -0,0 +1,36 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2019 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_library(hysteresis hysteresis.cpp)
|
||||
|
||||
px4_add_gtest(SRC hysteresis_test.cpp LINKLIBS hysteresis)
|
||||
@@ -32,89 +32,56 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file test_hysteresis.cpp
|
||||
* @file hysteresis_test.cpp
|
||||
* Tests for system timing hysteresis.
|
||||
*/
|
||||
|
||||
#include <unit_test.h>
|
||||
#include <gtest/gtest.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <systemlib/hysteresis/hysteresis.h>
|
||||
#include "hysteresis.h"
|
||||
|
||||
class HysteresisTest : public UnitTest
|
||||
{
|
||||
public:
|
||||
virtual bool run_tests();
|
||||
|
||||
private:
|
||||
bool _init_false();
|
||||
bool _init_true();
|
||||
bool _zero_case();
|
||||
bool _change_after_time();
|
||||
bool _hysteresis_changed();
|
||||
bool _change_after_multiple_sets();
|
||||
bool _take_change_back();
|
||||
|
||||
// timing on MacOS and Cygwin isn't great
|
||||
#if defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
|
||||
static const int f = 10;
|
||||
static const int f = 10;
|
||||
#else
|
||||
static const int f = 1;
|
||||
static const int f = 1;
|
||||
#endif
|
||||
};
|
||||
|
||||
bool HysteresisTest::run_tests()
|
||||
{
|
||||
ut_run_test(_init_false);
|
||||
ut_run_test(_init_true);
|
||||
ut_run_test(_zero_case);
|
||||
ut_run_test(_change_after_time);
|
||||
ut_run_test(_hysteresis_changed);
|
||||
ut_run_test(_change_after_multiple_sets);
|
||||
ut_run_test(_take_change_back);
|
||||
|
||||
return (_tests_failed == 0);
|
||||
}
|
||||
|
||||
bool HysteresisTest::_init_false()
|
||||
TEST(Hysteresis, InitFalse)
|
||||
{
|
||||
systemlib::Hysteresis hysteresis(false);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
bool HysteresisTest::_init_true()
|
||||
TEST(Hysteresis, InitTrue)
|
||||
{
|
||||
systemlib::Hysteresis hysteresis(true);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
bool HysteresisTest::_zero_case()
|
||||
TEST(Hysteresis, Zero)
|
||||
{
|
||||
// Default is 0 hysteresis.
|
||||
systemlib::Hysteresis hysteresis(false);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
|
||||
// Change and see result immediately.
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
|
||||
// A wait won't change anything.
|
||||
px4_usleep(1000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
bool HysteresisTest::_change_after_time()
|
||||
TEST(Hysteresis, ChangeAfterTime)
|
||||
{
|
||||
|
||||
systemlib::Hysteresis hysteresis(false);
|
||||
@@ -123,28 +90,26 @@ bool HysteresisTest::_change_after_time()
|
||||
|
||||
// Change to true.
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(4000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(2000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
|
||||
// Change back to false.
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
px4_usleep(1000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
bool HysteresisTest::_hysteresis_changed()
|
||||
TEST(Hysteresis, HysteresisChanged)
|
||||
{
|
||||
systemlib::Hysteresis hysteresis(false);
|
||||
hysteresis.set_hysteresis_time_from(true, 2000 * f);
|
||||
@@ -152,31 +117,29 @@ bool HysteresisTest::_hysteresis_changed()
|
||||
|
||||
// Change to true.
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
|
||||
// Change hysteresis time.
|
||||
hysteresis.set_hysteresis_time_from(true, 10000 * f);
|
||||
|
||||
// Change back to false.
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
px4_usleep(7000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
px4_usleep(5000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
bool HysteresisTest::_change_after_multiple_sets()
|
||||
TEST(Hysteresis, ChangeAfterMultipleSets)
|
||||
{
|
||||
systemlib::Hysteresis hysteresis(false);
|
||||
hysteresis.set_hysteresis_time_from(true, 5000 * f);
|
||||
@@ -184,60 +147,54 @@ bool HysteresisTest::_change_after_multiple_sets()
|
||||
|
||||
// Change to true.
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
|
||||
// Change to false.
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
bool HysteresisTest::_take_change_back()
|
||||
TEST(Hysteresis, TakeChangeBack)
|
||||
{
|
||||
systemlib::Hysteresis hysteresis(false);
|
||||
hysteresis.set_hysteresis_time_from(false, 5000 * f);
|
||||
|
||||
// Change to true.
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
// Change your mind to false.
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(6000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
|
||||
// And true again
|
||||
hysteresis.set_state_and_update(true);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
px4_usleep(3000 * f);
|
||||
hysteresis.update();
|
||||
ut_assert_true(hysteresis.get_state());
|
||||
EXPECT_TRUE(hysteresis.get_state());
|
||||
|
||||
// The other directory is immediate.
|
||||
hysteresis.set_state_and_update(false);
|
||||
ut_assert_false(hysteresis.get_state());
|
||||
|
||||
return true;
|
||||
EXPECT_FALSE(hysteresis.get_state());
|
||||
}
|
||||
|
||||
ut_declare_test_c(test_hysteresis, HysteresisTest)
|
||||
@@ -35,7 +35,6 @@ set(SRCS
|
||||
conversions.c
|
||||
cpuload.c
|
||||
crc.c
|
||||
hysteresis/hysteresis.cpp
|
||||
mavlink_log.c
|
||||
otp.c
|
||||
)
|
||||
|
||||
@@ -62,6 +62,7 @@ px4_add_module(
|
||||
failure_detector
|
||||
git_ecl
|
||||
ecl_geo
|
||||
hysteresis
|
||||
)
|
||||
|
||||
if(PX4_TESTING)
|
||||
|
||||
@@ -41,7 +41,7 @@
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <px4_module.h>
|
||||
#include <px4_module_params.h>
|
||||
#include <systemlib/hysteresis/hysteresis.h>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
|
||||
// publications
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
@@ -43,5 +43,6 @@ px4_add_module(
|
||||
VtolLandDetector.cpp
|
||||
RoverLandDetector.cpp
|
||||
DEPENDS
|
||||
hysteresis
|
||||
)
|
||||
|
||||
|
||||
@@ -44,7 +44,7 @@
|
||||
|
||||
#include <px4_workqueue.h>
|
||||
#include <px4_module.h>
|
||||
#include <systemlib/hysteresis/hysteresis.h>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <parameters/param.h>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <uORB/uORB.h>
|
||||
|
||||
@@ -43,7 +43,7 @@
|
||||
#include <px4_module.h>
|
||||
#include <px4_posix.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <systemlib/hysteresis/hysteresis.h>
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
#include <commander/px4_custom_mode.h>
|
||||
|
||||
#include <uORB/topics/home_position.h>
|
||||
|
||||
@@ -44,7 +44,6 @@ set(srcs
|
||||
test_float.cpp
|
||||
test_hott_telemetry.c
|
||||
test_hrt.cpp
|
||||
test_hysteresis.cpp
|
||||
test_int.cpp
|
||||
test_IntrusiveQueue.cpp
|
||||
test_jig_voltages.c
|
||||
|
||||
@@ -93,7 +93,6 @@ const struct {
|
||||
{"float", test_float, 0},
|
||||
{"hott_telemetry", test_hott_telemetry, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"hrt", test_hrt, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"hysteresis", test_hysteresis, 0},
|
||||
{"int", test_int, 0},
|
||||
{"IntrusiveQueue", test_IntrusiveQueue, 0},
|
||||
{"jig_voltages", test_jig_voltages, OPT_NOALLTEST},
|
||||
|
||||
@@ -55,7 +55,6 @@ extern int test_file2(int argc, char *argv[]);
|
||||
extern int test_float(int argc, char *argv[]);
|
||||
extern int test_hott_telemetry(int argc, char *argv[]);
|
||||
extern int test_hrt(int argc, char *argv[]);
|
||||
extern int test_hysteresis(int argc, char *argv[]);
|
||||
extern int test_int(int argc, char *argv[]);
|
||||
extern int test_IntrusiveQueue(int argc, char *argv[]);
|
||||
extern int test_jig_voltages(int argc, char *argv[]);
|
||||
|
||||
Reference in New Issue
Block a user