cblock.cpp

Go to the documentation of this file.

// Copyright (c) 2024 anabrid GmbH
// Contact: https://www.anabrid.com/licensing/
// SPDX-License-Identifier: MIT OR GPL-2.0-or-later
 
#include <block/cblock.h>
#include <cstring>
#include <entity/visitor.h>
#include <utils/logging.h>
 
blocks::CBlock::CBlock(CBlockHAL *hardware) : FunctionBlock("C", hardware), hardware(hardware) {
  classifier.class_enum = CLASS_;
}
 
blocks::CBlock::CBlock() : CBlock(new CBlockHALDummy()) {}
 
float blocks::CBlock::get_factor(uint8_t idx) {
  if (idx >= NUM_COEFF)
    return 0.0f;
  return factors_[idx];
}
 
const std::array<float, blocks::CBlock::NUM_COEFF> &blocks::CBlock::get_factors() const {
  return factors_;
}
 
UnitResult blocks::CBlock::set_factor(uint8_t idx, float factor) {
  if (idx >= NUM_COEFF)
    return UnitResult::err_fmt("Error c-block factor idx to high %d", idx);
  if (factor > MAX_FACTOR or factor < MIN_FACTOR)
    return UnitResult::err_fmt("Error c-block factor outside range %f", factor);
  factors_[idx] = factor;
  return UnitResult::ok();
}
 
void blocks::CBlock::set_factors(const std::array<float, NUM_COEFF> &factors) { factors_ = factors; }
 
UnitResult blocks::CBlock::write_to_hardware() {
  if (!write_factors_to_hardware()) {
    LOG(ANABRID_PEDANTIC, __PRETTY_FUNCTION__);
    return UnitResult::err("Writing to c-block factors failed");
  }
  return UnitResult::ok();
}
 
bool blocks::CBlock::write_factors_to_hardware() {
  for (size_t i = 0; i < factors_.size(); i++) {
    auto factor = factors_[i] * gain_corrections_[i];
    if (!hardware->write_factor(i, factor))
      return false;
  }
  return true;
}
 
void blocks::CBlock::reset(entities::ResetAction action) {
  FunctionBlock::reset(action);
 
  if (action.has(entities::ResetAction::CIRCUIT_RESET))
    for (size_t i = 0; i < NUM_COEFF; i++)
      (void)set_factor(i, 1.0f);
 
  if (action.has(entities::ResetAction::CALIBRATION_RESET))
    reset_gain_corrections();
}
 
float blocks::CBlock::get_gain_correction(uint8_t idx) const {
  if (idx > NUM_COEFF)
    return -2.0f;
  return gain_corrections_[idx];
}
 
const std::array<float, blocks::CBlock::NUM_COEFF> &blocks::CBlock::get_gain_corrections() const {
  return gain_corrections_;
}
 
void blocks::CBlock::reset_gain_corrections() {
  std::fill(gain_corrections_.begin(), gain_corrections_.end(), 1.0f);
}
 
UnitResult blocks::CBlock::set_gain_corrections(const std::array<float, NUM_COEFF> &corrections) {
  for (size_t idx = 0; idx < NUM_COEFF; idx++) {
    TRY(set_gain_correction(idx, corrections[idx]));
  }
  return UnitResult::ok();
}
 
UnitResult blocks::CBlock::set_gain_correction(const uint8_t coeff_idx, const float correction) {
  if (coeff_idx > NUM_COEFF)
    return UnitResult::err_fmt("Error c-block coef idx out of range: %d", coeff_idx);
  // Gain correction must be positive and close to 1
  if (correction < 0.0f || correction > MAX_GAIN_CORRECTION_ABS + 1.0f)
    return UnitResult::err_fmt("Error c-block coef gain correction out of range: %d %f", coeff_idx, correction);
 
  gain_corrections_[coeff_idx] = correction;
  return UnitResult::ok();
};
 
ConfigResult blocks::CBlock::config(const pb_Item &item) {
#ifdef ANABRID_DEBUG_ENTITY_CONFIG
  Serial.println(__PRETTY_FUNCTION__);
#endif
  if (item.which_kind != pb_Item_coef_config_tag)
    return ConfigResult::err("expected coef block config");
 
  auto& coef_config = item.kind.coef_config;
  return _apply_elements(coef_config);
}
 
ConfigResult blocks::CBlock::_apply_elements(const pb_CoefConfig &item) {
  // Handle an array of factors
  for (size_t elem_idx = 0; elem_idx < item.elements_count; ++elem_idx) {
    auto& element = item.elements[elem_idx];
    TRY(set_factor(element.idx, element.factor));
    if (element.has_gain_correction )
      TRY(set_gain_correction(element.idx, element.gain_correction));
  }
 
  return ConfigResult::ok(true);
}
 
void blocks::CBlock::extract(entities::ExtractVisitor &collector) {
 
  if (!collector.include_configuration() && !collector.include_calibration()) return;
  auto& item = collector.create(pb_Item_coef_config_tag);
  auto& coef_config = item.kind.coef_config;
  coef_config.elements_count = NUM_COEFF;
  auto& idx = coef_config.elements_count = 0;
  for (;idx < NUM_COEFF; ++idx) {
    auto& elem = coef_config.elements[idx];
    elem.idx = idx;
 
    if (collector.include_configuration()) {
      elem.factor = factors_[idx];
    }
 
    if (collector.include_calibration()) {
      elem.has_gain_correction = true;
      elem.gain_correction = gain_corrections_[idx];
    }
  }
}