daq.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 <Arduino.h>
#include <DMAChannel.h>
#include <algorithm>
#include <bitset>
 
#include <carrier/carrier.h>
#include <daq/teensy/daq.h>
#include <utils/logging.h>
#include <utils/running_avg.h>
 
namespace daq {
 
namespace stream {
 
namespace details {
 
// We need a memory segment to implement a ring buffer.
// Its size should be a power-of-2-multiple of NUM_CHANNELS.
// The DMA implements ring buffer by restricting the N lower bits of the destination address to change.
// That means the memory segment must start at a memory address with enough lower bits being zero,
// see manual "data queues [with] power-of-2 size bytes, [...] should start at a 0-modulo-size address".
// One can put this into DMAMEM for increased performance, but that did not work for me right away.
// *MUST* be at least 2*NUM_CHANNELS, otherwise some math later does not work.
__attribute__((aligned(BUFFER_SIZE * 4))) std::array<volatile uint16_t, BUFFER_SIZE> buffer = {0};
 
DMAChannel channel(false);
const run::Run *run = nullptr;
run::RunDataHandler *run_data_handler = nullptr;
//volatile bool first_data = false;
//volatile bool last_data = false;
volatile bool overflow = false;
// Pointer to the part of the buffer which (may) contain partial data at end of acquisition time
//auto partial_buffer_part = buffer.data();
 
volatile uint16_t head = 0;
volatile uint16_t tail = 0;
 
// NOT
FASTRUN void interrupt() {
  head = channel.TCD->BITER - channel.TCD->CITER;
  overflow |= head == tail;
 
  // Clear interrupt
  channel.clearInterrupt();
  // Memory barrier
#ifdef ARDUINO
  asm("DSB");
#endif
}
 
} // namespace details
 
} // namespace stream
 
} // namespace daq
 
void daq::enable() { details::flexio::get_flexio()->port().CTRL |= FLEXIO_CTRL_FLEXEN; }
 
void daq::disable() { details::flexio::get_flexio()->port().CTRL &= ~FLEXIO_CTRL_FLEXEN; }
 
void daq::reset() {
  disable();
  details::flexio::get_flexio()->port().CTRL |= FLEXIO_CTRL_SWRST;
  delayNanoseconds(100);
  details::flexio::get_flexio()->port().CTRL &= ~FLEXIO_CTRL_SWRST;
  delayNanoseconds(100);
}
 
status daq::details::flexio::_init_once() {
  // Configure and enable clock
  get_flexio()->setClockSettings(3, 1, 1);
  get_flexio()->hardware().clock_gate_register |= get_flexio()->hardware().clock_gate_mask;
 
  // Put MOSI pins into flexio mode
  uint8_t _flexio_pin_cnvst = get_flexio()->mapIOPinToFlexPin(PIN_CNVST);
  uint8_t _flexio_pin_clk = get_flexio()->mapIOPinToFlexPin(PIN_CLK);
  uint8_t _flexio_pin_gate = get_flexio()->mapIOPinToFlexPin(PIN_GATE);
  // Check if any of the pins could not be mapped to the same FlexIO module
  if (_flexio_pin_cnvst == 0xff or _flexio_pin_clk == 0xff or _flexio_pin_gate == 0xff)
    return status("Could not map pins to FlexIO module.");
  get_flexio()->setIOPinToFlexMode(PIN_CNVST);
  get_flexio()->setIOPinToFlexMode(PIN_CLK);
  get_flexio()->setIOPinToFlexMode(PIN_GATE);
 
  // Put MISO pins into flexio mode
  for (auto pin_miso : PINS_MISO) {
    if (get_flexio()->mapIOPinToFlexPin(pin_miso) == 0xff)
      return status("Could not map pins to FlexIO module.");
    get_flexio()->setIOPinToFlexMode(pin_miso);
  }
 
  return status::success();
}
 
void daq::details::flexio::_init_timers() {
  get_flexio()->port().TIMCTL[_gated_timer_idx] = 0;
  get_flexio()->port().TIMCFG[_gated_timer_idx] = 0;
  get_flexio()->port().TIMCMP[_gated_timer_idx] = 0;
 
  get_flexio()->port().TIMCTL[_sample_timer_idx] = 0;
  get_flexio()->port().TIMCFG[_sample_timer_idx] = 0;
  get_flexio()->port().TIMCMP[_sample_timer_idx] = 0;
 
  get_flexio()->port().TIMCTL[_cnvst_timer_idx] =
      FLEXIO_TIMCTL_PINSEL(get_flexio()->mapIOPinToFlexPin(PIN_CNVST)) | FLEXIO_TIMCTL_PINCFG(0b11) |
      FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TRGSEL(4 * _sample_timer_idx + 3) | FLEXIO_TIMCTL_TIMOD(0b10);
  get_flexio()->port().TIMCFG[_cnvst_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010) | FLEXIO_TIMCFG_TIMENA(0b111);
  get_flexio()->port().TIMCMP[_cnvst_timer_idx] = 0x0000'10'60;
 
  // Add a delay timer to control delay between CNVST and first CLK.
  // Basically a second CNVST timer which is slightly longer and used to trigger first CLK.
  get_flexio()->port().TIMCTL[_delay_timer_idx] =
      FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TRGSEL(4 * _sample_timer_idx + 3) | FLEXIO_TIMCTL_TIMOD(0b11);
  get_flexio()->port().TIMCFG[_delay_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010) | FLEXIO_TIMCFG_TIMENA(0b111);
  get_flexio()->port().TIMCMP[_delay_timer_idx] = 105;
 
  get_flexio()->port().TIMCTL[_clk_timer_idx] =
      FLEXIO_TIMCTL_PINSEL(get_flexio()->mapIOPinToFlexPin(PIN_CLK)) | FLEXIO_TIMCTL_PINCFG(0b11) |
      FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TRGSEL(4 * _delay_timer_idx + 3) | FLEXIO_TIMCTL_TRGPOL |
      FLEXIO_TIMCTL_TIMOD(0b01);
  get_flexio()->port().TIMCFG[_clk_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010) | FLEXIO_TIMCFG_TIMENA(0b110);
  get_flexio()->port().TIMCMP[_clk_timer_idx] = 0x0000'1B'03;
}
 
void daq::details::flexio::_init_shifters() {
  for (auto _pin_miso_idx = 0u; _pin_miso_idx < PINS_MISO.size(); _pin_miso_idx++) {
    // Trigger all shifters from CLK
    get_flexio()->port().SHIFTCTL[_pin_miso_idx] =
        FLEXIO_SHIFTCTL_TIMSEL(_clk_timer_idx) | FLEXIO_SHIFTCTL_TIMPOL |
        FLEXIO_SHIFTCTL_PINSEL(get_flexio()->mapIOPinToFlexPin(PINS_MISO[_pin_miso_idx])) |
        FLEXIO_SHIFTCTL_SMOD(1);
    get_flexio()->port().SHIFTCFG[_pin_miso_idx] = 0;
  }
}
 
status daq::init() {
  RETURN_IF_FAILED(details::flexio::_init_once());
  details::flexio::_init_timers();
  details::flexio::_init_shifters();
  enable();
 
  pinMode(details::PIN_EN, OUTPUT);
  digitalWriteFast(details::PIN_EN, HIGH);
 
  delay(1000);
 
  return status::success();
}
 
void daq::reboot() {
  digitalWriteFast(details::PIN_EN, LOW);
  delay(100);
  digitalWriteFast(details::PIN_EN, HIGH);
  delay(1000);
 
  // First thing contains some more or less junk data
  for (auto _ = 0; _ < 10; _++)
    sample_raw();
}
 
status daq::calibrate(carrier::Carrier &carrier) {
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  reboot();
 
  LOG(ANABRID_DEBUG_CALIBRATION, "Resetting current ADC connections...");
  // Disconnect all signal connections in ADC switching matrix to force them to a zero
  auto old_adc_channels = carrier.get_adc_channels();
  carrier.reset_adc_channels();
  // Ensure the ADC bus is actually the one connected to the ADCs (and not the SH gain measurement)
  auto old_adc_bus = carrier.ctrl_block->get_adc_bus();
  carrier.ctrl_block->reset_adc_bus();
  // Write to hardware
  if (!carrier.write_to_hardware())
    return status("Error writing disconnect of ADCs to hardware.");
 
  // Read zero offsets and save them
  LOG(ANABRID_DEBUG_CALIBRATION, "Reading ADC zero offsets:");
  auto zeros_raw = daq::average(daq::sample_raw);
  std::transform(zeros_raw.begin(), zeros_raw.end(), details::raw_zero_offsets.begin(),
                 [](auto zero_raw) { return details::RAW_ZERO_IDEAL - zero_raw; });
  for (auto raw_zero_offset : details::raw_zero_offsets)
    LOG(ANABRID_DEBUG_CALIBRATION, raw_zero_offset);
 
  // Reset previous changes
  LOG(ANABRID_DEBUG_CALIBRATION, "Restoring previous ADC connections...");
  static_cast<void>(carrier.set_adc_channels(old_adc_channels));
  carrier.ctrl_block->set_adc_bus(old_adc_bus);
  // Write to hardware
  if (!carrier.write_to_hardware())
    return status("Error writing previous connections of ADCs to hardware.");
 
  // Sanity check, otherwise return successful
  if (std::any_of(details::raw_zero_offsets.begin(), details::raw_zero_offsets.end(),
                  [](auto value) { return abs(value) > 100; }))
    return status("Zero offsets of ADCs are suspiciously large.");
 
  LOG(ANABRID_DEBUG_CALIBRATION, __PRETTY_FUNCTION__);
 
  return status::success();
}
 
std::array<uint16_t, daq::NUM_CHANNELS> daq::sample_raw() {
  using namespace details::flexio;
 
  // Clear SHIFTSTAT (by writing 0xFF (sic!)), which might be partially set by a previous acquisition
  get_flexio()->port().SHIFTSTAT = 0xFF;
 
  // Disable and reset _sample_timer_idx
  get_flexio()->port().TIMCTL[_sample_timer_idx] = 0;
  get_flexio()->port().TIMCFG[_sample_timer_idx] = 0;
  delayNanoseconds(42);
  // Generate just one sample pulse
  get_flexio()->port().TIMCMP[_sample_timer_idx] = 1;
  get_flexio()->port().TIMCTL[_sample_timer_idx] = FLEXIO_TIMCTL_TIMOD(0b11);
  get_flexio()->port().TIMCFG[_sample_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b010);
 
  // Wait until shift buffer is filled or timeout in case of hardware errors
  for (auto _ = 0u; _ < 50; _++) {
    if (get_flexio()->port().SHIFTSTAT & 0xFF)
      return {static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[0]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[1]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[2]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[3]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[4]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[5]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[6]),
              static_cast<uint16_t>(get_flexio()->port().SHIFTBUFBIS[7])};
    delayNanoseconds(100);
  }
  // Returning zero in error case may hide the error
  // Of course, returning RAW_SUSPICIOUS_VALUE is only slightly better :)
  return {details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE,
          details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE,
          details::RAW_SUSPICIOUS_VALUE, details::RAW_SUSPICIOUS_VALUE};
}
 
std::array<float, daq::NUM_CHANNELS> daq::sample() { return raw_to_float(sample_raw()); }
 
std::array<float, daq::NUM_CHANNELS> daq::average(size_t samples, unsigned int delay_us) {
  return average(daq::sample, samples, delay_us);
}
 
daq::stream::Scope daq::stream::get(const run::Run &run, run::RunDataHandler *const data_handler,
                                             bool start) {
  return {run, data_handler, start};
}
 
unsigned int daq::stream::details::remaining_data_count() {
  uint16_t citer = channel.TCD->CITER;
  uint16_t biter = channel.TCD->BITER;
 
  uint16_t halt_biter = biter >> 1;
  uint16_t count = biter - citer;
  if (count > halt_biter) {
    count -= halt_biter;
  }
  return count;
}
 
std::array<volatile uint16_t, daq::stream::details::BUFFER_SIZE> daq::stream::details::get_buffer() {
  return buffer;
}
 
// NOT
FASTRUN run::RunHandleResult daq::stream::process(const run::Run &run, run::RunDataHandler *const data_handler, bool partial) {
  if (!run.daq_config)
    return run::RunHandleResult::err("Invalid DAQ configuration.");
  if (details::overflow)
    return run::RunHandleResult::err("DMA overflow.");
 
  auto alignment = run.daq_config.get_num_channels_min_power_of_two();
 
  if (details::head != details::tail){
    size_t sample_count = details::HALF_BUFFER_SIZE / alignment;
    auto data_ptr = const_cast<uint16_t*>(details::buffer.data() + alignment * details::tail);
    auto result = data_handler->handle(run, data_ptr, sample_count);
    details::tail = details::head;
    if (result.is_err())
      return result;
  }
 
  if (partial) {
    size_t sample_count = details::remaining_data_count();
    auto data_ptr = const_cast<uint16_t*>(details::buffer.data() + alignment * details::tail);
    auto result = data_handler->handle(run, data_ptr, sample_count);
    if (result.is_err())
      return result;
  }
 
  return run::RunHandleResult::ok({});
}
 
FASTRUN status daq::stream::stop(const run::Run &run) {
  details::channel.disable();
 
  // TODO: In case of streams not synchronized with OP state,
  //       this function should probably actually stop the timers.
 
  auto flexio = FlexIOHandler::flexIOHandler_list[1];
  // Disable DMA trigger generation
  flexio->port().SHIFTSDEN = 0;
  // Clear global data for ::daq::dma::interrupt
  details::run_data_handler = nullptr;
  details::run = nullptr;
  details::head = details::tail = 0;
  details::overflow = false;
 
  // Check shifter errors
  // Unused channels (when get_num_channels() < 8) always go into SHIFTERR,
  // since they are written to due to always being active, but not read from by the DMA.
  uint32_t _shifterr_mask = (run.daq_config.get_num_channels_min_power_of_two() > 1)
                                ? ((1u << run.daq_config.get_num_channels_min_power_of_two()) - 1)
                                : 1u;
  if (flexio->port().SHIFTERR & _shifterr_mask)
    return status("SHIFTERR was asserted during stream (%d).", flexio->port().SHIFTERR);
  // Clear all SHIFTERR
  flexio->port().SHIFTERR = 0xFF;
 
  return status::success();
}
 
FASTRUN status daq::stream::start(const run::Run &run, run::RunDataHandler *const data_handler) {
  using namespace daq::details::flexio;
  // Care: FlexIO clock is enabled in _init_once() and this function will get stuck without it
 
  disable();
  // Reset is necessary before each continuous acquisition,
  // otherwise internal counter values are not reset and result in unwanted behaviour
  reset();
  _init_timers();
  _init_shifters();
 
  auto& port = get_flexio()->port();
 
  auto _flexio_pin_gate = get_flexio()->mapIOPinToFlexPin(daq::details::PIN_GATE);
  port.TIMCTL[_gated_timer_idx] = FLEXIO_TIMCTL_TRGSEL(2 * _flexio_pin_gate) |
                                                  FLEXIO_TIMCTL_TRGPOL | FLEXIO_TIMCTL_TRGSRC |
                                                  FLEXIO_TIMCTL_TIMOD(0b11);
  port.TIMCFG[_gated_timer_idx] = FLEXIO_TIMCFG_TIMDIS(0b011) | FLEXIO_TIMCFG_TIMENA(0b110);
  port.TIMCMP[_gated_timer_idx] = 59;
 
  port.TIMCTL[_sample_timer_idx] =
      FLEXIO_TIMCTL_TRGSEL(4 * _gated_timer_idx + 3) | FLEXIO_TIMCTL_TRGSRC | FLEXIO_TIMCTL_TIMOD(0b11);
  port.TIMCFG[_sample_timer_idx] = FLEXIO_TIMCFG_TIMDEC(0b01) | FLEXIO_TIMCFG_TIMENA(0b110);
  port.TIMCMP[_sample_timer_idx] = (1'000'000 / run.daq_config.get_sample_rate()) * 2 - 1;
 
  enable();
 
  // Update global pointers for ::daq::dma::interrupt
  details::run_data_handler = data_handler;
  details::run = &run;
  details::head = details::tail = 0;
  details::overflow = false;
 
  /*
   *  Configure DMA
   */
 
  // Select shifter zero to generate DMA events.
  // Which shifter is selected should not matter, as long as it is used.
  uint8_t shifter_dma_idx = 0;
  // Set shifter to generate DMA events.
  port.SHIFTSDEN = 1 << shifter_dma_idx;
  // Configure DMA channel
  details::channel.begin();
 
  // Configure minor and major loop of DMA process.
  // One DMA request (SHIFTBUF store) triggers one major loop.
  // A major loop consists of several minor loops.
  // For each loop, source address and destination address is adjusted and data is copied.
  // The number of minor loops is implicitly defined by how much data they should copy.
  // The approach here is to use the minor loops to copy all shift registers when one triggers the DMA.
  // That means each major loop copies over NUM_CHANNELS data points.
  // Triggering multiple major loops fills up the ring buffer.
  // Once all major loops are done, the ring buffer is full and an interrupt is triggered to handle the data.
  // For such configurations DMAChannel provides sourceCircular() and destinationCircular() functions,
  // which are not quite able to handle our case (BITER/CITER and ATTR_DST must be different).
  // That's why we do it by hand :)
 
 
  // BITER "beginning iteration count" is the number of major loops.
  // Each major loop fills part (see TCD->NBYTES) of the ring buffer.
  details::channel.TCD->BITER = details::buffer.size() / run.daq_config.get_num_channels_min_power_of_two();
  // CITER "current major loop iteration count" is the current number of major loops left to perform.
  // It can be used to check progress of the process. It's reset to BITER whe we filled the buffer once.
  details::channel.TCD->CITER = details::channel.TCD->BITER;
 
  // Configure source address and its adjustments.
  // We want to circularly copy from SHIFTBUFBIS.
  // In principle, we are only interested in the last 16 bits of each SHIFTBUFBIS.
  // Unfortunately, configuring it in such a way was not successful -- maybe in the future.
  // Instead, we copy the full 32 bit of data.
  // SADDR "source address start" is where the DMA process starts.
  // Set it to the beginning of the SHIFTBUFBIS array.
  details::channel.TCD->SADDR = port.SHIFTBUFBIS;
  // NBYTES "minor byte transfer count" is the number of bytes transferred in one minor loop.
  // Set it such that the whole SHIFTBUFBIS array is copied, which is 2 bytes * NUM_CHANNELS
  details::channel.TCD->NBYTES = 2 * run.daq_config.get_num_channels_min_power_of_two();
  // SOFF "source address offset" is the offset added onto SADDR for each minor loop.
  // Set it to 4 bytes, equaling the 32 bits each shift register has.
  details::channel.TCD->SOFF = 4;
  // ATTR_SRC "source address attribute" is an attribute setting the circularity and the transfer size.
  // The format is [5bit MOD][3bit SIZE].
  // The 5bit MOD is the number of lower address bites allowed to change, effectively circling back to SADDR.
  // The 3bit SIZE defines the source data transfer size.
  // Set MOD to 5, allowing the address bits to change until the end of the SHIFTBUFBIS array and cycling.
  // MOD = 5 because 2^5 = 32, meaning address cycles after 32 bytes, which are 8*32 bits.
  // Set SIZE to 0b010 for 32 bit transfer size.
  uint8_t MOD_SRC = __builtin_ctz(run.daq_config.get_num_channels_min_power_of_two() * 4);
  details::channel.TCD->ATTR_SRC = (MOD_SRC << 3) | 0b001;
  // SLAST "last source address adjustment" is an adjustment applied to SADDR after all major iterations.
  // We don't want any adjustments, since we already use ATTR_SRC to implement a circular buffer.
  details::channel.TCD->SLAST = 0;
 
  // Configure destination address and its adjustments.
  // We want to circularly copy into a memory ring buffer.
  // Since we always copy 32 bit from SADDR, we will have the memory buffer in a 32 bit layout as well,
  // even though we are again only interested in the lower 16 bits.
  // DADDR "destination address start" is the start of the destination ring buffer.
  // Set to address of ring buffer.
  details::channel.TCD->DADDR = details::buffer.data();
  // DOOFF "destination address offset" is the offset added to DADDR for each minor loop.
  // Set to 2 bytes, equaling the 16 bits each shift register has.
  details::channel.TCD->DOFF = 2;
  // ATTR_SRC "destination address attribute" is analogous to ATTR_SRC
  // Set first 5 bit MOD according to size of ring buffer.
  // Since only power-of-two number of channels N<=8 is allowed, N*NUM_CHANNELS will always fit for each N.
  // Set last 3 bits to 0b001 for 16 bit transfer size.
  uint8_t MOD = __builtin_ctz(details::BUFFER_SIZE * 2);
  details::channel.TCD->ATTR_DST = (MOD << 3) | 0b001;
  // DLASTSGA "last destination address adjustment or next TCD" is similar to SLAST.
  // We don't want any adjustments, since we already use ATTR_DST to implement a circular buffer.
  details::channel.TCD->SLAST = 0;
 
  // Call an interrupt when done
  details::channel.attachInterrupt(details::interrupt);
  details::channel.interruptAtCompletion();
  details::channel.interruptAtHalf();
  // Trigger from "shifter full" DMA event
  details::channel.triggerAtHardwareEvent(get_flexio()->shiftersDMAChannel(shifter_dma_idx));
  // Enable dma channel
  details::channel.enable();
  if (details::channel.error()) {
    return ("dma::channel.error");
  }
 
  return status::success();
}