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/teensy/mblock_int.h>
#include <utils/logging.h>
 
#include <carrier/carrier.h>
#include <carrier/cluster.h>
#include <mode/teensy/mode.h>
 
#include <chips/AD840X.h>
#include <chips/DAC60508.h>
#include <chips/SR74HC16X.h>
#include <chips/SR74HCT595.h>
 
extern int abs_clamp(float in, int min, int max);
 
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
 
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] = {};
  }
 
  if (action.has(entities::ResetAction::CIRCUIT_RESET))
    _limiters_enabled.reset();
}
 
 
const std::array<blocks::IntegratorCalibration, blocks::MIntBlock_V_1_1_X::NUM_INTEGRATORS> &
blocks::MIntBlock_V_1_1_X::get_calibration() const {
  return calibration;
}
 
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];
}
 
UnitResult blocks::MIntBlock_V_1_1_X::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 UnitResult::err("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_and_limiters_enable(time_factor_switches, _limiters_enabled))
    return UnitResult::err("Error writing time factors or limiters");
  return write_calibration_to_hardware();
}
 
UnitResult blocks::MIntBlock_V_1_1_X::calibrate(platform::Cluster *cluster, carrier::Carrier *carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  TRY(MBlock::calibrate(cluster, carrier));
 
  // TODO: keep old circuits alive
  cluster->reset(entities::ResetAction::CIRCUIT_RESET);
  _limiters_enabled.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()) {
    TRY(cluster->add_constant(
      UBlock::Transmission_Mode::POS_REF,
      slot_to_global_io_index(idx),
      1.0f,
      slot_to_global_io_index(idx)
    ));
  }
 
  cluster->ublock->change_reference_magnitude(UBlock::Reference_Magnitude::ONE_TENTH);
 
  TRY(cluster->write_to_hardware());
 
  // When setting routes we need to calibrate them
  TRY(carrier->calibrate_routes());
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Starting ramp integration on fast mode!");
  set_time_factors(10000);
  set_ic_values(-1.0f);
  TRY(write_to_hardware());
 
  TRY(_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);
  TRY(write_to_hardware());
 
  TRY(_gain_calibration(true)); // Slow time constant
  LOG(ANABRID_DEBUG_CALIBRATION, "Finished ramp integration on slow mode!");
 
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  // Measure end value
  return UnitResult::ok();
}
 
UnitResult blocks::MIntBlock_V_1_1_X::write_calibration_to_hardware() {
  for (size_t i = 0; i < NUM_INTEGRATORS; i++) {
    auto gain = time_factors[i] == 10000
      ? calibration[i].time_factor_gain_fast
      : calibration[i].time_factor_gain_slow;
    if (!hardware->write_time_factor_gain(i, gain))
      return UnitResult::err_fmt("Error mint-block writing time factor gain: %d %f", i, gain);
  }
  return UnitResult::ok();
}
 
UnitResult blocks::MIntBlock_V_1_1_X::_gain_calibration(bool use_slow_integration) {
  const float target_precision = 0.01f;
  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(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(static_cast<int>(*time_factor_gain) + step, 0, 0xff);
        done_channels[idx] = true;
        return UnitResult::err("Time factor / gain out of bounds");
      } else {
        *time_factor_gain += step;
      }
 
      if (!hardware->write_time_factor_gain(idx, *time_factor_gain)) {
        done_channels[idx] = true;
        return UnitResult::err("Writing of time factor gain failed");
        continue;
      }
    }
    delay(10); // Small transient time
 
    loop_count++;
    if (loop_count > max_loops)
      return UnitResult::err("Calibration timed out!");
  }
 
  return UnitResult::ok();
}
 
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) {}
 
ConfigResult blocks::MIntBlock_V_1_1_X::config(const pb_Item &config) {
  auto result = MIntBlock::config(config);
  if (result.is_err() || result.ok_value())
    return result;
 
  if (config.which_kind == pb_Item_limiter_config_tag) {
    auto& limiter_config = config.kind.limiter_config;
    for (size_t idx = 0; idx < limiter_config.elements_count; idx++) {
      auto& elem = limiter_config.elements[idx];
      _limiters_enabled[elem.idx] = elem.enable;
    }
 
    return ConfigResult::ok(true);
  }
 
  return ConfigResult::ok(false);
}
 
// Hardware abstraction layer
 
blocks::MIntBlockHAL_V_1_0_X::MIntBlockHAL_V_1_0_X(bus::addr_t block_address)
    : blocks::MIntBlockHAL_Parent(block_address, 14),
      f_ic_dac(bus::replace_function_idx(block_address, 4), 14, 2.0f),
      f_time_factor(bus::replace_function_idx(block_address, 5), 14, 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), 14),
      f_overload_flags_reset(bus::replace_function_idx(block_address, 3)) {}
 
UnitResult blocks::MIntBlockHAL_V_1_0_X::init() {
  TRY(MIntBlockHAL::init());
  if (!f_ic_dac.init())
    return UnitResult::err("Failed to initialize IC dac");
  if (!f_ic_dac.set_external_reference(true))
    return UnitResult::err("Failed to set external reference");
  if (!f_ic_dac.set_double_gain(true))
    return UnitResult::err("Failed to set double gain");
  return UnitResult::ok();
}
 
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);
}
 
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;
}
 
std::bitset<8> blocks::MIntBlockHAL_V_1_0_X::read_overload_flags() {
  return f_overload_flags.read8();
}
 
void blocks::MIntBlockHAL_V_1_0_X::reset_overload_flags() { f_overload_flags_reset.trigger(); }
 
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), 14),
      f_time_factor_gain_4_7(bus::replace_function_idx(block_address, 9), 14) {}
 
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;
}
 
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);
}
 
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);
}