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_receive);
  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: Instead of saving adc channels, switch to direct hardware access as well
  _orig_adc_channels = redac.get_adc_channels();
 
  // 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) {
  if (!broadcast_output->write(envelope))
    return UnitResult::err("Unable to send UDP data");
  return UnitResult::ok();
}
 
// 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::separate_lane(uint8_t i_in_lane) {
  for (const auto &cluster : redac.clusters) {
    // 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] & cluster.iblock->hardware->INPUT_BITMASK(i_in_lane)) {
        original_output_lane = lane;
        break;
      }
    }
 
    // i_in_lane doesn't get used in this cluster so do not connect anything
    if (original_output_lane == 0xff) {
      cluster.iblock->hardware->write_outputs({});
      continue;
    }
    // Write to new sum directly to hardware
    std::array<uint32_t, 16> new_outputs = {};
    uint8_t id_in_lane = used_id_lanes[cluster.get_cluster_idx()].first;
    new_outputs[id_in_lane] = original_i_config[original_output_lane];
 
    cluster.iblock->hardware->write_outputs(new_outputs);
  }
 
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::do_initial() {
  // Entry into this function is synchronized with +-10μs jitter
 
  // Set all signals to zero
  // Takes ~2ms for all clusters
  for (auto &cluster : redac.clusters) {
    for (auto lane : blocks::CBlock::OUTPUT_IDX_RANGE())
      if (!cluster.cblock->hardware->write_factor(lane, 0.0))
        return UnitResult::err("Error setting factor on C-block");
 
    if (!cluster.ublock->hardware->write_transmission_modes_and_ref(
            {blocks::UBlock::Transmission_Mode::POS_REF, blocks::UBlock::Transmission_Mode::POS_REF},
            blocks::UBlock::Reference_Magnitude::ONE))
      return UnitResult::err("Error setting constant on U-block");
  }
 
  // Account for timing variances on other boards
  // This could most likely be reduced further, especially considering track_time below
  delayMicroseconds(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);
  }
  delayMicroseconds(10000);
  for (auto &cluster : redac.clusters) {
    cluster.shblock->hardware->set_state(blocks::SHState::INJECT);
  }
  delayMicroseconds(5000);
 
  // TODO: Move into prepare function
  redac.reset_adc_channels();
 
  for (auto &cluster : redac.clusters) {
    // TODO: reset cblock gain correction factor
 
    // 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!");
 
    uint8_t id_in_lane = 0, id_out_lane = 0;
 
    auto id_connections = id_lane_block->ID_OUTPUT_CONNECTIONS();
    for (uint8_t lane = 0; lane < id_connections.size(); lane++) {
      if (id_connections[lane] != -1) {
        id_in_lane = id_lane_block->slot_to_global_io_index(id_connections[lane]);
        id_out_lane = id_lane_block->slot_to_global_io_index(lane);
        break;
      }
    }
 
    used_id_lanes.emplace_back(id_in_lane, id_out_lane);
 
    TRY(redac.set_adc_channel(cluster.get_cluster_idx(), static_cast<int8_t>(id_out_lane),
                               cluster.get_cluster_idx()));
  }
  if (!redac.write_adcs_to_hardware())
    return UnitResult::err("Error writing ADCs to hardware");
 
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::do_lane(uint8_t lane) {
  GateTimer 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
  for (auto &cluster : redac.clusters) {
    // Find out whether any of the targets of the signal is up-scaling
    // TODO: We can only calibrate signals where all targets are either up-scaling or not.
    //       So it would be nice to check this here.
    //       But of course, we could only check the local usages.
    bool signal_upscaled = TRY(redac.routing.is_upscaled(cluster.get_cluster_idx(), lane));
    // Set lane to constant 0.1 or 1
    if (!cluster.ublock->hardware->write_transmission_modes_and_ref(
            {blocks::UBlock::Transmission_Mode::POS_REF, blocks::UBlock::Transmission_Mode::POS_REF},
            signal_upscaled ? blocks::UBlock::Reference_Magnitude::ONE_TENTH
                            : blocks::UBlock::Reference_Magnitude::ONE
 
            ))
      return UnitResult::err("Error setting constant on U-block");
    if (!cluster.cblock->hardware->write_factor(lane, 1.0))
      return UnitResult::err("Error setting factor on C-block");
  }
 
  // TODO: Maybe move pre-computation into prepare() function
  TRY(separate_lane(lane));
 
  // Account for timing variances on other boards and possible signal transients
  // This could most likely be reduced further
  timer.release_after_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)
  timer.release_after_us(1000);
  // Reset signal to zero and measure offset
  // Takes ~100μs
  for (auto &cluster : redac.clusters) {
    if (!cluster.cblock->hardware->write_factor(lane, 0.0))
      return UnitResult::err("Error setting factor on C-block");
  }
 
  // Account for timing variances on other boards and possible signal transients
  // This could most likely be reduced further
  timer.release_after_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_upscaled(cluster_idx, lane));
    float difference = offset_measure[cluster_idx] - gain_measure[cluster_idx];
    float gain_correction = (signal_upscaled ? 0.1f : 1.0f) / difference;
 
    if (gain_correction > 1.1f) {
      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::distribute_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) {
    received_gain_correction[cluster_idx][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);
  if (src_sector != blocks::TBlock::Sector::BPL) {
    // Signal is local, no need to send data over network
    auto src_cluster_idx = static_cast<uint8_t>(src_sector) - 1;
    received_gain_correction[src_cluster_idx][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
 
  pb_Envelope envelope;
  auto& msg_out = transport::init_v1_message(envelope, pb_MessageV1_calibrate_data_command_tag);
  auto& data_cmd = msg_out.kind.calibrate_data_command;
  data_cmd.has_data = true;
 
  auto src_entity_path = TRY(redac.routing.source_path(cluster_idx, lane));
  auto signal_source = TRY(redac.routing.ip_address(src_entity_path.segment(0)));
 
  data_cmd.data = pb_CalibrationData{
    .lane = lane,
    .gain_correction = gain_correction
  };
 
  transport::DatagramMessageOutputStream<UDP*> data_output(&net_receive, signal_source, net_receive_port);
  data_output.write(envelope);
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::receive_gain_corrections(uint32_t timeout) {
  pb_Envelope envelope;
  while (true) {
    if (!TRY(data_input->read(envelope, timeout)))
      return UnitResult::ok();
 
    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) {
      LOG_ERROR("Expected calibration data but message is of different kind.")
      return UnitResult::err("Expected calibration data but message is of different kind");
    }
 
    auto& data_in = msg_in.kind.calibrate_data_command.data;
    // We must determine which cluster generates the signal
    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("Not expected calibration data for not connected backplane!");
 
    auto cluster_idx = static_cast<uint8_t>(source) - 1;
    received_gain_correction[cluster_idx][data_in.lane].add(data_in.gain_correction);
  }
}
 
UnitResult CalibrationBase::send_receive_apply_gain_corrections() {
  // Entry into this function is synchronized with +-10μs jitter
 
  GateTimer 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(distribute_gain_corrections(cluster_idx, lane_idx, gain_correction));
 
      TRY(receive_gain_corrections(5));
 
      timer.release_after_us(10);
    }
  }
 
  TRY(receive_gain_corrections(5000));
 
 
  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 = received_gain_correction[cluster_idx][lane_idx];
      auto count = TRY(redac.routing.use_count(cluster_idx, lane_idx));
 
      if (count != gain_correction.get_n()) {
        auto node = TRY(redac.routing.cluster2node(cluster_idx));
 
        return UnitResult::err_fmt(
          "Mismatch signal use and gain correction count of carrier %s/%d (%d) on lane %d. Received %d but expected %d.",
          redac.get_entity_id().c_str(),
          cluster_idx,
          node,
          lane_idx,
          gain_correction.get_n(),
          count);
      }
 
      if (!gain_correction.get_n()) continue;
 
      auto gain_correction_avg = gain_correction.get_average();
      if (gain_correction_avg > 1.1f) {
        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.");
 
  // Restore original configuration
  TRY(redac.set_adc_channels(_orig_adc_channels));
  TRY(redac.write_to_hardware());
 
  return UnitResult::ok();
}
 
UnitResult CalibrationBase::do_final_offset_correction() {
  // Entry into this function is synchronized with +-10μs jitter
  for (auto &cluster : redac.clusters) {
    if (!cluster.ublock->hardware->write_transmission_modes_and_ref(
            {blocks::UBlock::Transmission_Mode::GROUND, blocks::UBlock::Transmission_Mode::GROUND},
            blocks::UBlock::Reference_Magnitude::ONE))
      return UnitResult::err("Error setting ground on U-block");
  }
 
  // Account for timing variances on other boards
  // This could most likely be reduced further, especially considering track_time below
  delayMicroseconds(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);
  }
  delayMicroseconds(10000);
  for (auto &cluster : redac.clusters) {
    cluster.shblock->hardware->set_state(blocks::SHState::INJECT);
  }
  // TODO: This delay could probably be removed
  delayMicroseconds(5000);
 
  // Restore U-block
  for (auto &cluster : redac.clusters) {
    // TODO: Could be optimized by only writing transmission modes
    TRY(cluster.ublock->write_to_hardware());
  }
 
  return UnitResult::ok();
}
 
UnitResult CalibrationLeader::do_() {
  GateTimer timer;
  LOG_ALWAYS("Init calibration routes for run as leader...")
  timer.release_after_ms(500);
  TRY(do_initial());
  LOG_ALWAYS("Started calibration routes for run as leader...")
 
  for (uint8_t lane = 0u; lane < 32; lane++) {
    timer.release_after_us(1000);
    TRY(do_lane(lane));
  }
  timer.release_after_us(1000);
  TRY(send_receive_apply_gain_corrections());
  // TODO: Determine how long we should actually wait here.
  //       There can be a lot of variance in the previous send_receive_apply_gain_corrections(),
  //       so for now we stick with a large -- and safe -- delay.
  timer.release_after_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::send_receive_apply_gain_corrections() {
  pb_Envelope envelope;
  transport::init_v1_message(envelope, pb_MessageV1_calibrate_finalize_command_tag);
  TRY(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::send_receive_apply_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) {
  if (TRY(broadcast_input->read(envelope, timeout_ms)))
    return UnitResult::ok();
 
  return UnitResult::err("Timout while waiting for incoming calibration command");
}
 
UnitResult CalibrationFollower::do_() {
  LOG_ALWAYS("Init calibrating routes for run as follower...")
  TRY(do_initial());
  LOG_ALWAYS("Starting calibrating routes for run as follower...")
  while (true) {
    TRY(do_lane_as_told());
    if (is_done())
      break;
  }
  TRY(send_receive_apply_gain_corrections());
  TRY(do_final_offset_correction());
  return UnitResult::ok();
}
 
UnitResult CalibrationFollower::do_initial() {
  pb_Envelope envelope;
  TRY(receive_command(envelope));
  return CalibrationBase::do_initial();
}
 
UnitResult CalibrationFollower::do_lane_as_told() {
  pb_Envelope envelope = pb_Envelope_init_default;
  TRY(receive_command(envelope));
  auto& msg = envelope.kind.message_v1;
  if (msg.which_kind != pb_MessageV1_calibrate_lane_command_tag) {
    return UnitResult::err_fmt(
      "Expected calibration lane command but message is of different kind: %d",
      msg.which_kind
    );
  }
  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::send_receive_apply_gain_corrections() {
  pb_Envelope envelope;
  TRY(receive_command(envelope));
  return CalibrationBase::send_receive_apply_gain_corrections();
}
 
UnitResult CalibrationFollower::do_final_offset_correction() {
  pb_Envelope envelope;
  TRY(receive_command(envelope));
  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();
}