mul_calibration.cpp

Go to the documentation of this file.

#include "block/mul_calibration.h"
 
#include <algorithm>
#include <bitset>
 
#include "carrier/carrier.h"
#include "carrier/cluster.h"
#include "block/mblock.h"
 
#include <ArduinoEigenDense.h>
 
using Eigen::MatrixXd;
using Eigen::VectorXd;
using Eigen::Matrix4d;
using Eigen::Vector4d;
using Eigen::Matrix;
using Eigen::Vector;
 
 
UnitResult setup_calibration(platform::Cluster* cluster, blocks::MBlock* mblock, blocks::UBlock::Transmission_Mode mode, float lhs, float rhs) {
  auto offset = static_cast<uint32_t>(mblock->slot) * 8;
  cluster->reset(entities::ResetAction::CIRCUIT_RESET);
  for (auto idx = 0u; idx < 4; ++idx) {
    auto lhs_lane = idx * 2;
    auto rhs_lane = idx * 2 + 1;
    cluster->add_constant(mode, lhs_lane, lhs, offset + lhs_lane);
    cluster->add_constant(mode, rhs_lane, rhs, offset + rhs_lane);
  }
  TRY_TRUE(cluster->write_to_hardware());
  delay(10);
  return UnitResult::ok();
}
 
MulGainOffset vec_to_gain_offset(const Vector4d& x) {
  MulGainOffset current;
  auto gain = current.gain = x[0];
  if (std::fabs(gain) < 1e-8)
    gain = 1.0;
  current.offset_x = x[1] / gain;
  current.offset_y = x[2] / gain;
  current.offset_z = x[3] - gain * current.offset_x * current.offset_y;
  return current;
}
 
void prepare_calibration_data(VectorXd& x_vec, double x_min, double x_max, size_t x_samples,
                              VectorXd& y_vec, double y_min, double y_max, size_t y_samples) {
  const size_t C = x_samples * y_samples;
  x_vec.resize(C);
  y_vec.resize(C);
 
  const double x_step = (x_samples > 1) ? (x_max - x_min) / (x_samples - 1) : 0.0;
  const double y_step = (y_samples > 1) ? (y_max - y_min) / (y_samples - 1) : 0.0;
 
  size_t sample_idx = 0;
  for (size_t ix = 0; ix < x_samples; ++ix) {
    const double x = (x_samples > 1) ? x_min + ix * x_step : 0.5 * (x_min + x_max);
    for (size_t iy = 0; iy < y_samples; ++iy) {
      const double y = (y_samples > 1) ? y_min + iy * y_step : 0.5 * (y_min + y_max);
      x_vec[sample_idx] = x;
      y_vec[sample_idx] = y;
      ++sample_idx;
    }
  }
}