(Re)implemented digital highpass filter on input data.

This commit is contained in:
Anne de Jong 2022-10-20 17:12:34 +02:00
parent cb72c2ba74
commit bcf30044e6
4 changed files with 213 additions and 36 deletions

View File

@ -1,8 +1,8 @@
/* #define DEBUGTRACE_ENABLED */ /* #define DEBUGTRACE_ENABLED */
#include "debugtrace.hpp"
#include <armadillo>
#include "lasp_daqdata.h" #include "lasp_daqdata.h"
#include "debugtrace.hpp"
#include "lasp_mathtypes.h" #include "lasp_mathtypes.h"
#include <armadillo>
#include <cassert> #include <cassert>
#include <memory> #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) { DaqData::DaqData(const DaqData &o) : DaqData(o.nframes, o.nchannels, o.dtype) {
DEBUGTRACE_ENTER; DEBUGTRACE_ENTER;
/* std::copy(o._data, &o._data[sw * nchannels * nframes], _data); */
memcpy(_data, o._data, sw * nchannels * nframes); 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() { DaqData::~DaqData() {
DEBUGTRACE_ENTER; DEBUGTRACE_ENTER;
@ -59,12 +60,15 @@ void DaqData::copyInFromRaw(const std::vector<byte_t *> &ptrs) {
us ch = 0; us ch = 0;
assert(ptrs.size() == nchannels); assert(ptrs.size() == nchannels);
for (auto &ptr : ptrs) { 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++; 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) { void DaqData::copyToRaw(const us channel, byte_t *ptr) {
/* std::copy(raw_ptr(0, channel), raw_ptr(nframes, channel), 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 { vd DaqData::toFloat(const us channel_no) const {
DEBUGTRACE_ENTER; DEBUGTRACE_ENTER;
using DataType = DataTypeDescriptor::DataType; using DataType = DataTypeDescriptor::DataType;
cerr << (int) dtype << endl; /* cerr << (int)dtype << endl; */
switch (dtype) { switch (dtype) {
case (DataType::dtype_int8): { case (DataType::dtype_int8): {
return toFloat<int8_t>(channel_no); return toFloat<int8_t>(channel_no);
@ -166,7 +170,6 @@ vd DaqData::toFloat(const us channel_no) const {
return vd(); return vd();
} }
dmat DaqData::toFloat() const { dmat DaqData::toFloat() const {
DEBUGTRACE_ENTER; DEBUGTRACE_ENTER;
@ -204,17 +207,92 @@ dmat DaqData::toFloat() const {
return dmat(); 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 { void DaqData::print() const {
cout << "Number of frames: " << nframes << endl; cout << "Number of frames: " << nframes << endl;
cout << "Number of channels: " << nchannels << endl; cout << "Number of channels: " << nchannels << endl;
cout << "DataType: " << dtype_map.at(dtype).name << endl; cout << "DataType: " << dtype_map.at(dtype).name << endl;
cout << "First sample of first channel (as float)" << toFloat(0, 0) << 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(); dmat data = toFloat();
vrd max = arma::max(data, 0); vrd max = arma::max(data, 0);
vrd min = arma::min(data, 0); vrd min = arma::min(data, 0);
cout << "Maximum value in buf: " << max << endl; cout << "Maximum value in buf: " << max << endl;
cout << "Minumum value in buf: " << min << endl; cout << "Minumum value in buf: " << min << endl;
} }

View File

@ -65,7 +65,7 @@ public:
* @brief Initialize using no allocation * @brief Initialize using no allocation
*/ */
DaqData(const DaqData &); DaqData(const DaqData &);
/* DaqData(DaqData &&); */ DaqData(DaqData &&);
DaqData &operator=(const DaqData &) = delete; DaqData &operator=(const DaqData &) = delete;
virtual ~DaqData(); virtual ~DaqData();
@ -104,6 +104,15 @@ public:
*/ */
void copyInFromRaw(const std::vector<byte_t *> &ptrs); 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. * @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; 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 // Return value based on type
template <typename T> T &value(const us frame, const us channel) { template <typename T> T &value(const us frame, const us channel) {
#if LASP_DEBUG == 1 #if LASP_DEBUG == 1
@ -188,6 +217,11 @@ protected:
template <typename T> arma::Col<d> toFloat(const us channel_no) const; 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> 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 * @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. * float, if the stored type is integer. Also scales by its maximum value.

View File

@ -1,6 +1,7 @@
/* #define DEBUGTRACE_ENABLED */ /* #define DEBUGTRACE_ENABLED */
#include "lasp_streammgr.h" #include "lasp_streammgr.h"
#include "debugtrace.hpp" #include "debugtrace.hpp"
#include "lasp_biquadbank.h"
#include "lasp_thread.h" #include "lasp_thread.h"
#include <algorithm> #include <algorithm>
#include <assert.h> #include <assert.h>
@ -92,12 +93,47 @@ bool StreamMgr::inCallback(const DaqData &data) {
std::scoped_lock<std::mutex> lck(_inDataHandler_mtx); 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) { for (auto &handler : _inDataHandlers) {
bool res = handler->inCallback(data); bool res = handler->inCallback(data);
if (!res) if (!res) {
return false; return false;
} }
}
}
return true; return true;
} }
@ -258,8 +294,8 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
"first stop existing stream"); "first stop existing stream");
} else if (_inputStream) { } else if (_inputStream) {
if (_inputStream->duplexMode() && isOutput) { 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"); "first stop existing stream");
} }
} }
@ -270,6 +306,8 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
using namespace std::placeholders; using namespace std::placeholders;
std::unique_ptr<Daq> daq = Daq::createDaq(devinfo, config); std::unique_ptr<Daq> daq = Daq::createDaq(devinfo, config);
assert(daq);
if (isInput) { if (isInput) {
/// Give incallback as parameter to stream /// Give incallback as parameter to stream
inCallback = std::bind(&StreamMgr::inCallback, this, _1); inCallback = std::bind(&StreamMgr::inCallback, this, _1);
@ -278,6 +316,25 @@ void StreamMgr::startStream(const DaqConfiguration &config) {
for (auto &handler : _inDataHandlers) { for (auto &handler : _inDataHandlers) {
handler->reset(daq.get()); 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) { 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); daq->start(inCallback, outCallback);
// Move daq ptr to right place // Move daq ptr to right place

View File

@ -65,6 +65,8 @@ public:
void stop(); void stop();
}; };
class SeriesBiquad;
/** /**
* @brief Stream manager. Used to manage the input and output streams. * @brief Stream manager. Used to manage the input and output streams.
* Implemented as a singleton: only one stream manager can be in existance for * 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. * implemented as to generate the same data for all output channels.
*/ */
std::shared_ptr<Siggen> _siggen; 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 _siggen_mtx;
std::mutex _devices_mtx; std::mutex _devices_mtx;