(Re)implemented digital highpass filter on input data.

src/lasp/device/lasp_daqdata.cpp

@@ -1,8 +1,8 @@
 /* #define DEBUGTRACE_ENABLED */
-#include "debugtrace.hpp"
-#include <armadillo>
 #include "lasp_daqdata.h"
+#include "debugtrace.hpp"
 #include "lasp_mathtypes.h"
+#include <armadillo>
 #include <cassert>
 #include <memory>
 
@@ -34,19 +34,20 @@ DaqData::DaqData(const us nframes, const us nchannels,
 DaqData::DaqData(const DaqData &o) : DaqData(o.nframes, o.nchannels, o.dtype) {
 
   DEBUGTRACE_ENTER;
-  /* std::copy(o._data, &o._data[sw * nchannels * nframes], _data); */
   memcpy(_data, o._data, sw * nchannels * nframes);
 }
 
-/* DaqData::DaqData(DaqData &&o) */
+DaqData::DaqData(DaqData &&o)
-/*     : nframes(o.nframes), nchannels(o.nchannels), dtype(o.dtype), */
+    : nframes(o.nframes), nchannels(o.nchannels), dtype(o.dtype),
-/*       dtype_descr(std::move(o.dtype_descr)), sw(o.sw) { */
+      dtype_descr(std::move(o.dtype_descr)), sw(o.sw) {
 
-/*   DEBUGTRACE_ENTER; */
+  /// Steal from other one
-/*   _data = o._data; */
+
-/*   /// Nullptrs do not get deleted */
+  DEBUGTRACE_ENTER;
-/*   o._data = nullptr; */
+  _data = o._data;
-/* } */
+  /// Nullptrs do not get deleted
+  o._data = nullptr;
+}
 
 DaqData::~DaqData() {
   DEBUGTRACE_ENTER;
@@ -59,12 +60,15 @@ void DaqData::copyInFromRaw(const std::vector<byte_t *> &ptrs) {
   us ch = 0;
   assert(ptrs.size() == nchannels);
   for (auto &ptr : ptrs) {
-    assert(ch < nchannels);
+    copyInFromRaw(ch, ptr);
-    memcpy(&_data[sw * ch * nframes], ptr, sw * nframes);
-    /* std::copy(ptr, ptr + sw * nframes, &_data[sw * ch * nframes]); */
     ch++;
   }
 }
+void DaqData::copyInFromRaw(const us channel, const byte_t *ptr) {
+  DEBUGTRACE_ENTER;
+  assert(ptr);
+  memcpy(&_data[sw * channel * nframes], ptr, sw * nframes);
+}
 
 void DaqData::copyToRaw(const us channel, byte_t *ptr) {
   /* std::copy(raw_ptr(0, channel), raw_ptr(nframes, channel), ptr); */
@@ -141,7 +145,7 @@ d DaqData::toFloat(const us frame, const us channel) const {
 vd DaqData::toFloat(const us channel_no) const {
   DEBUGTRACE_ENTER;
   using DataType = DataTypeDescriptor::DataType;
-  cerr << (int) dtype << endl;
+  /* cerr << (int)dtype << endl; */
   switch (dtype) {
   case (DataType::dtype_int8): {
     return toFloat<int8_t>(channel_no);
@@ -166,7 +170,6 @@ vd DaqData::toFloat(const us channel_no) const {
   return vd();
 }
 
-
 dmat DaqData::toFloat() const {
 
   DEBUGTRACE_ENTER;
@@ -204,17 +207,92 @@ dmat DaqData::toFloat() const {
   return dmat();
 }
 
+template <typename T>
+void DaqData::fromFloat(const us frame, const us channel, const d val) {
+  DEBUGTRACE_ENTER;
+#if LASP_DEBUG == 1
+  check_type<T>();
+#endif
+  if constexpr (std::is_integral<T>::value) {
+    value<T>(frame, channel) =
+        static_cast<T>(val * std::numeric_limits<T>::max());
+  } else {
+    value<T>(frame, channel) = static_cast<T>(val);
+  }
+}
+
+void DaqData::fromFloat(const us frame, const us channel, const d val) {
+
+  using DataType = DataTypeDescriptor::DataType;
+
+  switch (dtype) {
+  case (DataType::dtype_int8):
+    return fromFloat<int8_t>(frame, channel, val);
+    break;
+  case (DataType::dtype_int16):
+    return fromFloat<int16_t>(frame, channel, val);
+    break;
+  case (DataType::dtype_int32):
+    return fromFloat<int32_t>(frame, channel, val);
+    break;
+  case (DataType::dtype_fl32):
+    return fromFloat<float>(frame, channel, val);
+    break;
+  case (DataType::dtype_fl64):
+    return fromFloat<float>(frame, channel, val);
+    break;
+  default:
+    throw std::runtime_error("BUG");
+  } // End of switch
+}
+void DaqData::fromFloat(const us channel, const vd &vals) {
+  if (vals.size() != nframes) {
+    throw rte("Invalid number of frames in channel data");
+  }
+  using DataType = DataTypeDescriptor::DataType;
+  switch (dtype) {
+  case (DataType::dtype_int8):
+    for (us frame = 0; frame < nframes; frame++) {
+      fromFloat<int8_t>(frame, channel, vals(frame));
+    }
+    break;
+  case (DataType::dtype_int16):
+    for (us frame = 0; frame < nframes; frame++) {
+      fromFloat<int16_t>(frame, channel, vals(frame));
+    }
+    break;
+  case (DataType::dtype_int32):
+    for (us frame = 0; frame < nframes; frame++) {
+      fromFloat<int32_t>(frame, channel, vals(frame));
+    }
+    break;
+  case (DataType::dtype_fl32):
+    for (us frame = 0; frame < nframes; frame++) {
+      fromFloat<float>(frame, channel, vals(frame));
+    }
+    break;
+  case (DataType::dtype_fl64):
+    for (us frame = 0; frame < nframes; frame++) {
+      fromFloat<double>(frame, channel, vals(frame));
+    }
+    break;
+  default:
+    throw std::runtime_error("BUG");
+  } // End of switch
+}
+
 void DaqData::print() const {
   cout << "Number of frames: " << nframes << endl;
   cout << "Number of channels: " << nchannels << endl;
   cout << "DataType: " << dtype_map.at(dtype).name << endl;
   cout << "First sample of first channel (as float)" << toFloat(0, 0) << endl;
-  cout << "Last sample of first channel (as float)" << toFloat(nframes-1,0) << endl;
+  cout << "Last sample of first channel (as float)" << toFloat(nframes - 1, 0)
-  cout << "Last sample of last channel (as float)" << toFloat(nframes-1,nchannels-1) << endl;
+       << endl;
+  cout << "Last sample of last channel (as float)"
+       << toFloat(nframes - 1, nchannels - 1) << endl;
   dmat data = toFloat();
   vrd max = arma::max(data, 0);
   vrd min = arma::min(data, 0);
   cout << "Maximum value in buf: " << max << endl;
   cout << "Minumum value in buf: " << min << endl;
-
 }

src/lasp/device/lasp_daqdata.h

@@ -65,7 +65,7 @@ public:
    * @brief Initialize using no allocation
    */
   DaqData(const DaqData &);
-  /* DaqData(DaqData &&); */
+  DaqData(DaqData &&);
   DaqData &operator=(const DaqData &) = delete;
   virtual ~DaqData();
 
@@ -104,6 +104,15 @@ public:
    */
   void copyInFromRaw(const std::vector<byte_t *> &ptrs);
 
+  /**
+   * @brief Copy data from a set of raw pointers of *uninterleaved* data.
+   * Overwrites any existing available data.
+   *
+   * @param channel The channel to copy to
+   * @param ptrs Pointers to data from channels
+   */
+  void copyInFromRaw(const us channel, const byte_t *ptr);
+
   /**
    * @brief Copy contents of DaqData for a certain channel to a raw pointer.
    *
@@ -144,6 +153,26 @@ public:
    */
   d toFloat(const us frame_no, const us channel_no) const;
 
+  /**
+   * @brief Convert to channel data of native type from floating point values.
+   * Useful for 'changing' raw data in any way.
+   *
+   * @param channel_no
+   * @param channel_no
+   * @param data
+   */
+  void fromFloat(const us frame_no, const us channel_no,
+                 const d data);
+
+  /**
+   * @brief Convert to channel data of native type from floating point values.
+   * Useful for 'changing' raw data in any way.
+   *
+   * @param channel The channel to convert
+   * @param data
+   */
+  void fromFloat(const us channel, const arma::Col<d> &data);
+
   //  Return value based on type
   template <typename T> T &value(const us frame, const us channel) {
 #if LASP_DEBUG == 1
@@ -188,6 +217,11 @@ protected:
   template <typename T> arma::Col<d> toFloat(const us channel_no) const;
   template <typename T> d toFloat(const us frame_no, const us channel_no) const;
 
+  /* template <typename T> void fromFloat(const dmat&); */
+  template <typename T>
+  void fromFloat(const us channel_no, const arma::Col<d> &vals);
+  template <typename T>
+  void fromFloat(const us frame_no, const us channel_no, const d val);
   /**
    * @brief Return a value as floating point. Does a conversion from integer to
    * float, if the stored type is integer. Also scales by its maximum value.

src/lasp/device/lasp_streammgr.cpp

@@ -1,6 +1,7 @@
 /* #define DEBUGTRACE_ENABLED */
 #include "lasp_streammgr.h"
 #include "debugtrace.hpp"
+#include "lasp_biquadbank.h"
 #include "lasp_thread.h"
 #include <algorithm>
 #include <assert.h>
@@ -92,12 +93,47 @@ bool StreamMgr::inCallback(const DaqData &data) {
 
   std::scoped_lock<std::mutex> lck(_inDataHandler_mtx);
 
+  assert(_inputFilters.size() == data.nchannels);
+
+  if (std::count_if(_inputFilters.cbegin(), _inputFilters.cend(),
+                    [](const auto &a) { return bool(a); }) > 0) {
+
+    /// Found a filter in vector of input filters. So we have to apply the
+    /// filters to each channel.
+
+    DaqData input_filtered(data.nframes, data.nchannels, data.dtype);
+
+    for (us ch = 0; ch < data.nchannels; ch++) {
+      if (_inputFilters[ch]) {
+        DEBUGTRACE_PRINT("Filter ch:");
+        DEBUGTRACE_PRINT(ch);
+        vd inout = data.toFloat(ch);
+        _inputFilters[ch]->filter(inout);
+        input_filtered.fromFloat(ch, inout);
+      } else {
+        DEBUGTRACE_PRINT("No filter ch:");
+        DEBUGTRACE_PRINT(ch);
+        input_filtered.copyInFromRaw(ch, data.raw_ptr(0, ch));
+      }
+    }
+
+    for (auto &handler : _inDataHandlers) {
+      bool res = handler->inCallback(input_filtered);
+      if (!res) {
+        return false;
+      }
+    }
+
+  } else {
+    /// No input filters
     for (auto &handler : _inDataHandlers) {
 
       bool res = handler->inCallback(data);
-    if (!res)
+      if (!res) {
         return false;
       }
+    }
+  }
   return true;
 }
 
@@ -258,8 +294,8 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
               "first stop existing stream");
   } else if (_inputStream) {
     if (_inputStream->duplexMode() && isOutput) {
-      throw rte(
+      throw rte("Error: output stream is already running (in duplex mode). "
-          "Error: output stream is already running (in duplex mode). Please "
+                "Please "
                 "first stop existing stream");
     }
   }
@@ -270,6 +306,8 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
   using namespace std::placeholders;
   std::unique_ptr<Daq> daq = Daq::createDaq(devinfo, config);
 
+  assert(daq);
+
   if (isInput) {
     /// Give incallback as parameter to stream
     inCallback = std::bind(&StreamMgr::inCallback, this, _1);
@@ -278,6 +316,25 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
     for (auto &handler : _inDataHandlers) {
       handler->reset(daq.get());
     }
+
+    d fs = daq->samplerate();
+    /// Create input filters
+    _inputFilters.clear();
+    /// No input filter for monitor channel.
+    if (config.monitorOutput && devinfo.hasInternalOutputMonitor) {
+      _inputFilters.push_back(nullptr);
+    }
+    for (auto &ch : daq->inchannel_config) {
+      if (ch.digitalHighPassCutOn < 0) {
+        _inputFilters.push_back(nullptr);
+      } else if (ch.digitalHighPassCutOn == 0) {
+        throw rte("Digital highpass cuton should be > 0 if activated");
+      } else {
+        // Put in a digital high-pass filter.
+        _inputFilters.emplace_back(std::make_unique<SeriesBiquad>(
+            SeriesBiquad::firstOrderHighPass(fs, ch.digitalHighPassCutOn)));
+      }
+    } // End of input filter creation
   }
 
   if (isOutput) {
@@ -292,8 +349,8 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
     }
   }
 
-  /// Start the DAQ. If it fails, everything is still nicely cleaned up and the
+  /// Start the DAQ. If it fails, everything is still nicely cleaned up and
-  /// daq unique_ptr cleans up resources nicely.
+  /// the daq unique_ptr cleans up resources nicely.
   daq->start(inCallback, outCallback);
 
   // Move daq ptr to right place

src/lasp/device/lasp_streammgr.h

@@ -65,6 +65,8 @@ public:
   void stop();
 };
 
+class SeriesBiquad;
+
 /**
  * @brief Stream manager. Used to manage the input and output streams.
  * Implemented as a singleton: only one stream manager can be in existance for
@@ -96,6 +98,12 @@ class StreamMgr {
    * implemented as to generate the same data for all output channels.
    */
   std::shared_ptr<Siggen> _siggen;
+
+  /**
+   * @brief Filters on input stream. For example, a digital high pass filter.
+   */
+  std::vector<std::unique_ptr<SeriesBiquad>> _inputFilters;
+
   std::mutex _siggen_mtx;
 
   std::mutex _devices_mtx;