calibration.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 "proto/main.pb.h"
#include "redac/calibration.h"
#include "handlers/calibration.h"
 
#include "utils/timer.h"
 
#include <Arduino.h>
#include <utility>
 
namespace platform{
 
net::EthernetUDP CalibrationBase::net_group;
//net::EthernetUDP CalibrationBase::net_receive;
 
Box<UdpMessageOutputStream> CalibrationBase::broadcast_output;
Box<UdpMessageInputStream> CalibrationBase::broadcast_input;
//Box<UdpMessageInputStream> CalibrationBase::data_input;
 
void CalibrationBase::begin() {
  broadcast_output = std::make_unique<UdpMessageOutputStream>(&net_group, net_group_ip, net_group_port);
  broadcast_input = std::make_unique<UdpMessageInputStream>(&net_group);
  //data_input = std::make_unique<UdpMessageInputStream>(&net_group);
  if (!net_group.beginMulticast(net_group_ip, net_group_port))
    LOG_ALWAYS("unable to open net_group_port")
 
  //if (!net_receive.begin(net_receive_port))
  //  LOG_ALWAYS("unable to open net_receive_port")
}
 
void CalibrationBase::clear(uint32_t timeout_ms) {
  pb_Envelope envelope = pb_Envelope_init_zero;
  while (broadcast_input->has_message())
    IGNORE(broadcast_input->read(envelope, timeout_ms));
 
  //while (data_input->has_message())
  //  IGNORE(data_input->read(envelope, timeout_ms));
}
 
CalibrationBase::CalibrationBase(REDAC &redac)
    : redac(redac){
  // TODO: Check block null pointer
  // TODO: Check assumptions of calibration (e.g. no signal is used locally and externally)
}
 
UnitResult CalibrationBase::send(const pb_Envelope &envelope) {
  return broadcast_output->write(envelope);
}
 
// searches where lane i_in_lane gets used in summation and prepares this summation node to be routed to an
// M-Block ID lane. This does not preserve any connections. It also automatically connects the ADCs to the
// chosen output. i.e. the signal in cluster 0 gets routed to ADC 0 and so on.
UnitResult CalibrationBase::prepare_lane(uint8_t in_lane) {
  for (const auto &cluster : redac.clusters) {
    bool signal_upscaled = TRY(redac.routing.is_source_upscaled(cluster.get_cluster_idx(), in_lane));
    // Set lane to constant 0.1 or 1
 
    auto ref = signal_upscaled
        ? blocks::UBlock::Reference_Magnitude::ONE_TENTH
        : blocks::UBlock::Reference_Magnitude::ONE;
 
    constexpr auto mode = blocks::UBlockHAL::Transmission_Mode::POS_REF;
    TRY_TRUE(cluster.ublock->hardware->write_transmission_modes_and_ref({mode, mode}, ref));
    TRY_TRUE(cluster.cblock->hardware->write_factor(in_lane, 1.0));
 
    // Find used output lane
    auto original_i_config = cluster.iblock->get_outputs();
    uint8_t original_output_lane = 0xff;
 
    for (uint8_t lane = 0; lane < original_i_config.size(); lane++) {
      if (original_i_config[lane] & blocks::IBlockHAL::INPUT_BITMASK(in_lane)) {
        original_output_lane = lane;
        break;
      }
    }
 
    std::array<uint32_t, 16> new_outputs{};
    if (original_output_lane < 16) {
      uint8_t id_in_lane = cluster2id_lane[cluster.get_cluster_idx()];
      new_outputs[id_in_lane] = original_i_config[original_output_lane];
    }
 
    TRY_TRUE(cluster.iblock->hardware->write_outputs(new_outputs));
  }
 
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::do_initial() {
  // Entry into this function is synchronized with +-10μs jitter
 
  Sync timer;
  // Set all signals to zero
  // Takes ~2ms for all clusters
 
  constexpr auto pos = blocks::UBlock::Transmission_Mode::POS_REF;
  constexpr auto one = blocks::UBlock::Reference_Magnitude::ONE;
  for (auto &cluster : redac.clusters) {
    TRY_TRUE(cluster.ublock->hardware->write_transmission_modes_and_ref({pos, pos}, one));
 
    for (size_t lane = 0; lane < 32 ; ++lane) {
      TRY_TRUE(cluster.cblock->hardware->write_factor(lane, 0.0));
    }
  }
 
  // Account for timing variances on other boards
  // This could most likely be reduced further, especially considering track_time below
  timer.sync_at_us(100);
 
  // Do SH-block offset correction for each cluster in parallel
  // Takes track_delay + inject_delay + 100μs for write_to_hardware
  // TODO: Refactor into carrier convenience function
  for (auto &cluster : redac.clusters) {
    cluster.shblock->hardware->set_state(blocks::SHState::TRACK);
  }
  timer.sync_at_us(10000);
  for (auto &cluster : redac.clusters) {
    cluster.shblock->hardware->set_state(blocks::SHState::INJECT);
  }
  timer.sync_at_us(5000);
 
  // TODO: Move into prepare function
  std::array<carrier::ADCChannel, 8> adc_channels;
  for (auto &cluster : redac.clusters) {
    // Check if M-Block with ID lane is reachable
    blocks::MBlock *id_lane_block = nullptr;
    if (cluster.m0block->has_id_lanes())
      id_lane_block = cluster.m0block;
    if (cluster.m1block->has_id_lanes())
      id_lane_block = cluster.m1block;
 
    if (id_lane_block == nullptr)
      return UnitResult::err("No M-Block with ID lane found in this cluster!");
 
    auto id_connections = id_lane_block->ID_OUTPUT_CONNECTIONS();
    size_t lane = 0;
    for (; lane < id_connections.size(); lane++) {
      if (id_connections[lane] == -1) continue;
      uint8_t id_in_lane = id_lane_block->slot_to_global_io_index(id_connections[lane]);
      uint8_t id_out_lane = id_lane_block->slot_to_global_io_index(lane);
 
      cluster2id_lane.push_back(id_in_lane);
      adc_channels[cluster.get_cluster_idx()].src = id_out_lane + cluster.get_cluster_idx() * 16;
      break;
    }
 
    if (lane == id_connections.size())
      return UnitResult::err("No M-Block with ID lane found!");
  }
 
  TRY(redac.write_adcs_to_hardware(adc_channels));
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::do_lane(uint8_t lane) {
  Sync timer;
  // Entry into this function is synchronized with +-10μs jitter
  // Set this signal to 1 or 0.1
  // Takes ~100μs for all clusters, mostly in write_to_hardware
  TRY(prepare_lane(lane));
 
  // Account for timing variances on other boards and possible signal transients
  // This could most likely be reduced further
  TRY_TRUE(timer.sync_at_us(5000));
 
  // Measure signal here
  auto gain_measure = daq::average(daq::sample, 4, 10);
 
  // Wait before resetting signal (important to reduce problems caused by jitter)
  TRY_TRUE(timer.sync_at_us(5000));
  // Reset signal to zero and measure offset
  // Takes ~100μs
  for (auto &cluster : redac.clusters) {
    TRY_TRUE(cluster.cblock->hardware->write_factor(lane, 0.0));
  }
 
  // Account for timing variances on other boards and possible signal transients
  // This could most likely be reduced further
  TRY_TRUE(timer.sync_at_us(5000));
 
  auto offset_measure = daq::average(daq::sample, 4, 10);
 
  // Analyse data
  bool gain_correction_exceeding = false;
  for (size_t cluster_idx = 0; cluster_idx < redac.clusters.size(); cluster_idx++) {
    if (!TRY(redac.routing.is_sink_present(cluster_idx, lane)))
      continue;
 
    bool signal_upscaled = TRY(redac.routing.is_source_upscaled(cluster_idx, lane));
 
    float difference = offset_measure[cluster_idx] - gain_measure[cluster_idx];
    float gain_correction = (signal_upscaled ? 0.8f : 1.0f) / difference;
 
    if (gain_correction < 0.7f || 1.2f < gain_correction) {
      bool carrier_idx = redac.routing.self_carrier();
      return UnitResult::err_fmt("Gain correction far beyond normal %f at %d/%d/%d", gain_correction, carrier_idx, cluster_idx, lane);
    }
 
    if (gain_correction > 1.1f) {
      bool carrier_idx = redac.routing.self_carrier();
      LOGV("Gain correction exceeding 1.1 at %s(%d)/%d/%d which is %f at", redac.get_entity_id().c_str(), carrier_idx, cluster_idx, lane, gain_correction);
      gain_correction = 1.1f;
      gain_correction_exceeding = true;
    }
 
    measured_gain_correction[cluster_idx][lane] = gain_correction;
  }
 
  if (gain_correction_exceeding) {
    LOG_ERROR("Gain correction exceeding 1.1.");
  }
 
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::collect_gain_corrections(uint8_t cluster_idx, uint8_t lane, float gain_correction) {
  if (!TRY(redac.routing.is_sink_present(cluster_idx, lane))) {
    return UnitResult::ok();
  }
 
  if (lane < 8) {
    agg_gain_corrs[cluster_idx + 1][lane].add(gain_correction);
    return UnitResult::ok();
  }
 
  auto dst_sector = static_cast<blocks::TBlock::Sector>(cluster_idx + 1);
  auto src_sector = redac.carrier_t_block->src_of_signal(dst_sector, lane - 8);
  auto& agg_gain_corr = agg_gain_corrs[src_sector][lane];
 
  if (src_sector == 0) {
    auto dst_lane = TRY(redac.routing.cluster2lane(cluster_idx, lane));
    auto src_lane = TRY(redac.routing.source(dst_lane));
    if (agg_gain_corr.get_n() == 0) {
      bpl_srcs[lane] = src_lane;
    }else if (bpl_srcs[lane] != src_lane) {
      return UnitResult::err_fmt("Expected source lane be unique %s:%d:%d from %d:%d:%d - %d:%d:%d", redac.get_entity_id().c_str(), cluster_idx, lane,  src_lane.carrier(), src_lane.cluster(), src_lane.lane(), bpl_srcs[lane].carrier(), bpl_srcs[lane].cluster(), bpl_srcs[lane].lane());
    }
  }
 
  agg_gain_corr.add(gain_correction);
 
 
  /*if (src_sector != blocks::TBlock::Sector::BPL) {
    // Signal is local, no need to send data over network
    aggregate_gain_correction[src_sector][lane].add(gain_correction);
    return UnitResult::ok();
  }*/
 
  // Send signal to other redac
  // Package can be very simplistic, containing only:
  // [lane_idx, gain_correction]
  // The receiving side must determine which cluster it came from.
  // Send every measurement as separate package. Allows for easy error correction when we get to that point
 
 
  //auto src_node_path = TRY(redac.routing.node2path(src_lane.node()));
  //stage.add(src_node_path, lane, gain_correction);
 
  return UnitResult::ok();
}
 
BoolResult CalibrationBase::receive_gain_corrections(uint32_t timeout) {
  pb_Envelope envelope;
  if (!TRY(broadcast_input->read(envelope, timeout)))
    return BoolResult::ok(false);
 
  if (envelope.which_kind != pb_Envelope_message_v1_tag)
    return UnitResult::err("Expected message v1 tag");
 
  auto& msg_in = envelope.kind.message_v1;
  if (msg_in.which_kind != pb_MessageV1_calibrate_data_command_tag)
    return UnitResult::err_fmt("Expected calibration data but message is of different kind: %d", msg_in.which_kind);
 
  auto& cmd = msg_in.kind.calibrate_data_command;
  for (auto data_idx = 0; data_idx < cmd.data_count; ++data_idx) {
    auto& data_in = cmd.data[data_idx];
    // We must determine which cluster generates the signal
 
    if (data_in.carrier != redac.routing.self_carrier())
      continue;
 
    if (data_in.lane < 8)
      return UnitResult::err("Not expected calibration data for first 8 lanes via network!");
 
    auto source = redac.carrier_t_block->src_of_signal(blocks::TBlock::Sector::BPL, data_in.lane - 8);
    if (source == blocks::TBlock::Sector::BPL)
      return UnitResult::err_fmt("Not expected calibration data for not connected backplane! %d", static_cast<int>(redac.carrier_t_block->get_connections()[0]));
 
    agg_gain_corrs[source][data_in.lane].add(data_in.gain_correction, data_in.weight);
  }
 
  return BoolResult::ok(true);
}
 
UnitResult CalibrationBase::exchange_gain_corrections() {
  // Entry into this function is synchronized with +-10μs jitter
 
  Sync timer;
  for (uint8_t cluster_idx = 0; cluster_idx < 3; ++cluster_idx) {
    for (uint8_t lane_idx = 0; lane_idx < 32; ++lane_idx) {
      // sending gain corrections others wait for
      auto gain_correction = measured_gain_correction[cluster_idx][lane_idx];
      TRY(collect_gain_corrections(cluster_idx, lane_idx, gain_correction));
    }
  }
 
  CalibrationStaging staging;
  for (uint8_t lane_idx = 0; lane_idx < 32; ++lane_idx) {
    auto src = bpl_srcs[lane_idx];
    if (!src.valid()) continue;
    auto& correction = agg_gain_corrs[0][lane_idx];
    TRY(staging.add(src, correction.get_average(), correction.get_n()));
  }
 
  TRY_TRUE(timer.sync_at_ms(10));
  for (auto idx = 0; idx < redac.routing.num_carrier(); ++idx) {
    if (idx == redac.routing.self_carrier()) {
      TRY(broadcast_output->write(staging.envelope));
    }else{
      TRY(receive_gain_corrections(9));
    }
 
    TRY_TRUE(timer.sync_at_ms(10));
  }
 
  bool gain_correction_exceeding = false;
  // Actually apply the gathered values
  for (auto cluster_idx = 0; cluster_idx < 3 ; ++cluster_idx) {
    for (auto lane_idx = 0; lane_idx < 32 ; ++lane_idx) {
      auto& gain_correction = agg_gain_corrs[cluster_idx + 1][lane_idx];
      auto expected_count = TRY(redac.routing.use_count(cluster_idx, lane_idx));
 
      if(expected_count != gain_correction.get_n())
          return UnitResult::err_fmt(
            "Mismatch signal use and gain correction count of carrier %s/%d on lane %d. Received %d but expected %d.",
            redac.get_entity_id().c_str(),
            cluster_idx,
            lane_idx,
            gain_correction.get_n(),
            expected_count);
 
      if (gain_correction.get_n() == 0)
        continue;
 
      auto gain_correction_avg = gain_correction.get_average();
      if (gain_correction_avg > 1.1f) {
        LOGV("Received gain correction exceeding 1.1 which is %f", gain_correction_avg);
        gain_correction_avg = 1.1f;
        gain_correction_exceeding = true;
      }
 
      TRY (redac.clusters[cluster_idx].cblock->set_gain_correction(lane_idx, gain_correction_avg));
    }
  }
 
  if (gain_correction_exceeding)
    LOG_ERROR("Gain correction exceeding 1.1.");
 
  TRY_TRUE(redac.write_to_hardware());
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::do_final_offset_correction() {
  // Entry into this function is synchronized with +-10μs jitter
  Sync timer;
  constexpr auto ground = blocks::UBlock::Transmission_Mode::GROUND;
  constexpr auto one = blocks::UBlock::Reference_Magnitude::ONE;
  for (auto &cluster : redac.clusters)
    TRY_TRUE(cluster.ublock->hardware->write_transmission_modes_and_ref({ground, ground}, one));
 
  // Account for timing variances on other boards
  // This could most likely be reduced further, especially considering track_time below
  TRY_TRUE(timer.sync_at_us(1000));
 
  // Do SH-block offset correction for each cluster in parallel
  // Takes track_delay + inject_delay + 100μs for write_to_hardware
  // TODO: Refactor into carrier convenience function
  for (auto &cluster : redac.clusters)
    cluster.shblock->hardware->set_state(blocks::SHState::TRACK);
  TRY_TRUE(timer.sync_at_us(10000));
  for (auto &cluster : redac.clusters)
    cluster.shblock->hardware->set_state(blocks::SHState::INJECT);
 
  TRY_TRUE(timer.sync_at_us(5000));
 
  for (auto &cluster : redac.clusters)
    TRY_TRUE(cluster.ublock->write_to_hardware());
  return UnitResult::ok();
}
 
UnitResult CalibrationLeader::do_() {
  Sync timer;
  LOG_ALWAYS("Init calibration routes for run as leader...")
  TRY_TRUE(timer.sync_at_ms(500));
  LOG_ALWAYS("Starting calibration routes for run as leader...")
  TRY(do_initial());
  LOG_ALWAYS("Started calibration routes for run as leader...")
  for (uint8_t lane = 0u; lane < 32; lane++) {
    TRY_TRUE(timer.sync_at_us(20000));
    TRY(do_lane(lane));
  }
  TRY_TRUE(timer.sync_at_us(1000));
  TRY(exchange_gain_corrections());
  TRY_TRUE(timer.sync_at_us(1000));
  LOG_ALWAYS("Finishing calibration routes for run as leader...")
  TRY_TRUE(redac.write_to_hardware());
  TRY_TRUE(timer.sync_at_us(10000));
  TRY(do_final_offset_correction());
  return UnitResult::ok();
}
 
UnitResult CalibrationLeader::do_initial() {
  pb_Envelope envelope;
  auto& msg = transport::init_v1_message(envelope, pb_MessageV1_calibrate_init_command_tag);
  msg.kind.calibrate_init_command = pb_CalibrateInitCommand_init_default;
  TRY(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::do_initial();
}
 
UnitResult CalibrationLeader::do_lane(uint8_t lane) {
  pb_Envelope envelope;
  auto& msg = transport::init_v1_message(envelope, pb_MessageV1_calibrate_lane_command_tag);
  auto& lane_cmd = msg.kind.calibrate_lane_command = pb_CalibrateLaneCommand_init_default;
  lane_cmd.lane = lane;
 
  TRY(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::do_lane(lane);
}
 
UnitResult CalibrationLeader::exchange_gain_corrections() {
  pb_Envelope envelope;
  transport::init_v1_message(envelope, pb_MessageV1_calibrate_finalize_command_tag);
  TRY(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::exchange_gain_corrections();
}
 
UnitResult CalibrationLeader::do_final_offset_correction() {
  pb_Envelope envelope;
  transport::init_v1_message(envelope, pb_MessageV1_calibrate_offset_command_tag);
 
  TRY(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::do_final_offset_correction();
}
 
CalibrationFollower::CalibrationFollower(REDAC &redac,
                                                   unsigned int timeout_ms)
    : CalibrationBase(redac), timeout_ms(timeout_ms) {}
 
UnitResult CalibrationFollower::receive_command(pb_Envelope &envelope) const {
  if (TRY(broadcast_input->read(envelope, timeout_ms)))
    return UnitResult::ok();
 
  return UnitResult::err("Timeout while waiting for incoming calibration command");
}
 
UnitResult CalibrationFollower::do_() {
  LOG_ALWAYS("Init calibrating routes for run as follower...")
  TRY(do_initial());
  LOG_ALWAYS("Started calibrating routes for run as follower...")
  while (true) {
    TRY(do_lane_as_told());
    if (is_done())
      break;
  }
  TRY(exchange_gain_corrections());
  TRY_TRUE(redac.write_to_hardware());
  TRY(do_final_offset_correction());
  return UnitResult::ok();
}
 
UnitResult CalibrationFollower::wait_for(pb_Envelope& envelope, int v1_tag) const {
  Sync timer;
  while (true){
    if (timer.elapsed_ms() > timeout_ms)
      return UnitResult::err("Error initializing calibration");
    TRY(receive_command(envelope));
    if (envelope.which_kind != pb_Envelope_message_v1_tag) continue;
    if (envelope.kind.message_v1.which_kind != v1_tag) continue;
    break;
  }
 
  return UnitResult::ok();
}
 
UnitResult CalibrationFollower::do_initial() {
  pb_Envelope envelope;
  TRY(wait_for(envelope, pb_MessageV1_calibrate_init_command_tag));
  return CalibrationBase::do_initial();
}
 
UnitResult CalibrationFollower::do_lane_as_told() {
  pb_Envelope envelope = pb_Envelope_init_default;
  TRY(wait_for(envelope, pb_MessageV1_calibrate_lane_command_tag));
 
  auto& msg = envelope.kind.message_v1;
  auto& lane_cmd = msg.kind.calibrate_lane_command;
  uint8_t lane = lane_cmd.lane;
  if (lane >= 32)
    return UnitResult::err("Error: Told to calibrate a lane >= 32.");
  done |= 1 << lane;
  return CalibrationBase::do_lane(lane);
}
 
UnitResult CalibrationFollower::exchange_gain_corrections() {
  pb_Envelope envelope;
  TRY(wait_for(envelope, pb_MessageV1_calibrate_finalize_command_tag));
  return CalibrationBase::exchange_gain_corrections();
}
 
UnitResult CalibrationFollower::do_final_offset_correction() {
  pb_Envelope envelope;
  TRY(wait_for(envelope, pb_MessageV1_calibrate_offset_command_tag));
  return CalibrationBase::do_final_offset_correction();
}
 
} // namespace platform
 
msg::handlers::HandleResult msg::handlers::RegisterExternalEntitiesRequestHandler::handle(const pb_Envelope &envelope_in, pb_Envelope &envelope_out) {
  transport::init_v1_message(envelope_out, pb_MessageV1_success_message_tag);
  auto& msg_in = envelope_in.kind.message_v1;
  auto redac = dynamic_cast<platform::REDAC*>(&platform::REDAC::get());
 
  auto& register_external_entities_cmd = msg_in.kind.register_external_entities_command;
  for (size_t idx = 0; idx < register_external_entities_cmd.entities_count; ++idx) {
    auto& entity = register_external_entities_cmd.entities[idx];
    if (!entity.has_value) continue;
    auto& data = entity.value.data.bytes;
    IPAddress address(data[0], data[1], data[2], data[3]);
    if (redac == nullptr) continue;
    redac->routing.set_path_address(std::string_view(entity.key), address);
  }
  return HandleResult::ok();
}