From bd10436283ab08ebddf40ca797edff4adc2582af Mon Sep 17 00:00:00 2001 From: Georgi Gerganov Date: Sat, 28 May 2022 22:16:32 +0300 Subject: [PATCH] wip --- examples/arduino-rx/.gitignore | 5 + examples/arduino-rx/CMakeLists.txt | 1 + examples/arduino-rx/ggwave.cpp | 1452 --------------------- examples/arduino-rx/ggwave/ggwave.h | 592 --------- examples/arduino-rx/reed-solomon/LICENSE | 21 - examples/arduino-rx/reed-solomon/gf.hpp | 235 ---- examples/arduino-rx/reed-solomon/poly.hpp | 94 -- examples/arduino-rx/reed-solomon/rs.hpp | 538 -------- examples/arduino-rx/resampler.cpp | 161 --- examples/arduino-rx/resampler.h | 49 - 10 files changed, 6 insertions(+), 3142 deletions(-) create mode 100644 examples/arduino-rx/.gitignore delete mode 100644 examples/arduino-rx/ggwave.cpp delete mode 100644 examples/arduino-rx/ggwave/ggwave.h delete mode 100644 examples/arduino-rx/reed-solomon/LICENSE delete mode 100644 examples/arduino-rx/reed-solomon/gf.hpp delete mode 100644 examples/arduino-rx/reed-solomon/poly.hpp delete mode 100644 examples/arduino-rx/reed-solomon/rs.hpp delete mode 100644 examples/arduino-rx/resampler.cpp delete mode 100644 examples/arduino-rx/resampler.h diff --git a/examples/arduino-rx/.gitignore b/examples/arduino-rx/.gitignore new file mode 100644 index 0000000..efd47f2 --- /dev/null +++ b/examples/arduino-rx/.gitignore @@ -0,0 +1,5 @@ +ggwave +ggwave.cpp +resampler.h +resampler.cpp +reed-solomon diff --git a/examples/arduino-rx/CMakeLists.txt b/examples/arduino-rx/CMakeLists.txt index 76bd66f..898fa28 100644 --- a/examples/arduino-rx/CMakeLists.txt +++ b/examples/arduino-rx/CMakeLists.txt @@ -8,3 +8,4 @@ configure_file(${CMAKE_SOURCE_DIR}/src/resampler.cpp ${CMAKE_CURRENT_SOU configure_file(${CMAKE_SOURCE_DIR}/src/reed-solomon/gf.hpp ${CMAKE_CURRENT_SOURCE_DIR}/reed-solomon/gf.hpp COPYONLY) configure_file(${CMAKE_SOURCE_DIR}/src/reed-solomon/rs.hpp ${CMAKE_CURRENT_SOURCE_DIR}/reed-solomon/rs.hpp COPYONLY) configure_file(${CMAKE_SOURCE_DIR}/src/reed-solomon/poly.hpp ${CMAKE_CURRENT_SOURCE_DIR}/reed-solomon/poly.hpp COPYONLY) +configure_file(${CMAKE_SOURCE_DIR}/src/reed-solomon/LICENSE ${CMAKE_CURRENT_SOURCE_DIR}/reed-solomon/LICENSE COPYONLY) diff --git a/examples/arduino-rx/ggwave.cpp b/examples/arduino-rx/ggwave.cpp deleted file mode 100644 index 000bd22..0000000 --- a/examples/arduino-rx/ggwave.cpp +++ /dev/null @@ -1,1452 +0,0 @@ -#include "ggwave/ggwave.h" - -#include "resampler.h" - -#include "reed-solomon/rs.hpp" - -#include -#include -#include -#include -#include -//#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -#define ggprintf(...) \ - g_fptr && fprintf(g_fptr, __VA_ARGS__) - -// -// C interface -// - -namespace { -FILE * g_fptr = stderr; -std::map g_instances; -std::map g_rxProtocols; -} - -extern "C" -void ggwave_setLogFile(void * fptr) { - GGWave::setLogFile((FILE *) fptr); -} - -extern "C" -ggwave_Parameters ggwave_getDefaultParameters(void) { - return GGWave::getDefaultParameters(); -} - -extern "C" -ggwave_Instance ggwave_init(const ggwave_Parameters parameters) { - static ggwave_Instance curId = 0; - - g_instances[curId] = new GGWave({ - parameters.payloadLength, - parameters.sampleRateInp, - parameters.sampleRateOut, - parameters.sampleRate, - parameters.samplesPerFrame, - parameters.soundMarkerThreshold, - parameters.sampleFormatInp, - parameters.sampleFormatOut, - parameters.operatingMode}); - - return curId++; -} - -extern "C" -void ggwave_free(ggwave_Instance instance) { - delete (GGWave *) g_instances[instance]; - g_instances.erase(instance); -} - -extern "C" -int ggwave_encode( - ggwave_Instance instance, - const char * dataBuffer, - int dataSize, - ggwave_TxProtocolId txProtocolId, - int volume, - char * outputBuffer, - int query) { - GGWave * ggWave = (GGWave *) g_instances[instance]; - - if (ggWave == nullptr) { - ggprintf("Invalid GGWave instance %d\n", instance); - return -1; - } - - if (ggWave->init(dataSize, dataBuffer, ggWave->getTxProtocol(txProtocolId), volume) == false) { - ggprintf("Failed to initialize GGWave instance %d\n", instance); - return -1; - } - - if (query != 0) { - if (query == 1) { - return ggWave->encodeSize_bytes(); - } - - return ggWave->encodeSize_samples(); - } - - int nSamples = 0; - - GGWave::CBWaveformOut cbWaveformOut = [&](const void * data, uint32_t nBytes) { - char * p = (char *) data; - std::copy(p, p + nBytes, outputBuffer); - - nSamples = nBytes/ggWave->getSampleSizeBytesOut(); - }; - - if (ggWave->encode(cbWaveformOut) == false) { - ggprintf("Failed to encode data - GGWave instance %d\n", instance); - return -1; - } - - return nSamples; -} - -extern "C" -int ggwave_decode( - ggwave_Instance instance, - const char * dataBuffer, - int dataSize, - char * outputBuffer) { - GGWave * ggWave = (GGWave *) g_instances[instance]; - - GGWave::CBWaveformInp cbWaveformInp = [&](void * data, uint32_t nMaxBytes) -> uint32_t { - uint32_t nCopied = std::min((uint32_t) dataSize, nMaxBytes); - std::copy(dataBuffer, dataBuffer + nCopied, (char *) data); - - dataSize -= nCopied; - dataBuffer += nCopied; - - return nCopied; - }; - - ggWave->decode(cbWaveformInp); - - // TODO : avoid allocation - GGWave::TxRxData rxData; - - auto rxDataLength = ggWave->takeRxData(rxData); - if (rxDataLength == -1) { - // failed to decode message - return -1; - } else if (rxDataLength > 0) { - memcpy(outputBuffer, rxData.data(), rxDataLength); - } - - return rxDataLength; -} - -extern "C" -int ggwave_ndecode( - ggwave_Instance instance, - const char * dataBuffer, - int dataSize, - char * outputBuffer, - int outputSize) { - // TODO : avoid duplicated code - GGWave * ggWave = (GGWave *) g_instances[instance]; - - GGWave::CBWaveformInp cbWaveformInp = [&](void * data, uint32_t nMaxBytes) -> uint32_t { - uint32_t nCopied = std::min((uint32_t) dataSize, nMaxBytes); - std::copy(dataBuffer, dataBuffer + nCopied, (char *) data); - - dataSize -= nCopied; - dataBuffer += nCopied; - - return nCopied; - }; - - ggWave->decode(cbWaveformInp); - - // TODO : avoid allocation - GGWave::TxRxData rxData; - - auto rxDataLength = ggWave->takeRxData(rxData); - if (rxDataLength == -1) { - // failed to decode message - return -1; - } else if (rxDataLength > outputSize) { - // the outputBuffer is not big enough to store the data - return -2; - } else if (rxDataLength > 0) { - memcpy(outputBuffer, rxData.data(), rxDataLength); - } - - return rxDataLength; -} - -extern "C" -void ggwave_toggleRxProtocol( - ggwave_Instance instance, - ggwave_TxProtocolId rxProtocolId, - int state) { - // if never called - initialize with all available protocols - if (g_rxProtocols.find(instance) == g_rxProtocols.end()) { - g_rxProtocols[instance] = GGWave::getTxProtocols(); - } - - if (state == 0) { - // disable Rx protocol - g_rxProtocols[instance].erase(rxProtocolId); - } else if (state == 1) { - // enable Rx protocol - g_rxProtocols[instance][rxProtocolId] = GGWave::getTxProtocols().at(rxProtocolId); - } - - g_instances[instance]->setRxProtocols(g_rxProtocols[instance]); -} - -// -// C++ implementation -// - -namespace { - -// FFT routines taken from https://stackoverflow.com/a/37729648/4039976 - -int log2(int N) { - int k = N, i = 0; - while(k) { - k >>= 1; - i++; - } - return i - 1; -} - -int reverse(int N, int n) { - int j, p = 0; - for(j = 1; j <= log2(N); j++) { - if(n & (1 << (log2(N) - j))) - p |= 1 << (j - 1); - } - return p; -} - -void ordina(float * f1, int N) { - static thread_local float f2[2*GGWave::kMaxSamplesPerFrame]; - for (int i = 0; i < N; i++) { - int ir = reverse(N, i); - f2[2*i + 0] = f1[2*ir + 0]; - f2[2*i + 1] = f1[2*ir + 1]; - } - for (int j = 0; j < N; j++) { - f1[2*j + 0] = f2[2*j + 0]; - f1[2*j + 1] = f2[2*j + 1]; - } -} - -void transform(float * f, int N) { - ordina(f, N); //first: reverse order - float * W; - W = (float *)malloc(N*sizeof(float)); - W[2*1 + 0] = cos(-2.*M_PI/N); - W[2*1 + 1] = sin(-2.*M_PI/N); - W[2*0 + 0] = 1; - W[2*0 + 1] = 0; - for (int i = 2; i < N / 2; i++) { - W[2*i + 0] = cos(-2.*i*M_PI/N); - W[2*i + 1] = sin(-2.*i*M_PI/N); - } - int n = 1; - int a = N / 2; - for(int j = 0; j < log2(N); j++) { - for(int i = 0; i < N; i++) { - if(!(i & n)) { - int wi = (i * a) % (n * a); - int fi = i + n; - float a = W[2*wi + 0]; - float b = W[2*wi + 1]; - float c = f[2*fi + 0]; - float d = f[2*fi + 1]; - float temp[2] = { f[2*i + 0], f[2*i + 1] }; - float Temp[2] = { a*c - b*d, b*c + a*d }; - f[2*i + 0] = temp[0] + Temp[0]; - f[2*i + 1] = temp[1] + Temp[1]; - f[2*fi + 0] = temp[0] - Temp[0]; - f[2*fi + 1] = temp[1] - Temp[1]; - } - } - n *= 2; - a = a / 2; - } - free(W); -} - -void FFT(float * f, int N, float d) { - transform(f, N); - for (int i = 0; i < N; i++) { - f[2*i + 0] *= d; - f[2*i + 1] *= d; - } -} - -void FFT(const float * src, float * dst, int N, float d) { - for (int i = 0; i < N; ++i) { - dst[2*i + 0] = src[i]; - dst[2*i + 1] = 0.0f; - } - FFT(dst, N, d); -} - -inline void addAmplitudeSmooth( - const GGWave::AmplitudeData & src, - GGWave::AmplitudeData & dst, - float scalar, int startId, int finalId, int cycleMod, int nPerCycle) { - int nTotal = nPerCycle*finalId; - float frac = 0.15f; - float ds = frac*nTotal; - float ids = 1.0f/ds; - int nBegin = frac*nTotal; - int nEnd = (1.0f - frac)*nTotal; - for (int i = startId; i < finalId; i++) { - float k = cycleMod*finalId + i; - if (k < nBegin) { - dst[i] += scalar*src[i]*(k*ids); - } else if (k > nEnd) { - dst[i] += scalar*src[i]*(((float)(nTotal) - k)*ids); - } else { - dst[i] += scalar*src[i]; - } - } -} - -template -float getTime_ms(const T & tStart, const T & tEnd) { - return ((float)(std::chrono::duration_cast(tEnd - tStart).count()))/1000.0; -} - -int getECCBytesForLength(int len) { - return len < 4 ? 2 : std::max(4, 2*(len/5)); -} - -int bytesForSampleFormat(GGWave::SampleFormat sampleFormat) { - switch (sampleFormat) { - case GGWAVE_SAMPLE_FORMAT_UNDEFINED: return 0; break; - case GGWAVE_SAMPLE_FORMAT_U8: return sizeof(uint8_t); break; - case GGWAVE_SAMPLE_FORMAT_I8: return sizeof(int8_t); break; - case GGWAVE_SAMPLE_FORMAT_U16: return sizeof(uint16_t); break; - case GGWAVE_SAMPLE_FORMAT_I16: return sizeof(int16_t); break; - case GGWAVE_SAMPLE_FORMAT_F32: return sizeof(float); break; - }; - - ggprintf("Invalid sample format: %d\n", (int) sampleFormat); - - return 0; -} - -} - -struct GGWave::Impl { - Resampler resampler; -}; - -void GGWave::setLogFile(FILE * fptr) { - g_fptr = fptr; -} - -const GGWave::Parameters & GGWave::getDefaultParameters() { - static ggwave_Parameters result { - -1, // vaiable payload length - kDefaultSampleRate, - kDefaultSampleRate, - kDefaultSampleRate, - kDefaultSamplesPerFrame, - kDefaultSoundMarkerThreshold, - GGWAVE_SAMPLE_FORMAT_F32, - GGWAVE_SAMPLE_FORMAT_F32, - GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX, - }; - - return result; -} - -GGWave::GGWave(const Parameters & parameters) : - m_sampleRateInp (parameters.sampleRateInp), - m_sampleRateOut (parameters.sampleRateOut), - m_sampleRate (parameters.sampleRate), - m_samplesPerFrame (parameters.samplesPerFrame), - m_isamplesPerFrame (1.0f/m_samplesPerFrame), - m_sampleSizeBytesInp (bytesForSampleFormat(parameters.sampleFormatInp)), - m_sampleSizeBytesOut (bytesForSampleFormat(parameters.sampleFormatOut)), - m_sampleFormatInp (parameters.sampleFormatInp), - m_sampleFormatOut (parameters.sampleFormatOut), - m_hzPerSample (m_sampleRate/m_samplesPerFrame), - m_ihzPerSample (1.0f/m_hzPerSample), - m_freqDelta_bin (1), - m_freqDelta_hz (2*m_hzPerSample), - m_nBitsInMarker (16), - m_nMarkerFrames (parameters.payloadLength > 0 ? 0 : kDefaultMarkerFrames), - m_encodedDataOffset (parameters.payloadLength > 0 ? 0 : kDefaultEncodedDataOffset), - m_soundMarkerThreshold(parameters.soundMarkerThreshold), - - // common - m_isFixedPayloadLength(parameters.payloadLength > 0), - m_payloadLength (parameters.payloadLength), - m_dataEncoded (kMaxDataSize), - - // Rx - m_isRxEnabled(parameters.operatingMode == GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX || - parameters.operatingMode == GGWAVE_OPERATING_MODE_ONLY_RX), - - m_samplesNeeded (m_samplesPerFrame), - m_fftInp (m_samplesPerFrame), - m_fftOut (2*m_samplesPerFrame), - m_hasNewSpectrum (false), - m_hasNewAmplitude (false), - m_sampleSpectrum (m_samplesPerFrame), - m_sampleAmplitude (m_sampleRateInp == m_sampleRate ? m_samplesPerFrame : m_samplesPerFrame + 128), // small extra space because sometimes resampling needs a few more samples - m_sampleAmplitudeResampled(m_sampleRateInp == m_sampleRate ? m_samplesPerFrame : 8*m_samplesPerFrame), // min input sampling rate is 0.125*m_sampleRate - m_sampleAmplitudeTmp (m_sampleRateInp == m_sampleRate ? m_samplesPerFrame*m_sampleSizeBytesInp : 8*m_samplesPerFrame*m_sampleSizeBytesInp), - m_hasNewRxData (false), - m_lastRxDataLength (0), - m_rxData (kMaxDataSize), - m_rxProtocol (getDefaultTxProtocol()), - m_rxProtocolId (getDefaultTxProtocolId()), - m_rxProtocols (getTxProtocols()), - m_historyId (0), - m_sampleAmplitudeAverage (parameters.payloadLength > 0 ? 0 : m_samplesPerFrame), - m_sampleAmplitudeHistory (parameters.payloadLength > 0 ? 0 : kMaxSpectrumHistory), - m_historyIdFixed (0), - - // Tx - m_isTxEnabled(parameters.operatingMode == GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX), - - m_hasNewTxData (false), - m_sendVolume (0.1), - m_txDataLength (0), - m_txData (m_isTxEnabled ? kMaxDataSize : 0), - m_outputBlock (m_isTxEnabled ? m_samplesPerFrame : 0), - m_outputBlockResampled(m_isTxEnabled ? 2*m_samplesPerFrame : 0), - m_outputBlockTmp (m_isTxEnabled ? kMaxRecordedFrames*m_samplesPerFrame*m_sampleSizeBytesOut : 0), - m_outputBlockI16 (m_isTxEnabled ? kMaxRecordedFrames*m_samplesPerFrame : 0), - m_impl(new Impl()) { - - if (m_payloadLength > 0) { - // fixed payload length - if (m_payloadLength > kMaxLengthFixed) { - ggprintf("Invalid payload legnth: %d, max: %d\n", m_payloadLength, kMaxLengthFixed); - return; - } - - m_txDataLength = m_payloadLength; - - int totalLength = m_txDataLength + getECCBytesForLength(m_txDataLength); - int totalTxs = (totalLength + minBytesPerTx() - 1)/minBytesPerTx(); - - m_spectrumHistoryFixed.resize(totalTxs*maxFramesPerTx()); - } else { - // variable payload length - m_recordedAmplitude.resize(kMaxRecordedFrames*m_samplesPerFrame); - } - - if (m_sampleSizeBytesInp == 0) { - ggprintf("Invalid or unsupported capture sample format: %d\n", (int) parameters.sampleFormatInp); - return; - } - - if (m_sampleSizeBytesOut == 0) { - ggprintf("Invalid or unsupported playback sample format: %d\n", (int) parameters.sampleFormatOut); - return; - } - - if (parameters.samplesPerFrame > kMaxSamplesPerFrame) { - ggprintf("Invalid samples per frame: %d, max: %d\n", parameters.samplesPerFrame, kMaxSamplesPerFrame); - return; - } - - if (m_sampleRateInp < kSampleRateMin) { - ggprintf("Error: capture sample rate (%g Hz) must be >= %g Hz\n", m_sampleRateInp, kSampleRateMin); - return; - } - - if (m_sampleRateInp > kSampleRateMax) { - ggprintf("Error: capture sample rate (%g Hz) must be <= %g Hz\n", m_sampleRateInp, kSampleRateMax); - return; - } - - init("", getDefaultTxProtocol(), 0); -} - -GGWave::~GGWave() { -} - -bool GGWave::init(const std::string & text, const int volume) { - return init((int) text.size(), text.data(), getDefaultTxProtocol(), volume); -} - -bool GGWave::init(const std::string & text, const TxProtocol & txProtocol, const int volume) { - return init((int) text.size(), text.data(), txProtocol, volume); -} - -bool GGWave::init(int dataSize, const char * dataBuffer, const int volume) { - return init(dataSize, dataBuffer, getDefaultTxProtocol(), volume); -} - -bool GGWave::init(int dataSize, const char * dataBuffer, const TxProtocol & txProtocol, const int volume) { - if (dataSize < 0) { - ggprintf("Negative data size: %d\n", dataSize); - return false; - } - - auto maxLength = m_isFixedPayloadLength ? m_payloadLength : kMaxLengthVarible; - if (dataSize > maxLength) { - ggprintf("Truncating data from %d to %d bytes\n", dataSize, maxLength); - dataSize = maxLength; - } - - if (volume < 0 || volume > 100) { - ggprintf("Invalid volume: %d\n", volume); - return false; - } - - // Tx - if (m_isTxEnabled) { - m_txProtocol = txProtocol; - m_txDataLength = dataSize; - m_sendVolume = ((double)(volume))/100.0f; - - const uint8_t * text = reinterpret_cast(dataBuffer); - - m_hasNewTxData = false; - std::fill(m_txData.begin(), m_txData.end(), 0); - std::fill(m_dataEncoded.begin(), m_dataEncoded.end(), 0); - - if (m_txDataLength > 0) { - m_txData[0] = m_txDataLength; - for (int i = 0; i < m_txDataLength; ++i) m_txData[i + 1] = text[i]; - - m_hasNewTxData = true; - } - - if (m_isFixedPayloadLength) { - m_txDataLength = m_payloadLength; - } - } - - // Rx - if (m_isRxEnabled) { - m_receivingData = false; - m_analyzingData = false; - - m_framesToAnalyze = 0; - m_framesLeftToAnalyze = 0; - m_framesToRecord = 0; - m_framesLeftToRecord = 0; - - std::fill(m_sampleSpectrum.begin(), m_sampleSpectrum.end(), 0); - std::fill(m_sampleAmplitude.begin(), m_sampleAmplitude.end(), 0); - for (auto & s : m_sampleAmplitudeHistory) { - s.resize(m_samplesPerFrame); - std::fill(s.begin(), s.end(), 0); - } - - std::fill(m_rxData.begin(), m_rxData.end(), 0); - - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_fftOut[2*i + 0] = 0.0f; - m_fftOut[2*i + 1] = 0.0f; - } - - for (auto & s : m_spectrumHistoryFixed) { - s.resize(m_samplesPerFrame); - std::fill(s.begin(), s.end(), 0); - } - } - - return true; -} - -uint32_t GGWave::encodeSize_bytes() const { - return encodeSize_samples()*m_sampleSizeBytesOut; -} - -uint32_t GGWave::encodeSize_samples() const { - if (m_hasNewTxData == false) { - return 0; - } - - float factor = 1.0f; - int samplesPerFrameOut = m_samplesPerFrame; - if (m_sampleRateOut != m_sampleRate) { - factor = m_sampleRate/m_sampleRateOut; - // note : +1 extra sample in order to overestimate the buffer size - samplesPerFrameOut = m_impl->resampler.resample(factor, m_samplesPerFrame, m_outputBlock.data(), nullptr) + 1; - } - int nECCBytesPerTx = getECCBytesForLength(m_txDataLength); - int sendDataLength = m_txDataLength + m_encodedDataOffset; - int totalBytes = sendDataLength + nECCBytesPerTx; - int totalDataFrames = ((totalBytes + m_txProtocol.bytesPerTx - 1)/m_txProtocol.bytesPerTx)*m_txProtocol.framesPerTx; - - return ( - m_nMarkerFrames + totalDataFrames + m_nMarkerFrames - )*samplesPerFrameOut; -} - -bool GGWave::encode(const CBWaveformOut & cbWaveformOut) { - int frameId = 0; - - m_impl->resampler.reset(); - - std::vector phaseOffsets(kMaxDataBits); - - for (int k = 0; k < (int) phaseOffsets.size(); ++k) { - phaseOffsets[k] = (M_PI*k)/(m_txProtocol.nDataBitsPerTx()); - } - - // note : what is the purpose of this shuffle ? I forgot .. :( - //std::random_device rd; - //std::mt19937 g(rd()); - - //std::shuffle(phaseOffsets.begin(), phaseOffsets.end(), g); - - std::vector dataBits(kMaxDataBits); - - std::vector bit1Amplitude(kMaxDataBits); - std::vector bit0Amplitude(kMaxDataBits); - - for (int k = 0; k < (int) dataBits.size(); ++k) { - double freq = bitFreq(m_txProtocol, k); - - bit1Amplitude[k].resize(m_samplesPerFrame); - bit0Amplitude[k].resize(m_samplesPerFrame); - - double phaseOffset = phaseOffsets[k]; - double curHzPerSample = m_hzPerSample; - double curIHzPerSample = 1.0/curHzPerSample; - for (int i = 0; i < m_samplesPerFrame; i++) { - double curi = i; - bit1Amplitude[k][i] = std::sin((2.0*M_PI)*(curi*m_isamplesPerFrame)*(freq*curIHzPerSample) + phaseOffset); - } - for (int i = 0; i < m_samplesPerFrame; i++) { - double curi = i; - bit0Amplitude[k][i] = std::sin((2.0*M_PI)*(curi*m_isamplesPerFrame)*((freq + m_hzPerSample*m_freqDelta_bin)*curIHzPerSample) + phaseOffset); - } - } - - int nECCBytesPerTx = getECCBytesForLength(m_txDataLength); - int sendDataLength = m_txDataLength + m_encodedDataOffset; - int totalBytes = sendDataLength + nECCBytesPerTx; - int totalDataFrames = ((totalBytes + m_txProtocol.bytesPerTx - 1)/m_txProtocol.bytesPerTx)*m_txProtocol.framesPerTx; - - if (m_isFixedPayloadLength == false) { - RS::ReedSolomon rsLength(1, m_encodedDataOffset - 1); - rsLength.Encode(m_txData.data(), m_dataEncoded.data()); - } - - // first byte of m_txData contains the length of the payload, so we skip it: - RS::ReedSolomon rsData = RS::ReedSolomon(m_txDataLength, nECCBytesPerTx); - rsData.Encode(m_txData.data() + 1, m_dataEncoded.data() + m_encodedDataOffset); - - const float factor = m_sampleRate/m_sampleRateOut; - - uint32_t offset = 0; - m_waveformTones.clear(); - - while (m_hasNewTxData) { - std::fill(m_outputBlock.begin(), m_outputBlock.end(), 0.0f); - - std::uint16_t nFreq = 0; - m_waveformTones.push_back({}); - - if (frameId < m_nMarkerFrames) { - nFreq = m_nBitsInMarker; - - for (int i = 0; i < m_nBitsInMarker; ++i) { - m_waveformTones.back().push_back({}); - m_waveformTones.back().back().duration_ms = (1000.0*m_samplesPerFrame)/m_sampleRate; - if (i%2 == 0) { - ::addAmplitudeSmooth(bit1Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, frameId, m_nMarkerFrames); - m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i); - } else { - ::addAmplitudeSmooth(bit0Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, frameId, m_nMarkerFrames); - m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i) + m_hzPerSample; - } - } - } else if (frameId < m_nMarkerFrames + totalDataFrames) { - int dataOffset = frameId - m_nMarkerFrames; - int cycleModMain = dataOffset%m_txProtocol.framesPerTx; - dataOffset /= m_txProtocol.framesPerTx; - dataOffset *= m_txProtocol.bytesPerTx; - - std::fill(dataBits.begin(), dataBits.end(), 0); - - for (int j = 0; j < m_txProtocol.bytesPerTx; ++j) { - { - uint8_t d = m_dataEncoded[dataOffset + j] & 15; - dataBits[(2*j + 0)*16 + d] = 1; - } - { - uint8_t d = m_dataEncoded[dataOffset + j] & 240; - dataBits[(2*j + 1)*16 + (d >> 4)] = 1; - } - } - - for (int k = 0; k < 2*m_txProtocol.bytesPerTx*16; ++k) { - if (dataBits[k] == 0) continue; - - ++nFreq; - m_waveformTones.back().push_back({}); - m_waveformTones.back().back().duration_ms = (1000.0*m_samplesPerFrame)/m_sampleRate; - if (k%2) { - ::addAmplitudeSmooth(bit0Amplitude[k/2], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, cycleModMain, m_txProtocol.framesPerTx); - m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, k/2) + m_hzPerSample; - } else { - ::addAmplitudeSmooth(bit1Amplitude[k/2], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, cycleModMain, m_txProtocol.framesPerTx); - m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, k/2); - } - } - } else if (frameId < m_nMarkerFrames + totalDataFrames + m_nMarkerFrames) { - nFreq = m_nBitsInMarker; - - int fId = frameId - (m_nMarkerFrames + totalDataFrames); - for (int i = 0; i < m_nBitsInMarker; ++i) { - m_waveformTones.back().push_back({}); - m_waveformTones.back().back().duration_ms = (1000.0*m_samplesPerFrame)/m_sampleRate; - if (i%2 == 0) { - addAmplitudeSmooth(bit0Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, fId, m_nMarkerFrames); - m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i) + m_hzPerSample; - } else { - addAmplitudeSmooth(bit1Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, fId, m_nMarkerFrames); - m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i); - } - } - } else { - m_hasNewTxData = false; - break; - } - - if (nFreq == 0) nFreq = 1; - float scale = 1.0f/nFreq; - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_outputBlock[i] *= scale; - } - - int samplesPerFrameOut = m_samplesPerFrame; - if (m_sampleRateOut != m_sampleRate) { - samplesPerFrameOut = m_impl->resampler.resample(factor, m_samplesPerFrame, m_outputBlock.data(), m_outputBlockResampled.data()); - } else { - m_outputBlockResampled = m_outputBlock; - } - - // default output is in 16-bit signed int so we always compute it - for (int i = 0; i < samplesPerFrameOut; ++i) { - m_outputBlockI16[offset + i] = 32768*m_outputBlockResampled[i]; - } - - // convert from 32-bit float - switch (m_sampleFormatOut) { - case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break; - case GGWAVE_SAMPLE_FORMAT_U8: - { - auto p = reinterpret_cast(m_outputBlockTmp.data()); - for (int i = 0; i < samplesPerFrameOut; ++i) { - p[offset + i] = 128*(m_outputBlockResampled[i] + 1.0f); - } - } break; - case GGWAVE_SAMPLE_FORMAT_I8: - { - auto p = reinterpret_cast(m_outputBlockTmp.data()); - for (int i = 0; i < samplesPerFrameOut; ++i) { - p[offset + i] = 128*m_outputBlockResampled[i]; - } - } break; - case GGWAVE_SAMPLE_FORMAT_U16: - { - auto p = reinterpret_cast(m_outputBlockTmp.data()); - for (int i = 0; i < samplesPerFrameOut; ++i) { - p[offset + i] = 32768*(m_outputBlockResampled[i] + 1.0f); - } - } break; - case GGWAVE_SAMPLE_FORMAT_I16: - { - // skip because we already have the data in m_outputBlockI16 - //auto p = reinterpret_cast(m_outputBlockTmp.data()); - //for (int i = 0; i < samplesPerFrameOut; ++i) { - // p[offset + i] = 32768*m_outputBlockResampled[i]; - //} - } break; - case GGWAVE_SAMPLE_FORMAT_F32: - { - auto p = reinterpret_cast(m_outputBlockTmp.data()); - for (int i = 0; i < samplesPerFrameOut; ++i) { - p[offset + i] = m_outputBlockResampled[i]; - } - } break; - } - - ++frameId; - offset += samplesPerFrameOut; - } - - switch (m_sampleFormatOut) { - case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break; - case GGWAVE_SAMPLE_FORMAT_I16: - { - cbWaveformOut(m_outputBlockI16.data(), offset*m_sampleSizeBytesOut); - } break; - case GGWAVE_SAMPLE_FORMAT_U8: - case GGWAVE_SAMPLE_FORMAT_I8: - case GGWAVE_SAMPLE_FORMAT_U16: - case GGWAVE_SAMPLE_FORMAT_F32: - { - cbWaveformOut(m_outputBlockTmp.data(), offset*m_sampleSizeBytesOut); - } break; - } - - m_txAmplitudeDataI16.resize(offset); - for (uint32_t i = 0; i < offset; ++i) { - m_txAmplitudeDataI16[i] = m_outputBlockI16[i]; - } - - return true; -} - -void GGWave::decode(const CBWaveformInp & cbWaveformInp) { - while (m_hasNewTxData == false) { - // read capture data - float factor = m_sampleRateInp/m_sampleRate; - uint32_t nBytesNeeded = m_samplesNeeded*m_sampleSizeBytesInp; - - if (m_sampleRateInp != m_sampleRate) { - // note : predict 4 extra samples just to make sure we have enough data - nBytesNeeded = (m_impl->resampler.resample(1.0f/factor, m_samplesNeeded, m_sampleAmplitudeResampled.data(), nullptr) + 4)*m_sampleSizeBytesInp; - } - - uint32_t nBytesRecorded = 0; - - switch (m_sampleFormatInp) { - case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break; - case GGWAVE_SAMPLE_FORMAT_U8: - case GGWAVE_SAMPLE_FORMAT_I8: - case GGWAVE_SAMPLE_FORMAT_U16: - case GGWAVE_SAMPLE_FORMAT_I16: - { - nBytesRecorded = cbWaveformInp(m_sampleAmplitudeTmp.data(), nBytesNeeded); - } break; - case GGWAVE_SAMPLE_FORMAT_F32: - { - nBytesRecorded = cbWaveformInp(m_sampleAmplitudeResampled.data(), nBytesNeeded); - } break; - } - - if (nBytesRecorded % m_sampleSizeBytesInp != 0) { - ggprintf("Failure during capture - provided bytes (%d) are not multiple of sample size (%d)\n", - nBytesRecorded, m_sampleSizeBytesInp); - m_samplesNeeded = m_samplesPerFrame; - break; - } - - if (nBytesRecorded > nBytesNeeded) { - ggprintf("Failure during capture - more samples were provided (%d) than requested (%d)\n", - nBytesRecorded/m_sampleSizeBytesInp, nBytesNeeded/m_sampleSizeBytesInp); - m_samplesNeeded = m_samplesPerFrame; - break; - } - - // convert to 32-bit float - int nSamplesRecorded = nBytesRecorded/m_sampleSizeBytesInp; - switch (m_sampleFormatInp) { - case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break; - case GGWAVE_SAMPLE_FORMAT_U8: - { - constexpr float scale = 1.0f/128; - auto p = reinterpret_cast(m_sampleAmplitudeTmp.data()); - for (int i = 0; i < nSamplesRecorded; ++i) { - m_sampleAmplitudeResampled[i] = float(int16_t(*(p + i)) - 128)*scale; - } - } break; - case GGWAVE_SAMPLE_FORMAT_I8: - { - constexpr float scale = 1.0f/128; - auto p = reinterpret_cast(m_sampleAmplitudeTmp.data()); - for (int i = 0; i < nSamplesRecorded; ++i) { - m_sampleAmplitudeResampled[i] = float(*(p + i))*scale; - } - } break; - case GGWAVE_SAMPLE_FORMAT_U16: - { - constexpr float scale = 1.0f/32768; - auto p = reinterpret_cast(m_sampleAmplitudeTmp.data()); - for (int i = 0; i < nSamplesRecorded; ++i) { - m_sampleAmplitudeResampled[i] = float(int32_t(*(p + i)) - 32768)*scale; - } - } break; - case GGWAVE_SAMPLE_FORMAT_I16: - { - constexpr float scale = 1.0f/32768; - auto p = reinterpret_cast(m_sampleAmplitudeTmp.data()); - for (int i = 0; i < nSamplesRecorded; ++i) { - m_sampleAmplitudeResampled[i] = float(*(p + i))*scale; - } - } break; - case GGWAVE_SAMPLE_FORMAT_F32: break; - } - - if (nSamplesRecorded == 0) { - break; - } - - uint32_t offset = m_samplesPerFrame - m_samplesNeeded; - - if (m_sampleRateInp != m_sampleRate) { - if (nSamplesRecorded <= 2*Resampler::kWidth) { - m_samplesNeeded = m_samplesPerFrame; - break; - } - - // reset resampler state every minute - if (!m_receivingData && m_impl->resampler.nSamplesTotal() > 60.0f*factor*m_sampleRate) { - m_impl->resampler.reset(); - } - - int nSamplesResampled = offset + m_impl->resampler.resample(factor, nSamplesRecorded, m_sampleAmplitudeResampled.data(), m_sampleAmplitude.data() + offset); - nSamplesRecorded = nSamplesResampled; - } else { - for (int i = 0; i < nSamplesRecorded; ++i) { - m_sampleAmplitude[offset + i] = m_sampleAmplitudeResampled[i]; - } - } - - // we have enough bytes to do analysis - if (nSamplesRecorded >= m_samplesPerFrame) { - m_hasNewAmplitude = true; - - if (m_isFixedPayloadLength) { - decode_fixed(); - } else { - decode_variable(); - } - - int nExtraSamples = nSamplesRecorded - m_samplesPerFrame; - for (int i = 0; i < nExtraSamples; ++i) { - m_sampleAmplitude[i] = m_sampleAmplitude[m_samplesPerFrame + i]; - } - - m_samplesNeeded = m_samplesPerFrame - nExtraSamples; - } else { - m_samplesNeeded = m_samplesPerFrame - nSamplesRecorded; - break; - } - } -} - -bool GGWave::takeTxAmplitudeI16(AmplitudeDataI16 & dst) { - if (m_txAmplitudeDataI16.size() == 0) return false; - - dst = std::move(m_txAmplitudeDataI16); - - return true; -} - -bool GGWave::stopReceiving() { - if (m_receivingData == false) { - return false; - } - - m_receivingData = false; - - return true; -} - -int GGWave::takeRxData(TxRxData & dst) { - if (m_lastRxDataLength == 0) return 0; - - auto res = m_lastRxDataLength; - m_lastRxDataLength = 0; - - if (res != -1) { - dst = m_rxData; - } - - return res; -} - -bool GGWave::takeRxSpectrum(SpectrumData & dst) { - if (m_hasNewSpectrum == false) return false; - - m_hasNewSpectrum = false; - dst = m_sampleSpectrum; - - return true; -} - -bool GGWave::takeRxAmplitude(AmplitudeData & dst) { - if (m_hasNewAmplitude == false) return false; - - m_hasNewAmplitude = false; - dst = m_sampleAmplitude; - - return true; -} - -bool GGWave::computeFFTR(const float * src, float * dst, int N, float d) { - if (N > kMaxSamplesPerFrame) { - ggprintf("computeFFTR: N (%d) must be <= %d\n", N, GGWave::kMaxSamplesPerFrame); - return false; - } - - FFT(src, dst, N, d); - - return true; -} - -// -// Variable payload length -// - -void GGWave::decode_variable() { - m_sampleAmplitudeHistory[m_historyId] = m_sampleAmplitude; - - if (++m_historyId >= kMaxSpectrumHistory) { - m_historyId = 0; - } - - if (m_historyId == 0 || m_receivingData) { - m_hasNewSpectrum = true; - - std::fill(m_sampleAmplitudeAverage.begin(), m_sampleAmplitudeAverage.end(), 0.0f); - for (auto & s : m_sampleAmplitudeHistory) { - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_sampleAmplitudeAverage[i] += s[i]; - } - } - - float norm = 1.0f/kMaxSpectrumHistory; - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_sampleAmplitudeAverage[i] *= norm; - } - - // calculate spectrum - FFT(m_sampleAmplitudeAverage.data(), m_fftOut.data(), m_samplesPerFrame, 1.0); - - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_sampleSpectrum[i] = (m_fftOut[2*i + 0]*m_fftOut[2*i + 0] + m_fftOut[2*i + 1]*m_fftOut[2*i + 1]); - } - for (int i = 1; i < m_samplesPerFrame/2; ++i) { - m_sampleSpectrum[i] += m_sampleSpectrum[m_samplesPerFrame - i]; - } - } - - if (m_framesLeftToRecord > 0) { - std::copy(m_sampleAmplitude.begin(), - m_sampleAmplitude.begin() + m_samplesPerFrame, - m_recordedAmplitude.data() + (m_framesToRecord - m_framesLeftToRecord)*m_samplesPerFrame); - - if (--m_framesLeftToRecord <= 0) { - m_analyzingData = true; - } - } - - if (m_analyzingData) { - ggprintf("Analyzing captured data ..\n"); - auto tStart = std::chrono::high_resolution_clock::now(); - - const int stepsPerFrame = 16; - const int step = m_samplesPerFrame/stepsPerFrame; - - bool isValid = false; - for (const auto & rxProtocolPair : m_rxProtocols) { - const auto & rxProtocolId = rxProtocolPair.first; - const auto & rxProtocol = rxProtocolPair.second; - - // skip Rx protocol if start frequency is different from detected one - if (rxProtocol.freqStart != m_markerFreqStart) { - continue; - } - - std::fill(m_sampleSpectrum.begin(), m_sampleSpectrum.end(), 0.0f); - - m_framesToAnalyze = m_nMarkerFrames*stepsPerFrame; - m_framesLeftToAnalyze = m_framesToAnalyze; - - // note : not sure if looping backwards here is more meaningful than looping forwards - for (int ii = m_nMarkerFrames*stepsPerFrame - 1; ii >= 0; --ii) { - bool knownLength = false; - - int decodedLength = 0; - const int offsetStart = ii; - for (int itx = 0; itx < 1024; ++itx) { - int offsetTx = offsetStart + itx*rxProtocol.framesPerTx*stepsPerFrame; - if (offsetTx >= m_recvDuration_frames*stepsPerFrame || (itx + 1)*rxProtocol.bytesPerTx >= (int) m_dataEncoded.size()) { - break; - } - - std::copy( - m_recordedAmplitude.begin() + offsetTx*step, - m_recordedAmplitude.begin() + offsetTx*step + m_samplesPerFrame, m_fftInp.data()); - - // note : should we skip the first and last frame here as they are amplitude-smoothed? - for (int k = 1; k < rxProtocol.framesPerTx; ++k) { - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_fftInp[i] += m_recordedAmplitude[(offsetTx + k*stepsPerFrame)*step + i]; - } - } - - FFT(m_fftInp.data(), m_fftOut.data(), m_samplesPerFrame, 1.0); - - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_sampleSpectrum[i] = (m_fftOut[2*i + 0]*m_fftOut[2*i + 0] + m_fftOut[2*i + 1]*m_fftOut[2*i + 1]); - } - for (int i = 1; i < m_samplesPerFrame/2; ++i) { - m_sampleSpectrum[i] += m_sampleSpectrum[m_samplesPerFrame - i]; - } - - uint8_t curByte = 0; - for (int i = 0; i < 2*rxProtocol.bytesPerTx; ++i) { - double freq = m_hzPerSample*rxProtocol.freqStart; - int bin = std::round(freq*m_ihzPerSample) + 16*i; - - int kmax = 0; - double amax = 0.0; - for (int k = 0; k < 16; ++k) { - if (m_sampleSpectrum[bin + k] > amax) { - kmax = k; - amax = m_sampleSpectrum[bin + k]; - } - } - - if (i%2) { - curByte += (kmax << 4); - m_dataEncoded[itx*rxProtocol.bytesPerTx + i/2] = curByte; - curByte = 0; - } else { - curByte = kmax; - } - } - - if (itx*rxProtocol.bytesPerTx > m_encodedDataOffset && knownLength == false) { - RS::ReedSolomon rsLength(1, m_encodedDataOffset - 1); - if ((rsLength.Decode(m_dataEncoded.data(), m_rxData.data()) == 0) && (m_rxData[0] > 0 && m_rxData[0] <= 140)) { - knownLength = true; - decodedLength = m_rxData[0]; - - const int nTotalBytesExpected = m_encodedDataOffset + decodedLength + ::getECCBytesForLength(decodedLength); - const int nTotalFramesExpected = 2*m_nMarkerFrames + ((nTotalBytesExpected + rxProtocol.bytesPerTx - 1)/rxProtocol.bytesPerTx)*rxProtocol.framesPerTx; - if (m_recvDuration_frames > nTotalFramesExpected || - m_recvDuration_frames < nTotalFramesExpected - 2*m_nMarkerFrames) { - knownLength = false; - break; - } - } else { - break; - } - } - - { - const int nTotalBytesExpected = m_encodedDataOffset + decodedLength + ::getECCBytesForLength(decodedLength); - if (knownLength && itx*rxProtocol.bytesPerTx > nTotalBytesExpected + 1) { - break; - } - } - } - - if (knownLength) { - RS::ReedSolomon rsData(decodedLength, ::getECCBytesForLength(decodedLength)); - - if (rsData.Decode(m_dataEncoded.data() + m_encodedDataOffset, m_rxData.data()) == 0) { - if (m_rxData[0] != 0) { - std::string s((char *) m_rxData.data(), decodedLength); - - ggprintf("Decoded length = %d, protocol = '%s' (%d)\n", decodedLength, rxProtocol.name, rxProtocolId); - ggprintf("Received sound data successfully: '%s'\n", s.c_str()); - - isValid = true; - m_hasNewRxData = true; - m_lastRxDataLength = decodedLength; - m_rxProtocol = rxProtocol; - m_rxProtocolId = TxProtocolId(rxProtocolId); - } - } - } - - if (isValid) { - break; - } - --m_framesLeftToAnalyze; - } - - if (isValid) break; - } - - m_framesToRecord = 0; - - if (isValid == false) { - ggprintf("Failed to capture sound data. Please try again (length = %d)\n", m_rxData[0]); - m_lastRxDataLength = -1; - m_framesToRecord = -1; - } - - m_receivingData = false; - m_analyzingData = false; - - std::fill(m_sampleSpectrum.begin(), m_sampleSpectrum.end(), 0.0f); - - m_framesToAnalyze = 0; - m_framesLeftToAnalyze = 0; - - auto tEnd = std::chrono::high_resolution_clock::now(); - ggprintf("Time to analyze: %g ms\n", getTime_ms(tStart, tEnd)); - } - - // check if receiving data - if (m_receivingData == false) { - bool isReceiving = false; - - for (const auto & rxProtocol : getTxProtocols()) { - int nDetectedMarkerBits = m_nBitsInMarker; - - for (int i = 0; i < m_nBitsInMarker; ++i) { - double freq = bitFreq(rxProtocol.second, i); - int bin = std::round(freq*m_ihzPerSample); - - if (i%2 == 0) { - if (m_sampleSpectrum[bin] <= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) --nDetectedMarkerBits; - } else { - if (m_sampleSpectrum[bin] >= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) --nDetectedMarkerBits; - } - } - - if (nDetectedMarkerBits == m_nBitsInMarker) { - m_markerFreqStart = rxProtocol.second.freqStart; - isReceiving = true; - break; - } - } - - if (isReceiving) { - if (++m_nMarkersSuccess >= 1) { - } else { - isReceiving = false; - } - } else { - m_nMarkersSuccess = 0; - } - - if (isReceiving) { - std::time_t timestamp = std::time(nullptr); - ggprintf("%sReceiving sound data ...\n", std::asctime(std::localtime(×tamp))); - - m_receivingData = true; - std::fill(m_rxData.begin(), m_rxData.end(), 0); - - // max recieve duration - m_recvDuration_frames = - 2*m_nMarkerFrames + - maxFramesPerTx()*((kMaxLengthVarible + ::getECCBytesForLength(kMaxLengthVarible))/minBytesPerTx() + 1); - - m_nMarkersSuccess = 0; - m_framesToRecord = m_recvDuration_frames; - m_framesLeftToRecord = m_recvDuration_frames; - } - } else { - bool isEnded = false; - - for (const auto & rxProtocol : getTxProtocols()) { - int nDetectedMarkerBits = m_nBitsInMarker; - - for (int i = 0; i < m_nBitsInMarker; ++i) { - double freq = bitFreq(rxProtocol.second, i); - int bin = std::round(freq*m_ihzPerSample); - - if (i%2 == 0) { - if (m_sampleSpectrum[bin] >= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) nDetectedMarkerBits--; - } else { - if (m_sampleSpectrum[bin] <= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) nDetectedMarkerBits--; - } - } - - if (nDetectedMarkerBits == m_nBitsInMarker) { - isEnded = true; - break; - } - } - - if (isEnded) { - if (++m_nMarkersSuccess >= 1) { - } else { - isEnded = false; - } - } else { - m_nMarkersSuccess = 0; - } - - if (isEnded && m_framesToRecord > 1) { - std::time_t timestamp = std::time(nullptr); - m_recvDuration_frames -= m_framesLeftToRecord - 1; - ggprintf("%sReceived end marker. Frames left = %d, recorded = %d\n", std::asctime(std::localtime(×tamp)), m_framesLeftToRecord, m_recvDuration_frames); - m_nMarkersSuccess = 0; - m_framesLeftToRecord = 1; - } - } -} - -// -// Fixed payload length - -void GGWave::decode_fixed() { - m_hasNewSpectrum = true; - - // calculate spectrum - FFT(m_sampleAmplitude.data(), m_fftOut.data(), m_samplesPerFrame, 1.0); - - for (int i = 0; i < m_samplesPerFrame; ++i) { - m_sampleSpectrum[i] = (m_fftOut[2*i + 0]*m_fftOut[2*i + 0] + m_fftOut[2*i + 1]*m_fftOut[2*i + 1]); - } - for (int i = 1; i < m_samplesPerFrame/2; ++i) { - m_sampleSpectrum[i] += m_sampleSpectrum[m_samplesPerFrame - i]; - } - - m_spectrumHistoryFixed[m_historyIdFixed] = m_sampleSpectrum; - - if (++m_historyIdFixed >= (int) m_spectrumHistoryFixed.size()) { - m_historyIdFixed = 0; - } - - bool isValid = false; - for (const auto & rxProtocolPair : m_rxProtocols) { - const auto & rxProtocolId = rxProtocolPair.first; - const auto & rxProtocol = rxProtocolPair.second; - - const int binStart = rxProtocol.freqStart; - const int binDelta = 16; - - if (binStart > m_samplesPerFrame) { - continue; - } - - const int totalLength = m_payloadLength + getECCBytesForLength(m_payloadLength); - const int totalTxs = (totalLength + rxProtocol.bytesPerTx - 1)/rxProtocol.bytesPerTx; - - int historyStartId = m_historyIdFixed - totalTxs*rxProtocol.framesPerTx; - if (historyStartId < 0) { - historyStartId += m_spectrumHistoryFixed.size(); - } - - const int nTones = 2*rxProtocol.bytesPerTx; - std::vector detectedBins(2*totalLength); - - struct ToneData { - int nMax[16]; - }; - - std::vector tones(nTones); - - bool detectedSignal = true; - int txDetectedTotal = 0; - int txNeededTotal = 0; - for (int k = 0; k < totalTxs; ++k) { - for (auto & tone : tones) { - std::fill(tone.nMax, tone.nMax + 16, 0); - } - - for (int i = 0; i < rxProtocol.framesPerTx; ++i) { - int historyId = historyStartId + k*rxProtocol.framesPerTx + i; - if (historyId >= (int) m_spectrumHistoryFixed.size()) { - historyId -= m_spectrumHistoryFixed.size(); - } - - for (int j = 0; j < rxProtocol.bytesPerTx; ++j) { - int f0bin = -1; - int f1bin = -1; - - double f0max = 0.0; - double f1max = 0.0; - - for (int b = 0; b < 16; ++b) { - { - const auto & v = m_spectrumHistoryFixed[historyId][binStart + 2*j*binDelta + b]; - - if (f0max <= v) { - f0max = v; - f0bin = b; - } - } - - { - const auto & v = m_spectrumHistoryFixed[historyId][binStart + 2*j*binDelta + binDelta + b]; - - if (f1max <= v) { - f1max = v; - f1bin = b; - } - } - } - - tones[2*j + 0].nMax[f0bin]++; - tones[2*j + 1].nMax[f1bin]++; - } - } - - int txDetected = 0; - int txNeeded = 0; - for (int j = 0; j < rxProtocol.bytesPerTx; ++j) { - if (k*rxProtocol.bytesPerTx + j >= totalLength) break; - txNeeded += 2; - for (int b = 0; b < 16; ++b) { - if (tones[2*j + 0].nMax[b] > rxProtocol.framesPerTx/2) { - detectedBins[2*(k*rxProtocol.bytesPerTx + j) + 0] = b; - txDetected++; - } - if (tones[2*j + 1].nMax[b] > rxProtocol.framesPerTx/2) { - detectedBins[2*(k*rxProtocol.bytesPerTx + j) + 1] = b; - txDetected++; - } - } - } - - txDetectedTotal += txDetected; - txNeededTotal += txNeeded; - } - - if (txDetectedTotal < 0.75*txNeededTotal) { - detectedSignal = false; - } - - if (detectedSignal) { - RS::ReedSolomon rsData(m_payloadLength, getECCBytesForLength(m_payloadLength)); - - for (int j = 0; j < totalLength; ++j) { - m_dataEncoded[j] = (detectedBins[2*j + 1] << 4) + detectedBins[2*j + 0]; - } - - if (rsData.Decode(m_dataEncoded.data(), m_rxData.data()) == 0) { - if (m_rxData[0] != 0) { - ggprintf("Received sound data successfully: '%s'\n", m_rxData.data()); - - isValid = true; - m_hasNewRxData = true; - m_lastRxDataLength = m_payloadLength; - m_rxProtocol = rxProtocol; - m_rxProtocolId = TxProtocolId(rxProtocolId); - } - } - } - - if (isValid) { - break; - } - } -} - -int GGWave::maxFramesPerTx() const { - int res = 0; - for (const auto & protocol : getTxProtocols()) { - res = std::max(res, protocol.second.framesPerTx); - } - return res; -} - -int GGWave::minBytesPerTx() const { - int res = getTxProtocols().begin()->second.bytesPerTx; - for (const auto & protocol : getTxProtocols()) { - res = std::min(res, protocol.second.bytesPerTx); - } - return res; -} - diff --git a/examples/arduino-rx/ggwave/ggwave.h b/examples/arduino-rx/ggwave/ggwave.h deleted file mode 100644 index 80e7d25..0000000 --- a/examples/arduino-rx/ggwave/ggwave.h +++ /dev/null @@ -1,592 +0,0 @@ -#ifndef GGWAVE_H -#define GGWAVE_H - -#ifdef GGWAVE_SHARED -# ifdef _WIN32 -# ifdef GGWAVE_BUILD -# define GGWAVE_API __declspec(dllexport) -# else -# define GGWAVE_API __declspec(dllimport) -# endif -# else -# define GGWAVE_API __attribute__ ((visibility ("default"))) -# endif -#else -# define GGWAVE_API -#endif - -#ifdef __cplusplus -extern "C" { -#endif - - // - // C interface - // - - // Data format of the audio samples - typedef enum { - GGWAVE_SAMPLE_FORMAT_UNDEFINED, - GGWAVE_SAMPLE_FORMAT_U8, - GGWAVE_SAMPLE_FORMAT_I8, - GGWAVE_SAMPLE_FORMAT_U16, - GGWAVE_SAMPLE_FORMAT_I16, - GGWAVE_SAMPLE_FORMAT_F32, - } ggwave_SampleFormat; - - // TxProtocol ids - typedef enum { - GGWAVE_TX_PROTOCOL_AUDIBLE_NORMAL, - GGWAVE_TX_PROTOCOL_AUDIBLE_FAST, - GGWAVE_TX_PROTOCOL_AUDIBLE_FASTEST, - GGWAVE_TX_PROTOCOL_ULTRASOUND_NORMAL, - GGWAVE_TX_PROTOCOL_ULTRASOUND_FAST, - GGWAVE_TX_PROTOCOL_ULTRASOUND_FASTEST, - GGWAVE_TX_PROTOCOL_DT_NORMAL, - GGWAVE_TX_PROTOCOL_DT_FAST, - GGWAVE_TX_PROTOCOL_DT_FASTEST, - - GGWAVE_TX_PROTOCOL_CUSTOM_0, - GGWAVE_TX_PROTOCOL_CUSTOM_1, - GGWAVE_TX_PROTOCOL_CUSTOM_2, - GGWAVE_TX_PROTOCOL_CUSTOM_3, - GGWAVE_TX_PROTOCOL_CUSTOM_4, - GGWAVE_TX_PROTOCOL_CUSTOM_5, - GGWAVE_TX_PROTOCOL_CUSTOM_6, - GGWAVE_TX_PROTOCOL_CUSTOM_7, - GGWAVE_TX_PROTOCOL_CUSTOM_8, - GGWAVE_TX_PROTOCOL_CUSTOM_9, - } ggwave_TxProtocolId; - - typedef enum { - GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX, - GGWAVE_OPERATING_MODE_ONLY_RX, - //GGWAVE_OPERATING_MODE_ONLY_TX, // Not supported yet - } ggwave_OperatingMode; - - // GGWave instance parameters - // - // If payloadLength <= 0, then GGWave will transmit with variable payload length - // depending on the provided payload. Sound markers are used to identify the - // start and end of the transmission. - // - // If payloadLength > 0, then the transmitted payload will be of the specified - // fixed length. In this case, no sound markers are emitted and a slightly - // different decoding scheme is applied. This is useful in cases where the - // length of the payload is known in advance. - // - // The sample rates are values typically between 1000 and 96000. - // Default value: GGWave::kDefaultSampleRate - // - // The captured audio is resampled to the specified sampleRate if sampleRatInp - // is different from sampleRate. Same applies to the transmitted audio. - // - // The samplesPerFrame is the number of samples on which ggwave performs FFT. - // This affects the number of bins in the Fourier spectrum. - // Default value: GGWave::kDefaultSamplesPerFrame - // - // The operatingMode controls which functions of the ggwave instance are enabled. - // Use this parameter to reduce the memory footprint of the ggwave instance. For - // example, if only Rx is enabled, then the memory buffers needed for the Tx will - // not be allocated. - // - typedef struct { - int payloadLength; // payload length - float sampleRateInp; // capture sample rate - float sampleRateOut; // playback sample rate - float sampleRate; // the operating sample rate - int samplesPerFrame; // number of samples per audio frame - float soundMarkerThreshold; // sound marker detection threshold - ggwave_SampleFormat sampleFormatInp; // format of the captured audio samples - ggwave_SampleFormat sampleFormatOut; // format of the playback audio samples - ggwave_OperatingMode operatingMode; // operating mode - } ggwave_Parameters; - - // GGWave instances are identified with an integer and are stored - // in a private map container. Using void * caused some issues with - // the python module and unfortunately had to do it this way - typedef int ggwave_Instance; - - // Change file stream for internal ggwave logging. NULL - disable logging - // - // Intentionally passing it as void * instead of FILE * to avoid including a header - // - // // log to standard error - // ggwave_setLogFile(stderr); - // - // // log to standard output - // ggwave_setLogFile(stdout); - // - // // disable logging - // ggwave_setLogFile(NULL); - // - // Note: not thread-safe. Do not call while any GGWave instances are running - // - GGWAVE_API void ggwave_setLogFile(void * fptr); - - // Helper method to get default instance parameters - GGWAVE_API ggwave_Parameters ggwave_getDefaultParameters(void); - - // Create a new GGWave instance with the specified parameters - // - // The newly created instance is added to the internal map container. - // This function returns an id that can be used to identify this instance. - // Make sure to deallocate the instance at the end by calling ggwave_free() - // - GGWAVE_API ggwave_Instance ggwave_init(const ggwave_Parameters parameters); - - // Free a GGWave instance - GGWAVE_API void ggwave_free(ggwave_Instance instance); - - // Encode data into audio waveform - // - // instance - the GGWave instance to use - // dataBuffer - the data to encode - // dataSize - number of bytes in the input dataBuffer - // txProtocolId - the protocol to use for encoding - // volume - the volume of the generated waveform [0, 100] - // usually 25 is OK and you should not go over 50 - // outputBuffer - the generated audio waveform. must be big enough to fit the generated data - // query - if != 0, do not perform encoding. - // if == 1, return waveform size in bytes - // if != 1, return waveform size in samples - // - // returns the number of generated bytes or samples (see query) - // - // returns -1 if there was an error - // - // This function can be used to encode some binary data (payload) into an audio waveform. - // - // payload -> waveform - // - // When calling it, make sure that the outputBuffer is big enough to store the - // generated waveform. This means that its size must be at least: - // - // nSamples*sizeOfSample_bytes - // - // Where nSamples is the number of audio samples in the waveform and sizeOfSample_bytes - // is the size of a single sample in bytes based on the sampleFormatOut parameter - // specified during the initialization of the GGWave instance. - // - // If query != 0, then this function does not perform the actual encoding and just - // outputs the expected size of the waveform that would be generated if you call it - // with query == 0. This mechanism can be used to ask ggwave how much memory to - // allocate for the outputBuffer. For example: - // - // // this is the data to encode - // const char * payload = "test"; - // - // // query the number of bytes in the waveform - // int n = ggwave_encode(instance, payload, 4, GGWAVE_TX_PROTOCOL_AUDIBLE_FAST, 25, NULL, 1); - // - // // allocate the output buffer - // char waveform[n]; - // - // // generate the waveform - // ggwave_encode(instance, payload, 4, GGWAVE_TX_PROTOCOL_AUDIBLE_FAST, 25, waveform, 0); - // - // The dataBuffer can be any binary data that you would like to transmit (i.e. the payload). - // Usually, this is some text, but it can be any sequence of bytes. - // - // todo: - // - change the type of dataBuffer to const void * - // - change the type of outputBuffer to void * - // - rename dataBuffer to payloadBuffer - // - rename dataSize to payloadSize - // - rename outputBuffer to waveformBuffer - // - GGWAVE_API int ggwave_encode( - ggwave_Instance instance, - const char * dataBuffer, - int dataSize, - ggwave_TxProtocolId txProtocolId, - int volume, - char * outputBuffer, - int query); - - // Decode an audio waveform into data - // - // instance - the GGWave instance to use - // dataBuffer - the audio waveform - // dataSize - number of bytes in the input dataBuffer - // outputBuffer - stores the decoded data on success - // the maximum size of the output is GGWave::kMaxDataSize - // - // returns the number of decoded bytes - // - // Use this function to continuously provide audio samples to a GGWave instance. - // On each call, GGWave will analyze the provided data and if it detects a payload, - // it will return a non-zero result. - // - // waveform -> payload - // - // If the return value is -1 then there was an error during the decoding process. - // Usually can occur if there is a lot of background noise in the audio. - // - // If the return value is greater than 0, then there are that number of bytes decoded. - // - // IMPORTANT: - // Notice that the decoded data written to the outputBuffer is NOT null terminated. - // - // Example: - // - // char payload[256]; - // - // while (true) { - // ... capture samplesPerFrame audio samples into waveform ... - // - // int ret = ggwave_decode(instance, waveform, samplesPerFrame*sizeOfSample_bytes, payload); - // if (ret > 0) { - // printf("Received payload: '%s'\n", payload); - // } - // } - // - // todo: - // - change the type of dataBuffer to const void * - // - change the type of outputBuffer to void * - // - rename dataBuffer to waveformBuffer - // - rename dataSize to waveformSize - // - rename outputBuffer to payloadBuffer - // - GGWAVE_API int ggwave_decode( - ggwave_Instance instance, - const char * dataBuffer, - int dataSize, - char * outputBuffer); - - // Memory-safe overload of ggwave_decode - // - // outputSize - optionally specify the size of the output buffer - // - // If the return value is -2 then the provided outputBuffer was not big enough to - // store the decoded data. - // - // See ggwave_decode for more information - // - GGWAVE_API int ggwave_ndecode( - ggwave_Instance instance, - const char * dataBuffer, - int dataSize, - char * outputBuffer, - int outputSize); - - // Toggle Rx protocols on and off - // - // instance - the GGWave instance to use - // rxProtocolId - Id of the Rx protocol to modify - // state - 0 - disable, 1 - enable - // - // If an Rx protocol is enabled, the GGWave instance will attempt to decode received - // data using this protocol. By default, all protocols are enabled. - // Use this function to restrict the number of Rx protocols used in the decoding - // process. This helps to reduce the number of false positives and improves the transmission - // accuracy, especially when the Tx/Rx protocol is known in advance. - // - GGWAVE_API void ggwave_toggleRxProtocol( - ggwave_Instance instance, - ggwave_TxProtocolId rxProtocolId, - int state); - -#ifdef __cplusplus -} - -// -// C++ interface -// - -#include -#include -#include -#include -#include -#include - -class GGWave { -public: - static constexpr auto kSampleRateMin = 1000.0f; - static constexpr auto kSampleRateMax = 96000.0f; - static constexpr auto kDefaultSampleRate = 48000.0f; - static constexpr auto kDefaultSamplesPerFrame = 1024; - static constexpr auto kDefaultVolume = 10; - static constexpr auto kDefaultSoundMarkerThreshold = 3.0f; - static constexpr auto kDefaultMarkerFrames = 16; - static constexpr auto kDefaultEncodedDataOffset = 3; - static constexpr auto kMaxSamplesPerFrame = 1024; - static constexpr auto kMaxDataBits = 256; - static constexpr auto kMaxDataSize = 256; - static constexpr auto kMaxLengthVarible = 140; - static constexpr auto kMaxLengthFixed = 16; - static constexpr auto kMaxSpectrumHistory = 4; - static constexpr auto kMaxRecordedFrames = 2048; - - using Parameters = ggwave_Parameters; - using SampleFormat = ggwave_SampleFormat; - using TxProtocolId = ggwave_TxProtocolId; - using RxProtocolId = ggwave_TxProtocolId; - - struct TxProtocol { - const char * name; // string identifier of the protocol - - int freqStart; // FFT bin index of the lowest frequency - int framesPerTx; // number of frames to transmit a single chunk of data - int bytesPerTx; // number of bytes in a chunk of data - - int nDataBitsPerTx() const { return 8*bytesPerTx; } - }; - - using RxProtocol = TxProtocol; - - using TxProtocols = std::map; - using RxProtocols = std::map; - - static const TxProtocols & getTxProtocols() { - static const TxProtocols kTxProtocols { - { GGWAVE_TX_PROTOCOL_AUDIBLE_NORMAL, { "Normal", 40, 9, 3, } }, - { GGWAVE_TX_PROTOCOL_AUDIBLE_FAST, { "Fast", 40, 6, 3, } }, - { GGWAVE_TX_PROTOCOL_AUDIBLE_FASTEST, { "Fastest", 40, 3, 3, } }, - { GGWAVE_TX_PROTOCOL_ULTRASOUND_NORMAL, { "[U] Normal", 320, 9, 3, } }, - { GGWAVE_TX_PROTOCOL_ULTRASOUND_FAST, { "[U] Fast", 320, 6, 3, } }, - { GGWAVE_TX_PROTOCOL_ULTRASOUND_FASTEST, { "[U] Fastest", 320, 3, 3, } }, - { GGWAVE_TX_PROTOCOL_DT_NORMAL, { "[DT] Normal", 24, 9, 1, } }, - { GGWAVE_TX_PROTOCOL_DT_FAST, { "[DT] Fast", 24, 6, 1, } }, - { GGWAVE_TX_PROTOCOL_DT_FASTEST, { "[DT] Fastest", 24, 3, 1, } }, - }; - - return kTxProtocols; - } - - struct ToneData { - double freq_hz; - double duration_ms; - }; - - using Tones = std::vector; - using WaveformTones = std::vector; - - using AmplitudeData = std::vector; - using AmplitudeDataI16 = std::vector; - using SpectrumData = std::vector; - using RecordedData = std::vector; - using TxRxData = std::vector; - - using CBWaveformOut = std::function; - using CBWaveformInp = std::function; - - GGWave(const Parameters & parameters); - ~GGWave(); - - // set file stream for the internal ggwave logging - // - // By default, ggwave prints internal log messages to stderr. - // To disable logging all together, call this method with nullptr. - // - // Note: not thread-safe. Do not call while any GGWave instances are running - // - static void setLogFile(FILE * fptr); - - static const Parameters & getDefaultParameters(); - - // set Tx data to encode - // - // This prepares the GGWave instance for transmission. - // To perform the actual encoding, the encode() method must be called - // - // returns false upon invalid parameters or failure to initialize - // - bool init(const std::string & text, const int volume = kDefaultVolume); - bool init(const std::string & text, const TxProtocol & txProtocol, const int volume = kDefaultVolume); - bool init(int dataSize, const char * dataBuffer, const int volume = kDefaultVolume); - bool init(int dataSize, const char * dataBuffer, const TxProtocol & txProtocol, const int volume = kDefaultVolume); - - // expected waveform size of the encoded Tx data in bytes - // - // When the output sampling rate is not equal to operating sample rate the result of this method is overestimation of - // the actual number of bytes that would be produced - // - uint32_t encodeSize_bytes() const; - - // expected waveform size of the encoded Tx data in samples - // - // When the output sampling rate is not equal to operating sample rate the result of this method is overestimation of - // the actual number of samples that would be produced - // - uint32_t encodeSize_samples() const; - - // encode Tx data into an audio waveform - // - // The generated waveform is returned by calling the cbWaveformOut callback. - // - // returns false if the encoding fails - // - bool encode(const CBWaveformOut & cbWaveformOut); - - // decode an audio waveform - // - // This methods calls cbWaveformInp multiple times (at least once) until it returns 0. - // Use the Rx methods to check if any data was decoded successfully. - // - void decode(const CBWaveformInp & cbWaveformInp); - - // instance state - const bool & hasTxData() const { return m_hasNewTxData; } - const bool & isReceiving() const { return m_receivingData; } - const bool & isAnalyzing() const { return m_analyzingData; } - - const int & getFramesToRecord() const { return m_framesToRecord; } - const int & getFramesLeftToRecord() const { return m_framesLeftToRecord; } - const int & getFramesToAnalyze() const { return m_framesToAnalyze; } - const int & getFramesLeftToAnalyze() const { return m_framesLeftToAnalyze; } - const int & getSamplesPerFrame() const { return m_samplesPerFrame; } - const int & getSampleSizeBytesInp() const { return m_sampleSizeBytesInp; } - const int & getSampleSizeBytesOut() const { return m_sampleSizeBytesOut; } - - const float & getSampleRateInp() const { return m_sampleRateInp; } - const float & getSampleRateOut() const { return m_sampleRateOut; } - const SampleFormat & getSampleFormatInp() const { return m_sampleFormatInp; } - const SampleFormat & getSampleFormatOut() const { return m_sampleFormatOut; } - - // Tx - - static TxProtocolId getDefaultTxProtocolId() { return GGWAVE_TX_PROTOCOL_AUDIBLE_FAST; } - static const TxProtocol & getDefaultTxProtocol() { return getTxProtocols().at(getDefaultTxProtocolId()); } - static const TxProtocol & getTxProtocol(int id) { return getTxProtocols().at(TxProtocolId(id)); } - static const TxProtocol & getTxProtocol(TxProtocolId id) { return getTxProtocols().at(id); } - - // get a list of the tones generated for the last waveform - // - // Call this method after calling encode() to get a list of the tones participating in the generated waveform - // - const WaveformTones & getWaveformTones() { return m_waveformTones; } - - bool takeTxAmplitudeI16(AmplitudeDataI16 & dst); - - // Rx - - bool stopReceiving(); - void setRxProtocols(const RxProtocols & rxProtocols) { m_rxProtocols = rxProtocols; } - const RxProtocols & getRxProtocols() const { return m_rxProtocols; } - - int lastRxDataLength() const { return m_lastRxDataLength; } - - const TxRxData & getRxData() const { return m_rxData; } - const RxProtocol & getRxProtocol() const { return m_rxProtocol; } - const RxProtocolId & getRxProtocolId() const { return m_rxProtocolId; } - - int takeRxData(TxRxData & dst); - bool takeRxSpectrum(SpectrumData & dst); - bool takeRxAmplitude(AmplitudeData & dst); - - // compute FFT of real values - // - // src - input real-valued data, size is N - // dst - output complex-valued data, size is 2*N - // - // d is scaling factor - // N must be <= kMaxSamplesPerFrame - // - static bool computeFFTR(const float * src, float * dst, int N, float d); - -private: - void decode_fixed(); - void decode_variable(); - - int maxFramesPerTx() const; - int minBytesPerTx() const; - - double bitFreq(const TxProtocol & p, int bit) const { - return m_hzPerSample*p.freqStart + m_freqDelta_hz*bit; - } - - const float m_sampleRateInp; - const float m_sampleRateOut; - const float m_sampleRate; - const int m_samplesPerFrame; - const float m_isamplesPerFrame; - const int m_sampleSizeBytesInp; - const int m_sampleSizeBytesOut; - const SampleFormat m_sampleFormatInp; - const SampleFormat m_sampleFormatOut; - - const float m_hzPerSample; - const float m_ihzPerSample; - - const int m_freqDelta_bin; - const float m_freqDelta_hz; - - const int m_nBitsInMarker; - const int m_nMarkerFrames; - const int m_encodedDataOffset; - - const float m_soundMarkerThreshold; - - // common - - bool m_isFixedPayloadLength; - int m_payloadLength; - TxRxData m_dataEncoded; - - // Rx - bool m_isRxEnabled; - bool m_receivingData; - bool m_analyzingData; - - int m_nMarkersSuccess; - int m_markerFreqStart; - int m_recvDuration_frames; - - int m_framesLeftToAnalyze; - int m_framesLeftToRecord; - int m_framesToAnalyze; - int m_framesToRecord; - int m_samplesNeeded; - - std::vector m_fftInp; // real - std::vector m_fftOut; // complex - - bool m_hasNewSpectrum; - bool m_hasNewAmplitude; - SpectrumData m_sampleSpectrum; - AmplitudeData m_sampleAmplitude; - AmplitudeData m_sampleAmplitudeResampled; - TxRxData m_sampleAmplitudeTmp; - - bool m_hasNewRxData; - int m_lastRxDataLength; - TxRxData m_rxData; - TxProtocol m_rxProtocol; - TxProtocolId m_rxProtocolId; - TxProtocols m_rxProtocols; - - int m_historyId; - AmplitudeData m_sampleAmplitudeAverage; - std::vector m_sampleAmplitudeHistory; - - RecordedData m_recordedAmplitude; - - int m_historyIdFixed; - std::vector m_spectrumHistoryFixed; - - // Tx - bool m_isTxEnabled; - bool m_hasNewTxData; - float m_sendVolume; - - int m_txDataLength; - TxRxData m_txData; - TxProtocol m_txProtocol; - - AmplitudeData m_outputBlock; - AmplitudeData m_outputBlockResampled; - TxRxData m_outputBlockTmp; - AmplitudeDataI16 m_outputBlockI16; - AmplitudeDataI16 m_txAmplitudeDataI16; - WaveformTones m_waveformTones; - - // Impl - // todo : move all members inside Impl - struct Impl; - std::unique_ptr m_impl; -}; - -#endif - -#endif diff --git a/examples/arduino-rx/reed-solomon/LICENSE b/examples/arduino-rx/reed-solomon/LICENSE deleted file mode 100644 index aaffd33..0000000 --- a/examples/arduino-rx/reed-solomon/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -Copyright © 2015 Mike Lubinets, github.com/mersinvald - -Permission is hereby granted, free of charge, to any person -obtaining a copy of this software and associated documentation files -(the “Software”), to deal in the Software without restriction, -including without limitation the rights to use, copy, modify, merge, -publish, distribute, sublicense, and/or sell copies of the Software, -and to permit persons to whom the Software is furnished to do so, -subject to the following conditions: - -The above copyright notice and this permission notice shall be -included in all copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, -EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF -MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND -NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS -BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN -ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN -CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/examples/arduino-rx/reed-solomon/gf.hpp b/examples/arduino-rx/reed-solomon/gf.hpp deleted file mode 100644 index 14a2831..0000000 --- a/examples/arduino-rx/reed-solomon/gf.hpp +++ /dev/null @@ -1,235 +0,0 @@ -/* Author: Mike Lubinets (aka mersinvald) - * Date: 29.12.15 - * - * See LICENSE */ - -#ifndef GF_H -#define GF_H - -#include "poly.hpp" - -#include -#include -#include - -namespace RS { - -namespace gf { - - -/* GF tables pre-calculated for 0x11d primitive polynomial */ - -const uint8_t exp[512] = { - 0x1, 0x2, 0x4, 0x8, 0x10, 0x20, 0x40, 0x80, 0x1d, 0x3a, 0x74, 0xe8, 0xcd, 0x87, 0x13, 0x26, 0x4c, - 0x98, 0x2d, 0x5a, 0xb4, 0x75, 0xea, 0xc9, 0x8f, 0x3, 0x6, 0xc, 0x18, 0x30, 0x60, 0xc0, 0x9d, - 0x27, 0x4e, 0x9c, 0x25, 0x4a, 0x94, 0x35, 0x6a, 0xd4, 0xb5, 0x77, 0xee, 0xc1, 0x9f, 0x23, 0x46, - 0x8c, 0x5, 0xa, 0x14, 0x28, 0x50, 0xa0, 0x5d, 0xba, 0x69, 0xd2, 0xb9, 0x6f, 0xde, 0xa1, 0x5f, - 0xbe, 0x61, 0xc2, 0x99, 0x2f, 0x5e, 0xbc, 0x65, 0xca, 0x89, 0xf, 0x1e, 0x3c, 0x78, 0xf0, 0xfd, - 0xe7, 0xd3, 0xbb, 0x6b, 0xd6, 0xb1, 0x7f, 0xfe, 0xe1, 0xdf, 0xa3, 0x5b, 0xb6, 0x71, 0xe2, 0xd9, - 0xaf, 0x43, 0x86, 0x11, 0x22, 0x44, 0x88, 0xd, 0x1a, 0x34, 0x68, 0xd0, 0xbd, 0x67, 0xce, 0x81, - 0x1f, 0x3e, 0x7c, 0xf8, 0xed, 0xc7, 0x93, 0x3b, 0x76, 0xec, 0xc5, 0x97, 0x33, 0x66, 0xcc, 0x85, - 0x17, 0x2e, 0x5c, 0xb8, 0x6d, 0xda, 0xa9, 0x4f, 0x9e, 0x21, 0x42, 0x84, 0x15, 0x2a, 0x54, 0xa8, - 0x4d, 0x9a, 0x29, 0x52, 0xa4, 0x55, 0xaa, 0x49, 0x92, 0x39, 0x72, 0xe4, 0xd5, 0xb7, 0x73, 0xe6, - 0xd1, 0xbf, 0x63, 0xc6, 0x91, 0x3f, 0x7e, 0xfc, 0xe5, 0xd7, 0xb3, 0x7b, 0xf6, 0xf1, 0xff, 0xe3, - 0xdb, 0xab, 0x4b, 0x96, 0x31, 0x62, 0xc4, 0x95, 0x37, 0x6e, 0xdc, 0xa5, 0x57, 0xae, 0x41, 0x82, - 0x19, 0x32, 0x64, 0xc8, 0x8d, 0x7, 0xe, 0x1c, 0x38, 0x70, 0xe0, 0xdd, 0xa7, 0x53, 0xa6, 0x51, - 0xa2, 0x59, 0xb2, 0x79, 0xf2, 0xf9, 0xef, 0xc3, 0x9b, 0x2b, 0x56, 0xac, 0x45, 0x8a, 0x9, 0x12, - 0x24, 0x48, 0x90, 0x3d, 0x7a, 0xf4, 0xf5, 0xf7, 0xf3, 0xfb, 0xeb, 0xcb, 0x8b, 0xb, 0x16, 0x2c, - 0x58, 0xb0, 0x7d, 0xfa, 0xe9, 0xcf, 0x83, 0x1b, 0x36, 0x6c, 0xd8, 0xad, 0x47, 0x8e, 0x1, 0x2, - 0x4, 0x8, 0x10, 0x20, 0x40, 0x80, 0x1d, 0x3a, 0x74, 0xe8, 0xcd, 0x87, 0x13, 0x26, 0x4c, 0x98, - 0x2d, 0x5a, 0xb4, 0x75, 0xea, 0xc9, 0x8f, 0x3, 0x6, 0xc, 0x18, 0x30, 0x60, 0xc0, 0x9d, 0x27, - 0x4e, 0x9c, 0x25, 0x4a, 0x94, 0x35, 0x6a, 0xd4, 0xb5, 0x77, 0xee, 0xc1, 0x9f, 0x23, 0x46, 0x8c, - 0x5, 0xa, 0x14, 0x28, 0x50, 0xa0, 0x5d, 0xba, 0x69, 0xd2, 0xb9, 0x6f, 0xde, 0xa1, 0x5f, 0xbe, - 0x61, 0xc2, 0x99, 0x2f, 0x5e, 0xbc, 0x65, 0xca, 0x89, 0xf, 0x1e, 0x3c, 0x78, 0xf0, 0xfd, 0xe7, - 0xd3, 0xbb, 0x6b, 0xd6, 0xb1, 0x7f, 0xfe, 0xe1, 0xdf, 0xa3, 0x5b, 0xb6, 0x71, 0xe2, 0xd9, 0xaf, - 0x43, 0x86, 0x11, 0x22, 0x44, 0x88, 0xd, 0x1a, 0x34, 0x68, 0xd0, 0xbd, 0x67, 0xce, 0x81, 0x1f, - 0x3e, 0x7c, 0xf8, 0xed, 0xc7, 0x93, 0x3b, 0x76, 0xec, 0xc5, 0x97, 0x33, 0x66, 0xcc, 0x85, 0x17, - 0x2e, 0x5c, 0xb8, 0x6d, 0xda, 0xa9, 0x4f, 0x9e, 0x21, 0x42, 0x84, 0x15, 0x2a, 0x54, 0xa8, 0x4d, - 0x9a, 0x29, 0x52, 0xa4, 0x55, 0xaa, 0x49, 0x92, 0x39, 0x72, 0xe4, 0xd5, 0xb7, 0x73, 0xe6, 0xd1, - 0xbf, 0x63, 0xc6, 0x91, 0x3f, 0x7e, 0xfc, 0xe5, 0xd7, 0xb3, 0x7b, 0xf6, 0xf1, 0xff, 0xe3, 0xdb, - 0xab, 0x4b, 0x96, 0x31, 0x62, 0xc4, 0x95, 0x37, 0x6e, 0xdc, 0xa5, 0x57, 0xae, 0x41, 0x82, 0x19, - 0x32, 0x64, 0xc8, 0x8d, 0x7, 0xe, 0x1c, 0x38, 0x70, 0xe0, 0xdd, 0xa7, 0x53, 0xa6, 0x51, 0xa2, - 0x59, 0xb2, 0x79, 0xf2, 0xf9, 0xef, 0xc3, 0x9b, 0x2b, 0x56, 0xac, 0x45, 0x8a, 0x9, 0x12, 0x24, - 0x48, 0x90, 0x3d, 0x7a, 0xf4, 0xf5, 0xf7, 0xf3, 0xfb, 0xeb, 0xcb, 0x8b, 0xb, 0x16, 0x2c, 0x58, - 0xb0, 0x7d, 0xfa, 0xe9, 0xcf, 0x83, 0x1b, 0x36, 0x6c, 0xd8, 0xad, 0x47, 0x8e, 0x1, 0x2 -}; - -const uint8_t log[256] = { - 0x0, 0x0, 0x1, 0x19, 0x2, 0x32, 0x1a, 0xc6, 0x3, 0xdf, 0x33, 0xee, 0x1b, 0x68, 0xc7, 0x4b, 0x4, - 0x64, 0xe0, 0xe, 0x34, 0x8d, 0xef, 0x81, 0x1c, 0xc1, 0x69, 0xf8, 0xc8, 0x8, 0x4c, 0x71, 0x5, - 0x8a, 0x65, 0x2f, 0xe1, 0x24, 0xf, 0x21, 0x35, 0x93, 0x8e, 0xda, 0xf0, 0x12, 0x82, 0x45, 0x1d, - 0xb5, 0xc2, 0x7d, 0x6a, 0x27, 0xf9, 0xb9, 0xc9, 0x9a, 0x9, 0x78, 0x4d, 0xe4, 0x72, 0xa6, 0x6, - 0xbf, 0x8b, 0x62, 0x66, 0xdd, 0x30, 0xfd, 0xe2, 0x98, 0x25, 0xb3, 0x10, 0x91, 0x22, 0x88, 0x36, - 0xd0, 0x94, 0xce, 0x8f, 0x96, 0xdb, 0xbd, 0xf1, 0xd2, 0x13, 0x5c, 0x83, 0x38, 0x46, 0x40, 0x1e, - 0x42, 0xb6, 0xa3, 0xc3, 0x48, 0x7e, 0x6e, 0x6b, 0x3a, 0x28, 0x54, 0xfa, 0x85, 0xba, 0x3d, 0xca, - 0x5e, 0x9b, 0x9f, 0xa, 0x15, 0x79, 0x2b, 0x4e, 0xd4, 0xe5, 0xac, 0x73, 0xf3, 0xa7, 0x57, 0x7, - 0x70, 0xc0, 0xf7, 0x8c, 0x80, 0x63, 0xd, 0x67, 0x4a, 0xde, 0xed, 0x31, 0xc5, 0xfe, 0x18, 0xe3, - 0xa5, 0x99, 0x77, 0x26, 0xb8, 0xb4, 0x7c, 0x11, 0x44, 0x92, 0xd9, 0x23, 0x20, 0x89, 0x2e, 0x37, - 0x3f, 0xd1, 0x5b, 0x95, 0xbc, 0xcf, 0xcd, 0x90, 0x87, 0x97, 0xb2, 0xdc, 0xfc, 0xbe, 0x61, 0xf2, - 0x56, 0xd3, 0xab, 0x14, 0x2a, 0x5d, 0x9e, 0x84, 0x3c, 0x39, 0x53, 0x47, 0x6d, 0x41, 0xa2, 0x1f, - 0x2d, 0x43, 0xd8, 0xb7, 0x7b, 0xa4, 0x76, 0xc4, 0x17, 0x49, 0xec, 0x7f, 0xc, 0x6f, 0xf6, 0x6c, - 0xa1, 0x3b, 0x52, 0x29, 0x9d, 0x55, 0xaa, 0xfb, 0x60, 0x86, 0xb1, 0xbb, 0xcc, 0x3e, 0x5a, 0xcb, - 0x59, 0x5f, 0xb0, 0x9c, 0xa9, 0xa0, 0x51, 0xb, 0xf5, 0x16, 0xeb, 0x7a, 0x75, 0x2c, 0xd7, 0x4f, - 0xae, 0xd5, 0xe9, 0xe6, 0xe7, 0xad, 0xe8, 0x74, 0xd6, 0xf4, 0xea, 0xa8, 0x50, 0x58, 0xaf -}; - - - -/* ################################ - * # OPERATIONS OVER GALUA FIELDS # - * ################################ */ - -/* @brief Addition in Galua Fields - * @param x - left operand - * @param y - right operand - * @return x + y */ -inline uint8_t add(uint8_t x, uint8_t y) { - return x^y; -} - -/* ##### GF substraction ###### */ -/* @brief Substraction in Galua Fields - * @param x - left operand - * @param y - right operand - * @return x - y */ -inline uint8_t sub(uint8_t x, uint8_t y) { - return x^y; -} - -/* @brief Multiplication in Galua Fields - * @param x - left operand - * @param y - rifht operand - * @return x * y */ -inline uint8_t mul(uint16_t x, uint16_t y){ - if (x == 0 || y == 0) - return 0; - return exp[log[x] + log[y]]; -} - -/* @brief Division in Galua Fields - * @param x - dividend - * @param y - divisor - * @return x / y */ -inline uint8_t div(uint8_t x, uint8_t y){ - assert(y != 0); - if(x == 0) return 0; - return exp[(log[x] + 255 - log[y]) % 255]; -} - -/* @brief X in power Y w - * @param x - operand - * @param power - power - * @return x^power */ -inline uint8_t pow(uint8_t x, intmax_t power){ - intmax_t i = log[x]; - i *= power; - i %= 255; - if(i < 0) i = i + 255; - return exp[i]; -} - -/* @brief Inversion in Galua Fields - * @param x - number - * @return inversion of x */ -inline uint8_t inverse(uint8_t x){ - return exp[255 - log[x]]; /* == div(1, x); */ -} - -/* ########################## - * # POLYNOMIALS OPERATIONS # - * ########################## */ - -/* @brief Multiplication polynomial by scalar - * @param &p - source polynomial - * @param &newp - destination polynomial - * @param x - scalar */ -inline void -poly_scale(const Poly *p, Poly *newp, uint16_t x) { - newp->length = p->length; - for(uint16_t i = 0; i < p->length; i++){ - newp->at(i) = mul(p->at(i), x); - } -} - -/* @brief Addition of two polynomials - * @param &p - right operand polynomial - * @param &q - left operand polynomial - * @param &newp - destination polynomial */ -inline void -poly_add(const Poly *p, const Poly *q, Poly *newp) { - newp->length = poly_max(p->length, q->length); - memset(newp->ptr(), 0, newp->length * sizeof(uint8_t)); - - for(uint8_t i = 0; i < p->length; i++){ - newp->at(i + newp->length - p->length) = p->at(i); - } - - for(uint8_t i = 0; i < q->length; i++){ - newp->at(i + newp->length - q->length) ^= q->at(i); - } -} - - -/* @brief Multiplication of two polynomials - * @param &p - right operand polynomial - * @param &q - left operand polynomial - * @param &newp - destination polynomial */ -inline void -poly_mul(const Poly *p, const Poly *q, Poly *newp) { - newp->length = p->length + q->length - 1; - memset(newp->ptr(), 0, newp->length * sizeof(uint8_t)); - /* Compute the polynomial multiplication (just like the outer product of two vectors, - * we multiply each coefficients of p with all coefficients of q) */ - for(uint8_t j = 0; j < q->length; j++){ - for(uint8_t i = 0; i < p->length; i++){ - newp->at(i+j) ^= mul(p->at(i), q->at(j)); /* == r[i + j] = gf_add(r[i+j], gf_mul(p[i], q[j])) */ - } - } -} - -/* @brief Division of two polynomials - * @param &p - right operand polynomial - * @param &q - left operand polynomial - * @param &newp - destination polynomial */ -inline void -poly_div(const Poly *p, const Poly *q, Poly *newp) { - if(p->ptr() != newp->ptr()) { - memcpy(newp->ptr(), p->ptr(), p->length*sizeof(uint8_t)); - } - - newp->length = p->length; - - uint8_t coef; - - for(int i = 0; i < (p->length-(q->length-1)); i++){ - coef = newp->at(i); - if(coef != 0){ - for(uint8_t j = 1; j < q->length; j++){ - if(q->at(j) != 0) - newp->at(i+j) ^= mul(q->at(j), coef); - } - } - } - - size_t sep = p->length-(q->length-1); - memmove(newp->ptr(), newp->ptr()+sep, (newp->length-sep) * sizeof(uint8_t)); - newp->length = newp->length-sep; -} - -/* @brief Evaluation of polynomial in x - * @param &p - polynomial to evaluate - * @param x - evaluation point */ -inline int8_t -poly_eval(const Poly *p, uint16_t x) { - uint8_t y = p->at(0); - for(uint8_t i = 1; i < p->length; i++){ - y = mul(y, x) ^ p->at(i); - } - return y; -} - -} /* end of gf namespace */ - -} -#endif // GF_H - diff --git a/examples/arduino-rx/reed-solomon/poly.hpp b/examples/arduino-rx/reed-solomon/poly.hpp deleted file mode 100644 index 9e1b65f..0000000 --- a/examples/arduino-rx/reed-solomon/poly.hpp +++ /dev/null @@ -1,94 +0,0 @@ -/* Author: Mike Lubinets (aka mersinvald) - * Date: 29.12.15 - * - * See LICENSE */ - -#ifndef POLY_H -#define POLY_H - -#include -#include -#include - -namespace RS { - -struct Poly { - Poly() - : length(0), _memory(NULL) {} - - Poly(uint8_t id, uint16_t offset, uint8_t size) \ - : length(0), _id(id), _size(size), _offset(offset), _memory(NULL) {} - - /* @brief Append number at the end of polynomial - * @param num - number to append - * @return false if polynomial can't be stretched */ - inline bool Append(uint8_t num) { - assert(length < _size); - ptr()[length++] = num; - return true; - } - - /* @brief Polynomial initialization */ - inline void Init(uint8_t id, uint16_t offset, uint8_t size, uint8_t** memory_ptr) { - this->_id = id; - this->_offset = offset; - this->_size = size; - this->length = 0; - this->_memory = memory_ptr; - } - - /* @brief Polynomial memory zeroing */ - inline void Reset() { - memset((void*)ptr(), 0, this->_size); - } - - /* @brief Copy polynomial to memory - * @param src - source byte-sequence - * @param size - size of polynomial - * @param offset - write offset */ - inline void Set(const uint8_t* src, uint8_t len, uint8_t offset = 0) { - assert(src && len <= this->_size-offset); - memcpy(ptr()+offset, src, len * sizeof(uint8_t)); - length = len + offset; - } - - #define poly_max(a, b) ((a > b) ? (a) : (b)) - - inline void Copy(const Poly* src) { - length = poly_max(length, src->length); - Set(src->ptr(), length); - } - - inline uint8_t& at(uint8_t i) const { - assert(i < _size); - return ptr()[i]; - } - - inline uint8_t id() const { - return _id; - } - - inline uint8_t size() const { - return _size; - } - - // Returns pointer to memory of this polynomial - inline uint8_t* ptr() const { - assert(_memory && *_memory); - return (*_memory) + _offset; - } - - uint8_t length; - -protected: - - uint8_t _id; - uint8_t _size; // Size of reserved memory for this polynomial - uint16_t _offset; // Offset in memory - uint8_t** _memory; // Pointer to pointer to memory -}; - - -} - -#endif // POLY_H diff --git a/examples/arduino-rx/reed-solomon/rs.hpp b/examples/arduino-rx/reed-solomon/rs.hpp deleted file mode 100644 index 202cdd5..0000000 --- a/examples/arduino-rx/reed-solomon/rs.hpp +++ /dev/null @@ -1,538 +0,0 @@ -/* Author: Mike Lubinets (aka mersinvald) - * Date: 29.12.15 - * - * See LICENSE */ - -#ifndef RS_HPP -#define RS_HPP - -#include "poly.hpp" -#include "gf.hpp" - -#include -#include -#include -#include - -namespace RS { - -#define MSG_CNT 3 // message-length polynomials count -#define POLY_CNT 14 // (ecc_length*2)-length polynomialc count - -class ReedSolomon { -public: - const uint8_t msg_length; - const uint8_t ecc_length; - - uint8_t * generator_cache = nullptr; - bool generator_cached = false; - - ReedSolomon(uint8_t msg_length_p, uint8_t ecc_length_p) : - msg_length(msg_length_p), ecc_length(ecc_length_p) { - generator_cache = new uint8_t[ecc_length + 1]; - - const uint8_t enc_len = msg_length + ecc_length; - const uint8_t poly_len = ecc_length * 2; - uint8_t** memptr = &memory; - uint16_t offset = 0; - - /* Initialize first six polys manually cause their amount depends on template parameters */ - - polynoms[0].Init(ID_MSG_IN, offset, enc_len, memptr); - offset += enc_len; - - polynoms[1].Init(ID_MSG_OUT, offset, enc_len, memptr); - offset += enc_len; - - for(uint8_t i = ID_GENERATOR; i < ID_MSG_E; i++) { - polynoms[i].Init(i, offset, poly_len, memptr); - offset += poly_len; - } - - polynoms[5].Init(ID_MSG_E, offset, enc_len, memptr); - offset += enc_len; - - for(uint8_t i = ID_TPOLY3; i < ID_ERR_EVAL+2; i++) { - polynoms[i].Init(i, offset, poly_len, memptr); - offset += poly_len; - } - } - - ~ReedSolomon() { - delete [] generator_cache; - // Dummy destructor, gcc-generated one crashes programm - memory = NULL; - } - - /* @brief Message block encoding - * @param *src - input message buffer (msg_lenth size) - * @param *dst - output buffer for ecc (ecc_length size at least) */ - void EncodeBlock(const void* src, void* dst) { - assert(msg_length + ecc_length < 256); - - ///* Allocating memory on stack for polynomials storage */ - //uint8_t stack_memory[MSG_CNT * msg_length + POLY_CNT * ecc_length * 2]; - //this->memory = stack_memory; - - // gg : allocation is now on the heap - std::vector stack_memory(MSG_CNT * msg_length + POLY_CNT * ecc_length * 2); - this->memory = stack_memory.data(); - - const uint8_t* src_ptr = (const uint8_t*) src; - uint8_t* dst_ptr = (uint8_t*) dst; - - Poly *msg_in = &polynoms[ID_MSG_IN]; - Poly *msg_out = &polynoms[ID_MSG_OUT]; - Poly *gen = &polynoms[ID_GENERATOR]; - - // Weird shit, but without reseting msg_in it simply doesn't work - msg_in->Reset(); - msg_out->Reset(); - - // Using cached generator or generating new one - if(generator_cached) { - gen->Set(generator_cache, ecc_length + 1); - } else { - GeneratorPoly(); - memcpy(generator_cache, gen->ptr(), gen->length); - generator_cached = true; - } - - // Copying input message to internal polynomial - msg_in->Set(src_ptr, msg_length); - msg_out->Set(src_ptr, msg_length); - msg_out->length = msg_in->length + ecc_length; - - // Here all the magic happens - uint8_t coef = 0; // cache - for(uint8_t i = 0; i < msg_length; i++){ - coef = msg_out->at(i); - if(coef != 0){ - for(uint32_t j = 1; j < gen->length; j++){ - msg_out->at(i+j) ^= gf::mul(gen->at(j), coef); - } - } - } - - // Copying ECC to the output buffer - memcpy(dst_ptr, msg_out->ptr()+msg_length, ecc_length * sizeof(uint8_t)); - } - - /* @brief Message encoding - * @param *src - input message buffer (msg_lenth size) - * @param *dst - output buffer (msg_length + ecc_length size at least) */ - void Encode(const void* src, void* dst) { - uint8_t* dst_ptr = (uint8_t*) dst; - - // Copying message to the output buffer - memcpy(dst_ptr, src, msg_length * sizeof(uint8_t)); - - // Calling EncodeBlock to write ecc to out[ut buffer - EncodeBlock(src, dst_ptr+msg_length); - } - - /* @brief Message block decoding - * @param *src - encoded message buffer (msg_length size) - * @param *ecc - ecc buffer (ecc_length size) - * @param *msg_out - output buffer (msg_length size at least) - * @param *erase_pos - known errors positions - * @param erase_count - count of known errors - * @return RESULT_SUCCESS if successfull, error code otherwise */ - int DecodeBlock(const void* src, const void* ecc, void* dst, uint8_t* erase_pos = NULL, size_t erase_count = 0) { - assert(msg_length + ecc_length < 256); - - const uint8_t *src_ptr = (const uint8_t*) src; - const uint8_t *ecc_ptr = (const uint8_t*) ecc; - uint8_t *dst_ptr = (uint8_t*) dst; - - const uint8_t src_len = msg_length + ecc_length; - const uint8_t dst_len = msg_length; - - bool ok; - - ///* Allocation memory on stack */ - //uint8_t stack_memory[MSG_CNT * msg_length + POLY_CNT * ecc_length * 2]; - //this->memory = stack_memory; - - // gg : allocation is now on the heap - std::vector stack_memory(MSG_CNT * msg_length + POLY_CNT * ecc_length * 2); - this->memory = stack_memory.data(); - - Poly *msg_in = &polynoms[ID_MSG_IN]; - Poly *msg_out = &polynoms[ID_MSG_OUT]; - Poly *epos = &polynoms[ID_ERASURES]; - - // Copying message to polynomials memory - msg_in->Set(src_ptr, msg_length); - msg_in->Set(ecc_ptr, ecc_length, msg_length); - msg_out->Copy(msg_in); - - // Copying known errors to polynomial - if(erase_pos == NULL) { - epos->length = 0; - } else { - epos->Set(erase_pos, erase_count); - for(uint8_t i = 0; i < epos->length; i++){ - msg_in->at(epos->at(i)) = 0; - } - } - - // Too many errors - if(epos->length > ecc_length) return 1; - - Poly *synd = &polynoms[ID_SYNDROMES]; - Poly *eloc = &polynoms[ID_ERRORS_LOC]; - Poly *reloc = &polynoms[ID_TPOLY1]; - Poly *err = &polynoms[ID_ERRORS]; - Poly *forney = &polynoms[ID_FORNEY]; - - // Calculating syndrome - CalcSyndromes(msg_in); - - // Checking for errors - bool has_errors = false; - for(uint8_t i = 0; i < synd->length; i++) { - if(synd->at(i) != 0) { - has_errors = true; - break; - } - } - - // Going to exit if no errors - if(!has_errors) goto return_corrected_msg; - - CalcForneySyndromes(synd, epos, src_len); - FindErrorLocator(forney, NULL, epos->length); - - // Reversing syndrome - // TODO optimize through special Poly flag - reloc->length = eloc->length; - for(int8_t i = eloc->length-1, j = 0; i >= 0; i--, j++){ - reloc->at(j) = eloc->at(i); - } - - // Fing errors - ok = FindErrors(reloc, src_len); - if(!ok) return 1; - - // Error happened while finding errors (so helpfull :D) - if(err->length == 0) return 1; - - /* Adding found errors with known */ - for(uint8_t i = 0; i < err->length; i++) { - epos->Append(err->at(i)); - } - - // Correcting errors - CorrectErrata(synd, epos, msg_in); - - return_corrected_msg: - // Wrighting corrected message to output buffer - msg_out->length = dst_len; - memcpy(dst_ptr, msg_out->ptr(), msg_out->length * sizeof(uint8_t)); - return 0; - } - - /* @brief Message block decoding - * @param *src - encoded message buffer (msg_length + ecc_length size) - * @param *msg_out - output buffer (msg_length size at least) - * @param *erase_pos - known errors positions - * @param erase_count - count of known errors - * @return RESULT_SUCCESS if successfull, error code otherwise */ - int Decode(const void* src, void* dst, uint8_t* erase_pos = NULL, size_t erase_count = 0) { - const uint8_t *src_ptr = (const uint8_t*) src; - const uint8_t *ecc_ptr = src_ptr + msg_length; - - return DecodeBlock(src, ecc_ptr, dst, erase_pos, erase_count); - } - -#ifndef DEBUG -private: -#endif - - enum POLY_ID { - ID_MSG_IN = 0, - ID_MSG_OUT, - ID_GENERATOR, // 3 - ID_TPOLY1, // T for Temporary - ID_TPOLY2, - - ID_MSG_E, // 5 - - ID_TPOLY3, // 6 - ID_TPOLY4, - - ID_SYNDROMES, - ID_FORNEY, - - ID_ERASURES_LOC, - ID_ERRORS_LOC, - - ID_ERASURES, - ID_ERRORS, - - ID_COEF_POS, - ID_ERR_EVAL - }; - - // Pointer for polynomials memory on stack - uint8_t* memory; - Poly polynoms[MSG_CNT + POLY_CNT]; - - void GeneratorPoly() { - Poly *gen = polynoms + ID_GENERATOR; - gen->at(0) = 1; - gen->length = 1; - - Poly *mulp = polynoms + ID_TPOLY1; - Poly *temp = polynoms + ID_TPOLY2; - mulp->length = 2; - - for(int8_t i = 0; i < ecc_length; i++){ - mulp->at(0) = 1; - mulp->at(1) = gf::pow(2, i); - - gf::poly_mul(gen, mulp, temp); - - gen->Copy(temp); - } - } - - void CalcSyndromes(const Poly *msg) { - Poly *synd = &polynoms[ID_SYNDROMES]; - synd->length = ecc_length+1; - synd->at(0) = 0; - for(uint8_t i = 1; i < ecc_length+1; i++){ - synd->at(i) = gf::poly_eval(msg, gf::pow(2, i-1)); - } - } - - void FindErrataLocator(const Poly *epos) { - Poly *errata_loc = &polynoms[ID_ERASURES_LOC]; - Poly *mulp = &polynoms[ID_TPOLY1]; - Poly *addp = &polynoms[ID_TPOLY2]; - Poly *apol = &polynoms[ID_TPOLY3]; - Poly *temp = &polynoms[ID_TPOLY4]; - - errata_loc->length = 1; - errata_loc->at(0) = 1; - - mulp->length = 1; - addp->length = 2; - - for(uint8_t i = 0; i < epos->length; i++){ - mulp->at(0) = 1; - addp->at(0) = gf::pow(2, epos->at(i)); - addp->at(1) = 0; - - gf::poly_add(mulp, addp, apol); - gf::poly_mul(errata_loc, apol, temp); - - errata_loc->Copy(temp); - } - } - - void FindErrorEvaluator(const Poly *synd, const Poly *errata_loc, Poly *dst, uint8_t ecclen) { - Poly *mulp = &polynoms[ID_TPOLY1]; - gf::poly_mul(synd, errata_loc, mulp); - - Poly *divisor = &polynoms[ID_TPOLY2]; - divisor->length = ecclen+2; - - divisor->Reset(); - divisor->at(0) = 1; - - gf::poly_div(mulp, divisor, dst); - } - - void CorrectErrata(const Poly *synd, const Poly *err_pos, const Poly *msg_in) { - Poly *c_pos = &polynoms[ID_COEF_POS]; - Poly *corrected = &polynoms[ID_MSG_OUT]; - c_pos->length = err_pos->length; - - for(uint8_t i = 0; i < err_pos->length; i++) - c_pos->at(i) = msg_in->length - 1 - err_pos->at(i); - - /* uses t_poly 1, 2, 3, 4 */ - FindErrataLocator(c_pos); - Poly *errata_loc = &polynoms[ID_ERASURES_LOC]; - - /* reversing syndromes */ - Poly *rsynd = &polynoms[ID_TPOLY3]; - rsynd->length = synd->length; - - for(int8_t i = synd->length-1, j = 0; i >= 0; i--, j++) { - rsynd->at(j) = synd->at(i); - } - - /* getting reversed error evaluator polynomial */ - Poly *re_eval = &polynoms[ID_TPOLY4]; - - /* uses T_POLY 1, 2 */ - FindErrorEvaluator(rsynd, errata_loc, re_eval, errata_loc->length-1); - - /* reversing it back */ - Poly *e_eval = &polynoms[ID_ERR_EVAL]; - e_eval->length = re_eval->length; - for(int8_t i = re_eval->length-1, j = 0; i >= 0; i--, j++) { - e_eval->at(j) = re_eval->at(i); - } - - Poly *X = &polynoms[ID_TPOLY1]; /* this will store errors positions */ - X->length = 0; - - int16_t l; - for(uint8_t i = 0; i < c_pos->length; i++){ - l = 255 - c_pos->at(i); - X->Append(gf::pow(2, -l)); - } - - /* Magnitude polynomial - Shit just got real */ - Poly *E = &polynoms[ID_MSG_E]; - E->Reset(); - E->length = msg_in->length; - - uint8_t Xi_inv; - - Poly *err_loc_prime_temp = &polynoms[ID_TPOLY2]; - - uint8_t err_loc_prime; - uint8_t y; - - for(uint8_t i = 0; i < X->length; i++){ - Xi_inv = gf::inverse(X->at(i)); - - err_loc_prime_temp->length = 0; - for(uint8_t j = 0; j < X->length; j++){ - if(j != i){ - err_loc_prime_temp->Append(gf::sub(1, gf::mul(Xi_inv, X->at(j)))); - } - } - - err_loc_prime = 1; - for(uint8_t j = 0; j < err_loc_prime_temp->length; j++){ - err_loc_prime = gf::mul(err_loc_prime, err_loc_prime_temp->at(j)); - } - - y = gf::poly_eval(re_eval, Xi_inv); - y = gf::mul(gf::pow(X->at(i), 1), y); - - E->at(err_pos->at(i)) = gf::div(y, err_loc_prime); - } - - gf::poly_add(msg_in, E, corrected); - } - - bool FindErrorLocator(const Poly *synd, Poly *erase_loc = NULL, size_t erase_count = 0) { - Poly *error_loc = &polynoms[ID_ERRORS_LOC]; - Poly *err_loc = &polynoms[ID_TPOLY1]; - Poly *old_loc = &polynoms[ID_TPOLY2]; - Poly *temp = &polynoms[ID_TPOLY3]; - Poly *temp2 = &polynoms[ID_TPOLY4]; - - if(erase_loc != NULL) { - err_loc->Copy(erase_loc); - old_loc->Copy(erase_loc); - } else { - err_loc->length = 1; - old_loc->length = 1; - err_loc->at(0) = 1; - old_loc->at(0) = 1; - } - - uint8_t synd_shift = 0; - if(synd->length > ecc_length) { - synd_shift = synd->length - ecc_length; - } - - uint8_t K = 0; - uint8_t delta = 0; - uint8_t index; - - for(uint8_t i = 0; i < ecc_length - erase_count; i++){ - if(erase_loc != NULL) - K = erase_count + i + synd_shift; - else - K = i + synd_shift; - - delta = synd->at(K); - for(uint8_t j = 1; j < err_loc->length; j++) { - index = err_loc->length - j - 1; - delta ^= gf::mul(err_loc->at(index), synd->at(K-j)); - } - - old_loc->Append(0); - - if(delta != 0) { - if(old_loc->length > err_loc->length) { - gf::poly_scale(old_loc, temp, delta); - gf::poly_scale(err_loc, old_loc, gf::inverse(delta)); - err_loc->Copy(temp); - } - gf::poly_scale(old_loc, temp, delta); - gf::poly_add(err_loc, temp, temp2); - err_loc->Copy(temp2); - } - } - - uint32_t shift = 0; - while(err_loc->length && err_loc->at(shift) == 0) shift++; - - uint32_t errs = err_loc->length - shift - 1; - if(((errs - erase_count) * 2 + erase_count) > ecc_length){ - return false; /* Error count is greater then we can fix! */ - } - - memcpy(error_loc->ptr(), err_loc->ptr() + shift, (err_loc->length - shift) * sizeof(uint8_t)); - error_loc->length = (err_loc->length - shift); - return true; - } - - bool FindErrors(const Poly *error_loc, size_t msg_in_size) { - Poly *err = &polynoms[ID_ERRORS]; - - uint8_t errs = error_loc->length - 1; - err->length = 0; - - for(uint8_t i = 0; i < msg_in_size; i++) { - if(gf::poly_eval(error_loc, gf::pow(2, i)) == 0) { - err->Append(msg_in_size - 1 - i); - } - } - - /* Sanity check: - * the number of err/errata positions found - * should be exactly the same as the length of the errata locator polynomial */ - if(err->length != errs) - /* couldn't find error locations */ - return false; - return true; - } - - void CalcForneySyndromes(const Poly *synd, const Poly *erasures_pos, size_t msg_in_size) { - Poly *erase_pos_reversed = &polynoms[ID_TPOLY1]; - Poly *forney_synd = &polynoms[ID_FORNEY]; - erase_pos_reversed->length = 0; - - for(uint8_t i = 0; i < erasures_pos->length; i++){ - erase_pos_reversed->Append(msg_in_size - 1 - erasures_pos->at(i)); - } - - forney_synd->Reset(); - forney_synd->Set(synd->ptr()+1, synd->length-1); - - uint8_t x; - for(uint8_t i = 0; i < erasures_pos->length; i++) { - x = gf::pow(2, erase_pos_reversed->at(i)); - for(int8_t j = 0; j < forney_synd->length - 1; j++){ - forney_synd->at(j) = gf::mul(forney_synd->at(j), x) ^ forney_synd->at(j+1); - } - } - } -}; - -} - -#endif // RS_HPP - diff --git a/examples/arduino-rx/resampler.cpp b/examples/arduino-rx/resampler.cpp deleted file mode 100644 index 38508b0..0000000 --- a/examples/arduino-rx/resampler.cpp +++ /dev/null @@ -1,161 +0,0 @@ -#include "resampler.h" - -#include -#include -#include - -#ifndef M_PI -#define M_PI 3.14159265358979323846 -#endif - -namespace { -double linear_interp(double first_number, double second_number, double fraction) { - return (first_number + ((second_number - first_number)*fraction)); -} -} - -Resampler::Resampler() : - m_sincTable(kWidth*kSamplesPerZeroCrossing), - m_delayBuffer(3*kWidth), - m_edgeSamples(kWidth), - m_samplesInp(2048) { - make_sinc(); - reset(); -} - -void Resampler::reset() { - m_state = {}; - std::fill(m_edgeSamples.begin(), m_edgeSamples.end(), 0.0f); - std::fill(m_delayBuffer.begin(), m_delayBuffer.end(), 0.0f); - std::fill(m_samplesInp.begin(), m_samplesInp.end(), 0.0f); -} - -int Resampler::resample( - float factor, - int nSamples, - const float * samplesInp, - float * samplesOut) { - int idxInp = -1; - int idxOut = 0; - int notDone = 1; - float data_in = 0.0f; - float data_out = 0.0f; - double one_over_factor = 1.0; - - auto stateSave = m_state; - - m_state.nSamplesTotal += nSamples; - - if (samplesOut) { - assert(nSamples > kWidth); - if ((int) m_samplesInp.size() < nSamples + kWidth) { - m_samplesInp.resize(nSamples + kWidth); - } - for (int i = 0; i < kWidth; ++i) { - m_samplesInp[i] = m_edgeSamples[i]; - m_edgeSamples[i] = samplesInp[nSamples - kWidth + i]; - } - for (int i = 0; i < nSamples; ++i) { - m_samplesInp[i + kWidth] = samplesInp[i]; - } - samplesInp = m_samplesInp.data(); - } - - while (notDone) { - while (m_state.timeLast < m_state.timeInt) { - if (++idxInp >= nSamples) { - notDone = 0; - break; - } else { - data_in = samplesInp[idxInp]; - } - //printf("xxxx idxInp = %d\n", idxInp); - if (samplesOut) new_data(data_in); - m_state.timeLast += 1; - } - - if (notDone == false) break; - - double temp1 = 0.0; - int left_limit = m_state.timeNow - kWidth + 1; /* leftmost neighboring sample used for interp.*/ - int right_limit = m_state.timeNow + kWidth; /* rightmost leftmost neighboring sample used for interp.*/ - if (left_limit < 0) left_limit = 0; - if (right_limit > m_state.nSamplesTotal + kWidth) right_limit = m_state.nSamplesTotal + kWidth; - if (factor < 1.0) { - for (int j = left_limit; j < right_limit; j++) { - temp1 += gimme_data(j - m_state.timeInt)*sinc(m_state.timeNow - (double) j); - } - data_out = temp1; - } - else { - one_over_factor = 1.0 / factor; - for (int j = left_limit; j < right_limit; j++) { - temp1 += gimme_data(j - m_state.timeInt)*one_over_factor*sinc(one_over_factor*(m_state.timeNow - (double) j)); - } - data_out = temp1; - } - - if (samplesOut) { - //printf("inp = %d, l = %d, r = %d, n = %d, a = %d, b = %d\n", idxInp, left_limit, right_limit, m_state.nSamplesTotal, left_limit - m_state.timeInt, right_limit - m_state.timeInt - 1); - samplesOut[idxOut] = data_out; - } - ++idxOut; - - m_state.timeNow += factor; - m_state.timeLast = m_state.timeInt; - m_state.timeInt = m_state.timeNow; - while (m_state.timeLast < m_state.timeInt) { - if (++idxInp >= nSamples) { - notDone = 0; - break; - } else { - data_in = samplesInp[idxInp]; - } - if (samplesOut) new_data(data_in); - m_state.timeLast += 1; - } - //printf("last idxInp = %d, nSamples = %d\n", idxInp, nSamples); - } - - if (samplesOut == nullptr) { - m_state = stateSave; - } - - return idxOut; -} - -float Resampler::gimme_data(int j) const { - return m_delayBuffer[(int) j + kWidth]; -} - -void Resampler::new_data(float data) { - for (int i = 0; i < kDelaySize - 5; i++) { - m_delayBuffer[i] = m_delayBuffer[i + 1]; - } - m_delayBuffer[kDelaySize - 5] = data; -} - -void Resampler::make_sinc() { - double temp, win_freq, win; - win_freq = M_PI/kWidth/kSamplesPerZeroCrossing; - m_sincTable[0] = 1.0; - for (int i = 1; i < kWidth*kSamplesPerZeroCrossing; i++) { - temp = (double) i*M_PI/kSamplesPerZeroCrossing; - m_sincTable[i] = sin(temp)/temp; - win = 0.5 + 0.5*cos(win_freq*i); - m_sincTable[i] *= win; - } -} - -double Resampler::sinc(double x) const { - int low; - double temp, delta; - if (fabs(x) >= kWidth - 1) { - return 0.0; - } else { - temp = fabs(x)*(double) kSamplesPerZeroCrossing; - low = temp; /* these are interpolation steps */ - delta = temp - low; /* and can be ommited if desired */ - return linear_interp(m_sincTable[low], m_sincTable[low + 1], delta); - } -} diff --git a/examples/arduino-rx/resampler.h b/examples/arduino-rx/resampler.h deleted file mode 100644 index 87804c5..0000000 --- a/examples/arduino-rx/resampler.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include -#include - -class Resampler { -public: - // this controls the number of neighboring samples - // which are used to interpolate the new samples. The - // processing time is linearly related to this width - static const int kWidth = 64; - - Resampler(); - - void reset(); - - int nSamplesTotal() const { return m_state.nSamplesTotal; } - - int resample( - float factor, - int nSamples, - const float * samplesInp, - float * samplesOut); - -private: - float gimme_data(int j) const; - void new_data(float data); - void make_sinc(); - double sinc(double x) const; - - static const int kDelaySize = 140; - - // this defines how finely the sinc function is sampled for storage in the table - static const int kSamplesPerZeroCrossing = 32; - - std::vector m_sincTable; - std::vector m_delayBuffer; - std::vector m_edgeSamples; - std::vector m_samplesInp; - - struct State { - int nSamplesTotal = 0; - int timeInt = 0; - int timeLast = 0; - double timeNow = 0.0; - }; - - State m_state; -};