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 "block/mblock_mul.h"
#include "utils/logging.h"
 
#include "entity/entity.h"
#include "etl/crc.h"
 
#include "carrier/carrier.h"
#include "carrier/cluster.h"
 
extern int abs_clamp(float in, int min, int max);
 
FLASHMEM metadata::eui_t blocks::MMulBlock::get_entity_eui() const {
  if (hardware)
    return hardware->get_entity_eui();
  return {};
}
 
FLASHMEM utils::status blocks::MMulBlock::config_self_from_json(JsonObjectConst cfg) {
  // MMulBlock does not expect any configuration currently.
  // But due to automation, some may still be sent.
  // Thus we accept any configuration containing only empty values or similar.
  for (auto cfgItr = cfg.begin(); cfgItr != cfg.end(); ++cfgItr) {
    if (cfgItr->key() == "calibration") {
      auto res = _config_elements_from_json(cfgItr->value());
      if (!res)
        return res;
    } else {
      return utils::status(771, "MMulBlock: Unknown configuration key");
    }
  }
  return utils::status::success();
}
 
FLASHMEM utils::status blocks::MMulBlock::_config_elements_from_json(const JsonVariantConst &cfg) {
  return utils::status("MMulBlock currently does not accept configuration");
 
  // Note: The following is a workaround to get manual calibration data
  //       into the system, for release v1.0.
 
  auto map = cfg.as<JsonObjectConst>();
  if (!map.containsKey("offset_x") || map["offset_x"].size() != MMulBlock::NUM_MULTIPLIERS)
    return utils::status(772, "Missing offset_x (need 4 entries)");
  if (!map.containsKey("offset_y") || map["offset_y"].size() != MMulBlock::NUM_MULTIPLIERS)
    return utils::status(773, "Missing offset_y (need 4 entries)");
  if (!map.containsKey("offset_z") || map["offset_z"].size() != MMulBlock::NUM_MULTIPLIERS)
    return utils::status(774, "Missing offset_z (need 4 entries)");
  for (size_t i = 0; i < MMulBlock::NUM_MULTIPLIERS; i++) {
    calibration[i].offset_x = map["offset_x"][i];
    calibration[i].offset_y = map["offset_y"][i];
    calibration[i].offset_z = map["offset_z"][i];
  }
 
  auto res = write_calibration_to_hardware();
  if (!res) {
    return res.attach("via MMUlBlock::config_elements_from_json");
  }
 
  return utils::status::success();
}
 
FLASHMEM utils::status blocks::MMulBlock::write_to_hardware() { return write_calibration_to_hardware(); }
 
FLASHMEM utils::status blocks::MMulBlock::write_calibration_to_hardware() {
  for (size_t i = 0; i < MMulBlock::NUM_MULTIPLIERS; i++) {
    if (!hardware->write_calibration_input_offsets(i, calibration[i].offset_x, calibration[i].offset_y))
      return utils::status(
          775, "MMulBlock::calibration from json for multiplier %d values offset_x, offset_y not accepted", i);
    if (!hardware->write_calibration_output_offset(i, calibration[i].offset_z))
      return utils::status(
          776, "MMulBlock::calibration from json for multiplier %d values offset_z not accepted", i);
  }
  return utils::status::success();
}
 
FLASHMEM void blocks::MMulBlock::config_self_to_json(JsonObject &cfg) {
  auto json_calibration = cfg.createNestedObject("calibration");
  auto offset_x = json_calibration.createNestedArray("offset_x");
  auto offset_y = json_calibration.createNestedArray("offset_y");
  auto offset_z = json_calibration.createNestedArray("offset_z");
 
  for (int i = 0; i < MMulBlock::NUM_MULTIPLIERS; i++) {
    offset_x.add(calibration[i].offset_x);
    offset_y.add(calibration[i].offset_y);
    offset_z.add(calibration[i].offset_z);
  }
}
 
FLASHMEM 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;
}
 
FLASHMEM blocks::MMulBlock::MMulBlock(SLOT slot, MMulBlockHAL *hardware)
    : MBlock(slot, hardware), hardware(hardware) {
  classifier.type = static_cast<uint8_t>(TYPE);
}
 
FLASHMEM bool blocks::MMulBlock::init() {
  // TODO: Remove once hardware pointer is part of FunctionBlock base class
  return FunctionBlock::init() and hardware->init();
}
 
FLASHMEM void blocks::MMulBlock::reset(entities::ResetAction action) {
  FunctionBlock::reset(action);
 
  if (action.has(entities::ResetAction::CALIBRATION_RESET)) {
    for (auto idx = 0u; idx < MMulBlock::NUM_MULTIPLIERS; idx++)
      calibration[idx] = {};
  }
 
  if (action.has(entities::ResetAction::OVERLOAD_RESET)) {
    hardware->reset_overload_flags();
  }
}
 
FLASHMEM bool 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
 
  if (!MBlock::calibrate(cluster, carrier))
    return false;
 
  // TODO: keep old circuits alive
  cluster->reset(entities::ResetAction::CIRCUIT_RESET);
 
  // We can't iterativly calibrate, so we need to delete the old calibration values.
  reset(entities::ResetAction::CALIBRATION_RESET);
  bool success = true;
 
  // Connect zeros to all our inputs
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating output offsets...");
  for (auto idx : SLOT_INPUT_IDX_RANGE())
    if (!cluster->add_constant(UBlock::Transmission_Mode::GROUND, slot_to_global_io_index(idx), 0.0f,
                               slot_to_global_io_index(idx)))
      return false; // Fatal error
  if (!cluster->write_to_hardware())
    return false; // Fatal error
 
  // When setting routes we need to calibrate them
  if (!carrier->calibrate_routes_in_cluster(*cluster))
    LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating routes failed!");
 
  // 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))
      success = false; // out of range
  }
  delay(10); // Small transient time
 
  // Set some inputs to one
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating x input offsets...");
  cluster->ublock->change_b_side_transmission_mode(UBlock::Transmission_Mode::POS_REF); // Set constant source
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
    if (!cluster->cblock->set_factor(slot_to_global_io_index(idx * 2), 1.0f)) // Setting 1.0 to Y inputs
      return false;                                                           // fatal error
  }
  if (!cluster->cblock->write_to_hardware())
    return false; // fatal error
  // When changing a factor, we always have to calibrate offset
  if (!cluster->calibrate_offsets())
    return false; // fatal error
 
  delay(10);
 
  const float target_precision = 0.0005f;
  const int max_loops = 100;
 
  std::bitset<NUM_MULTIPLIERS> done_channels;
  int loop_count = 0;
 
  // start varying the x input offsets
  while (!done_channels.all()) {
    read_outputs = daq::average(daq::sample, 4, 10);
 
    for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
      if (fabs(read_outputs[idx]) < target_precision) {
        done_channels[idx] = true; // Already precice
        continue;
      }
      done_channels[idx] = false;
 
      // Little bounded proportional controller, to decrease calibration time
      calibration[idx].offset_x += abs_clamp(read_outputs[idx] * 6000.0f, 1, 50);
 
      if (!hardware->write_calibration_input_offsets(idx, calibration[idx].offset_x,
                                                     calibration[idx].offset_y)) {
        calibration[idx].offset_x =
            std::clamp(calibration[idx].offset_x, functions::DAC60508::RAW_ZERO, functions::DAC60508::RAW_MAX);
        success = false; // Out of bounds for factor
        done_channels[idx] = true;
        LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating out of bounds!");
        continue;
      }
    }
    delay(10); // Small transient time
 
    loop_count++;
    if (loop_count > max_loops) {
      LOG(ANABRID_DEBUG_CALIBRATION, "Calibration timed out!");
      break;
    }
  }
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating y input offsets...");
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
    if (!cluster->cblock->set_factor(slot_to_global_io_index(idx * 2),
                                     0.0f)) // Setting 0.0 to Y inputs which previously were 1.0
      return false;                         // fatal error
 
    if (!cluster->cblock->set_factor(slot_to_global_io_index(idx * 2 + 1), 1.0f)) // Setting 1.0 to X inputs
      return false;                                                               // fatal error
  }
  if (!cluster->cblock->write_to_hardware())
    return false; // fatal error
  // When changing a factor, we always have to calibrate offset
  if (!cluster->calibrate_offsets())
    return false; // fatal error
 
  delay(10);
 
  read_outputs = daq::average(daq::sample, 4, 10);
 
  done_channels.reset();
  loop_count = 0;
 
  // start varying the y input offsets
  while (!done_channels.all()) {
    read_outputs = daq::average(daq::sample, 4, 10);
 
    for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
      if (fabs(read_outputs[idx]) < target_precision) {
        done_channels[idx] = true; // Already precice
        continue;
      }
      done_channels[idx] = false;
 
      // Little bounded proportional controller, to decrease calibration time
      calibration[idx].offset_y += abs_clamp(read_outputs[idx] * 6000.0f, 1, 50);
 
      if (!hardware->write_calibration_input_offsets(idx, calibration[idx].offset_x,
                                                     calibration[idx].offset_y)) {
        calibration[idx].offset_y =
            std::clamp(calibration[idx].offset_y, functions::DAC60508::RAW_ZERO, functions::DAC60508::RAW_MAX);
        success = false; // Out of bounds for factor
        done_channels[idx] = true;
        LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating out of bounds!");
        continue;
      }
    }
    delay(10); // Small transient time
 
    loop_count++;
    if (loop_count > max_loops) {
      LOG(ANABRID_DEBUG_CALIBRATION, "Calibration timed out!");
      break;
    }
  }
 
  return success;
}
 
FLASHMEM
const std::array<blocks::MultiplierCalibration, blocks::MMulBlock::NUM_MULTIPLIERS> &
blocks::MMulBlock::get_calibration() const {
  return calibration;
}
 
FLASHMEM blocks::MultiplierCalibration blocks::MMulBlock::get_calibration(uint8_t mul_idx) const {
  if (mul_idx >= NUM_MULTIPLIERS)
    return {};
  return calibration[mul_idx];
}
 
// V1.-1.0
 
FLASHMEM utils::status 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 utils::status(777, "MMulBlock::calibration from json for multiplier %d values gain not accepted",
                           i);
  }
  return utils::status::success();
}
 
FLASHMEM bool blocks::MMulBlock_FullAutoCalibration::calibrate(platform::Cluster *cluster,
                                                               carrier::Carrier *carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  // First do regular calibration
  bool success = MMulBlock::calibrate(cluster, carrier);
 
  // Automatic gain calibration can only be done by version 1.1
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating output gains...");
 
  for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
    if (!cluster->cblock->set_factor(slot_to_global_io_index(idx * 2),
                                     1.0f)) // Setting 1.0 to Y inputs which previously were 0.0
      return false;                         // fatal error
  }
  if (!cluster->cblock->write_to_hardware())
    return false; // fatal error
  // When changing a factor, we always have to calibrate offset
  if (!cluster->calibrate_offsets())
    return false; // fatal error
 
  delay(10);
 
  const float target_precision = 0.0005f;
  const int max_loops = 100;
 
  std::bitset<NUM_MULTIPLIERS> done_channels;
  int loop_count = 0;
 
  // start varying the gain factors
  while (!done_channels.all()) {
    auto read_outputs = daq::average(daq::sample, 4, 10);
 
    for (auto idx = 0u; idx < NUM_MULTIPLIERS; idx++) {
      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) * -1000.0f, 1, 70);
 
      if (calibration[idx].gain + step > 0xff || calibration[idx].gain + step < 0) {
        success = false; // Out of bounds for factor
        done_channels[idx] = true;
        LOG(ANABRID_DEBUG_CALIBRATION, "Calibrating out of bounds!");
        continue;
      }
 
      calibration[idx].gain += step;
 
      if (!hardware->write_calibration_gain(idx, calibration[idx].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!");
      break;
    }
  }
 
  return success;
}
 
// Hardware abstraction layer
 
FLASHMEM blocks::MMulBlockHAL_V_1_0_X::MMulBlockHAL_V_1_0_X(bus::addr_t block_address)
    : f_meta(block_address), f_overload_flags_reset(bus::address_from_tuple(block_address, 3)),
      f_overload_flags(bus::replace_function_idx(block_address, 2)),
      f_calibration_dac_0(bus::address_from_tuple(block_address, 4), 2.0f),
      f_calibration_dac_1(bus::address_from_tuple(block_address, 5), 2.0f) {}
 
FLASHMEM bool blocks::MMulBlockHAL::init() { return MBlockHAL::init(); }
 
FLASHMEM bool 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();
  return error;
}
 
FLASHMEM void blocks::MMulBlockHAL_V_1_0_X::reset_overload_flags() { f_overload_flags_reset.trigger(); }
 
FLASHMEM 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);
}
 
FLASHMEM 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);
}
 
FLASHMEM std::bitset<8> blocks::MMulBlockHAL_V_1_0_X::read_overload_flags() {
  return f_overload_flags.read8();
}
 
blocks::MMulBlock_FullAutoCalibration::MMulBlock_FullAutoCalibration(
    SLOT slot, blocks::MMulBlockHAL_FullAutoCalibration *hardware)
    : MMulBlock(slot, hardware), hardware(hardware) {}
 
FLASHMEM blocks::MMulBlockHAL_V_1_M1_X::MMulBlockHAL_V_1_M1_X(bus::addr_t block_address)
    : MMulBlockHAL_FullAutoCalibration(block_address),
      f_gain_ch0_1(bus::replace_function_idx(block_address, 6)),
      f_gain_ch2_3(bus::replace_function_idx(block_address, 7)) {}
 
FLASHMEM 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;
}
 
FLASHMEM blocks::MMulBlockHAL_V_1_1_X::MMulBlockHAL_V_1_1_X(bus::addr_t block_address)
    : MMulBlockHAL_FullAutoCalibration(block_address), f_gain(bus::replace_function_idx(block_address, 6)) {}
 
FLASHMEM 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;
}