lasp/lasp/device/lasp_uldaq.cpp

#include "lasp_uldaq.h"
#include "lasp_daqconfig.h"
#include <algorithm>
#include <atomic>
#include <cassert>
#include <chrono>
#include <gsl/gsl-lite.hpp>
#include <iostream>
#include <memory>
#include <mutex>
#include <span>
#include <stdexcept>
#include <thread>
#include <uldaq.h>
#include <vector>

using namespace std::literals::chrono_literals;
using std::atomic;
using std::cerr;
using std::endl;
using std::runtime_error;

#include "debugtrace.hpp"
DEBUGTRACE_VARIABLES;

const us MAX_DEV_COUNT_PER_API = 100;
/**
 * @brief Reserve some space for an error message from UlDaq
 */
const us UL_ERR_MSG_LEN = 512;

/**
 * @brief Show the error to default error stream and return a string
 * corresponding to the error
 *
 * @param err Error string
 */
string showErr(UlError err) {
  string errstr;
  errstr.reserve(UL_ERR_MSG_LEN);
  if (err != ERR_NO_ERROR) {
    char errmsg[UL_ERR_MSG_LEN];
    errstr = "UlDaq API Error: ";
    ulGetErrMsg(err, errmsg);
    errstr += errmsg;
    std::cerr << errstr << std::endl;
    return errstr;
  }
  return errstr;
}

class DT9837A : public Daq {

  DaqDeviceHandle _handle = 0;
  std::mutex _daqmutex;

  std::thread _thread;
  atomic<bool> _stopThread{false};

  const us _nFramesPerBlock;

  void threadFcn(std::optional<DaqCallback> inCallback,
                 std::optional<DaqCallback> outcallback);

public:
  DT9837A(const DeviceInfo &devinfo, const DaqConfiguration &config);

  ~DT9837A() {
    UlError err;
    if (isRunning()) {
      stop();
    }

    if (_handle) {
      err = ulDisconnectDaqDevice(_handle);
      showErr(err);
      err = ulReleaseDaqDevice(_handle);
      showErr(err);
    }
  }

  bool isRunning() const override final { return _thread.joinable(); }
  virtual void start(std::optional<DaqCallback> inCallback,
                     std::optional<DaqCallback> outCallback) override final;

  void stop() override final {
    DEBUGTRACE_ENTER;
    if (!isRunning()) {
      throw runtime_error("No data acquisition running");
    }

    _stopThread = true;
    if (_thread.joinable()) {
      _thread.join();
    }
    _stopThread = false;
  }
  friend class InBufHandler;
  friend class OutBufHandler;
};

void DT9837A::start(std::optional<DaqCallback> inCallback,
                    std::optional<DaqCallback> outCallback) {
  DEBUGTRACE_ENTER;
  if (isRunning()) {
    throw runtime_error("DAQ is already running");
  }
  if (neninchannels() > 0) {
    if (!inCallback)
      throw runtime_error("DAQ requires a callback for input data");
  }
  if (nenoutchannels() > 0) {
    if (!outCallback)
      throw runtime_error("DAQ requires a callback for output data");
  }
  assert(neninchannels() + nenoutchannels() > 0);
  _thread = std::thread(&DT9837A::threadFcn, this, inCallback, outCallback);
}

class BufHandler {
protected:
  DaqDeviceHandle _handle;
  const DataTypeDescriptor dtype_descr;
  us nchannels, nFramesPerBlock;
  DaqCallback cb;
  double samplerate;
  dvec buf;
  bool topenqueued, botenqueued;
  us increment = 0;
  us totalFramesCount = 0;
  long long buffer_mid_idx;

public:
  BufHandler(DaqDeviceHandle handle, const DataTypeDescriptor dtype_descr,
             const us nchannels, const us nFramesPerBlock, DaqCallback cb,
             const double samplerate)
      : _handle(handle), dtype_descr(dtype_descr), nchannels(nchannels),
        nFramesPerBlock(nFramesPerBlock), cb(cb), samplerate(samplerate),
        buf(2 * nchannels *
                nFramesPerBlock, // Watch the two here, the top and the bottom!
            0),
        buffer_mid_idx(nchannels * nFramesPerBlock) {
    assert(nchannels > 0);
  }
};
class InBufHandler : public BufHandler {
  bool monitorOutput;

public:
  InBufHandler(DT9837A &daq, DaqCallback cb)
      : BufHandler(daq._handle, daq.dtypeDescr(), daq.neninchannels(),
                   daq._nFramesPerBlock, cb, daq.samplerate())

  {
    DEBUGTRACE_ENTER;
    assert(_handle != 0);

    monitorOutput = daq.monitorOutput;

    DaqInScanFlag inscanflags = DAQINSCAN_FF_DEFAULT;
    ScanOption scanoptions = SO_CONTINUOUS;
    UlError err = ERR_NO_ERROR;

    std::vector<DaqInChanDescriptor> indescs;

    boolvec eninchannels = daq.eninchannels;

    // Initialize input, if any
    for (us chin = 0; chin < 4; chin++) {
      if (eninchannels[chin] == true) {
        DaqInChanDescriptor indesc;
        indesc.type = DAQI_ANALOG_SE;
        indesc.channel = chin;

        double rangeval = daq.inputRangeForChannel(chin);
        Range rangenum;
        if (fabs(rangeval - 1.0) < 1e-8) {
          rangenum = BIP1VOLTS;
        } else if (fabs(rangeval - 10.0) < 1e-8) {
          rangenum = BIP10VOLTS;
        } else {
          std::cerr << "Fatal: input range value is invalid" << endl;
          return;
        }
        indesc.range = rangenum;
        indescs.push_back(indesc);
      }
    }
    // Overwrite last channel
    if (monitorOutput) {
      DaqInChanDescriptor indesc;
      indesc.type = DAQI_DAC;
      indesc.channel = 0;
      indesc.range = BIP10VOLTS;
      indescs.push_back(indesc);
    }
    assert(indescs.size() == nchannels);
    DEBUGTRACE_MESSAGE("Starting input scan");
    err = ulDaqInScan(_handle, indescs.data(), nchannels,
                      2 * nFramesPerBlock, // Watch the 2 here!
                      &samplerate, scanoptions, inscanflags, buf.data());
    if (err != ERR_NO_ERROR) {
      showErr(err);
      throw std::runtime_error("Could not start input DAQ");
    }

    botenqueued = false;
    topenqueued = false;
  }

  bool operator()() {

    auto runCallback = ([&](us totalOffset) {
      us monitoroffset = monitorOutput ? 1 : 0;

      DaqData data(nchannels, nFramesPerBlock,
                   DataTypeDescriptor::DataType::dtype_fl64);
      us ch_no = 0;
      if (monitorOutput) {

          reinterpret_cast<uint8_t *>(
            &buf[totalOffset + (nchannels - 1) * nFramesPerBlock]),
          nFramesPerBlock * sizeof(double));
      }
      /* if(mon */

      /* for (us channel = monitoroffset; channel < (nchannels - monitoroffset);
       */
      /*     channel++) { */
      /* cv[channel] = */
      /* gsl::span(reinterpret_cast<uint8_t *>( */
      /*       &buf[totalOffset + channel * nFramesPerBlock]), */
      /*     nFramesPerBlock * sizeof(double)); */
      /* } */

      /* cv[0] = gsl::span( */
      /* cb(cv, dtype_descr); */
    });

    ScanStatus status;
    TransferStatus transferStatus;

    UlError err = ulDaqInScanStatus(_handle, &status, &transferStatus);
    if (err != ERR_NO_ERROR) {
      showErr(err);
      return;
    }

    increment = transferStatus.currentTotalCount - totalFramesCount;
    totalFramesCount += increment;

    if (increment > nFramesPerBlock) {
      cerr << "Error: overrun for input of DAQ!" << endl;
      return;
    }
    assert(status == SS_RUNNING);

    if (transferStatus.currentIndex < (long long)buffer_mid_idx) {
      topenqueued = false;
      if (!botenqueued) {
        runCallback(nchannels * nFramesPerBlock);
        botenqueued = true;
      }
    } else {
      botenqueued = false;
      if (!topenqueued) {
        runCallback(0);
        topenqueued = true;
      }
    }
  }
  ~InBufHandler() {
    // At exit of the function, stop scanning.
    DEBUGTRACE_ENTER;
    UlError err = ulDaqInScanStop(_handle);
    if (err != ERR_NO_ERROR) {
      showErr(err);
    }
  }
};

class OutBufHandler : public BufHandler {
public:
  OutBufHandler(DT9837A &daq, DaqCallback cb)
      : BufHandler(daq._handle, daq.dtypeDescr(), daq.neninchannels(),
                   daq._nFramesPerBlock, cb, daq.samplerate()) {

    DEBUGTRACE_MESSAGE("Starting output scan");
    AOutScanFlag outscanflags = AOUTSCAN_FF_DEFAULT;
    ScanOption scanoptions = SO_CONTINUOUS;
    UlError err =
        ulAOutScan(_handle, 0, 0, BIP10VOLTS,
                   2 * nFramesPerBlock, // Watch the 2 here!
                   &samplerate, scanoptions, outscanflags, buf.data());
    if (err != ERR_NO_ERROR) {
      showErr(err);
      throw runtime_error("Unable to start output on DAQ");
    }

    botenqueued = false, topenqueued = true;

    // Run callback to first fill top part
    ChannelView cv{gsl::span(reinterpret_cast<uint8_t *>(&buf[0]),
                             nFramesPerBlock * sizeof(double))};
    cb(cv, dtype_descr);
  }
  void operator()() {

    assert(_handle != 0);

    UlError err = ERR_NO_ERROR;

    ScanStatus status;
    TransferStatus transferStatus;

    err = ulAOutScanStatus(_handle, &status, &transferStatus);
    if (err != ERR_NO_ERROR) {
      showErr(err);
      return;
    }
    if (status != SS_RUNNING) {
      return;
    }
    increment = transferStatus.currentTotalCount - totalFramesCount;
    totalFramesCount += increment;

    if (increment > nFramesPerBlock) {
      cerr << "Error: underrun for output of DAQ!" << endl;
      return;
    }

    if (transferStatus.currentIndex < buffer_mid_idx) {
      topenqueued = false;
      if (!botenqueued) {

        ChannelView cv{
            gsl::span(reinterpret_cast<uint8_t *>(&buf[buffer_mid_idx]),
                      nFramesPerBlock * sizeof(double))};
        cb(cv, dtype_descr);
        botenqueued = true;
      }
    } else {
      botenqueued = false;
      if (!topenqueued) {

        ChannelView cv{gsl::span(reinterpret_cast<uint8_t *>(&buf[0]),
                                 nFramesPerBlock * sizeof(double))};
        cb(cv, dtype_descr);

        topenqueued = true;
      }
    }
  }

  ~OutBufHandler() {
    DEBUGTRACE_ENTER;
    UlError err = ulAOutScanStop(_handle);
    if (err != ERR_NO_ERROR) {
      showErr(err);
    }
  }
};

void DT9837A::threadFcn(std::optional<DaqCallback> inCallback,
                        std::optional<DaqCallback> outCallback) {

  DEBUGTRACE_ENTER;
  std::unique_ptr<InBufHandler> ibh;
  std::unique_ptr<OutBufHandler> obh;

  if (neninchannels() > 0) {
    ibh = std::make_unique<InBufHandler>(*this, inCallback.value());
  }
  if (nenoutchannels() > 0) {
    obh = std::make_unique<OutBufHandler>(*this, outCallback.value());
  }

  const double sleeptime =
      static_cast<double>(_nFramesPerBlock) / (16 * samplerate());
  const us sleeptime_us = static_cast<us>(sleeptime * 1e6);

  while (!_stopThread) {
    if (ibh) {
      (*ibh)();
    }
    if (obh) {
      (*obh)();
    }
    std::this_thread::sleep_for(std::chrono::microseconds(sleeptime_us));
  }
}

std::unique_ptr<Daq> createUlDaqDevice(const DeviceInfo &devinfo,
                                       const DaqConfiguration &config) {
  return std::make_unique<DT9837A>(devinfo, config);
}

DT9837A::DT9837A(const DeviceInfo &devinfo, const DaqConfiguration &config)
    : Daq(devinfo, config),
      _nFramesPerBlock(availableFramesPerBlock.at(framesPerBlockIndex)) {

  // Some sanity checks
  if (eninchannels.size() != 4) {
    throw runtime_error("Invalid length of enabled inChannels vector");
  }

  if (enoutchannels.size() != 1) {
    throw runtime_error("Invalid length of enabled outChannels vector");
  }

  if (_nFramesPerBlock < 24 || _nFramesPerBlock > 8192) {
    throw runtime_error("Unsensible number of samples per block chosen");
  }

  if (samplerate() < 10000 || samplerate() > 51000) {
    throw runtime_error("Invalid sample rate");
  }

  DaqDeviceDescriptor devdescriptors[MAX_DEV_COUNT_PER_API];
  DaqDeviceDescriptor descriptor;
  DaqDeviceInterface interfaceType = ANY_IFC;

  UlError err;

  us numdevs = MAX_DEV_COUNT_PER_API;
  err = ulGetDaqDeviceInventory(interfaceType, devdescriptors,
                                (unsigned *)&numdevs);
  if (err != ERR_NO_ERROR) {
    throw runtime_error("Device inventarization failed");
  }

  if ((us)api_specific_devindex >= numdevs) {
    throw runtime_error("Device number {deviceno} too high {err}. This could "
                        "happen when the device is currently not connected");
  }

  descriptor = devdescriptors[api_specific_devindex];

  // get a handle to the DAQ device associated with the first descriptor
  _handle = ulCreateDaqDevice(descriptor);

  if (_handle == 0) {
    throw runtime_error(
        "Unable to create a handle to the specified DAQ "
        "device. Is the device currently in use? Please make sure to set "
        "the DAQ configuration in duplex mode if simultaneous input and "
        "output is required.");
  }

  err = ulConnectDaqDevice(_handle);
  if (err != ERR_NO_ERROR) {
    ulReleaseDaqDevice(_handle);
    _handle = 0;
    throw runtime_error(string("Unable to connect to device: " + showErr(err)));
  }

  for (us ch = 0; ch < 4; ch++) {

    err = ulAISetConfigDbl(_handle, AI_CFG_CHAN_SENSOR_SENSITIVITY, ch, 1.0);
    showErr(err);
    if (err != ERR_NO_ERROR) {
      throw runtime_error("Fatal: could normalize channel sensitivity");
    }

    CouplingMode cm = inputACCouplingMode[ch] ? CM_AC : CM_DC;
    err = ulAISetConfig(_handle, AI_CFG_CHAN_COUPLING_MODE, ch, cm);
    if (err != ERR_NO_ERROR) {
      showErr(err);
      throw runtime_error("Fatal: could not set AC/DC coupling mode");
    }

    IepeMode iepe = inputIEPEEnabled[ch] ? IEPE_ENABLED : IEPE_DISABLED;
    err = ulAISetConfig(_handle, AI_CFG_CHAN_IEPE_MODE, ch, iepe);
    if (err != ERR_NO_ERROR) {
      showErr(err);
      throw runtime_error("Fatal: could not set IEPE mode");
    }
  }
}
void fillUlDaqDeviceInfo(std::vector<DeviceInfo> &devinfolist) {

  DEBUGTRACE_ENTER;

  UlError err;
  unsigned int numdevs = MAX_DEV_COUNT_PER_API;

  DaqDeviceDescriptor devdescriptors[MAX_DEV_COUNT_PER_API];
  DaqDeviceDescriptor descriptor;
  DaqDeviceInterface interfaceType = ANY_IFC;

  err = ulGetDaqDeviceInventory(interfaceType, devdescriptors,
                                static_cast<unsigned *>(&numdevs));

  if (err != ERR_NO_ERROR) {
    throw runtime_error("UlDaq device inventarization failed");
  }

  for (unsigned i = 0; i < numdevs; i++) {

    descriptor = devdescriptors[i];

    DeviceInfo devinfo;
    devinfo.api = uldaqapi;
    string name, interface;
    if (string(descriptor.productName) != "DT9837A") {
      throw runtime_error("Unknown UlDAQ type");
    }

    switch (descriptor.devInterface) {
    case USB_IFC:
      name = "USB - ";
      break;
    case BLUETOOTH_IFC:
      /* devinfo. */
      name = "Bluetooth - ";
      break;

    case ETHERNET_IFC:
      /* devinfo. */
      name = "Ethernet - ";
      break;
    default:
      name = "Uknown interface = ";
    }

    name += string(descriptor.productName) + " " + string(descriptor.uniqueId);
    devinfo.device_name = std::move(name);

    devinfo.api_specific_devindex = i;
    devinfo.availableDataTypes.push_back(
        DataTypeDescriptor::DataType::dtype_fl64);
    devinfo.prefDataTypeIndex = 0;

    devinfo.availableSampleRates = {8000,  10000, 11025, 16000, 20000,
                                    22050, 24000, 32000, 44056, 44100,
                                    47250, 48000, 50000, 50400, 51000};

    devinfo.prefSampleRateIndex = 11;

    devinfo.availableFramesPerBlock = {512, 1024, 2048, 4096, 8192};

    devinfo.availableInputRanges = {1.0, 10.0};
    devinfo.prefInputRangeIndex = 0;

    devinfo.ninchannels = 4;
    devinfo.noutchannels = 1;

    devinfo.hasInputIEPE = true;
    devinfo.hasInputACCouplingSwitch = true;
    devinfo.hasInputTrigger = true;

    // Finally, this devinfo is pushed back in list
    devinfolist.push_back(devinfo);
  }
}