mblock_mul.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 "carrier/carrier.h"
#include "carrier/cluster.h"
 
#include "block/mblock.h"
#include "teensy/mblock_mul.h"
 
extern int abs_clamp(float in, int min, int max);
 
blocks::MMulBlock *blocks::MMulBlock::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;
  // Return default implementation
  if (classifier.version < entities::Version(1))
    return nullptr;
  if (classifier.version < entities::Version(1, 1)) {
    auto *new_block = new MMulBlock(slot, new MMulBlockHAL_V_1_0_X(block_address));
    new_block->classifier = classifier;
    return new_block;
  }
  if (classifier.version <= entities::Version(1, 2)) {
    auto *new_block = new MMulBlock_FullAutoCalibration(slot, new MMulBlockHAL_V_1_1_X(block_address));
    new_block->classifier = classifier;
    return new_block;
  }
  if (classifier.version == entities::Version(1, -1)) {
    auto *new_block = new MMulBlock_FullAutoCalibration(slot, new MMulBlockHAL_V_1_M1_X(block_address));
    new_block->classifier = classifier;
    return new_block;
  }
  return nullptr;
}
 
UnitResult blocks::MMulBlock::calibrate(platform::Cluster *cluster, carrier::Carrier *carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  // Our first simple calibration process has been developed empirically and goes
  // 1. Set all inputs to zero
  //    - The input offsets are rather small, so inputs are 0+-epsilon
  //    - The output is then roughly 0 + epsilon^2 - offset_z ~= -offset_z
  // 2. Measure output and use it as a first estimate of offset_z
  // 3. Set inputs to 0 and 1 and change the first one's offset to get an output close to zero
 
  std::function find_zeros = [&](size_t input) {
    float lhs = input == 0 ? 1.0f : 0.0f;
    float rhs = input == 1 ? 1.0f : 0.0f;
    // Set some inputs to one
 
    cluster->reset(entities::ResetAction::CIRCUIT_RESET);
    for (auto idx = 0u; idx < NUM_MULTIPLIERS; ++idx) {
      auto lhs_lane = idx * 2;
      auto rhs_lane = idx * 2 + 1;
      cluster->add_constant(UBlockHAL::Transmission_Mode::POS_REF, lhs_lane, lhs, slot_to_global_io_index(lhs_lane));
      cluster->add_constant(UBlockHAL::Transmission_Mode::POS_REF, rhs_lane, rhs, slot_to_global_io_index(rhs_lane));
    }
 
    TRY(cluster->write_to_hardware());
 
    for (size_t step = 1024; step > 0; step >>= 1) {
      for (auto idx = 0u; idx < NUM_MULTIPLIERS; ++idx) {
        auto &cal = calibration[idx];
        hardware->write_calibration_input_offsets(idx, cal.offset_x, cal.offset_y);
      }
 
      delay(10);
      auto read_outputs = daq::average(daq::sample, 4, 10);
 
      for (auto idx = 0u; idx < NUM_MULTIPLIERS; ++idx) {
        auto& cal = calibration[idx];
 
        bool is_neg = read_outputs[idx] < 0.0f;
        if (input == 0) {
          cal.offset_x -= is_neg ? step : -step;
        } else {
          cal.offset_y -= is_neg ? step : -step;
        }
      }
    }
 
    return UnitResult::ok();
  };
 
  // TODO: keep old circuits alive
  cluster->reset(entities::ResetAction::CIRCUIT_RESET);
  reset(entities::ResetAction::CALIBRATION_RESET);
 
  delay(10);
 
  // Connect zeros to all our inputs
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating output offsets...");
  for (auto idx : SLOT_INPUT_IDX_RANGE()) {
    TRY(cluster->add_constant(
      UBlock::Transmission_Mode::GROUND,
      slot_to_global_io_index(idx),
      0.0f,
      slot_to_global_io_index(idx)
    ));
  }
 
  TRY(cluster->write_to_hardware());
 
  // When setting routes we need to calibrate them
  TRY(carrier->calibrate_offsets());
 
  // When setting routes we need to calibrate them
  TRY(carrier->calibrate_routes());
 
  // Measure offset_z and set it
  auto read_outputs = daq::average(daq::sample, 4, 10);
 
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
    calibration[idx].offset_z = MMulBlockHAL_V_1_0_X::float_to_raw_calibration(-read_outputs[idx]);
    if (!hardware->write_calibration_output_offset(idx, calibration[idx].offset_z))
      return UnitResult::err("Writing output offsets failed");
  }
 
  delay(10); // Small transient time
 
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
    auto& cal = calibration[idx];
    cal.offset_x = 2047;
    cal.offset_y = 2047;
  }
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating x input offsets...");
  TRY(find_zeros(0));
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating y input offsets...");
  TRY(find_zeros(1));
 
  return UnitResult::ok();
}
 
// V1.-1.0
 
blocks::MMulBlock_FullAutoCalibration::MMulBlock_FullAutoCalibration(
    blocks::MBlock::SLOT slot, blocks::MMulBlockHAL_FullAutoCalibration *hardware)
    : MMulBlock(slot, hardware), hardware(hardware) {}
 
UnitResult blocks::MMulBlock_FullAutoCalibration::write_calibration_to_hardware() {
  auto res = MMulBlock::write_calibration_to_hardware();
  if (!res)
    return res;
 
  for (size_t i = 0; i < MMulBlock::NUM_MULTIPLIERS; i++) {
    if (!hardware->write_calibration_gain(i, calibration[i].gain))
      return UnitResult::err_fmt("MMulBlock::calibration from json for multiplier %d values gain not accepted", i);
  }
  return UnitResult::ok();
}
 
UnitResult blocks::MMulBlock_FullAutoCalibration::calibrate(platform::Cluster *cluster, carrier::Carrier *carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  // First do regular calibration
  TRY(MMulBlock::calibrate(cluster, carrier));
 
  cluster->reset(entities::ResetAction::CIRCUIT_RESET);
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; ++idx) {
    auto lhs_lane = idx * 2;
    auto rhs_lane = idx * 2 + 1;
    cluster->add_constant(UBlockHAL::Transmission_Mode::POS_REF, lhs_lane, 1.0, slot_to_global_io_index(lhs_lane));
    cluster->add_constant(UBlockHAL::Transmission_Mode::POS_REF, rhs_lane, 1.0, slot_to_global_io_index(rhs_lane));
  }
 
  TRY(cluster->write_to_hardware());
 
  TRY(cluster->calibrate_offsets());
  delay(10);
 
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
    calibration[idx].gain = 127;
  }
 
  for (size_t step = 64; step > 0; step >>= 1) {
    for (auto idx = 0u; idx < NUM_MULTIPLIERS; ++idx) {
      hardware->write_calibration_gain(idx, calibration[idx].gain);
    }
 
    delay(10);
    auto read_outputs = daq::average(daq::sample, 4, 10);
    for (auto idx = 0u; idx < NUM_MULTIPLIERS; ++idx) {
      calibration[idx].gain += read_outputs[idx] < 1.0f ? step : -step;
    }
  }
 
  return UnitResult::ok();
}
 
// Hardware abstraction layer
 
blocks::MMulBlockHAL_V_1_0_X::MMulBlockHAL_V_1_0_X(bus::addr_t block_address)
    : MMulBlockHAL_Parent(block_address, 7), f_overload_flags_reset(bus::address_from_tuple(block_address, 3)),
      f_overload_flags(bus::replace_function_idx(block_address, 2), 7),
      f_calibration_dac_0(bus::address_from_tuple(block_address, 4), 7, 2.0f),
      f_calibration_dac_1(bus::address_from_tuple(block_address, 5), 7, 2.0f) {}
 
UnitResult blocks::MMulBlockHAL::init() { return MBlockHAL::init(); }
 
UnitResult blocks::MMulBlockHAL_V_1_0_X::init() {
  bool error = true;
  error &= f_calibration_dac_0.init();
  error &= f_calibration_dac_0.set_external_reference();
  error &= f_calibration_dac_0.set_double_gain();
  error &= f_calibration_dac_1.init();
  error &= f_calibration_dac_1.set_external_reference();
  error &= f_calibration_dac_1.set_double_gain();
  error &= MMulBlockHAL::init();
  if (!error)
    return UnitResult::err("Failed mmul-blck writing hardware");
  return UnitResult::ok();
}
 
void blocks::MMulBlockHAL_V_1_0_X::reset_overload_flags() { f_overload_flags_reset.trigger(); }
 
bool blocks::MMulBlockHAL_V_1_0_X::write_calibration_input_offsets(uint8_t idx, uint16_t offset_x,
                                                                            uint16_t offset_y) {
  return f_calibration_dac_0.set_channel_raw(idx * 2 + 1, offset_x) and
         f_calibration_dac_0.set_channel_raw(idx * 2, offset_y);
}
 
bool blocks::MMulBlockHAL_V_1_0_X::write_calibration_output_offset(uint8_t idx, uint16_t offset_z) {
  return f_calibration_dac_1.set_channel_raw(idx, offset_z);
}
 
std::bitset<8> blocks::MMulBlockHAL_V_1_0_X::read_overload_flags() {
  return f_overload_flags.read8();
}
 
blocks::MMulBlockHAL_V_1_M1_X::MMulBlockHAL_V_1_M1_X(bus::addr_t block_address)
    : MMulBlockHAL_V_1_0_X(block_address), f_gain_ch0_1(bus::replace_function_idx(block_address, 6), 7),
      f_gain_ch2_3(bus::replace_function_idx(block_address, 7), 7) {}
 
bool blocks::MMulBlockHAL_V_1_M1_X::write_calibration_gain(uint8_t idx, uint8_t gain) {
  if (idx < 2)
    return f_gain_ch0_1.write_channel_raw(idx, gain);
  else if (idx < 4)
    return f_gain_ch2_3.write_channel_raw(idx - 2, gain);
  else
    return false;
}
 
blocks::MMulBlockHAL_V_1_1_X::MMulBlockHAL_V_1_1_X(bus::addr_t block_address)
    : MMulBlockHAL_V_1_0_X(block_address), f_gain(bus::replace_function_idx(block_address, 6), 7) {}
 
bool blocks::MMulBlockHAL_V_1_1_X::write_calibration_gain(uint8_t idx, uint8_t gain) {
  if (idx == 0)
    return f_gain.write_channel_raw(2, gain);
  else if (idx == 1)
    return f_gain.write_channel_raw(0, gain);
  else if (idx == 2)
    return f_gain.write_channel_raw(1, gain);
  if (idx == 3)
    return f_gain.write_channel_raw(3, gain);
  return false;
}