mblock_int.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 <algorithm>
#include <bitset>
 
#include "block/mblock_int.h"
#include "utils/logging.h"
 
#include "entity/entity.h"
#include "etl/crc.h"
 
#include "carrier/carrier.h"
#include "carrier/cluster.h"
#include "mode/mode.h"
 
extern int abs_clamp(float in, int min, int max);
 
FLASHMEM metadata::eui_t blocks::MIntBlock::get_entity_eui() const {
  if (hardware)
    return hardware->get_entity_eui();
  return {};
}
 
FLASHMEM blocks::MIntBlock::MIntBlock(SLOT slot, MIntBlockHAL *hardware)
    : blocks::MBlock{slot, hardware}, hardware(hardware), ic_values{}, time_factors{} {
  classifier.type = static_cast<uint8_t>(TYPE);
  reset_ic_values();
  reset_time_factors();
}
 
FLASHMEM bool blocks::MIntBlock::init() {
  if (!FunctionBlock::init())
    return false;
  if (!hardware->init())
    return false;
  return true;
}
 
FLASHMEM const std::array<float, 8> &blocks::MIntBlock::get_ic_values() const { return ic_values; }
 
FLASHMEM float blocks::MIntBlock::get_ic_value(uint8_t idx) const {
  if (idx >= ic_values.size())
    return 0.0f;
  return ic_values[idx];
}
 
FLASHMEM bool blocks::MIntBlock::set_ic_values(const std::array<float, 8> &ic_values_) {
  for (auto idx = 0u; idx < ic_values_.size(); idx++)
    if (!set_ic_value(idx, ic_values_[idx]))
      return false;
  return true;
}
 
FLASHMEM bool blocks::MIntBlock::set_ic_values(float value) {
  for (auto idx = 0u; idx < ic_values.size(); idx++)
    if (!set_ic_value(idx, value))
      return false;
  return true;
}
 
FLASHMEM bool blocks::MIntBlock::set_ic_value(uint8_t idx, float value) {
  if (idx >= ic_values.size())
    return false;
  if (value > 1.0f or value < -1.0f)
    return false;
  ic_values[idx] = value;
  return true;
}
 
FLASHMEM void blocks::MIntBlock::reset_ic_values() { std::fill(ic_values.begin(), ic_values.end(), 0.0f); }
 
FLASHMEM const std::array<unsigned int, 8> &blocks::MIntBlock::get_time_factors() const {
  return time_factors;
}
 
FLASHMEM unsigned int blocks::MIntBlock::get_time_factor(uint8_t idx) const {
  if (idx >= time_factors.size())
    return 0;
  return time_factors[idx];
}
 
FLASHMEM bool blocks::MIntBlock::set_time_factors(const std::array<unsigned int, 8> &time_factors_) {
  for (auto idx = 0u; idx < time_factors_.size(); idx++)
    if (!set_time_factor(idx, time_factors_[idx]))
      return false;
  return true;
}
 
FLASHMEM bool blocks::MIntBlock::set_time_factors(unsigned int k) {
  for (auto idx = 0u; idx < time_factors.size(); idx++)
    if (!set_time_factor(idx, k))
      return false;
  return true;
}
 
FLASHMEM bool blocks::MIntBlock::set_time_factor(uint8_t int_idx, unsigned int k) {
  if (!(k == 100 or k == 10000))
    return false;
  if (int_idx >= NUM_INTEGRATORS)
    return false;
  time_factors[int_idx] = k;
  return true;
}
 
FLASHMEM void blocks::MIntBlock::reset_time_factors() {
  // Copying solves a strange linker issue "relocation against ... in read-only section `.text'"
  auto default_ = DEFAULT_TIME_FACTOR;
  std::fill(std::begin(time_factors), std::end(time_factors), default_);
}
 
FLASHMEM utils::status blocks::MIntBlock::write_to_hardware() {
  // Write IC values one channel at a time
  for (decltype(ic_values.size()) i = 0; i < ic_values.size(); i++) {
    if (!hardware->write_ic(i, ic_values[i])) {
      LOG(ANABRID_PEDANTIC, __PRETTY_FUNCTION__);
      return "Error writing IC values to hardware.";
    }
  }
  // Write time factor switches by converting to bitset
  std::bitset<NUM_INTEGRATORS> time_factor_switches{};
  for (auto idx = 0u; idx < time_factors.size(); idx++)
    if (time_factors[idx] != DEFAULT_TIME_FACTOR)
      time_factor_switches.set(idx);
  if (!hardware->write_time_factor_switches(time_factor_switches))
    return "Error writing time factor switches.";
 
  return utils::status::success();
}
 
FLASHMEM void blocks::MIntBlock::reset(entities::ResetAction action) {
  FunctionBlock::reset(action);
 
  if (action.has(entities::ResetAction::CIRCUIT_RESET)) {
    reset_ic_values();
    reset_time_factors();
  }
 
  if (action.has(entities::ResetAction::OVERLOAD_RESET)) {
    // !!ATTENTION!! This breaks a lot of things but is neccessary and the ONLY
    // way to reset overloads on the MIntBlock.
    mode::RealManualControl::enable();
    mode::RealManualControl::to_ic();
  }
}
 
FLASHMEM utils::status blocks::MIntBlock::config_self_from_json(JsonObjectConst cfg) {
#ifdef ANABRID_DEBUG_ENTITY_CONFIG
  Serial.println(__PRETTY_FUNCTION__);
#endif
  for (auto cfgItr = cfg.begin(); cfgItr != cfg.end(); ++cfgItr) {
    if (cfgItr->key() == "elements") {
      auto res = _config_elements_from_json(cfgItr->value());
      if (!res)
        return res;
    } else {
      return utils::status("Unknown configuration key.");
    }
  }
  return utils::status::success();
}
 
FLASHMEM utils::status blocks::MIntBlock::_config_elements_from_json(const JsonVariantConst &cfg) {
  if (cfg.is<JsonArrayConst>()) {
    auto ints_cfg = cfg.as<JsonArrayConst>();
    if (ints_cfg.size() != NUM_INTEGRATORS) {
      return utils::status("MIntBlock: Provided %d elments but NUM_INTEGRATORS=%d.", ints_cfg.size(),
                           NUM_INTEGRATORS);
    }
    for (size_t i = 0; i < ints_cfg.size(); i++) {
      if (!ints_cfg[i].containsKey("ic") or !ints_cfg[i]["ic"].is<float>())
        return utils::status("MIntBlock, element %d: Requiring IC as float", i);
      if (!set_ic_value(i, ints_cfg[i]["ic"]))
        return utils::status("MIntBlock, element %d: Illegal IC %f", i, ints_cfg[i]["ic"]);
      if (ints_cfg[i].containsKey("k")) {
        if (!ints_cfg[i]["k"].is<unsigned int>())
          return utils::status("MIntBlock, element %d: Requiring time factor 'k' as int", i);
        if (!set_time_factor(i, ints_cfg[i]["k"]))
          return utils::status("MIntBlock, element %d: Illegal time factor %d", i, ints_cfg[i]["k"]);
      }
    }
    return utils::status::success();
  } else if (cfg.is<JsonObjectConst>()) {
    for (JsonPairConst keyval : cfg.as<JsonObjectConst>()) {
      // Value is an object, with any key being optional
      if (!keyval.value().is<JsonObjectConst>())
        return utils::status("MIntBlock configuration value needs to be an object");
      // TODO: Check conversion from string to number
      auto int_idx = std::stoul(keyval.key().c_str());
      if (int_idx >= NUM_INTEGRATORS)
        return utils::status("MIntBlock: Integrator index %d >= NUM_INTEGRATORS = %d", int_idx,
                             NUM_INTEGRATORS);
      auto int_cfg = keyval.value().as<JsonObjectConst>();
      if (int_cfg.containsKey("ic")) {
        if (!int_cfg["ic"].is<float>())
          return utils::status("MIntBlock: Integrator %d IC must be float", int_idx);
        if (!set_ic_value(int_idx, int_cfg["ic"]))
          return utils::status("MIntBlock: Integrator %d IC is illegal: %f", int_idx, int_cfg["ic"]);
      }
      if (int_cfg.containsKey("k")) {
        if (!int_cfg["k"].is<unsigned int>())
          return utils::status("MIntBlock: Integrator %d k must be integer", int_idx);
        if (!set_time_factor(int_idx, int_cfg["k"]))
          return utils::status("MIntBlock: Integrator %d time factor k illegal: %d", int_idx, int_cfg["k"]);
      }
    }
    return utils::status::success();
  }
  return utils::status("MIntBlock: Configuration must either be array or object");
}
 
FLASHMEM void blocks::MIntBlock::config_self_to_json(JsonObject &cfg) {
  Entity::config_self_to_json(cfg);
  auto ints_cfg = cfg.createNestedArray("elements");
  for (size_t i = 0; i < NUM_INTEGRATORS; i++) {
    auto int_cfg = ints_cfg.createNestedObject();
    int_cfg["ic"] = ic_values[i];
    int_cfg["k"] = time_factors[i];
  }
}
 
FLASHMEM blocks::MIntBlock *blocks::MIntBlock::from_entity_classifier(entities::EntityClassifier classifier,
                                                                      const bus::addr_t block_address) {
  if (!classifier or classifier.class_enum != CLASS_ or classifier.type != static_cast<uint8_t>(TYPE))
    return nullptr;
 
  // Currently, there are no different variants
  if (classifier.variant != entities::EntityClassifier::DEFAULT_)
    return nullptr;
 
  SLOT slot = block_address % 8 == 4 ? SLOT::M0 : SLOT::M1;
  if (classifier.version < entities::Version(1))
    return nullptr;
  if (classifier.version < entities::Version(1, 1)) {
    auto *new_block = new MIntBlock(slot, new MIntBlockHAL_V_1_0_X(block_address));
    new_block->classifier = classifier;
    return new_block;
  }
  if (classifier.version < entities::Version(1, 2)) {
    auto *new_block = new MIntBlock_V_1_1_X(slot, new MIntBlockHAL_V_1_1_X(block_address));
    new_block->classifier = classifier;
    return new_block;
  }
  return nullptr;
}
 
// V1.1.0
 
FLASHMEM void blocks::MIntBlock_V_1_1_X::reset(entities::ResetAction action) {
  MIntBlock::reset(action);
 
  if (action.has(entities::ResetAction::CALIBRATION_RESET)) {
    for (auto idx = 0u; idx < MIntBlock::NUM_INTEGRATORS; idx++)
      calibration[idx] = {};
  }
}
 
FLASHMEM
const std::array<blocks::IntegratorCalibration, blocks::MIntBlock_V_1_1_X::NUM_INTEGRATORS> &
blocks::MIntBlock_V_1_1_X::get_calibration() const {
  return calibration;
}
 
FLASHMEM blocks::IntegratorCalibration blocks::MIntBlock_V_1_1_X::get_calibration(uint8_t int_idx) const {
  if (int_idx >= NUM_INTEGRATORS)
    return {};
  return calibration[int_idx];
}
 
FLASHMEM utils::status blocks::MIntBlock_V_1_1_X::write_to_hardware() {
  auto res = MIntBlock::write_to_hardware();
  if (!res)
    return res;
 
  return write_calibration_to_hardware();
}
 
FLASHMEM bool blocks::MIntBlock_V_1_1_X::calibrate(platform::Cluster *cluster, carrier::Carrier *carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  if (!MBlock::calibrate(cluster, carrier))
    return false;
 
  // Currently this is the only integrator version which can be calibrated automatically
  bool success = true;
 
  // TODO: keep old circuits alive
  cluster->reset(entities::ResetAction::CIRCUIT_RESET);
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Connecting calibration signals!");
 
  // Connect +0.1 to all integrator inputs. 0.1 because we have high precision and a long integration time.
  for (auto idx : SLOT_INPUT_IDX_RANGE())
    if (!cluster->add_constant(UBlock::Transmission_Mode::POS_REF, slot_to_global_io_index(idx), 1.0f,
                               slot_to_global_io_index(idx)))
      return "Could not connect constants to inputs."; // Fatal error
 
  cluster->ublock->change_reference_magnitude(UBlock::Reference_Magnitude::ONE_TENTH);
 
  if (!cluster->write_to_hardware())
    return "Error while writing new configuration to hardware."; // Fatal error
 
  // When setting routes we need to calibrate them
  if (!carrier->calibrate_routes_in_cluster(*cluster))
    LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating routes failed!");
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Starting ramp integration on fast mode!");
  set_time_factors(10000);
  set_ic_values(-1.0f);
  if (!write_to_hardware())
    return false;
 
  success &= _gain_calibration(false); // Fast time constant
  LOG(ANABRID_DEBUG_CALIBRATION, "Finished ramp integration on fast mode!");
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Starting ramp integration on slow mode!");
  set_time_factors(100);
  if (!write_to_hardware())
    return false;
 
  success &= _gain_calibration(true); // Slow time constant
  LOG(ANABRID_DEBUG_CALIBRATION, "Finished ramp integration on slow mode!");
 
  // Measure end value
  return success;
}
 
FLASHMEM utils::status blocks::MIntBlock_V_1_1_X::write_calibration_to_hardware() {
  for (size_t i = 0; i < NUM_INTEGRATORS; i++) {
    if (!hardware->write_time_factor_gain(i, time_factors[i] == 10000 ? calibration[i].time_factor_gain_fast
                                                                      : calibration[i].time_factor_gain_slow))
      return utils::status::failure();
  }
  return utils::status::success();
}
 
FLASHMEM bool blocks::MIntBlock_V_1_1_X::_gain_calibration(bool use_slow_integration) {
  bool success = true;
  const float target_precision = 0.001f;
  const int max_loops = 100;
 
  // TODO: currently we cant reach the other four channels
  std::bitset<NUM_INTEGRATORS> done_channels;
  int loop_count = 0;
 
  // start varying the x input offsets
  while (!done_channels.all()) {
    // Start calculation
    mode::FlexIOControl::reset();
    if (!mode::FlexIOControl::init(use_slow_integration ? mode::DEFAULT_IC_TIME * 100 : mode::DEFAULT_IC_TIME,
                                   use_slow_integration ? 200'000'000 : 2'000'000, mode::OnOverload::IGNORE,
                                   mode::OnExtHalt::IGNORE))
      LOG(ANABRID_DEBUG_CALIBRATION, "Timer failed!");
 
    mode::FlexIOControl::force_start();
    while (!mode::FlexIOControl::is_done()) {
    }
 
    auto read_outputs = daq::average(daq::sample, 4, 10);
 
    for (auto idx = 0u; idx < NUM_INTEGRATORS; idx++) {
      uint8_t *time_factor_gain = use_slow_integration ? &calibration[idx].time_factor_gain_slow
                                                       : &calibration[idx].time_factor_gain_fast;
      if (fabs(read_outputs[idx] + 1.0f) < target_precision) {
        done_channels[idx] = true; // Already precice
        continue;
      }
      done_channels[idx] = false;
 
      // Little bounded proportional controller, to decrease calibration time
      int step = abs_clamp((read_outputs[idx] + 1.0f) * -800.0f, 1, 70);
 
      if (*time_factor_gain + step > 0xff || *time_factor_gain + step < 0) {
        *time_factor_gain = std::clamp((int)(*time_factor_gain) + step, 0, 0xff);
        success = false; // Out of bounds for factor
        done_channels[idx] = true;
      } else {
        *time_factor_gain += step;
      }
 
      if (!hardware->write_time_factor_gain(idx, *time_factor_gain)) {
        success = false; // Other issues
        done_channels[idx] = true;
        continue;
      }
    }
    delay(10); // Small transient time
 
    loop_count++;
    if (loop_count > max_loops) {
      LOG(ANABRID_DEBUG_CALIBRATION, "Calibration timed out!");
      return false;
    }
  }
 
  return success;
}
 
blocks::MIntBlock_V_1_1_X::MIntBlock_V_1_1_X(SLOT slot, blocks::MIntBlockHAL_V_1_1_X *hardware)
    : MIntBlock(slot, hardware), hardware(hardware) {}
 
// Hardware abstraction layer
 
FLASHMEM blocks::MIntBlockHAL_V_1_0_X::MIntBlockHAL_V_1_0_X(bus::addr_t block_address)
    : f_meta(block_address), f_ic_dac(bus::replace_function_idx(block_address, 4), 2.0f),
      f_time_factor(bus::replace_function_idx(block_address, 5), true),
      f_time_factor_sync(bus::replace_function_idx(block_address, 6)),
      f_time_factor_reset(bus::replace_function_idx(block_address, 7)),
      f_overload_flags(bus::replace_function_idx(block_address, 2)),
      f_overload_flags_reset(bus::replace_function_idx(block_address, 3)) {}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_0_X::init() {
  if (!MIntBlockHAL::init())
    return false;
  return f_ic_dac.init() and f_ic_dac.set_external_reference(true) and f_ic_dac.set_double_gain(true);
}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_0_X::write_ic(uint8_t idx, float ic) {
  if (idx >= MIntBlock::NUM_INTEGRATORS)
    return false;
  // Note: The output is level-shifted, such that IC = 2V - output.
  return f_ic_dac.set_channel(idx, ic + 1.0f);
}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_0_X::write_time_factor_switches(std::bitset<8> switches) {
  if (!f_time_factor.transfer8(static_cast<uint8_t>(switches.to_ulong())))
    return false;
  f_time_factor_sync.trigger();
  return true;
}
 
FLASHMEM std::bitset<8> blocks::MIntBlockHAL_V_1_0_X::read_overload_flags() {
  return f_overload_flags.read8();
}
 
FLASHMEM void blocks::MIntBlockHAL_V_1_0_X::reset_overload_flags() { f_overload_flags_reset.trigger(); }
 
FLASHMEM blocks::MIntBlockHAL_V_1_1_X::MIntBlockHAL_V_1_1_X(bus::addr_t block_address)
    : MIntBlockHAL_V_1_0_X(block_address), f_time_factor_gain_0_3(bus::replace_function_idx(block_address, 8)),
      f_time_factor_gain_4_7(bus::replace_function_idx(block_address, 9)) {}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_1_X::write_time_factor_switches(std::bitset<8> switches) {
  // In version 1.1, a new shift register has been chained behind the time factors shift register,
  // instead of recieving a separate function address, so we can only set them together.
  // If we try to set only the time facors, we shift their old state into the shift register behind,
  // and would change the limeters enable randomly.
  // So we need to read out the current value and change only the lower 8 bits.
  uint16_t current;
  if (!f_time_factor.transfer16(static_cast<uint8_t>(switches.to_ulong()), &current))
    return false;
  auto new_ = (current & 0xFF00) | static_cast<uint8_t>(switches.to_ulong());
  if (!f_time_factor.transfer16(new_))
    return false;
  f_time_factor_sync.trigger();
  return true;
}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_1_X::write_limiters_enable(std::bitset<8> limiters) {
  // In version 1.1, a new shift register has been chained behind the time factors shift register,
  // instead of recieving a separate function address, so we can only set them together.
  // If we try to set only the limiters, we also shift data into the time factors switches,
  // and would change them.
  // So we need to read out the current value and change only the upper 8 bits.
  uint16_t current;
  if (!f_time_factor.transfer16(static_cast<uint16_t>(limiters.to_ulong()) << 8, &current))
    return false;
  auto new_ = (current & 0x00FF) | (static_cast<uint16_t>(limiters.to_ulong()) << 8);
  if (!f_time_factor.transfer16(new_))
    return false;
  f_time_factor_sync.trigger();
  return true;
}
 
FLASHMEM bool
blocks::MIntBlockHAL_V_1_1_X::write_time_factor_switches_and_limiters_enable(std::bitset<8> switches,
                                                                             std::bitset<8> limiters) {
  // In version 1.1, a new shift register has been chained behind the time factors shift register,
  // instead of recieving a separate function address, so we can only set them together.
  if (!f_time_factor.transfer16(static_cast<uint16_t>(limiters.to_ulong()) << 8 |
                                static_cast<uint8_t>(switches.to_ulong())))
    return false;
  f_time_factor_sync.trigger();
  return true;
}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_1_X::write_ic(uint8_t idx, float ic) {
  if (idx >= MIntBlock::NUM_INTEGRATORS)
    return false;
  // Note: The output is level-shifted differently than on Version 1.0, such that IC = output - 2V.
  return f_ic_dac.set_channel(idx, 1.0f - ic);
}
 
FLASHMEM bool blocks::MIntBlockHAL_V_1_1_X::write_time_factor_gain(uint8_t idx, uint8_t gain) {
  if (idx >= MIntBlock::NUM_INTEGRATORS)
    return false;
 
  if (idx < 4)
    return f_time_factor_gain_0_3.write_channel_raw(idx, gain);
  else
    return f_time_factor_gain_4_7.write_channel_raw(idx - 4, gain);
}