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 <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")
}
 
CalibrationBase::CalibrationBase(REDAC &redac, const run::Run &run)
    : 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)
}
 
status CalibrationBase::send(const pb_Envelope &envelope) {
  if (!broadcast_output->write(envelope))
    return "Unable to send UDP data";
  return status::success();
}
 
// 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.
status 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 status::success();
}
 
status 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 "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 "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 "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);
 
    if (!redac.set_adc_channel(cluster.get_cluster_idx(), static_cast<int8_t>(id_out_lane),
                               cluster.get_cluster_idx()))
      return "Error setting ADC lanes";
  }
  if (!redac.write_adcs_to_hardware())
    return "Error writing ADCs to hardware";
 
  return status::success();
}
 
status CalibrationBase::do_lane(uint8_t lane) {
  // 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 = false;
    for (auto dst : {blocks::TBlock::Sector::BPL, blocks::TBlock::Sector::CL0, blocks::TBlock::Sector::CL1,
                     blocks::TBlock::Sector::CL2})
      if (dst == blocks::TBlock::Sector::BPL)
        signal_upscaled |= redac.carrier_t_block->is_external_target_upscaled(lane - 8);
      else
        // TODO: It would be nice to additionally check whether the signal is actually in use here
        signal_upscaled |= redac.clusters[static_cast<uint8_t>(dst) - 1].iblock->get_upscaling(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 "Error setting constant on U-block";
    if (!cluster.cblock->hardware->write_factor(lane, 1.0))
      return "Error setting factor on C-block";
  }
 
  // TODO: Maybe move pre-computation into prepare() function
  RETURN_IF_FAILED(separate_lane(lane));
 
  // Account for timing variances on other boards and possible signal transients
  // This could most likely be reduced further
  delayMicroseconds(5000);
 
  // Measure signal here
  auto gain_measure = daq::average(daq::sample, 4, 10);
 
  // Wait before resetting signal (important to reduce problems caused by jitter)
  delayMicroseconds(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 "Error setting factor on C-block";
  }
 
  // Account for timing variances on other boards and possible signal transients
  // This could most likely be reduced further
  delayMicroseconds(5000);
 
  auto offset_measure = daq::average(daq::sample, 4, 10);
 
  // Analyse data
  bool gain_correction_exceeding = false;
  for (uint8_t cluster_idx = 0; cluster_idx < 3; cluster_idx++) {
    bool signal_upscaled = redac.clusters[cluster_idx].iblock->get_upscaling(lane);
    // Only calc if there is a signal there
    // TODO: Replace by boolean flag whether this channel is in use
    if (fabs(gain_measure[cluster_idx]) > 0.1) {
      float gain_correction =
          (signal_upscaled ? -0.8f : -1.0f) / (gain_measure[cluster_idx] - offset_measure[cluster_idx]);
 
      if (gain_correction > 1.1f) {
        gain_correction = 1.1f;
        gain_correction_exceeding = true;
      }
      if (lane < 8) {
        // Signals < 8 are always exclusively local inside the cluster, so store it right away
        if (!redac.clusters[cluster_idx].cblock->set_gain_correction(lane, gain_correction))
          return "Error setting local gain correction on C-block";
      } else {
        measured_gain_correction[(lane - 8) + cluster_idx * 24] = gain_correction;
      }
    }
  }
 
  if (gain_correction_exceeding) {
    LOG_ERROR("Gain correction exceeding 1.1.");
  }
 
  return status::success();
}
 
bool CalibrationBase::send_gain_corrections(uint8_t cluster_idx, uint8_t lane, float gain_correction) {
 
  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;
 
  // TODO: Replace by boolean flag whether this channel is in use
  if (gain_correction <= 0.0f)
    return true;
 
  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[(lane - 8) + src_cluster_idx * 24].add(gain_correction);
    return true;
  }
 
  // 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 source_entity_id = redac.carrier_t_block->get_external_signal_source_entity_id(dst_sector, lane - 8);
  auto signal_source = get_ip_of_external_device(source_entity_id);
  if (!signal_source) {
    LOG_ERROR("Source of signal is unknown.");
  } else {
    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 true;
}
 
bool CalibrationBase::receive_gain_corrections(uint32_t timeout) {
 
  pb_Envelope envelope;
  if (!data_input->read(envelope, timeout))
    return false;
 
  if (envelope.which_kind != pb_Envelope_message_v1_tag)
    return false;
 
  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 true;
  }
 
  auto& data_in = msg_in.kind.calibrate_data_command.data;
  // We must determine which cluster generates the signal
  auto source = redac.carrier_t_block->src_of_signal(blocks::TBlock::Sector::BPL, data_in.lane - 8);
  auto cluster_idx_ = static_cast<uint8_t>(source) - 1;
  received_gain_correction[(data_in.lane - 8) + cluster_idx_ * 24].add(data_in.gain_correction);
  return true;
}
 
status CalibrationBase::send_receive_apply_gain_corrections() {
  // Entry into this function is synchronized with +-10μs jitter
 
  // Lanes 0-7 have already been set directly in do_lane()
  for (uint8_t idx = 0; idx < measured_gain_correction.size();) {
    // priority is receiving gain corrections
    if (receive_gain_corrections(0)) continue;
 
    // sending gain corrections others wait for
    auto gain_correction = measured_gain_correction[idx];
    uint8_t cluster_idx = idx / 24;
    uint8_t lane_idx = idx % 24 + 8;
    send_gain_corrections(cluster_idx, lane_idx, gain_correction);
    ++idx;
  }
 
  while (receive_gain_corrections(10)) {}
 
 
  bool gain_correction_exceeding = false;
  // Actually apply the gathered values
  for (auto idx = 0; idx < received_gain_correction.size() ; ++idx) {
    auto& gain_correction = received_gain_correction[idx];
    uint8_t cluster_idx = idx / 24;
    uint8_t lane_idx = idx % 24 + 8;
    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;
    }
 
    if (!redac.clusters[cluster_idx].cblock->set_gain_correction(lane_idx, gain_correction_avg))
      LOG_ERROR("Gain correction could not be set.");
  }
 
  if (gain_correction_exceeding)
    LOG_ERROR("Gain correction exceeding 1.1.");
 
  // Restore original configuration
  if (!redac.set_adc_channels(_orig_adc_channels))
    LOG_ERROR("Error while restoring setting adc channels.");
 
  if (!redac.write_to_hardware())
    LOG_ERROR("Error while restoring original configuration.");
 
  return status::success();
}
 
status 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 "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
    RETURN_IF_FAILED(cluster.ublock->write_to_hardware());
  }
 
  return status::success();
}
 
status CalibrationLeader::do_() {
 
  RETURN_IF_FAILED(do_initial())
 
  elapsedMicros elapsed_micros;
  uint64_t sync_time = elapsed_micros;
 
  auto sync = [&](uint64_t offset) {
    uint64_t current = elapsed_micros;
    delayMicroseconds( current - sync_time + offset);
    sync_time += offset;
  };
 
  for (uint8_t lane = 0u; lane < 32; lane++) {
    delayMicroseconds(1000);
    RETURN_IF_FAILED(do_lane(lane))
  }
  delayMicroseconds(1000);
  RETURN_IF_FAILED(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.
  delayMicroseconds(10000);
  RETURN_IF_FAILED(do_final_offset_correction());
  return status::success();
}
 
status 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;
  RETURN_IF_FAILED(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::do_initial();
}
 
status 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;
 
  RETURN_IF_FAILED(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::do_lane(lane);
}
 
status CalibrationLeader::send_receive_apply_gain_corrections() {
  pb_Envelope envelope;
  transport::init_v1_message(envelope, pb_MessageV1_calibrate_finalize_command_tag);
  RETURN_IF_FAILED(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::send_receive_apply_gain_corrections();
}
 
status CalibrationLeader::do_final_offset_correction() {
  pb_Envelope envelope;
  transport::init_v1_message(envelope, pb_MessageV1_calibrate_offset_command_tag);
 
  RETURN_IF_FAILED(send(envelope));
  delayMicroseconds(19);
  return CalibrationBase::do_final_offset_correction();
}
 
CalibrationFollower::CalibrationFollower(REDAC &redac, const run::Run &run,
                                                   unsigned int timeout_ms)
    : CalibrationBase(redac, run), timeout_ms(timeout_ms) {}
 
status CalibrationFollower::receive_command(pb_Envelope &envelope) {
  if (!broadcast_input->read(envelope, timeout_ms))
    return "Timout while waiting for incoming calibration command";
 
  return status::success();
}
 
status CalibrationFollower::do_() {
  RETURN_IF_FAILED(do_initial())
  while (true) {
    RETURN_IF_FAILED(do_lane_as_told())
    if (is_done())
      break;
  }
  RETURN_IF_FAILED(send_receive_apply_gain_corrections());
  RETURN_IF_FAILED(do_final_offset_correction());
  return status::success();
}
 
status CalibrationFollower::do_initial() {
  pb_Envelope envelope;
  RETURN_IF_FAILED(receive_command(envelope));
  return CalibrationBase::do_initial();
}
 
status CalibrationFollower::do_lane_as_told() {
  pb_Envelope envelope = pb_Envelope_init_default;
  RETURN_IF_FAILED(receive_command(envelope));
  auto& msg = envelope.kind.message_v1;
  if (msg.which_kind != pb_MessageV1_calibrate_lane_command_tag) {
    std::string error_msg = "Expected calibration lane command but message is of different kind.";
    error_msg += " Received: " + std::to_string(msg.which_kind);
    return status(error_msg.c_str());
  }
  auto& lane_cmd = msg.kind.calibrate_lane_command;
  uint8_t lane = lane_cmd.lane;
  if (lane >= 32)
    return "Error: Told to calibrate a lane >= 32.";
  if (lane == 31)
    done = true;
  return CalibrationBase::do_lane(lane);
}
 
status CalibrationFollower::send_receive_apply_gain_corrections() {
  pb_Envelope envelope;
  RETURN_IF_FAILED(receive_command(envelope));
  return CalibrationBase::send_receive_apply_gain_corrections();
}
 
status CalibrationFollower::do_final_offset_correction() {
  pb_Envelope envelope;
  RETURN_IF_FAILED(receive_command(envelope));
  return CalibrationBase::do_final_offset_correction();
}
 
std::map<uint32_t, IPAddress> _external_entity_map{};
 
IPAddress get_ip_of_external_device(uint32_t entity_id) {
  if (auto search = _external_entity_map.find(entity_id); search != _external_entity_map.end())
    return search->second;
  else
    return {};
}
 
void set_ip_of_external_device(uint32_t entity_id, IPAddress ip_address) {
  _external_entity_map[entity_id] = std::move(ip_address);
}
 
}
 
 
utils::status 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& 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]);
    platform::set_ip_of_external_device(entity.key, address);
  }
  return {};
}