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 <io/io.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);
 
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)
    : blocks::MIntBlockHAL_Parent(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);
}