util.h

Go to the documentation of this file.

#pragma once
 
#ifdef LINUX_BUILD
#include "dl_syscalls.h"
#endif
 
#include <chrono>
#include <cmath>
#include <deque>
#include <future>
#include <list>
#include <nlohmann/json.hpp>
#include <numeric>
#include <semver.hpp>
#include <string>
#include <thread>
 
#include "cia402_drive.h"
#include "motion-master.pb.h"
#include "notifier.h"
#include "standalone_autotuning.h"
#include "virtual_device.h"
 
const char* const HARDWARE_DESCRIPTION_FILENAME = ".hardware_description";
const char* const STACK_INFO_FILENAME = "stack_info.json";
const char* const STACK_IMAGE_FILENAME = "stack_image.svg";
const char* const ESI_FILENAME = "SOMANET_CiA_402.xml";
const char* const CONFIG_FILENAME = "config.csv";
const char* const PLANT_MODEL_FILENAME = "plant_model.csv";
const char* const VARIANT_FILENAME = ".variant";
 
struct PlantModel {
  std::vector<double> numerator;
  std::vector<double> denominator;
  double start_frequency = 0.0;   // Hz
  double end_frequency = 0.0;     // Hz
  uint16_t torque_amplitude = 0;  // ‰ of rated torque
  double duration_seconds = 0.0;  // seconds
  uint8_t signal_type = 0;        // 0 - logarithmic, 1 - linear
  nlohmann::json to_json() {
    nlohmann::json j;
    j["numerator"] = numerator;
    j["denominator"] = denominator;
    j["start_frequency"] = start_frequency;
    j["end_frequency"] = end_frequency;
    j["torque_amplitude"] = torque_amplitude;
    j["duration_seconds"] = duration_seconds;
    j["signal_type"] = signal_type;
    return j;
  }
  std::string bode_file_content;
};
 
enum class ControllerType { kPiP, kPpi };
 
std::string controllerTypeToString(ControllerType ct);
 
enum class CirculoType {
  kUnspecified,
  kCirculo7,
  kCirculo7Smm,
  kCirculo9,
  kCirculo9Smm
};
 
enum class CirculoEncoderType {
  kUndefined = 0,
  kCirculo7Inner,
  kCirculo7Outer,
  kCirculo9Inner,
  kCirculo9Outer
};
 
enum class EncoderLocation { kUnspecified, kMotorShaft, kDrivingShaft };
 
#define JSON_TO_DOUBLE(jx) jx.is_number() ? static_cast<double>(jx) : 0.0
 
// clang-format off
        typedef struct AutoTuningDriveConfigurationClass {
            uint8_t vel_lp_type = 1;  // 0 - disabled, 1 - 1st order, 2 - 2nd order (0x2021, 1)
            uint32_t vel_lp_fc = 300;  // [Hz] (0x2021, 2)
            uint8_t pos_lp_type = 1;  // 0 - disabled, 1 - 1st order, 2 - 2nd order (0x2022, 1)
            uint32_t pos_lp_fc = 300;   // [Hz] (0x2022, 2)
            bool notch_en = true;       // Enabled / disabled (0x2023, 1)
            uint16_t notch_fw = 10;     // [Hz] (0x2023, 3)
            uint16_t notch_fc = 1000;   // [Hz] (0x2023, 2)
            int16_t vel_ff_gain = 0;    // Permill, 0 - disabled (0x2015, 1)
            uint16_t vel_ff_fc = 1000;  // [Hz] (0x2015, 2)
            int32_t L = 200;            // Inductance, [μH] (0x2003, 4)
            int32_t R = 300000;         // Resistance, [μOhm] (0x2003, 3)
            uint32_t vdc = 48000;       // DC voltage, [mV] (0x6079, 0)
            double current_ratio = 16384;   // ADC conversion factor, [mA / inc] (from BSP file)
            double tc_kp = 2500;   // kp gain from FW v4.x.x, [-] (0x2010, 1)
            double tc_ki = 40000;  // ki gain from FW v4.x.x, [-] (0x2010, 2)
            uint8_t switch_F = 0;  // Switching frequency: 0 - 16kHz, 1 - 32kHz, 2 - 64kHz (0x2010, 9)
            uint16_t tc_st = 1000;  // Torque controller settling time, FW v5.x.x, [us] (0x2010, 10)
            uint16_t tc_damping = 2000;  // Torque controller damping ratio, FW v5.x.x, [permill] (0x2010, 11)
            std::string software_version = "v4.4.1";  // FW version
            unsigned int rated_torque;  // Motor rated torque, [mNm]
            double max_vel_noise;  // Max value of the pre-recorded encoder noise, [RPM]
            nlohmann::json to_json() {
                nlohmann::json j;
                j["vel_lp_type"] = vel_lp_type;
                j["vel_lp_fc"] = vel_lp_fc;
                j["pos_lp_type"] = pos_lp_type;
                j["pos_lp_fc"] = pos_lp_fc;
                j["notch_en"] = notch_en;
                j["notch_fw"] = notch_fw;
                j["notch_fc"] = notch_fc;
                j["vel_ff_gain"] = vel_ff_gain;
                j["vel_ff_fc"] = vel_ff_fc;
                j["L"] = L;
                j["R"] = R;
                j["vdc"] = vdc;
                j["current_ratio"] = current_ratio;
                j["tc_kp"] = tc_kp;
                j["tc_ki"] = tc_ki;
                j["switch_F"] = switch_F;
                j["tc_st"] = tc_st;
                j["tc_damping"] = tc_damping;
                j["software_version"] = software_version;
                j["rated_torque"] = rated_torque;
                j["max_vel_noise"] = max_vel_noise;
                return j;
            }
        } AutoTuningDriveConfiguration;
// clang-format on
 
typedef struct SystemParametersClass {
  double time_step = 0.001;
  uint32_t nominal_torque = 0;
  uint32_t max_torque_mnm_ = 0;
  uint32_t singleturn_resolution = 0;
} SystemParameters;
 
typedef struct TuningParametersClass {
  // Chirp parameters
  double duration_seconds = 5.0;
  uint16_t torque_amplitude = 500;
  double start_frequency = 3.0;
  double end_frequency = 60.0;
  double cutoff_frequency = 30.0;
 
  // Position auto-tuning parameters
  ControllerType controller_type = ControllerType::kPpi;
  double settling_time = 0.2;
  double position_damping = 2.0;
  int16_t alpha_mult = 1;
  uint8_t order = 4;
 
  // Velocity auto-tuning parameters
  float velocity_loop_bandwidth = 5.0f;
  float velocity_damping = 0.7f;
 
  // Tuning sharpness limiter (nominal torque * warn_torque_const)
  double warn_torque_const = 3.0;
 
  // Position auto-tuning optimization boundaries
  double lb = 2.0;
  double ub = 314.0;
} TuningParameters;
 
enum class PositionControlStrategy { kNone = 0, kSimple, kCascaded };
 
typedef enum MotionMasterWarning : unsigned int {
  WAR_NONE = 0,
  WAR_PROFILE_TARGET_LIMITED,
  WAR_INTERNAL_LIMIT_REACHED
} MotionMasterWarning;
 
const std::map<MotionMasterWarning, std::string>
    motion_master_warning_to_message_map_ = {
        {WAR_NONE, "No warning!"},
        {WAR_PROFILE_TARGET_LIMITED,
         "Slave profile: The amplitude is outside of the configured limits - "
         "using a limit value as the new amplitude!"},
        {WAR_INTERNAL_LIMIT_REACHED, "Internal limit reached!"}};
 
const std::map<int32_t, std::string> slave_error_map_ = {
    {0x2110, "Short circuit/earth leakage (input)"},
    {0x2120, "Earth leakage (input)"},
    {0x2121, "Earth leakage phase L1"},
    {0x2122, "Earth leakage phase L2"},
    {0x2123, "Earth leakage phase L3"},
    {0x2130, "Short circuit (input)"},
    {0x2131, "Short circuit phases L1-L2"},
    {0x2132, "Short circuit phases L2-L3"},
    {0x2133, "Short circuit phases L3-L1"},
    {0x2211, "Internal current no.1"},
    {0x2212, "Internal current no.2"},
    {0x2213, "Over-current in ramp function"},
    {0x2214, "Over-current in the sequence"},
    {0x2220, "Continuous over current (device internal)"},
    {0x2221, "Continuous over current no.1"},
    {0x2222, "Continuous over current no.2"},
    {0x2230, "Short circuit/earth leakage (device internal)"},
    {0x2240, "Earth leakage (device internal)"},
    {0x2250, "Short circuit (device internal)"},
    {0x2310, "Continuous over current"},
    {0x2311, "Continuous over current no.1"},
    {0x2312, "Continuous over current no.2"},
    {0x2320, "Short circuit/earth leakage (motor-side)"},
    {0x2330, "Earth leakage (motor-side)"},
    {0x2331, "Earth leakage phase U"},
    {0x2332, "Earth leakage phase V"},
    {0x2333, "Earth leakage phase W"},
    {0x2340, "Short circuit (motor-side)"},
    {0x2341, "Short circuit phases U-V"},
    {0x2342, "Earth leakage phase V-W"},
    {0x2343, "Earth leakage phase W-U"},
    {0x2350, "Load level fault (I2t, thermal state)"},
    {0x2351, "Load level warning (I2t, thermal state)"},
    {0x3110, "Mains overvoltage"},
    {0x3111, "Mains overvoltage phase L1"},
    {0x3112, "Mains overvoltage phase L2"},
    {0x3113, "Mains overvoltage phase L3"},
    {0x3120, "Mains undervoltage"},
    {0x3121, "Mains undervoltage phase L1"},
    {0x3122, "Mains undervoltage phase L2"},
    {0x3123, "Mains undervoltage phase L3"},
    {0x3130, "Phase failure"},
    {0x3131, "Phase failure L1"},
    {0x3132, "Phase failure L2"},
    {0x3133, "Phase failure L3"},
    {0x3134, "Phase sequence"},
    {0x3140, "Mains frequency"},
    {0x3141, "Mains frequency too great"},
    {0x3142, "Mains frequency too small"},
    {0x3210, "DC link overvoltage"},
    {0x3211, "Overvoltage no. 1"},
    {0x3212, "Overvoltage no. 2"},
    {0x3220, "DC link undervoltage"},
    {0x3221, "Undervoltage no. 1"},
    {0x3222, "Undervoltage no. 2"},
    {0x3230, "Load error"},
    {0x3310, "Output overvoltage"},
    {0x3311, "Output overvoltage phase U"},
    {0x3312, "Output overvoltage phase V"},
    {0x3313, "Output overvoltage phase W"},
    {0x3320, "Armature circuit"},
    {0x3321, "Armature circuit interrupted"},
    {0x3330, "Field circuit"},
    {0x3331, "Field circuit interrupted"},
    {0x4110, "Excess ambient temperature"},
    {0x4120, "Too low ambient temperature"},
    {0x4130, "Temperature supply air"},
    {0x4140, "Temperature air outlet"},
    {0x4210, "Excess temperature device"},
    {0x4220, "Too low temperature device"},
    {0x4300, "Temperature drive"},
    {0x4310, "Excess temperature drive"},
    {0x4320, "Too low temperature drive"},
    {0x4400, "Temperature supply"},
    {0x4410, "Excess temperature supply"},
    {0x4420, "Too low temperature supply"},
    {0x5100, "Supply"},
    {0x5110, "Supply low voltage"},
    {0x5111, "U1 = supply ±15V"},
    {0x5112, "U2 = supply +24 V"},
    {0x5113, "U3 = supply +5 V"},
    {0x5114, "U4 = manufacturer-specific"},
    {0x5115, "U5 = manufacturer-specific"},
    {0x5116, "U6 = manufacturer-specific"},
    {0x5117, "U7 = manufacturer-specific"},
    {0x5118, "U8 = manufacturer-specific"},
    {0x5119, "U9 = manufacturer-specific"},
    {0x5120, "Supply intermediate circuit"},
    {0x5200, "Control"},
    {0x5210, "Measurement circuit"},
    {0x5220, "Computing circuit"},
    {0x5300, "Operating unit"},
    {0x5400, "Power section"},
    {0x5410, "Output stages"},
    {0x5420, "Chopper"},
    {0x5430, "Input stages"},
    {0x5440, "Contacts"},
    {0x5441, "Contact 1 = manufacturer-specific"},
    {0x5442, "Contact 2 = manufacturer-specific"},
    {0x5443, "Contact 3 = manufacturer-specific"},
    {0x5444, "Contact 4 = manufacturer-specific"},
    {0x5445, "Contact 5 = manufacturer-specific"},
    {0x5450, "Fuses"},
    {0x5451, "S1 = l1"},
    {0x5452, "S2 = l2"},
    {0x5453, "S3 = l3"},
    {0x5454, "S4 = manufacturer-specific"},
    {0x5455, "S5 = manufacturer-specific"},
    {0x5456, "S6 = manufacturer-specific"},
    {0x5457, "S7 = manufacturer-specific"},
    {0x5458, "S8 = manufacturer-specific"},
    {0x5459, "S9 = manufacturer-specific"},
    {0x5500, "Hardware memory"},
    {0x5510, "RAM"},
    {0x5520, "ROM/EPROM"},
    {0x5530, "EEPROM"},
    {0x6010, "Software reset (watchdog)"},
    {0x6301, "to 630Fh  Data record no. 1 to no. 15"},
    {0x6310, "Loss of parameters"},
    {0x6320, "Parameter error"},
    {0x7100, "Power"},
    {0x7110, "Brake chopper"},
    {0x7111, "Failure brake chopper"},
    {0x7112, "Over current brake chopper"},
    {0x7113, "Protective circuit brake chopper"},
    {0x7120, "Motor"},
    {0x7121, "Motor blocked"},
    {0x7122, "Motor error or commutation malfunction."},
    {0x7123, "Motor tilted"},
    {0x7200, "Measurement circuit"},
    {0x7300, "Sensor"},
    {0x7301, "Tacho fault"},
    {0x7302, "Tacho wrong polarity"},
    {0x7303, "Resolver 1 fault"},
    {0x7304, "Resolver 2 fault"},
    {0x7305, "Incremental encoder 1 fault"},
    {0x7306, "Incremental encoder 2 fault"},
    {0x7307, "Incremental encoder 3 fault"},
    {0x7310, "Speed"},
    {0x7320, "Position"},
    {0x7400, "Computation circuit"},
    {0x7500, "Communication"},
    {0x7510, "Serial interface no. 1"},
    {0x7520, "Serial interface no. 2"},
    {0x7600, "Data storage (external)"},
    {0x8300, "Torque control"},
    {0x8311, "Excess torque"},
    {0x8312, "Difficult start up"},
    {0x8313, "Standstill torque"},
    {0x8321, "Insufficient torque"},
    {0x8331, "Torque fault"},
    {0x8400, "Velocity speed controller"},
    {0x8500, "Position controller"},
    {0x8600, "Positioning controller"},
    {0x8611, "Following error"},
    {0x8612, "Reference limit"},
    {0x8700, "Sync controller"},
    {0x8800, "Winding controller"},
    {0x8900, "Process data monitoring"},
    {0x8A00, "Control"},
    {0xF001, "Deceleration"},
    {0xF002, "Sub-synchronous run"},
    {0xF003, "Stroke operation"},
    {0xF004, "Control"},
    {0xFF03, "Warning - Excess temperature device"},
    {0xFF04, "Pull brake voltage higher than measured on DC bus"},
    {0xFF05, "Hold brake voltage higher than measured on DC bus"},
    {0xFF06, "Open circuit: High FET in phase A"},
    {0xFF07, "Open circuit: Low FET in phase A"},
    {0xFF08, "Open circuit: High FET in phase B"},
    {0xFF09, "Open circuit: Low FET in phase B"},
    {0xFF0A, "Open circuit: High FET in phase C"},
    {0xFF0B, "Open circuit: Low FET in phase C"},
    {0xFF0C, "Commutation offset is missing"}};
 
const std::map<int32_t, std::map<int32_t, int32_t>>
    adc_current_ratio_fw_v4_4_1_map = {
        {8500, {{01, 20480}, {02, 24576}, {03, 16384}}},
        {8501, {{01, 8192}, {02, 8192}, {03, 16384}}},
        {8502, {{01, 20480}, {02, 24576}, {03, 16384}}},
        {8503, {{01, 8192}, {02, 8192}, {03, 16384}}},
        {8504, {{01, 20480}, {02, 24576}, {03, 16384}}},
        {8505, {{01, 8192}, {02, 8192}, {03, 16384}}},
        {9500, {{00, 32768}, {01, 32768}, {02, 32768}}},
        {9501, {{01, 81920}, {02, 81920}}},
        {9502, {{01, 16384}}},
        {9506, {{01, 32768}}},
        {9507, {{01, 81920}}},
        {9508, {{01, 16384}}},
        {9600, {{01, 32768}}},
        {9601, {{01, 81920}}},
        {9602, {{01, 16384}}},
        {9606, {{01, 32768}}},
        {9607, {{01, 81920}}},
        {9608, {{01, 16384}}}};
 
const std::map<int32_t, std::map<int32_t, int32_t>>
    adc_current_ratio_fw_v4_4_2_map = {
        {8500, {{01, 40960}, {02, 49152}, {03, 32768}}},
        {8501, {{01, 16384}, {02, 16384}, {03, 16384}}},
        {8502, {{01, 40960}, {02, 49152}, {03, 32768}}},
        {8503, {{01, 16384}, {02, 16384}, {03, 16384}}},
        {8504, {{01, 40960}, {02, 49152}, {03, 32768}}},
        {8505, {{01, 16384}, {02, 16384}, {03, 16384}}},
        {9500, {{00, 32768}, {01, 32768}, {02, 32768}}},
        {9501, {{01, 81920}, {02, 81920}}},
        {9502, {{01, 16384}}},
        {9506, {{01, 32768}}},
        {9507, {{01, 81920}}},
        {9508, {{01, 16384}}},
        {9600, {{01, 32768}}},
        {9601, {{01, 81920}}},
        {9602, {{01, 16384}}},
        {9606, {{01, 32768}}},
        {9607, {{01, 81920}}},
        {9608, {{01, 16384}}}};
 
enum class EcatSlaveFoeError {
  kNotFound = 0x00008001,
  kAccessDenied = 0x00008002,
  kDiskFull = 0x00008003,
  kIllegal = 0x00008004,
  kPacketNumberWrong = 0x00008005,
  kAlreadyExists = 0x00008006,
  kNoUser = 0x00008007,
  kBootstrapOnly = 0x00008008,
  kNotBootstrap = 0x00008009,
  kNoRights = 0x0000800a,
  kProgramError = 0x0000800b
};

std::string get_master_warning_message(MotionMasterWarning warning);

std::string get_slave_error_message(int32_t error_code);
 
#ifdef __linux__
int32_t set_default_thread_priority();

int32_t set_max_thread_priority();
#endif

std::string etg1004_unit_string(uint32_t etg1004_unit_value);

int32_t get_pdo_id(uint16_t index, uint8_t subindex,
                   std::map<int32_t, int32_t>& pdo_position_map);
 
#ifdef __linux__
std::string execute_system_command(const char* cmd);
#endif

bool abs_compare(float a, float b);

template <typename T>
bool opposite_signs(const T& a, const T& b) {
  return ((a ^ b) >> (sizeof(T) * 8 - 1));
}

void interruptable_wait(size_t seconds);

int32_t wrap_target_position(int32_t target, int32_t max_limit,
                             int32_t min_limit);

uint32_t register_sync_signal();

void unregister_sync_signal(uint32_t signal_id);

uint64_t wait_for_sync_signal(uint32_t signal_id);
uint64_t wait_for_signal(int signal_fd);
uint64_t wait_for_signal(uint32_t signal_id);

std::vector<double> linspace(double lb, double ub, size_t n);

ControllerGains get_position_controller_gains(Cia402Drive& cia402_drive);

bool set_position_controller_gains(Cia402Drive& cia402_drive,
                                   const ControllerGains& gains);

bool set_velocity_controller_gains(Cia402Drive& cia402_drive,
                                   const ControllerGains& gains);

bool check_gains(Cia402Drive& cia402_drive);

bool string_ends_with(const std::string& value, const std::string& ending);

std::vector<uint8_t> read_file_content(const std::string& path);

void set_csv_configuration(VirtualDevice* virtual_device,
                           const std::vector<uint8_t>& csv_data);

uint8_t calculate_crc(const std::vector<uint8_t>& data);

uint32_t random_uint32(uint32_t min = 0, uint32_t max = UINT32_MAX);

unsigned char random_char();

std::string generate_uuid(size_t length);

int32_t limit_int32_target(int64_t target);

std::string run_env_info();

bool validate_sii_data(const std::vector<uint8_t>& content, uint32_t device_id);

bool file_exists(const std::string& path);

std::string string_bytes_to_hex_array_string(const std::string& bytes);

std::string get_firmware_version_string(Cia402Drive& cia402_drive);

semver::version get_firmware_version(Cia402Drive& cia402_drive);

bool get_plant_model_from_flash(Cia402Drive& cia402_drive, PlantModel* m);

AutoTuningDriveConfiguration get_auto_tuning_drive_configuration(
    Cia402Drive& cia402_drive);

ControllerGains convert_auto_tuning_controller_gains(
    ControllerGains gains, uint32_t singleturn_resolution);

CirculoType get_circulo_type(int32_t hardware_id);

bool is_fw_based_on_v5(Cia402Drive& cia402_drive);

bool is_fw_at_least(Cia402Drive& cia402_drive, const std::string& version);

std::vector<uint8_t> zip(const std::string& file_name,
                         std::vector<uint8_t> data);

std::vector<uint8_t> unzip(const std::vector<uint8_t>& data);

uint32_t generate_od_entry_map_key(uint16_t index, uint8_t subindex);

std::list<std::string> get_file_parts(std::list<std::string> file_list,
                                      const std::string& file_name);

EncoderLocation get_encoder_location(Cia402Drive& cia402_drive,
                                     uint8_t encoder_ordinal);

template <typename T>
int32_t find_local_min_index_from_right(std::vector<T>& x) {
  T min_value = x[x.size() - 1];
 
  for (int32_t i = x.size() - 2; i >= 0; i--) {
    if (x[i] <= min_value) {
      min_value = x[i];
    } else {
      return i + 1;
    }
  }
 
  return 1;
}

template <typename T>
double compute_mean(const std::vector<T>& numbers) {
  if (numbers.empty()) {
    return 0.0;
  }
 
  return std::accumulate(numbers.begin(), numbers.end(), 0.0) / numbers.size();
}

template <typename T>
double compute_standard_deviation(const std::vector<T>& numbers) {
  if (numbers.empty()) {
    return 0.0;
  }
 
  double mean = compute_mean(numbers);
 
  std::vector<double> tmp(numbers.size());
  std::transform(numbers.begin(), numbers.end(), tmp.begin(),
                 [mean](double x) { return x - mean; });
 
  double sq_sum = std::inner_product(tmp.begin(), tmp.end(), tmp.begin(), 0.0);
  return std::sqrt(sq_sum / (numbers.size() - 1));
}

template <typename T>
double compute_section_mean(const std::vector<T>& numbers, size_t start,
                            size_t end) {
  std::vector<T> section(numbers.begin() + start, numbers.begin() + end);
 
  if (section.empty()) {
    return 0.0;
  }
 
  return std::accumulate(section.begin(), section.end(), 0.0) / section.size();
}

template <typename T>
std::vector<T> erase_edge_elements(std::vector<T> v, size_t length) {
  v.erase(v.begin(), v.begin() + length);
  v.erase(v.end() - length, v.end());
  return v;
}

template <typename R>
bool is_future_done(std::future<R> const& f) {
  try {
    return f.wait_for(std::chrono::seconds(0)) == std::future_status::ready;
  } catch (std::future_error& e) {
    return true;  // The future is not running since there is an error
  }
}

template <typename T>
auto to_integral(T e) {
  return static_cast<std::underlying_type_t<T>>(e);
}

template <typename T>
static std::vector<T> diff(std::vector<T> vector) {
  std::vector<T> result;
  result.reserve(vector.size() - 1);
 
  for (size_t i = 1; i < vector.size(); i++) {
    result.push_back(vector[i] - vector[i - 1]);
  }
 
  return result;
}

template <typename... Args>
std::string string_format(const std::string& format, Args... args) {
  int size_s = std::snprintf(nullptr, 0, format.c_str(), args...) +
               1;  // Extra space for '\0'
  if (size_s <= 0) {
    throw std::runtime_error("Error during formatting.");
  }
  auto size = static_cast<size_t>(size_s);
  std::unique_ptr<char[]> buf(new char[size]);
  std::snprintf(buf.get(), size, format.c_str(), args...);
  return std::string(buf.get(),
                     buf.get() + size - 1);  // We don't want the '\0' inside
}

class LowPassFilter {
 public:
  LowPassFilter(uint16_t cutoff_frequency, double ts);
 
  double filter(double value);
 
 private:
  std::deque<double> input_data_;
  std::deque<double> output_data_;
  double fc_, a0_, a1_, a2_, b0_, b1_, b2_;
};

class HighPassFilter {
 public:
  HighPassFilter(uint16_t cutoff_frequency, double ts);
 
  double filter(double value);
 
 private:
  std::deque<double> input_data_;
  std::deque<double> output_data_;
  double fc_, a0_, a1_, a2_, a3_, a4_, b0_, b1_, b2_, b3_, b4_;
};
 
int get_soem_slaves_responding();
 
bool compareStringsCaseInsensitive(const std::string& s1,
                                   const std::string& s2);