diff --git a/src/lasp/device/lasp_daqdata.cpp b/src/lasp/device/lasp_daqdata.cpp index bd4c56d..f191ef0 100644 --- a/src/lasp/device/lasp_daqdata.cpp +++ b/src/lasp/device/lasp_daqdata.cpp @@ -1,8 +1,8 @@ /* #define DEBUGTRACE_ENABLED */ -#include "debugtrace.hpp" -#include #include "lasp_daqdata.h" +#include "debugtrace.hpp" #include "lasp_mathtypes.h" +#include #include #include @@ -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) */ -/* : nframes(o.nframes), nchannels(o.nchannels), dtype(o.dtype), */ -/* dtype_descr(std::move(o.dtype_descr)), sw(o.sw) { */ +DaqData::DaqData(DaqData &&o) + : nframes(o.nframes), nchannels(o.nchannels), dtype(o.dtype), + dtype_descr(std::move(o.dtype_descr)), sw(o.sw) { -/* DEBUGTRACE_ENTER; */ -/* _data = o._data; */ -/* /// Nullptrs do not get deleted */ -/* o._data = nullptr; */ -/* } */ + /// Steal from other one + + DEBUGTRACE_ENTER; + _data = o._data; + /// Nullptrs do not get deleted + o._data = nullptr; +} DaqData::~DaqData() { DEBUGTRACE_ENTER; @@ -58,19 +59,22 @@ void DaqData::copyInFromRaw(const std::vector &ptrs) { DEBUGTRACE_ENTER; us ch = 0; assert(ptrs.size() == nchannels); - for (auto& ptr : ptrs) { - assert(ch < nchannels); - memcpy(&_data[sw * ch * nframes], ptr, sw * nframes); - /* std::copy(ptr, ptr + sw * nframes, &_data[sw * ch * nframes]); */ + for (auto &ptr : ptrs) { + copyInFromRaw(ch, ptr); 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); */ - assert(channel @@ -78,7 +82,7 @@ d DaqData::toFloat(const us frame, const us channel) const { /* DEBUGTRACE_ENTER; */ if constexpr (std::is_integral::value) { return static_cast(value(frame, channel)) / - std::numeric_limits::max(); + std::numeric_limits::max(); } else { return static_cast(value(frame, channel)); } @@ -87,7 +91,7 @@ d DaqData::toFloat(const us frame, const us channel) const { template vd DaqData::toFloat(const us channel) const { DEBUGTRACE_ENTER; #if LASP_DEBUG == 1 - check_type(); + check_type(); #endif vd res(nframes); for (us i = 0; i < nframes; i++) { @@ -99,7 +103,7 @@ template dmat DaqData::toFloat() const { DEBUGTRACE_ENTER; #if LASP_DEBUG == 1 - check_type(); + check_type(); #endif dmat res(nframes, nchannels); @@ -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(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 +void DaqData::fromFloat(const us frame, const us channel, const d val) { + DEBUGTRACE_ENTER; +#if LASP_DEBUG == 1 + check_type(); +#endif + if constexpr (std::is_integral::value) { + value(frame, channel) = + static_cast(val * std::numeric_limits::max()); + } else { + value(frame, channel) = static_cast(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(frame, channel, val); + break; + case (DataType::dtype_int16): + return fromFloat(frame, channel, val); + break; + case (DataType::dtype_int32): + return fromFloat(frame, channel, val); + break; + case (DataType::dtype_fl32): + return fromFloat(frame, channel, val); + break; + case (DataType::dtype_fl64): + return fromFloat(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(frame, channel, vals(frame)); + } + break; + case (DataType::dtype_int16): + for (us frame = 0; frame < nframes; frame++) { + fromFloat(frame, channel, vals(frame)); + } + break; + case (DataType::dtype_int32): + for (us frame = 0; frame < nframes; frame++) { + fromFloat(frame, channel, vals(frame)); + } + break; + case (DataType::dtype_fl32): + for (us frame = 0; frame < nframes; frame++) { + fromFloat(frame, channel, vals(frame)); + } + break; + case (DataType::dtype_fl64): + for (us frame = 0; frame < nframes; frame++) { + fromFloat(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 last channel (as float)" << toFloat(nframes-1,nchannels-1) << 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 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; - } diff --git a/src/lasp/device/lasp_daqdata.h b/src/lasp/device/lasp_daqdata.h index 034ebfa..45b0034 100644 --- a/src/lasp/device/lasp_daqdata.h +++ b/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 &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 &data); + // Return value based on type template T &value(const us frame, const us channel) { #if LASP_DEBUG == 1 @@ -188,6 +217,11 @@ protected: template arma::Col toFloat(const us channel_no) const; template d toFloat(const us frame_no, const us channel_no) const; + /* template void fromFloat(const dmat&); */ + template + void fromFloat(const us channel_no, const arma::Col &vals); + template + 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. diff --git a/src/lasp/device/lasp_streammgr.cpp b/src/lasp/device/lasp_streammgr.cpp index 4fc4d4b..f2dc33f 100644 --- a/src/lasp/device/lasp_streammgr.cpp +++ b/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 #include @@ -92,11 +93,46 @@ bool StreamMgr::inCallback(const DaqData &data) { std::scoped_lock lck(_inDataHandler_mtx); - for (auto &handler : _inDataHandlers) { + assert(_inputFilters.size() == data.nchannels); - bool res = handler->inCallback(data); - if (!res) - return false; + 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) { + return false; + } + } } return true; } @@ -258,9 +294,9 @@ void StreamMgr::startStream(const DaqConfiguration &config) { "first stop existing stream"); } else if (_inputStream) { if (_inputStream->duplexMode() && isOutput) { - throw rte( - "Error: output stream is already running (in duplex mode). Please " - "first stop existing stream"); + throw rte("Error: output stream is already running (in duplex mode). " + "Please " + "first stop existing stream"); } } @@ -270,6 +306,8 @@ void StreamMgr::startStream(const DaqConfiguration &config) { using namespace std::placeholders; std::unique_ptr 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::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 - /// daq unique_ptr cleans up resources nicely. + /// Start the DAQ. If it fails, everything is still nicely cleaned up and + /// the daq unique_ptr cleans up resources nicely. daq->start(inCallback, outCallback); // Move daq ptr to right place diff --git a/src/lasp/device/lasp_streammgr.h b/src/lasp/device/lasp_streammgr.h index ce155a1..05c49fa 100644 --- a/src/lasp/device/lasp_streammgr.h +++ b/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; + + /** + * @brief Filters on input stream. For example, a digital high pass filter. + */ + std::vector> _inputFilters; + std::mutex _siggen_mtx; std::mutex _devices_mtx;