carrier.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 "pb_decode.h"
 
#include <entity/visitor.h>
#include <lucidac/lucidac.h>
#include <mode/mode.h>
#include <net/settings.h>
#include <redac/redac.h>
#include <utils/is_number.h>
 
carrier::Carrier::Carrier(std::vector<platform::Cluster> clusters, carrier::Carrier_HAL *hardware)
    : Entity("", hardware), hardware(hardware), clusters(std::move(clusters)) {
  classifier.class_enum = CLASS_;
}
 
carrier::Carrier *carrier::Carrier::from_entity_classifier(entities::EntityClassifier classifier,
                                                                    __attribute__((__unused__))
                                                                    const bus::addr_t block_address) {
  if (!classifier or classifier.class_enum != entities::EntityClass::CARRIER)
    return nullptr;
 
  auto type = classifier.type_as<TYPES>();
  switch (type) {
  case TYPES::UNKNOWN:
    // This is already checked by !classifier above
    return nullptr;
  case TYPES::LUCIDAC:
    return platform::LUCIDAC::from_entity_classifier(classifier, block_address);
  case TYPES::mREDAC:
    return platform::REDAC::from_entity_classifier(classifier, block_address);
  }
  // Any unknown value results in a nullptr here.
  // Adding default case to switch suppresses warnings about missing cases.
  return nullptr;
}
 
carrier::Carrier *carrier::Carrier::detect() {
  return entities::detect<carrier::Carrier>(bus::address_from_tuple(bus::CARRIER_BADDR, 0));
}
 
status carrier::Carrier::better_init() {
  LOG(ANABRID_DEBUG_INIT, __PRETTY_FUNCTION__);
 
  entity_id = net::StartupConfig::get().mac;
 
  if (entity_id.empty())
    return status("Cannot determine Carrier MAC");
 
  // Detect CTRL-block
  ctrl_block = entities::detect<blocks::CTRLBlock>(bus::address_from_tuple(bus::CTRL_BLOCK_BADDR, 0));
  if (!ctrl_block) {
    return status("Missing CTRL Block");
  }
 
  for (auto &cluster : clusters) {
    if (!cluster.init())
      return status("Cannot initialize cluster");
  }
 
  reset(entities::ResetAction::EVERYTHING);
 
  return status::success();
}
 
uint8_t carrier::Carrier::get_active_adc_channel_range() const{
  uint8_t num_adc_channels = 0;
  for(size_t idx = 0; idx < adc_channels.size(); idx++){
    if(adc_channels[idx] != ADC_CHANNEL_DISABLED)
      num_adc_channels = idx + 1;
  }
  return num_adc_channels;
}
 
std::vector<entities::Entity *> carrier::Carrier::get_child_entities() {
  std::vector<entities::Entity *> children;
  for (auto &cluster : clusters) {
    children.push_back(&cluster);
  }
  if (ctrl_block)
    children.push_back(ctrl_block);
  return children;
}
 
entities::Entity *carrier::Carrier::get_child_entity(std::string_view child_id) {
  auto opt_int = utils::view_to_number(child_id);
  if (opt_int.has_value()) {
    auto cluster_idx = opt_int.value();
    if (cluster_idx < 0 or clusters.size() < cluster_idx)
      return nullptr;
    return &clusters[cluster_idx];
  }
  if (child_id == "CTRL")
    return ctrl_block;
  if (child_id.size() == 17)
    return this;
  return nullptr;
}
 
ConfigResult carrier::Carrier::config(const pb_Config &cfg) {
  if (!hardware)
    return ConfigResult::err("Carrier cannot be configured without hardware");
 
  if (cfg.which_kind == pb_Config_adc_config_tag) {
    auto& carrier_config = cfg.kind.adc_config;
 
    adc_channels.fill(ADC_CHANNEL_DISABLED);
    size_t min_size = std::min(
      static_cast<size_t>(carrier_config.channels_count),
      adc_channels.size()
    );
 
    for (size_t idx = 0; idx < min_size; ++idx) {
      auto& config_channel = carrier_config.channels[idx];
      adc_channels[idx] = config_channel.idx;
      adc_gains[idx] = config_channel.gain;
      adc_offsets[idx] = config_channel.offset;
    }
 
    auto res = hardware->write_adc_bus_mux(adc_channels);
    if (!res)
      return ConfigResult::err("Could not configure ACLs from configuration");
 
    return ConfigResult::ok(true);
  }
 
  return ConfigResult::ok(false);
}
 
void carrier::Carrier::extract(entities::ExtractVisitor &collector) {
  Entity::extract(collector);
 
  auto& cfg = collector.create(pb_Config_adc_config_tag);
  auto& carrier_config = cfg.kind.adc_config;
  for (size_t idx = 0; idx < adc_channels.size(); ++idx) {
    auto channel = adc_channels[idx];
    if (channel == ADC_CHANNEL_DISABLED) continue;
    carrier_config.channels[carrier_config.channels_count++] = {
      .idx = static_cast<uint32_t>(channel),
      .gain = adc_gains[idx],
      .offset = adc_offsets[idx],
    };
  }
}
 
status carrier::Carrier::write_to_hardware() {
  status error;
  int cluster_index = 1;
  for (auto &cluster : clusters) {
    if (!cluster.write_to_hardware()) {
      error.code += cluster_index; // maxes to 6 = 0b110
      error.msg += "Cluster write failed.";
    }
    cluster_index++;
  }
 
  error.attach(write_adcs_to_hardware());
 
  return error;
}
 
status carrier::Carrier::calibrate_offsets() {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  status success;
  for (auto &cluster : clusters)
    success.attach(cluster.calibrate_offsets());
  return success;
 
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
}
 
status carrier::Carrier::calibrate_routes() {
  entities::Setup setup;
  setup.extract(this);
  status result = calibrate_routes_raw();
  setup.apply(this, true);
  return result;
}
 
// This calibration function is working for a generic array of clusters that don't have any shared connections.
// LUCIDAC relies on this function, an mREDAC overwrites this function, as it can have cross cluster
// connections.
status carrier::Carrier::calibrate_routes_raw() {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  status success;
 
  bool gain_correction_exceeding = false;
  for (auto &cluster : clusters) {
    LOG_ANABRID_DEBUG_CALIBRATION(
        ("Calibrating routes in cluster " + std::to_string(cluster.get_cluster_idx())).c_str());
    // Save and change ADC bus selection
    auto old_adcbus = ctrl_block->get_adc_bus();
    auto old_adc_channels = get_adc_channels();
 
    reset_adc_channels();
 
    ctrl_block->set_adc_bus(blocks::CTRLBlock::ADCBus::ADC);
    if (!ctrl_block->write_to_hardware())
      success.attach("Writing to ctrl-block failed");
 
    // Save current U-block transmission modes and set them to zero
    LOG_ANABRID_DEBUG_CALIBRATION("Starting calibration");
    auto old_transmission_modes = cluster.ublock->get_all_transmission_modes();
    auto old_reference_magnitude = cluster.ublock->get_reference_magnitude();
 
    // Enable reference signal for calibration
    LOG_ANABRID_DEBUG_CALIBRATION("Enable u-block reference");
    cluster.ublock->change_all_transmission_modes(blocks::UBlock::Transmission_Mode::POS_REF);
    if (!cluster.ublock->write_to_hardware())
      success.attach("Resetting u-block failed");
 
    // Save C-Block factors
    LOG_ANABRID_DEBUG_CALIBRATION("Reset c-block");
    auto old_c_block_factors = cluster.cblock->get_factors();
    cluster.cblock->set_factors({});
    if (!cluster.cblock->write_to_hardware())
      success.attach("Resetting c-block failed");
 
    // Find an M block usable for its identity lanes
    blocks::MBlock *id_block = nullptr;
    if (cluster.m0block && cluster.m0block->has_id_lanes())
      id_block = cluster.m0block;
 
    if (!id_block && cluster.m1block && cluster.m1block->has_id_lanes())
      id_block = cluster.m1block;
 
    if (!id_block) {
      success.attach("No M Block with ID Lanes found, calibration impossible!");
      continue;
    }
 
    uint8_t id_in_lane = 0, id_out_lane = 4;
    for (auto i = 0; i < id_block->ID_OUTPUT_CONNECTIONS().size(); i++) {
      if (id_block->ID_OUTPUT_CONNECTIONS()[i] != -1) {
        id_out_lane = id_block->slot_to_global_io_index(i);
        id_in_lane = id_block->slot_to_global_io_index(id_block->ID_OUTPUT_CONNECTIONS()[i]);
        break;
      }
    }
 
    std::array<std::vector<uint8_t>, blocks::IBlock::NUM_OUTPUTS> old_i_block_connections;
    for (auto i_out_idx : blocks::IBlock::OUTPUT_IDX_RANGE()) {
      for (auto i_in_idx : blocks::IBlock::INPUT_IDX_RANGE()) {
 
        // Only do something is this lane is used in the original I-block configuration
        if (!cluster.iblock->is_connected(i_in_idx, i_out_idx))
          continue;
 
        // If we don't have a complete connection through the u block, we don't need / we can't calibrate this
        // route from here
        if (!cluster.ublock->is_output_connected(i_in_idx))
          continue;
 
        // Store this i block input to output connection
        old_i_block_connections[i_out_idx].emplace_back(i_in_idx);
      }
    }
 
    LOG_ANABRID_DEBUG_CALIBRATION("Reset i-block");
    cluster.iblock->reset_outputs();
    if (!cluster.iblock->write_to_hardware())
      success.attach("Resetting i-block failed");
 
    // Now we restore each summation isolated from the rest and route each summation output / i block output to
    // an M-Mul block
    for (auto i_out_idx : blocks::IBlock::OUTPUT_IDX_RANGE()) {
      for (auto i_in_idx : old_i_block_connections[i_out_idx])
        if (!cluster.iblock->connect(i_in_idx, id_in_lane))
          LOG_ANABRID_DEBUG_CALIBRATION("Connecting i-block failed");
 
      if (!cluster.iblock->write_to_hardware())
        success.attach("Writing to i-block failed");
 
      // Now switch through all relevant input signals and put one of them to 1.0
      for (auto i_in_idx : old_i_block_connections[i_out_idx]) {
        // Setup measure path
        if (!set_adc_channel(0, id_out_lane))
          success.attach("id_out_lane invalid");
        if (!hardware->write_adc_bus_mux(adc_channels))
          success.attach("Writing adcs failed!");
 
        // Depending on whether upscaling is enabled for this lane, we apply +1 or +0.1 reference
        // This is done on all lanes (but no other I-block connection exists, so no other current flows)
        bool upscaled_channel = cluster.iblock->get_upscaling(i_in_idx);
        cluster.ublock->change_reference_magnitude(upscaled_channel
                                                       ? blocks::UBlock::Reference_Magnitude::ONE_TENTH
                                                       : blocks::UBlock::Reference_Magnitude::ONE);
        if (!cluster.ublock->write_to_hardware())
          success.attach("Writing to u-block failed");
 
        // First measure the offset of each measuring lane. The SH Block can not calibrate offsets that occur
        // after himself so we need to take care of that.
        (void)cluster.cblock->set_factor(i_in_idx, 0.0f);
        (void)cluster.cblock->set_gain_correction(i_in_idx, 1.0f);
        if (!cluster.cblock->write_to_hardware())
          success.attach("Writing to c-block failed");
 
        // Calibrate offsets for this specific route
        success.attach(calibrate_offsets());
 
        auto measured_offset = -daq::average(daq::sample, 4, 10)[0];
 
        // Allow this connection to go up to full scale. Those values are allways legal so we can ignore the
        // return value
        (void)cluster.cblock->set_factor(i_in_idx, 1.0f);
        if (!cluster.cblock->write_to_hardware())
          success.attach("Writing to c-block failed");
 
        delay(10);
 
        // Measure gain output
        auto measured_gain = -daq::average(daq::sample, 4, 10)[0];
        LOG_ANABRID_DEBUG_CALIBRATION(measured_gain);
        // Calculate necessary gain correction
        auto gain_correction = (upscaled_channel ? 0.8f : 1.0f) / (measured_gain - measured_offset);
        LOG_ANABRID_DEBUG_CALIBRATION(gain_correction);
        if (gain_correction > 1.1f) {
          gain_correction_exceeding = true;
          gain_correction = 1.1f;
        }
        // Set gain correction on C-block, which will automatically get applied when writing to hardware
        if (!cluster.cblock->set_gain_correction(i_in_idx, gain_correction)) {
          (void)cluster.cblock->set_gain_correction(i_in_idx, 1.0f);
          success.attach("Gain correction could not be set as it is out of range. Resetting this "
                         "channel to default correction");
        }
        // Deactivate this lane again
        (void)cluster.cblock->set_factor(i_in_idx, 0.0f);
 
        LOG_ANABRID_DEBUG_CALIBRATION(" ");
      }
 
      cluster.iblock->reset_outputs();
      reset_adc_channels();
    }
 
    // Restore original U-block transmission modes and reference
    cluster.ublock->change_all_transmission_modes(old_transmission_modes);
    cluster.ublock->change_reference_magnitude(old_reference_magnitude);
    // Write them to hardware
    LOG_ANABRID_DEBUG_CALIBRATION("Restoring u-block");
    if (!cluster.ublock->write_to_hardware())
      success.attach("Restoring u-block failed");
 
    // Restore C-block factors
    cluster.cblock->set_factors(old_c_block_factors);
    LOG_ANABRID_DEBUG_CALIBRATION("Restoring c-block");
    if (!cluster.cblock->write_to_hardware())
      success.attach("Restoring c-block failed");
 
    // Restore I-block connections
    LOG_ANABRID_DEBUG_CALIBRATION("Restoring i-block");
    for (auto i_out_idx : blocks::IBlock::OUTPUT_IDX_RANGE())
      for (auto i_in_idx : old_i_block_connections[i_out_idx])
        if (!cluster.iblock->connect(i_in_idx, i_out_idx))
          LOG_ANABRID_DEBUG_CALIBRATION("Connecting i-block failed");
 
    if (!cluster.iblock->write_to_hardware())
      success.attach("Restoring i-block failed");
 
    // Restore ADC bus selection
    ctrl_block->set_adc_bus(old_adcbus);
    if (!ctrl_block->write_to_hardware())
      success.attach("Writing to ctrl-block failed");
 
    (void)set_adc_channels(old_adc_channels);
    if (!hardware->write_adc_bus_mux(adc_channels))
      success.attach("Writing adcs failed");
  }
 
  if (gain_correction_exceeding)
    success.attach("Gain correction is too high, this is probably a problem with the hardware");
 
 
  // Calibrate offsets for complete system
  success.attach(Carrier::calibrate_offsets());
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
  return success;
}
 
status carrier::Carrier::calibrate_mblock(platform::Cluster &cluster, blocks::MBlock &mblock) {
  // CARE: This function does not preserve the currently configured routes
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  status success;
 
  // The calibration for each M-Block is prepared by connecting all outputs
  // to the ADCs. This limits the calibration to one M-Block (and one cluster) at a time,
  // which is not a problem though.
  // Each M-Block must connect required reference signals by itself.
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Connecting outputs to ADC...");
  for (auto output_idx : blocks::MBlock::SLOT_OUTPUT_IDX_RANGE()) {
    auto lane_idx = mblock.slot_to_global_io_index(output_idx);
    if (!set_adc_channel(output_idx, lane_idx, cluster.get_cluster_idx()))
      success.attach("Writing adc select failed");
  }
  // Write to hardware
  success.attach(write_to_hardware());
 
  // Pass to calibration function
  LOG(ANABRID_DEBUG_CALIBRATION, "Passing control to M-block...");
  success.attach(mblock.calibrate(&cluster, this));
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Cleanup ADC connections...");
  reset_adc_channels();
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Cleaning up calibration signals...");
  reset(entities::ResetAction::CIRCUIT_RESET);
 
  // Write final clean-up to hardware
  success.attach(write_to_hardware());
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Calibration done.");
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  return success;
}
 
status carrier::Carrier::calibrate_m_blocks() {
  status success;
  for (auto &cluster : clusters)
    for (auto mblock : {cluster.m0block, cluster.m1block})
      if (mblock)
        success.attach(calibrate_mblock(cluster, *mblock));
 
  return success;
}
 
void carrier::Carrier::reset(entities::ResetAction action) {
  for (auto &cluster : clusters) {
    cluster.reset(action);
  }
  if (ctrl_block)
    ctrl_block->reset(action);
  reset_adc_channels();
}
 
const std::array<int8_t, 8> &carrier::Carrier::get_adc_channels() const { return adc_channels; }
const std::array<double, 8> &carrier::Carrier::get_adc_gains() const { return adc_gains; }
const std::array<double, 8> &carrier::Carrier::get_adc_offsets() const { return adc_offsets; }
 
bool carrier::Carrier::set_adc_channels(const std::array<int8_t, 8> &channels) {
  // Check that all inputs are in a valid range
  for (auto channel : channels) {
    if (channel < 0)
      continue;
    if (channel >= clusters.size() * 16)
      return false;
  }
  // Check that all inputs are different
  // This is actually not necessary, in principle one could split one signal to multiple ADC.
  // But for now we prohibit this, as it's more likely an error than an actual use-case.
  for (auto &channel : channels) {
    if (channel < 0)
      continue;
    for (auto &other_channel : channels) {
      if (std::addressof(channel) == std::addressof(other_channel))
        continue;
      if (channel == other_channel)
        return false;
    }
  }
  adc_channels = channels;
  return true;
}
 
bool carrier::Carrier::set_adc_channel(uint8_t adc_channel, int8_t src_channel) {
  if (adc_channel >= adc_channels.size())
    return false;
  if (src_channel < 0)
    src_channel = ADC_CHANNEL_DISABLED;
 
  // Check if channel is already routed to an ADC
  // This is not really necessary, but probably a good idea (see set_adc_channels)
  if (src_channel != ADC_CHANNEL_DISABLED)
    for (auto other_idx = 0u; other_idx < adc_channels.size(); other_idx++)
      if (adc_channel != other_idx and adc_channels[other_idx] == src_channel)
        return false;
 
  adc_channels[adc_channel] = src_channel;
  return true;
}
 
bool carrier::Carrier::set_adc_channel(uint8_t adc_channel, int8_t src_idx, uint8_t cluster_idx) {
  return Carrier::set_adc_channel(adc_channel, src_idx + cluster_idx * 16);
}
 
void carrier::Carrier::reset_adc_channels() {
  adc_channels.fill(ADC_CHANNEL_DISABLED);
}
 
status carrier::Carrier::measure_all_signals(std::array<std::array<float, 8>, 6> &data) {
  auto old_adc_channels = get_adc_channels();
  reset_adc_channels();
  size_t idx = static_cast<size_t>(-1);
  for (const auto &cluster : clusters) {
    for (const auto mblock : {cluster.m0block, cluster.m1block}) {
      ++idx;
      if (!mblock) continue;
 
      for (auto slot : blocks::MBlock::SLOT_OUTPUT_IDX_RANGE())
        if (!set_adc_channel(slot, mblock->slot_to_global_io_index(slot), cluster.get_cluster_idx()))
          return status("Error while setting ADC channels.");
      if (!write_to_hardware())
        return status("Error while writing ADC channels.");
      data[idx] = daq::average();
    }
  }
  if (!set_adc_channels(old_adc_channels) or !write_to_hardware())
    LOG_ERROR("Error while restoring ADC channels.");
  return status::success();
}
 
status carrier::Carrier::write_adcs_to_hardware() {
  status error;
 
  if (ctrl_block && !ctrl_block->write_to_hardware()) {
    error.code |= 0x8;
    error.msg += "CTRL Block write failed.";
  }
  if (!hardware->write_adc_bus_mux(adc_channels)) {
    error.code |= 0x10;
    error.msg += "ADC Bus write failed.";
  }
 
  return error;
}
 
status carrier::Carrier::user_set_extended_config(Carrier* carrier, const pb_ConfigCommand& msg_in, pb_ConfigResponse &msg_out) {
#ifdef ANABRID_DEBUG_COMMS
  Serial.println(__PRETTY_FUNCTION__);
#endif
 
  bool default_reset_before = true, default_calibrate_mblock = false, default_calibrate_offset = false, default_calibrate_routes = true;
 
  if (msg_in.reset_before | default_reset_before) {
    carrier->reset(entities::ResetAction::CIRCUIT_RESET | entities::ResetAction::OVERLOAD_RESET);
    (void)carrier->write_to_hardware();
  }
 
  if(msg_in.calibrate_mblock || default_calibrate_mblock)
    RETURN_IF_FAILED(carrier->calibrate_m_blocks());
 
  auto res = setup(carrier, msg_in, msg_out);
 
  if (!res)
    return res;
 
  if (msg_in.calibrate_offset || default_calibrate_offset)
    RETURN_IF_FAILED(carrier->calibrate_offsets());
 
  if (msg_in.calibrate_routes || default_calibrate_routes)
    RETURN_IF_FAILED(carrier->calibrate_routes());
 
  return status::success();
}
 
status carrier::Carrier::user_get_overload_status(pb_OverloadStatus &overload_status) {
  entities::OverloadVisitor visitor;
  visitor.visit(this, true);
  visitor.to(overload_status);
  return status::success();
}