LCOV - code coverage report
Current view: top level - src/common/signal_filters/include/signal_filters - low_pass_filter.hpp (source / functions) Hit Total Coverage
Test: lcov.total.filtered Lines: 11 11 100.0 %
Date: 2023-03-03 05:44:19 Functions: 18 24 75.0 %
Legend: Lines: hit not hit | Branches: + taken - not taken # not executed Branches: 5 6 83.3 %

           Branch data     Line data    Source code
       1                 :            : // Copyright 2020 the Autoware Foundation
       2                 :            : //
       3                 :            : // Licensed under the Apache License, Version 2.0 (the "License");
       4                 :            : // you may not use this file except in compliance with the License.
       5                 :            : // You may obtain a copy of the License at
       6                 :            : //
       7                 :            : //     http://www.apache.org/licenses/LICENSE-2.0
       8                 :            : //
       9                 :            : // Unless required by applicable law or agreed to in writing, software
      10                 :            : // distributed under the License is distributed on an "AS IS" BASIS,
      11                 :            : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      12                 :            : // See the License for the specific language governing permissions and
      13                 :            : // limitations under the License.
      14                 :            : //
      15                 :            : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
      16                 :            : /// \file
      17                 :            : /// \brief Interface definition for signal processing filters
      18                 :            : #ifndef SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_
      19                 :            : #define SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_
      20                 :            : 
      21                 :            : #include <signal_filters/signal_filter.hpp>
      22                 :            : #include <signal_filters/visibility_control.hpp>
      23                 :            : 
      24                 :            : #include <chrono>
      25                 :            : 
      26                 :            : namespace autoware
      27                 :            : {
      28                 :            : namespace common
      29                 :            : {
      30                 :            : namespace signal_filters
      31                 :            : {
      32                 :            : 
      33                 :            : /// Basic low pass filter implemented as an exponential moving average
      34                 :            : /// \tparam T A floating point type for the signal
      35                 :            : template<typename T, typename ClockT = std::chrono::steady_clock>
      36                 :            : class SIGNAL_FILTERS_PUBLIC LowPassFilter : public FilterBase<T, ClockT>
      37                 :            : {
      38                 :            : public:
      39         [ +  + ]:         24 :   explicit LowPassFilter(T cutoff_frequency_hz)
      40                 :         24 :   : FilterBase<T, ClockT>{}
      41                 :            :   {
      42         [ +  + ]:         24 :     if (T{} >= cutoff_frequency_hz) {
      43         [ +  - ]:          6 :       throw std::domain_error{"Cutoff frequency is non-positve"};
      44                 :            :     }
      45                 :            :     constexpr T TAU{static_cast<T>(2.0 * 3.14159)};
      46                 :         18 :     m_rc_inv = TAU * cutoff_frequency_hz;
      47                 :         18 :   }
      48                 :            :   /// Destructor
      49                 :         18 :   virtual ~LowPassFilter() = default;
      50                 :            : 
      51                 :            : protected:
      52                 :       1206 :   T filter_impl(T value, std::chrono::nanoseconds duration) override
      53                 :            :   {
      54                 :            :     const auto dt = std::chrono::duration_cast<std::chrono::duration<T>>(duration).count();
      55                 :            :     // From https://stackoverflow.com/a/1027808
      56                 :       1206 :     const auto alpha = T{1.0} - std::exp(-dt * m_rc_inv);
      57                 :       1206 :     m_signal += alpha * (value - m_signal);
      58                 :       1206 :     return m_signal;
      59                 :            :   }
      60                 :            : 
      61                 :            : private:
      62                 :            :   T m_rc_inv{};
      63                 :            :   T m_signal{};
      64                 :            : };
      65                 :            : }  // namespace signal_filters
      66                 :            : }  // namespace common
      67                 :            : }  // namespace autoware
      68                 :            : 
      69                 :            : #endif  // SIGNAL_FILTERS__LOW_PASS_FILTER_HPP_

Generated by: LCOV version 1.14