From 6b7134d3e4b2af18971e7d24c859c0b2bb7a85ac Mon Sep 17 00:00:00 2001 From: Georgi Gerganov Date: Sun, 15 May 2022 22:36:13 +0300 Subject: [PATCH] arduino-rx : first working example on RP2040 Connect ! --- examples/CMakeLists.txt | 2 +- .../CMakeLists.txt | 0 .../arduino-rx.cpp | 0 .../index-tmpl.html | 0 examples/arduino-rx/arduino-rx.ino | 127 ++ examples/arduino-rx/ggwave.cpp | 1436 +++++++++++++++++ examples/arduino-rx/ggwave/ggwave.h | 574 +++++++ 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 + 13 files changed, 3236 insertions(+), 1 deletion(-) rename examples/{arduino-rx => arduino-rx-web}/CMakeLists.txt (100%) rename examples/{arduino-rx => arduino-rx-web}/arduino-rx.cpp (100%) rename examples/{arduino-rx => arduino-rx-web}/index-tmpl.html (100%) create mode 100644 examples/arduino-rx/arduino-rx.ino create mode 100644 examples/arduino-rx/ggwave.cpp create mode 100644 examples/arduino-rx/ggwave/ggwave.h create mode 100644 examples/arduino-rx/reed-solomon/LICENSE create mode 100644 examples/arduino-rx/reed-solomon/gf.hpp create mode 100644 examples/arduino-rx/reed-solomon/poly.hpp create mode 100644 examples/arduino-rx/reed-solomon/rs.hpp create mode 100644 examples/arduino-rx/resampler.cpp create mode 100644 examples/arduino-rx/resampler.h diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index a7de83c..71dcc91 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -96,7 +96,7 @@ if (GGWAVE_SUPPORT_SDL2) add_subdirectory(r2t2) endif() - add_subdirectory(arduino-rx) + add_subdirectory(arduino-rx-web) if (EMSCRIPTEN) # emscripten sdl2 examples diff --git a/examples/arduino-rx/CMakeLists.txt b/examples/arduino-rx-web/CMakeLists.txt similarity index 100% rename from examples/arduino-rx/CMakeLists.txt rename to examples/arduino-rx-web/CMakeLists.txt diff --git a/examples/arduino-rx/arduino-rx.cpp b/examples/arduino-rx-web/arduino-rx.cpp similarity index 100% rename from examples/arduino-rx/arduino-rx.cpp rename to examples/arduino-rx-web/arduino-rx.cpp diff --git a/examples/arduino-rx/index-tmpl.html b/examples/arduino-rx-web/index-tmpl.html similarity index 100% rename from examples/arduino-rx/index-tmpl.html rename to examples/arduino-rx-web/index-tmpl.html diff --git a/examples/arduino-rx/arduino-rx.ino b/examples/arduino-rx/arduino-rx.ino new file mode 100644 index 0000000..637bb7c --- /dev/null +++ b/examples/arduino-rx/arduino-rx.ino @@ -0,0 +1,127 @@ +#include "ggwave/ggwave.h" + +#include + +// default number of output channels +static const char channels = 1; + +// default PCM output frequency +static const int frequency = 10000; + +const int qmax = 1024; +volatile int qhead = 0; +volatile int qtail = 0; +volatile int qsize = 0; + +// Buffer to read samples into, each sample is 16-bits +short sampleBuffer[qmax]; + +// Number of audio samples read +//volatile int samplesRead = 0; + +void setup() { + Serial.begin(57600); + while (!Serial); + + // Configure the data receive callback + PDM.onReceive(onPDMdata); + + // Optionally set the gain + // Defaults to 20 on the BLE Sense and -10 on the Portenta Vision Shields + //PDM.setGain(30); + + // Initialize PDM with: + // - one channel (mono mode) + // - a 16 kHz sample rate for the Arduino Nano 33 BLE Sense + // - a 32 kHz or 64 kHz sample rate for the Arduino Portenta Vision Shields + if (!PDM.begin(channels, frequency)) { + Serial.println("Failed to start PDM!"); + while (1); + } +} + +volatile int err = 0; + +void loop() { + Serial.println("hello4"); + + //delay(1000); + + Serial.println("trying to create ggwave instance"); + //delay(1000); + auto p = GGWave::getDefaultParameters(); + p.sampleRateInp = frequency; + p.sampleFormatInp = GGWAVE_SAMPLE_FORMAT_I16; + p.payloadLength = 4; + GGWave instance(p); + + static GGWave::CBWaveformInp cbWaveformInp = [&](void * data, uint32_t nMaxBytes) { + if (2*qsize < nMaxBytes) { + return 0; + } + //Serial.println(nMaxBytes); + //Serial.println(qsize); + int nCopied = std::min((uint32_t) 2*qsize, nMaxBytes); + //Serial.println(qsize); + qsize -= nCopied / 2; + //Serial.println(nCopied); + //Serial.println("---------"); + for (int i = 0; i < nCopied/2; ++i) { + //if (i == 0) Serial.println(sampleBuffer[qhead]); + //data[i] = sampleBuffer[qhead]; + memcpy(((char *)data) + 2*i, (char *)(sampleBuffer + qhead), 2); + qhead = (qhead + 1) % qmax; + } + //std::copy((char *) sampleBuffer, ((char *) sampleBuffer) + nCopied, (char *) data); + return nCopied; + }; + + int nr = 0; + GGWave::TxRxData result; + while (true) { + //Serial.println(sampleBuffer[10]); + if (qsize >= 512) { + //Serial.println(sampleBuffer[10]); + //Serial.println(qsize); + instance.decode(cbWaveformInp); + nr = instance.takeRxData(result); + if (nr > 0) { + Serial.println(nr); + Serial.println((char *)result.data()); + } + //samplesRead = 0; + } + if (err > 0) { + Serial.println("ERRROR"); + Serial.println(err); + } + } +} + +/** + Callback function to process the data from the PDM microphone. +NOTE: This callback is executed as part of an ISR. +Therefore using `Serial` to print messages inside this function isn't supported. + * */ +void onPDMdata() { + // Query the number of available bytes + int bytesAvailable = PDM.available(); + int ns = bytesAvailable / 2; + if (qsize + ns > qmax) { + qhead = 0; + qtail = 0; + qsize = 0; + } + + // Read into the sample buffer + PDM.read(sampleBuffer + qtail, bytesAvailable); + + qtail += ns; + qsize += ns; + if (qtail > qmax) { + ++err; + } + if (qtail >= qmax) { + qtail -= qmax; + } +} diff --git a/examples/arduino-rx/ggwave.cpp b/examples/arduino-rx/ggwave.cpp new file mode 100644 index 0000000..90d983e --- /dev/null +++ b/examples/arduino-rx/ggwave.cpp @@ -0,0 +1,1436 @@ +#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.samplesPerFrame, + parameters.soundMarkerThreshold, + parameters.sampleFormatInp, + parameters.sampleFormatOut}); + + 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 + kBaseSampleRate, + kBaseSampleRate, + kDefaultSamplesPerFrame, + kDefaultSoundMarkerThreshold, + GGWAVE_SAMPLE_FORMAT_F32, + GGWAVE_SAMPLE_FORMAT_F32, + }; + + return result; +} + +GGWave::GGWave(const Parameters & parameters) : + m_sampleRateInp(parameters.sampleRateInp), + m_sampleRateOut(parameters.sampleRateOut), + 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(kBaseSampleRate/parameters.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), + // Rx + m_samplesNeeded(m_samplesPerFrame), + m_fftInp(kMaxSamplesPerFrame), + m_fftOut(2*kMaxSamplesPerFrame), + m_hasNewSpectrum(false), + m_hasNewAmplitude(false), + m_sampleSpectrum(kMaxSamplesPerFrame), + m_sampleAmplitude(kMaxSamplesPerFrame + 128), // small extra space because sometimes resampling needs a few more samples + m_sampleAmplitudeResampled(8*kMaxSamplesPerFrame), // min input sampling rate is 0.125*kBaseSampleRate + m_sampleAmplitudeTmp(8*kMaxSamplesPerFrame*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(kMaxSamplesPerFrame), + //m_sampleAmplitudeHistory(kMaxSpectrumHistory), + m_historyIdFixed(0), + // Tx + m_hasNewTxData(false), + m_sendVolume(0.1), + m_txDataLength(0), + m_txData(kMaxDataSize), + m_txDataEncoded(kMaxDataSize), + //m_outputBlock(kMaxSamplesPerFrame), + //m_outputBlockResampled(2*kMaxSamplesPerFrame), + //m_outputBlockTmp(kMaxRecordedFrames*kMaxSamplesPerFrame*m_sampleSizeBytesOut), + //m_outputBlockI16(kMaxRecordedFrames*kMaxSamplesPerFrame), + m_impl(new Impl()) { + + if (m_payloadLength > 0) { + // fixed payload length + if (m_payloadLength > kMaxLengthFixed) { + //throw std::runtime_error("Invalid payload legnth"); + 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*kMaxSamplesPerFrame); + } + + if (m_sampleSizeBytesInp == 0) { + //throw std::runtime_error("Invalid or unsupported capture sample format"); + return; + } + + if (m_sampleSizeBytesOut == 0) { + //throw std::runtime_error("Invalid or unsupported playback sample format"); + return; + } + + if (parameters.samplesPerFrame > kMaxSamplesPerFrame) { + //throw std::runtime_error("Invalid samples per frame"); + return; + } + + if (m_sampleRateInp < kSampleRateMin) { + ggprintf("Error: capture sample rate (%g Hz) must be >= %g Hz\n", m_sampleRateInp, kSampleRateMin); + //throw std::runtime_error("Invalid capture/playback sample rate"); + return; + } + + if (m_sampleRateInp > kSampleRateMax) { + ggprintf("Error: capture sample rate (%g Hz) must be <= %g Hz\n", m_sampleRateInp, kSampleRateMax); + //throw std::runtime_error("Invalid capture/playback sample rate"); + 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; + } + + 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_txDataEncoded.begin(), m_txDataEncoded.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 + 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(kMaxSamplesPerFrame); + // 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(kMaxSamplesPerFrame); + 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 != kBaseSampleRate) { + factor = kBaseSampleRate/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(kMaxSamplesPerFrame); + bit0Amplitude[k].resize(kMaxSamplesPerFrame); + + 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_txDataEncoded.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_txDataEncoded.data() + m_encodedDataOffset); + + float factor = kBaseSampleRate/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)/kBaseSampleRate; + 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_txDataEncoded[dataOffset + j] & 15; + dataBits[(2*j + 0)*16 + d] = 1; + } + { + uint8_t d = m_txDataEncoded[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)/kBaseSampleRate; + 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)/kBaseSampleRate; + 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 != kBaseSampleRate) { + 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/kBaseSampleRate; + uint32_t nBytesNeeded = m_samplesNeeded*m_sampleSizeBytesInp; + + if (m_sampleRateInp != kBaseSampleRate) { + // 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 != kBaseSampleRate) { + 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*kBaseSampleRate) { + 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_txDataEncoded.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_txDataEncoded[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_txDataEncoded.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_txDataEncoded.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; + + 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 (rxProtocolId == GGWAVE_TX_PROTOCOL_DT_FAST) { + // printf("detected = %d, needed = %d\n", txDetectedTotal, txNeededTotal); + //} + + 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_txDataEncoded[j] = (detectedBins[2*j + 1] << 4) + detectedBins[2*j + 0]; + } + + if (rsData.Decode(m_txDataEncoded.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 new file mode 100644 index 0000000..0066b45 --- /dev/null +++ b/examples/arduino-rx/ggwave/ggwave.h @@ -0,0 +1,574 @@ +#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; + + // 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 8000 and 96000. + // Default value: GGWave::kBaseSampleRate + // + // 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 + // + typedef struct { + int payloadLength; // payload length + float sampleRateInp; // capture sample rate + float sampleRateOut; // playback 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_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 kBaseSampleRate = 10000.0f; + static constexpr auto kSampleRateMin = 6000.0f; + static constexpr auto kSampleRateMax = 10000.0f; + static constexpr auto kDefaultSamplesPerFrame = 256; + 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 = 256; + 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 kBaseSampleRate 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 kBaseSampleRate 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_NORMAL; } + 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 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; + + // Rx + 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_hasNewTxData; + float m_sendVolume; + + int m_txDataLength; + TxRxData m_txData; + TxRxData m_txDataEncoded; + + 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 new file mode 100644 index 0000000..aaffd33 --- /dev/null +++ b/examples/arduino-rx/reed-solomon/LICENSE @@ -0,0 +1,21 @@ +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 new file mode 100644 index 0000000..14a2831 --- /dev/null +++ b/examples/arduino-rx/reed-solomon/gf.hpp @@ -0,0 +1,235 @@ +/* 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 new file mode 100644 index 0000000..9e1b65f --- /dev/null +++ b/examples/arduino-rx/reed-solomon/poly.hpp @@ -0,0 +1,94 @@ +/* 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 new file mode 100644 index 0000000..202cdd5 --- /dev/null +++ b/examples/arduino-rx/reed-solomon/rs.hpp @@ -0,0 +1,538 @@ +/* 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 new file mode 100644 index 0000000..38508b0 --- /dev/null +++ b/examples/arduino-rx/resampler.cpp @@ -0,0 +1,161 @@ +#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 new file mode 100644 index 0000000..87804c5 --- /dev/null +++ b/examples/arduino-rx/resampler.h @@ -0,0 +1,49 @@ +#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; +};