ASCEE/lasp
ASCEE/lasp
5
1
Fork 0
Code Issues 5 Pull Requests Actions Releases 14 Activity

(Re)implemented digital highpass filter on input data.

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
src/lasp/device/lasp_daqdata.cpp
View File

@ -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) */
/* : 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<byte_t *> &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<nchannels);
assert(channel < nchannels);
assert(ptr);
memcpy(ptr, raw_ptr(0, channel), sw*nframes);
memcpy(ptr, raw_ptr(0, channel), sw * nframes);
}
template <typename T>
@ -78,7 +82,7 @@ d DaqData::toFloat(const us frame, const us channel) const {
/* DEBUGTRACE_ENTER; */
if constexpr (std::is_integral<T>::value) {
return static_cast<d>(value<T>(frame, channel)) /
std::numeric_limits<T>::max();
std::numeric_limits<T>::max();
} else {
return static_cast<d>(value<T>(frame, channel));
}
@ -87,7 +91,7 @@ d DaqData::toFloat(const us frame, const us channel) const {
template <typename T> vd DaqData::toFloat(const us channel) const {
DEBUGTRACE_ENTER;
#if LASP_DEBUG == 1
check_type<T>();
check_type<T>();
#endif
vd res(nframes);
for (us i = 0; i < nframes; i++) {
@ -99,7 +103,7 @@ template <typename T> dmat DaqData::toFloat() const {
DEBUGTRACE_ENTER;
#if LASP_DEBUG == 1
check_type<T>();
check_type<T>();
#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<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 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;
}

36
src/lasp/device/lasp_daqdata.h
View File

@ -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.

75
src/lasp/device/lasp_streammgr.cpp
View File

@ -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,11 +93,46 @@ bool StreamMgr::inCallback(const DaqData &data) {
std::scoped_lock<std::mutex> 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 = 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
/// 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

8
src/lasp/device/lasp_streammgr.h
View File

@ -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;