tlbmc/thermal/controller/algorithm/dff.cc

#include "tlbmc/thermal/controller/algorithm/dff.h"

#include <algorithm>
#include <limits>

namespace milotic_tlbmc {
namespace thermal {

double DffThermalLoop::ApplyLowPassFilterToDisturbance(
    double disturbance) const {
  double filtered_disturbance = GetCoeffH1() * disturbance +
                                GetCoeffH2() * GetLastDisturbance() +
                                GetCoeffH3() * GetLastFilteredDisturbance();

  return filtered_disturbance;
}

double DffThermalLoop::ExecuteDffLoop(double disturbance) {
  // Apply low pass filter on the disturbance measurement to attenuate
  // high-frequency components that the feedforward controller cannot
  // effectively handle.
  double filtered_disturbance = ApplyLowPassFilterToDisturbance(disturbance);

  double dff_value =
      GetCoeffF1() * (filtered_disturbance - GetReferencePower()) +
      GetCoeffF2() * (GetLastFilteredDisturbance() - GetReferencePower()) +
      GetCoeffF3() * GetLastDffValue();

  SetLastDisturbance(disturbance);
  SetLastFilteredDisturbance(filtered_disturbance);
  SetLastDffValue(dff_value);

  double output = dff_value;

  // Apply slew rate clamping
  double min_output = std::numeric_limits<double>::lowest();
  double max_output = std::numeric_limits<double>::max();
  if (GetSlewNeg() < -1.0) {
    min_output = GetLastOutput() + GetSlewNeg() * GetSampleTime();
  }
  if (GetSlewPos() > 1.0) {
    max_output = GetLastOutput() + GetSlewPos() * GetSampleTime();
  }
  output = std::clamp(output, min_output, max_output);

  SetLastOutput(output);

  return output;
}

}  // namespace thermal
}  // namespace milotic_tlbmc