fprime/Utils/RateLimiter.cpp

// ====================================================================== 
// \title  RateLimiter.cpp
// \author vwong
// \brief  cpp file for a rate limiter utility class
//
// \copyright
// Copyright (C) 2009-2020 California Institute of Technology.
// ALL RIGHTS RESERVED. United States Government Sponsorship
// acknowledged.
// ====================================================================== 

#include <Utils/RateLimiter.hpp>

namespace Utils {

  RateLimiter ::
    RateLimiter (
        U32 counterCycle,
        U32 timeCycle
    ) :
      m_counterCycle(counterCycle),
      m_timeCycle(timeCycle)
  {
    this->reset();
  }

  RateLimiter ::
    RateLimiter () :
      m_counterCycle(0),
      m_timeCycle(0)
  {
    this->reset();
  }

  void RateLimiter ::
    setCounterCycle(
        U32 counterCycle
    )
  {
    this->m_counterCycle = counterCycle;
  }

  void RateLimiter ::
    setTimeCycle(
        U32 timeCycle
    )
  {
    this->m_timeCycle = timeCycle;
  }

  void RateLimiter ::
    reset()
  {
    this->resetCounter();
    this->resetTime();
  }

  void RateLimiter ::
    resetCounter()
  {
    this->m_counter = 0;
  }

  void RateLimiter ::
    resetTime()
  {
    this->m_time = Fw::Time();
    this->m_timeAtNegativeInfinity = true;
  }

  void RateLimiter ::
    setCounter(
        U32 counter
    )
  {
    this->m_counter = counter;
  }

  void RateLimiter ::
    setTime(
        Fw::Time time
    )
  {
    this->m_time = time;
    this->m_timeAtNegativeInfinity = false;
  }

  bool RateLimiter ::
    trigger(
        Fw::Time time
    )
  {
    // NB: this implements a 4-bit decision, logically equivalent to this pseudo-code
    //
    // A = HAS_COUNTER, B = HAS_TIME, C = COUNTER_TRIGGER, D = TIME_TRIGGER
    //
    // if (!A && !B) => true
    // if (A && B) => C || D
    // if (A) => C
    // if (B) => D
    // false
    //
    if (this->m_counterCycle == 0 && this->m_timeCycle == 0) {
      return true;
    }

    // evaluate trigger criteria
    bool shouldTrigger = false;
    if (this->m_counterCycle > 0) {
      shouldTrigger = shouldTrigger || this->shouldCounterTrigger();
    }
    if (this->m_timeCycle > 0) {
      shouldTrigger = shouldTrigger || this->shouldTimeTrigger(time);
    }

    // update states
    if (this->m_counterCycle > 0) {
      this->updateCounter(shouldTrigger);
    }
    if (this->m_timeCycle > 0) {
      this->updateTime(shouldTrigger, time);
    }

    return shouldTrigger;
  }

  bool RateLimiter ::
    trigger()
  {
    FW_ASSERT(this->m_timeCycle == 0);
    return trigger(Fw::Time::zero());
  }

  bool RateLimiter ::
    shouldCounterTrigger()
  {
    FW_ASSERT(this->m_counterCycle > 0);

    // trigger at 0
    bool shouldTrigger = (this->m_counter == 0);

    return shouldTrigger;
  }

  bool RateLimiter ::
    shouldTimeTrigger(Fw::Time time)
  {
    FW_ASSERT(this->m_timeCycle > 0);

    // trigger at prev trigger time + time cycle seconds OR when time is at negative infinity
    Fw::Time timeCycle = Fw::Time(this->m_timeCycle, 0);
    Fw::Time nextTrigger = Fw::Time::add(this->m_time, timeCycle);
    bool shouldTrigger = (time >= nextTrigger) || this->m_timeAtNegativeInfinity;

    return shouldTrigger;
  }

  void RateLimiter ::
    updateCounter(bool triggered)
  {
    FW_ASSERT(this->m_counterCycle > 0);

    if (triggered) {
      // triggered, set to next state
      this->m_counter = 1;

    } else {
      // otherwise, just increment and maybe wrap
      if (++this->m_counter >= this->m_counterCycle) {
        this->m_counter = 0;
      }
    }
  }

  void RateLimiter ::
    updateTime(bool triggered, Fw::Time time)
  {
    FW_ASSERT(this->m_timeCycle > 0);

    if (triggered) {
      // mark time of trigger
      this->m_time = time;
    }
    this->m_timeAtNegativeInfinity = false;
  }

} // end namespace Utils