diff --git a/src/modules/systemlib/hysteresis/hysteresis.cpp b/src/modules/systemlib/hysteresis/hysteresis.cpp new file mode 100644 index 0000000000..a04c21883c --- /dev/null +++ b/src/modules/systemlib/hysteresis/hysteresis.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +/** + * @file hysteresis.cpp + * + * @author Julian Oes + */ + +#include +#include "systemlib/hysteresis/hysteresis.h" + + +namespace systemlib { + + +void +Hysteresis::set_state_and_update(const bool new_state) +{ + if (new_state != _state) { + if (new_state != _requested_state) { + _requested_state = new_state; + _last_time_to_change_state = hrt_absolute_time(); + } + } else { + _requested_state = _state; + } + update(); +} + +void +Hysteresis::update() +{ + + + if (_requested_state != _state && + hrt_elapsed_time(&_last_time_to_change_state) >= _hysteresis_time_us) { + + _state = _requested_state; + } +} + +} // namespace systemlib diff --git a/src/modules/systemlib/hysteresis/hysteresis.h b/src/modules/systemlib/hysteresis/hysteresis.h new file mode 100644 index 0000000000..ba841071e8 --- /dev/null +++ b/src/modules/systemlib/hysteresis/hysteresis.h @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2016 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. + * + ****************************************************************************/ + +/** + * @file hysteresis.h + * + * Hysteresis of a boolean value. + * + * @author Julian Oes + */ + +#pragma once + +#include + +namespace systemlib { + +class Hysteresis +{ +public: + Hysteresis(unsigned hysteresis_time_us, bool init_state) : + _hysteresis_time_us(hysteresis_time_us), + _state(init_state), + _requested_state(init_state), + _last_time_to_change_state(0) + {} + + ~Hysteresis() + {} + + bool get_state() const { + return _state; + } + + void set_state_and_update(const bool new_state); + + void update(); + +private: + + unsigned _hysteresis_time_us; + bool _state; + + bool _requested_state; + hrt_abstime _last_time_to_change_state; +}; + +} // namespace systemlib diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index 53f3922dad..c6bf0b90be 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -173,6 +173,11 @@ add_executable(param_test param_test.cpp uorb_stub.cpp target_link_libraries(param_test ${PX4_SITL_BUILD}/libmsg_gen.a) add_gtest(param_test) +# hysteresis_test +add_executable(hysteresis_test hysteresis_test.cpp + ${PX4_SRC}/modules/systemlib/hysteresis/hysteresis.cpp) +add_gtest(hysteresis_test) + # param_shmem_test #add_executable(param_shmem_test param_test.cpp uorb_stub.cpp # ${PX4_SRC}/modules/systemlib/bson/tinybson.c diff --git a/unittests/hysteresis_test.cpp b/unittests/hysteresis_test.cpp new file mode 100644 index 0000000000..a01fc3f2e2 --- /dev/null +++ b/unittests/hysteresis_test.cpp @@ -0,0 +1,114 @@ +#include + +#include "gtest/gtest.h" + +const static unsigned hysteresis_time_us = 5000; + +TEST(HysteresisTest, InitFalse) +{ + systemlib::Hysteresis hysteresis(hysteresis_time_us, false); + ASSERT_FALSE(hysteresis.get_state()); +} + +TEST(HysteresisTest, InitTrue) +{ + systemlib::Hysteresis hysteresis(hysteresis_time_us, true); + ASSERT_TRUE(hysteresis.get_state()); +} + +TEST(HysteresisTest, ZeroCase) +{ + systemlib::Hysteresis hysteresis(0, false); + ASSERT_FALSE(hysteresis.get_state()); + + // Change and see result immediately. + hysteresis.set_state_and_update(true); + ASSERT_TRUE(hysteresis.get_state()); + hysteresis.set_state_and_update(false); + ASSERT_FALSE(hysteresis.get_state()); + hysteresis.set_state_and_update(true); + ASSERT_TRUE(hysteresis.get_state()); + + // A wait won't change anything. + usleep(1000); + hysteresis.update(); + ASSERT_TRUE(hysteresis.get_state()); +} + +TEST(HysteresisTest, ChangeAfterTime) +{ + systemlib::Hysteresis hysteresis(hysteresis_time_us, false); + + // Change to true. + hysteresis.set_state_and_update(true); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_TRUE(hysteresis.get_state()); + + // Change back to false. + hysteresis.set_state_and_update(false); + ASSERT_TRUE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_TRUE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_FALSE(hysteresis.get_state()); +} + +TEST(HysteresisTest, ChangeAfterTimeMultipleSets) +{ + systemlib::Hysteresis hysteresis(hysteresis_time_us, false); + + // Change to true. + hysteresis.set_state_and_update(true); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.set_state_and_update(true); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.set_state_and_update(true); + ASSERT_TRUE(hysteresis.get_state()); + + // Change to false. + hysteresis.set_state_and_update(false); + ASSERT_TRUE(hysteresis.get_state()); + usleep(3000); + hysteresis.set_state_and_update(false); + ASSERT_TRUE(hysteresis.get_state()); + usleep(3000); + hysteresis.set_state_and_update(false); + ASSERT_FALSE(hysteresis.get_state()); +} + +TEST(HysteresisTest, TakeChangeBack) +{ + systemlib::Hysteresis hysteresis(hysteresis_time_us, false); + + // Change to true. + hysteresis.set_state_and_update(true); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_FALSE(hysteresis.get_state()); + // Change your mind to false. + hysteresis.set_state_and_update(false); + ASSERT_FALSE(hysteresis.get_state()); + usleep(6000); + hysteresis.update(); + ASSERT_FALSE(hysteresis.get_state()); + + // And true again + hysteresis.set_state_and_update(true); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_FALSE(hysteresis.get_state()); + usleep(3000); + hysteresis.update(); + ASSERT_TRUE(hysteresis.get_state()); +}