mirror of
https://github.com/ggerganov/ggwave.git
synced 2026-02-07 01:11:22 +08:00
1453 lines
50 KiB
C++
1453 lines
50 KiB
C++
#include "ggwave/ggwave.h"
|
|
|
|
#include "resampler.h"
|
|
|
|
#include "reed-solomon/rs.hpp"
|
|
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <algorithm>
|
|
#include <stdexcept>
|
|
#include <map>
|
|
//#include <random>
|
|
|
|
#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<ggwave_Instance, GGWave *> g_instances;
|
|
std::map<ggwave_Instance, GGWave::RxProtocols> g_rxProtocols;
|
|
}
|
|
|
|
extern "C"
|
|
void ggwave_setLogFile(void * fptr) {
|
|
GGWave::setLogFile((FILE *) fptr);
|
|
}
|
|
|
|
extern "C"
|
|
ggwave_Parameters ggwave_getDefaultParameters(void) {
|
|
return GGWave::getDefaultParameters();
|
|
}
|
|
|
|
extern "C"
|
|
ggwave_Instance ggwave_init(const ggwave_Parameters parameters) {
|
|
static ggwave_Instance curId = 0;
|
|
|
|
g_instances[curId] = new GGWave({
|
|
parameters.payloadLength,
|
|
parameters.sampleRateInp,
|
|
parameters.sampleRateOut,
|
|
parameters.sampleRate,
|
|
parameters.samplesPerFrame,
|
|
parameters.soundMarkerThreshold,
|
|
parameters.sampleFormatInp,
|
|
parameters.sampleFormatOut,
|
|
parameters.operatingMode});
|
|
|
|
return curId++;
|
|
}
|
|
|
|
extern "C"
|
|
void ggwave_free(ggwave_Instance instance) {
|
|
delete (GGWave *) g_instances[instance];
|
|
g_instances.erase(instance);
|
|
}
|
|
|
|
extern "C"
|
|
int ggwave_encode(
|
|
ggwave_Instance instance,
|
|
const char * dataBuffer,
|
|
int dataSize,
|
|
ggwave_TxProtocolId txProtocolId,
|
|
int volume,
|
|
char * outputBuffer,
|
|
int query) {
|
|
GGWave * ggWave = (GGWave *) g_instances[instance];
|
|
|
|
if (ggWave == nullptr) {
|
|
ggprintf("Invalid GGWave instance %d\n", instance);
|
|
return -1;
|
|
}
|
|
|
|
if (ggWave->init(dataSize, dataBuffer, ggWave->getTxProtocol(txProtocolId), volume) == false) {
|
|
ggprintf("Failed to initialize GGWave instance %d\n", instance);
|
|
return -1;
|
|
}
|
|
|
|
if (query != 0) {
|
|
if (query == 1) {
|
|
return ggWave->encodeSize_bytes();
|
|
}
|
|
|
|
return ggWave->encodeSize_samples();
|
|
}
|
|
|
|
int nSamples = 0;
|
|
|
|
GGWave::CBWaveformOut cbWaveformOut = [&](const void * data, uint32_t nBytes) {
|
|
char * p = (char *) data;
|
|
std::copy(p, p + nBytes, outputBuffer);
|
|
|
|
nSamples = nBytes/ggWave->getSampleSizeBytesOut();
|
|
};
|
|
|
|
if (ggWave->encode(cbWaveformOut) == false) {
|
|
ggprintf("Failed to encode data - GGWave instance %d\n", instance);
|
|
return -1;
|
|
}
|
|
|
|
return nSamples;
|
|
}
|
|
|
|
extern "C"
|
|
int ggwave_decode(
|
|
ggwave_Instance instance,
|
|
const char * dataBuffer,
|
|
int dataSize,
|
|
char * outputBuffer) {
|
|
GGWave * ggWave = (GGWave *) g_instances[instance];
|
|
|
|
GGWave::CBWaveformInp cbWaveformInp = [&](void * data, uint32_t nMaxBytes) -> uint32_t {
|
|
uint32_t nCopied = std::min((uint32_t) dataSize, nMaxBytes);
|
|
std::copy(dataBuffer, dataBuffer + nCopied, (char *) data);
|
|
|
|
dataSize -= nCopied;
|
|
dataBuffer += nCopied;
|
|
|
|
return nCopied;
|
|
};
|
|
|
|
ggWave->decode(cbWaveformInp);
|
|
|
|
// TODO : avoid allocation
|
|
GGWave::TxRxData rxData;
|
|
|
|
auto rxDataLength = ggWave->takeRxData(rxData);
|
|
if (rxDataLength == -1) {
|
|
// failed to decode message
|
|
return -1;
|
|
} else if (rxDataLength > 0) {
|
|
memcpy(outputBuffer, rxData.data(), rxDataLength);
|
|
}
|
|
|
|
return rxDataLength;
|
|
}
|
|
|
|
extern "C"
|
|
int ggwave_ndecode(
|
|
ggwave_Instance instance,
|
|
const char * dataBuffer,
|
|
int dataSize,
|
|
char * outputBuffer,
|
|
int outputSize) {
|
|
// TODO : avoid duplicated code
|
|
GGWave * ggWave = (GGWave *) g_instances[instance];
|
|
|
|
GGWave::CBWaveformInp cbWaveformInp = [&](void * data, uint32_t nMaxBytes) -> uint32_t {
|
|
uint32_t nCopied = std::min((uint32_t) dataSize, nMaxBytes);
|
|
std::copy(dataBuffer, dataBuffer + nCopied, (char *) data);
|
|
|
|
dataSize -= nCopied;
|
|
dataBuffer += nCopied;
|
|
|
|
return nCopied;
|
|
};
|
|
|
|
ggWave->decode(cbWaveformInp);
|
|
|
|
// TODO : avoid allocation
|
|
GGWave::TxRxData rxData;
|
|
|
|
auto rxDataLength = ggWave->takeRxData(rxData);
|
|
if (rxDataLength == -1) {
|
|
// failed to decode message
|
|
return -1;
|
|
} else if (rxDataLength > outputSize) {
|
|
// the outputBuffer is not big enough to store the data
|
|
return -2;
|
|
} else if (rxDataLength > 0) {
|
|
memcpy(outputBuffer, rxData.data(), rxDataLength);
|
|
}
|
|
|
|
return rxDataLength;
|
|
}
|
|
|
|
extern "C"
|
|
void ggwave_toggleRxProtocol(
|
|
ggwave_Instance instance,
|
|
ggwave_TxProtocolId rxProtocolId,
|
|
int state) {
|
|
// if never called - initialize with all available protocols
|
|
if (g_rxProtocols.find(instance) == g_rxProtocols.end()) {
|
|
g_rxProtocols[instance] = GGWave::getTxProtocols();
|
|
}
|
|
|
|
if (state == 0) {
|
|
// disable Rx protocol
|
|
g_rxProtocols[instance].erase(rxProtocolId);
|
|
} else if (state == 1) {
|
|
// enable Rx protocol
|
|
g_rxProtocols[instance][rxProtocolId] = GGWave::getTxProtocols().at(rxProtocolId);
|
|
}
|
|
|
|
g_instances[instance]->setRxProtocols(g_rxProtocols[instance]);
|
|
}
|
|
|
|
//
|
|
// C++ implementation
|
|
//
|
|
|
|
namespace {
|
|
|
|
// FFT routines taken from https://stackoverflow.com/a/37729648/4039976
|
|
|
|
int log2(int N) {
|
|
int k = N, i = 0;
|
|
while(k) {
|
|
k >>= 1;
|
|
i++;
|
|
}
|
|
return i - 1;
|
|
}
|
|
|
|
int reverse(int N, int n) {
|
|
int j, p = 0;
|
|
for(j = 1; j <= log2(N); j++) {
|
|
if(n & (1 << (log2(N) - j)))
|
|
p |= 1 << (j - 1);
|
|
}
|
|
return p;
|
|
}
|
|
|
|
void ordina(float * f1, int N) {
|
|
static thread_local float f2[2*GGWave::kMaxSamplesPerFrame];
|
|
for (int i = 0; i < N; i++) {
|
|
int ir = reverse(N, i);
|
|
f2[2*i + 0] = f1[2*ir + 0];
|
|
f2[2*i + 1] = f1[2*ir + 1];
|
|
}
|
|
for (int j = 0; j < N; j++) {
|
|
f1[2*j + 0] = f2[2*j + 0];
|
|
f1[2*j + 1] = f2[2*j + 1];
|
|
}
|
|
}
|
|
|
|
void transform(float * f, int N) {
|
|
ordina(f, N); //first: reverse order
|
|
float * W;
|
|
W = (float *)malloc(N*sizeof(float));
|
|
W[2*1 + 0] = cos(-2.*M_PI/N);
|
|
W[2*1 + 1] = sin(-2.*M_PI/N);
|
|
W[2*0 + 0] = 1;
|
|
W[2*0 + 1] = 0;
|
|
for (int i = 2; i < N / 2; i++) {
|
|
W[2*i + 0] = cos(-2.*i*M_PI/N);
|
|
W[2*i + 1] = sin(-2.*i*M_PI/N);
|
|
}
|
|
int n = 1;
|
|
int a = N / 2;
|
|
for(int j = 0; j < log2(N); j++) {
|
|
for(int i = 0; i < N; i++) {
|
|
if(!(i & n)) {
|
|
int wi = (i * a) % (n * a);
|
|
int fi = i + n;
|
|
float a = W[2*wi + 0];
|
|
float b = W[2*wi + 1];
|
|
float c = f[2*fi + 0];
|
|
float d = f[2*fi + 1];
|
|
float temp[2] = { f[2*i + 0], f[2*i + 1] };
|
|
float Temp[2] = { a*c - b*d, b*c + a*d };
|
|
f[2*i + 0] = temp[0] + Temp[0];
|
|
f[2*i + 1] = temp[1] + Temp[1];
|
|
f[2*fi + 0] = temp[0] - Temp[0];
|
|
f[2*fi + 1] = temp[1] - Temp[1];
|
|
}
|
|
}
|
|
n *= 2;
|
|
a = a / 2;
|
|
}
|
|
free(W);
|
|
}
|
|
|
|
void FFT(float * f, int N, float d) {
|
|
transform(f, N);
|
|
for (int i = 0; i < N; i++) {
|
|
f[2*i + 0] *= d;
|
|
f[2*i + 1] *= d;
|
|
}
|
|
}
|
|
|
|
void FFT(const float * src, float * dst, int N, float d) {
|
|
for (int i = 0; i < N; ++i) {
|
|
dst[2*i + 0] = src[i];
|
|
dst[2*i + 1] = 0.0f;
|
|
}
|
|
FFT(dst, N, d);
|
|
}
|
|
|
|
inline void addAmplitudeSmooth(
|
|
const GGWave::AmplitudeData & src,
|
|
GGWave::AmplitudeData & dst,
|
|
float scalar, int startId, int finalId, int cycleMod, int nPerCycle) {
|
|
int nTotal = nPerCycle*finalId;
|
|
float frac = 0.15f;
|
|
float ds = frac*nTotal;
|
|
float ids = 1.0f/ds;
|
|
int nBegin = frac*nTotal;
|
|
int nEnd = (1.0f - frac)*nTotal;
|
|
for (int i = startId; i < finalId; i++) {
|
|
float k = cycleMod*finalId + i;
|
|
if (k < nBegin) {
|
|
dst[i] += scalar*src[i]*(k*ids);
|
|
} else if (k > nEnd) {
|
|
dst[i] += scalar*src[i]*(((float)(nTotal) - k)*ids);
|
|
} else {
|
|
dst[i] += scalar*src[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
template <class T>
|
|
float getTime_ms(const T & tStart, const T & tEnd) {
|
|
return ((float)(std::chrono::duration_cast<std::chrono::microseconds>(tEnd - tStart).count()))/1000.0;
|
|
}
|
|
|
|
int getECCBytesForLength(int len) {
|
|
return len < 4 ? 2 : std::max(4, 2*(len/5));
|
|
}
|
|
|
|
int bytesForSampleFormat(GGWave::SampleFormat sampleFormat) {
|
|
switch (sampleFormat) {
|
|
case GGWAVE_SAMPLE_FORMAT_UNDEFINED: return 0; break;
|
|
case GGWAVE_SAMPLE_FORMAT_U8: return sizeof(uint8_t); break;
|
|
case GGWAVE_SAMPLE_FORMAT_I8: return sizeof(int8_t); break;
|
|
case GGWAVE_SAMPLE_FORMAT_U16: return sizeof(uint16_t); break;
|
|
case GGWAVE_SAMPLE_FORMAT_I16: return sizeof(int16_t); break;
|
|
case GGWAVE_SAMPLE_FORMAT_F32: return sizeof(float); break;
|
|
};
|
|
|
|
ggprintf("Invalid sample format: %d\n", (int) sampleFormat);
|
|
|
|
return 0;
|
|
}
|
|
|
|
}
|
|
|
|
struct GGWave::Impl {
|
|
Resampler resampler;
|
|
};
|
|
|
|
void GGWave::setLogFile(FILE * fptr) {
|
|
g_fptr = fptr;
|
|
}
|
|
|
|
const GGWave::Parameters & GGWave::getDefaultParameters() {
|
|
static ggwave_Parameters result {
|
|
-1, // vaiable payload length
|
|
kDefaultSampleRate,
|
|
kDefaultSampleRate,
|
|
kDefaultSampleRate,
|
|
kDefaultSamplesPerFrame,
|
|
kDefaultSoundMarkerThreshold,
|
|
GGWAVE_SAMPLE_FORMAT_F32,
|
|
GGWAVE_SAMPLE_FORMAT_F32,
|
|
GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX,
|
|
};
|
|
|
|
return result;
|
|
}
|
|
|
|
GGWave::GGWave(const Parameters & parameters) :
|
|
m_sampleRateInp (parameters.sampleRateInp),
|
|
m_sampleRateOut (parameters.sampleRateOut),
|
|
m_sampleRate (parameters.sampleRate),
|
|
m_samplesPerFrame (parameters.samplesPerFrame),
|
|
m_isamplesPerFrame (1.0f/m_samplesPerFrame),
|
|
m_sampleSizeBytesInp (bytesForSampleFormat(parameters.sampleFormatInp)),
|
|
m_sampleSizeBytesOut (bytesForSampleFormat(parameters.sampleFormatOut)),
|
|
m_sampleFormatInp (parameters.sampleFormatInp),
|
|
m_sampleFormatOut (parameters.sampleFormatOut),
|
|
m_hzPerSample (m_sampleRate/m_samplesPerFrame),
|
|
m_ihzPerSample (1.0f/m_hzPerSample),
|
|
m_freqDelta_bin (1),
|
|
m_freqDelta_hz (2*m_hzPerSample),
|
|
m_nBitsInMarker (16),
|
|
m_nMarkerFrames (parameters.payloadLength > 0 ? 0 : kDefaultMarkerFrames),
|
|
m_encodedDataOffset (parameters.payloadLength > 0 ? 0 : kDefaultEncodedDataOffset),
|
|
m_soundMarkerThreshold(parameters.soundMarkerThreshold),
|
|
|
|
// common
|
|
m_isFixedPayloadLength(parameters.payloadLength > 0),
|
|
m_payloadLength (parameters.payloadLength),
|
|
m_dataEncoded (kMaxDataSize),
|
|
|
|
// Rx
|
|
m_isRxEnabled(parameters.operatingMode == GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX ||
|
|
parameters.operatingMode == GGWAVE_OPERATING_MODE_ONLY_RX),
|
|
|
|
m_samplesNeeded (m_samplesPerFrame),
|
|
m_fftInp (m_samplesPerFrame),
|
|
m_fftOut (2*m_samplesPerFrame),
|
|
m_hasNewSpectrum (false),
|
|
m_hasNewAmplitude (false),
|
|
m_sampleSpectrum (m_samplesPerFrame),
|
|
m_sampleAmplitude (m_sampleRateInp == m_sampleRate ? m_samplesPerFrame : m_samplesPerFrame + 128), // small extra space because sometimes resampling needs a few more samples
|
|
m_sampleAmplitudeResampled(m_sampleRateInp == m_sampleRate ? m_samplesPerFrame : 8*m_samplesPerFrame), // min input sampling rate is 0.125*m_sampleRate
|
|
m_sampleAmplitudeTmp (m_sampleRateInp == m_sampleRate ? m_samplesPerFrame*m_sampleSizeBytesInp : 8*m_samplesPerFrame*m_sampleSizeBytesInp),
|
|
m_hasNewRxData (false),
|
|
m_lastRxDataLength (0),
|
|
m_rxData (kMaxDataSize),
|
|
m_rxProtocol (getDefaultTxProtocol()),
|
|
m_rxProtocolId (getDefaultTxProtocolId()),
|
|
m_rxProtocols (getTxProtocols()),
|
|
m_historyId (0),
|
|
m_sampleAmplitudeAverage (parameters.payloadLength > 0 ? 0 : m_samplesPerFrame),
|
|
m_sampleAmplitudeHistory (parameters.payloadLength > 0 ? 0 : kMaxSpectrumHistory),
|
|
m_historyIdFixed (0),
|
|
|
|
// Tx
|
|
m_isTxEnabled(parameters.operatingMode == GGWAVE_OPERATING_MODE_BOTH_RX_AND_TX),
|
|
|
|
m_hasNewTxData (false),
|
|
m_sendVolume (0.1),
|
|
m_txDataLength (0),
|
|
m_txData (m_isTxEnabled ? kMaxDataSize : 0),
|
|
m_outputBlock (m_isTxEnabled ? m_samplesPerFrame : 0),
|
|
m_outputBlockResampled(m_isTxEnabled ? 2*m_samplesPerFrame : 0),
|
|
m_outputBlockTmp (m_isTxEnabled ? kMaxRecordedFrames*m_samplesPerFrame*m_sampleSizeBytesOut : 0),
|
|
m_outputBlockI16 (m_isTxEnabled ? kMaxRecordedFrames*m_samplesPerFrame : 0),
|
|
m_impl(new Impl()) {
|
|
|
|
if (m_payloadLength > 0) {
|
|
// fixed payload length
|
|
if (m_payloadLength > kMaxLengthFixed) {
|
|
ggprintf("Invalid payload legnth: %d, max: %d\n", m_payloadLength, kMaxLengthFixed);
|
|
return;
|
|
}
|
|
|
|
m_txDataLength = m_payloadLength;
|
|
|
|
int totalLength = m_txDataLength + getECCBytesForLength(m_txDataLength);
|
|
int totalTxs = (totalLength + minBytesPerTx() - 1)/minBytesPerTx();
|
|
|
|
m_spectrumHistoryFixed.resize(totalTxs*maxFramesPerTx());
|
|
} else {
|
|
// variable payload length
|
|
m_recordedAmplitude.resize(kMaxRecordedFrames*m_samplesPerFrame);
|
|
}
|
|
|
|
if (m_sampleSizeBytesInp == 0) {
|
|
ggprintf("Invalid or unsupported capture sample format: %d\n", (int) parameters.sampleFormatInp);
|
|
return;
|
|
}
|
|
|
|
if (m_sampleSizeBytesOut == 0) {
|
|
ggprintf("Invalid or unsupported playback sample format: %d\n", (int) parameters.sampleFormatOut);
|
|
return;
|
|
}
|
|
|
|
if (parameters.samplesPerFrame > kMaxSamplesPerFrame) {
|
|
ggprintf("Invalid samples per frame: %d, max: %d\n", parameters.samplesPerFrame, kMaxSamplesPerFrame);
|
|
return;
|
|
}
|
|
|
|
if (m_sampleRateInp < kSampleRateMin) {
|
|
ggprintf("Error: capture sample rate (%g Hz) must be >= %g Hz\n", m_sampleRateInp, kSampleRateMin);
|
|
return;
|
|
}
|
|
|
|
if (m_sampleRateInp > kSampleRateMax) {
|
|
ggprintf("Error: capture sample rate (%g Hz) must be <= %g Hz\n", m_sampleRateInp, kSampleRateMax);
|
|
return;
|
|
}
|
|
|
|
init("", getDefaultTxProtocol(), 0);
|
|
}
|
|
|
|
GGWave::~GGWave() {
|
|
}
|
|
|
|
bool GGWave::init(const std::string & text, const int volume) {
|
|
return init((int) text.size(), text.data(), getDefaultTxProtocol(), volume);
|
|
}
|
|
|
|
bool GGWave::init(const std::string & text, const TxProtocol & txProtocol, const int volume) {
|
|
return init((int) text.size(), text.data(), txProtocol, volume);
|
|
}
|
|
|
|
bool GGWave::init(int dataSize, const char * dataBuffer, const int volume) {
|
|
return init(dataSize, dataBuffer, getDefaultTxProtocol(), volume);
|
|
}
|
|
|
|
bool GGWave::init(int dataSize, const char * dataBuffer, const TxProtocol & txProtocol, const int volume) {
|
|
if (dataSize < 0) {
|
|
ggprintf("Negative data size: %d\n", dataSize);
|
|
return false;
|
|
}
|
|
|
|
auto maxLength = m_isFixedPayloadLength ? m_payloadLength : kMaxLengthVarible;
|
|
if (dataSize > maxLength) {
|
|
ggprintf("Truncating data from %d to %d bytes\n", dataSize, maxLength);
|
|
dataSize = maxLength;
|
|
}
|
|
|
|
if (volume < 0 || volume > 100) {
|
|
ggprintf("Invalid volume: %d\n", volume);
|
|
return false;
|
|
}
|
|
|
|
// Tx
|
|
if (m_isTxEnabled) {
|
|
m_txProtocol = txProtocol;
|
|
m_txDataLength = dataSize;
|
|
m_sendVolume = ((double)(volume))/100.0f;
|
|
|
|
const uint8_t * text = reinterpret_cast<const uint8_t *>(dataBuffer);
|
|
|
|
m_hasNewTxData = false;
|
|
std::fill(m_txData.begin(), m_txData.end(), 0);
|
|
std::fill(m_dataEncoded.begin(), m_dataEncoded.end(), 0);
|
|
|
|
if (m_txDataLength > 0) {
|
|
m_txData[0] = m_txDataLength;
|
|
for (int i = 0; i < m_txDataLength; ++i) m_txData[i + 1] = text[i];
|
|
|
|
m_hasNewTxData = true;
|
|
}
|
|
|
|
if (m_isFixedPayloadLength) {
|
|
m_txDataLength = m_payloadLength;
|
|
}
|
|
}
|
|
|
|
// Rx
|
|
if (m_isRxEnabled) {
|
|
m_receivingData = false;
|
|
m_analyzingData = false;
|
|
|
|
m_framesToAnalyze = 0;
|
|
m_framesLeftToAnalyze = 0;
|
|
m_framesToRecord = 0;
|
|
m_framesLeftToRecord = 0;
|
|
|
|
std::fill(m_sampleSpectrum.begin(), m_sampleSpectrum.end(), 0);
|
|
std::fill(m_sampleAmplitude.begin(), m_sampleAmplitude.end(), 0);
|
|
for (auto & s : m_sampleAmplitudeHistory) {
|
|
s.resize(m_samplesPerFrame);
|
|
std::fill(s.begin(), s.end(), 0);
|
|
}
|
|
|
|
std::fill(m_rxData.begin(), m_rxData.end(), 0);
|
|
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_fftOut[2*i + 0] = 0.0f;
|
|
m_fftOut[2*i + 1] = 0.0f;
|
|
}
|
|
|
|
for (auto & s : m_spectrumHistoryFixed) {
|
|
s.resize(m_samplesPerFrame);
|
|
std::fill(s.begin(), s.end(), 0);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
uint32_t GGWave::encodeSize_bytes() const {
|
|
return encodeSize_samples()*m_sampleSizeBytesOut;
|
|
}
|
|
|
|
uint32_t GGWave::encodeSize_samples() const {
|
|
if (m_hasNewTxData == false) {
|
|
return 0;
|
|
}
|
|
|
|
float factor = 1.0f;
|
|
int samplesPerFrameOut = m_samplesPerFrame;
|
|
if (m_sampleRateOut != m_sampleRate) {
|
|
factor = m_sampleRate/m_sampleRateOut;
|
|
// note : +1 extra sample in order to overestimate the buffer size
|
|
samplesPerFrameOut = m_impl->resampler.resample(factor, m_samplesPerFrame, m_outputBlock.data(), nullptr) + 1;
|
|
}
|
|
int nECCBytesPerTx = getECCBytesForLength(m_txDataLength);
|
|
int sendDataLength = m_txDataLength + m_encodedDataOffset;
|
|
int totalBytes = sendDataLength + nECCBytesPerTx;
|
|
int totalDataFrames = ((totalBytes + m_txProtocol.bytesPerTx - 1)/m_txProtocol.bytesPerTx)*m_txProtocol.framesPerTx;
|
|
|
|
return (
|
|
m_nMarkerFrames + totalDataFrames + m_nMarkerFrames
|
|
)*samplesPerFrameOut;
|
|
}
|
|
|
|
bool GGWave::encode(const CBWaveformOut & cbWaveformOut) {
|
|
int frameId = 0;
|
|
|
|
m_impl->resampler.reset();
|
|
|
|
std::vector<double> 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<bool> dataBits(kMaxDataBits);
|
|
|
|
std::vector<AmplitudeData> bit1Amplitude(kMaxDataBits);
|
|
std::vector<AmplitudeData> bit0Amplitude(kMaxDataBits);
|
|
|
|
for (int k = 0; k < (int) dataBits.size(); ++k) {
|
|
double freq = bitFreq(m_txProtocol, k);
|
|
|
|
bit1Amplitude[k].resize(m_samplesPerFrame);
|
|
bit0Amplitude[k].resize(m_samplesPerFrame);
|
|
|
|
double phaseOffset = phaseOffsets[k];
|
|
double curHzPerSample = m_hzPerSample;
|
|
double curIHzPerSample = 1.0/curHzPerSample;
|
|
for (int i = 0; i < m_samplesPerFrame; i++) {
|
|
double curi = i;
|
|
bit1Amplitude[k][i] = std::sin((2.0*M_PI)*(curi*m_isamplesPerFrame)*(freq*curIHzPerSample) + phaseOffset);
|
|
}
|
|
for (int i = 0; i < m_samplesPerFrame; i++) {
|
|
double curi = i;
|
|
bit0Amplitude[k][i] = std::sin((2.0*M_PI)*(curi*m_isamplesPerFrame)*((freq + m_hzPerSample*m_freqDelta_bin)*curIHzPerSample) + phaseOffset);
|
|
}
|
|
}
|
|
|
|
int nECCBytesPerTx = getECCBytesForLength(m_txDataLength);
|
|
int sendDataLength = m_txDataLength + m_encodedDataOffset;
|
|
int totalBytes = sendDataLength + nECCBytesPerTx;
|
|
int totalDataFrames = ((totalBytes + m_txProtocol.bytesPerTx - 1)/m_txProtocol.bytesPerTx)*m_txProtocol.framesPerTx;
|
|
|
|
if (m_isFixedPayloadLength == false) {
|
|
RS::ReedSolomon rsLength(1, m_encodedDataOffset - 1);
|
|
rsLength.Encode(m_txData.data(), m_dataEncoded.data());
|
|
}
|
|
|
|
// first byte of m_txData contains the length of the payload, so we skip it:
|
|
RS::ReedSolomon rsData = RS::ReedSolomon(m_txDataLength, nECCBytesPerTx);
|
|
rsData.Encode(m_txData.data() + 1, m_dataEncoded.data() + m_encodedDataOffset);
|
|
|
|
const float factor = m_sampleRate/m_sampleRateOut;
|
|
|
|
uint32_t offset = 0;
|
|
m_waveformTones.clear();
|
|
|
|
while (m_hasNewTxData) {
|
|
std::fill(m_outputBlock.begin(), m_outputBlock.end(), 0.0f);
|
|
|
|
std::uint16_t nFreq = 0;
|
|
m_waveformTones.push_back({});
|
|
|
|
if (frameId < m_nMarkerFrames) {
|
|
nFreq = m_nBitsInMarker;
|
|
|
|
for (int i = 0; i < m_nBitsInMarker; ++i) {
|
|
m_waveformTones.back().push_back({});
|
|
m_waveformTones.back().back().duration_ms = (1000.0*m_samplesPerFrame)/m_sampleRate;
|
|
if (i%2 == 0) {
|
|
::addAmplitudeSmooth(bit1Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, frameId, m_nMarkerFrames);
|
|
m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i);
|
|
} else {
|
|
::addAmplitudeSmooth(bit0Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, frameId, m_nMarkerFrames);
|
|
m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i) + m_hzPerSample;
|
|
}
|
|
}
|
|
} else if (frameId < m_nMarkerFrames + totalDataFrames) {
|
|
int dataOffset = frameId - m_nMarkerFrames;
|
|
int cycleModMain = dataOffset%m_txProtocol.framesPerTx;
|
|
dataOffset /= m_txProtocol.framesPerTx;
|
|
dataOffset *= m_txProtocol.bytesPerTx;
|
|
|
|
std::fill(dataBits.begin(), dataBits.end(), 0);
|
|
|
|
for (int j = 0; j < m_txProtocol.bytesPerTx; ++j) {
|
|
{
|
|
uint8_t d = m_dataEncoded[dataOffset + j] & 15;
|
|
dataBits[(2*j + 0)*16 + d] = 1;
|
|
}
|
|
{
|
|
uint8_t d = m_dataEncoded[dataOffset + j] & 240;
|
|
dataBits[(2*j + 1)*16 + (d >> 4)] = 1;
|
|
}
|
|
}
|
|
|
|
for (int k = 0; k < 2*m_txProtocol.bytesPerTx*16; ++k) {
|
|
if (dataBits[k] == 0) continue;
|
|
|
|
++nFreq;
|
|
m_waveformTones.back().push_back({});
|
|
m_waveformTones.back().back().duration_ms = (1000.0*m_samplesPerFrame)/m_sampleRate;
|
|
if (k%2) {
|
|
::addAmplitudeSmooth(bit0Amplitude[k/2], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, cycleModMain, m_txProtocol.framesPerTx);
|
|
m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, k/2) + m_hzPerSample;
|
|
} else {
|
|
::addAmplitudeSmooth(bit1Amplitude[k/2], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, cycleModMain, m_txProtocol.framesPerTx);
|
|
m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, k/2);
|
|
}
|
|
}
|
|
} else if (frameId < m_nMarkerFrames + totalDataFrames + m_nMarkerFrames) {
|
|
nFreq = m_nBitsInMarker;
|
|
|
|
int fId = frameId - (m_nMarkerFrames + totalDataFrames);
|
|
for (int i = 0; i < m_nBitsInMarker; ++i) {
|
|
m_waveformTones.back().push_back({});
|
|
m_waveformTones.back().back().duration_ms = (1000.0*m_samplesPerFrame)/m_sampleRate;
|
|
if (i%2 == 0) {
|
|
addAmplitudeSmooth(bit0Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, fId, m_nMarkerFrames);
|
|
m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i) + m_hzPerSample;
|
|
} else {
|
|
addAmplitudeSmooth(bit1Amplitude[i], m_outputBlock, m_sendVolume, 0, m_samplesPerFrame, fId, m_nMarkerFrames);
|
|
m_waveformTones.back().back().freq_hz = bitFreq(m_txProtocol, i);
|
|
}
|
|
}
|
|
} else {
|
|
m_hasNewTxData = false;
|
|
break;
|
|
}
|
|
|
|
if (nFreq == 0) nFreq = 1;
|
|
float scale = 1.0f/nFreq;
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_outputBlock[i] *= scale;
|
|
}
|
|
|
|
int samplesPerFrameOut = m_samplesPerFrame;
|
|
if (m_sampleRateOut != m_sampleRate) {
|
|
samplesPerFrameOut = m_impl->resampler.resample(factor, m_samplesPerFrame, m_outputBlock.data(), m_outputBlockResampled.data());
|
|
} else {
|
|
m_outputBlockResampled = m_outputBlock;
|
|
}
|
|
|
|
// default output is in 16-bit signed int so we always compute it
|
|
for (int i = 0; i < samplesPerFrameOut; ++i) {
|
|
m_outputBlockI16[offset + i] = 32768*m_outputBlockResampled[i];
|
|
}
|
|
|
|
// convert from 32-bit float
|
|
switch (m_sampleFormatOut) {
|
|
case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break;
|
|
case GGWAVE_SAMPLE_FORMAT_U8:
|
|
{
|
|
auto p = reinterpret_cast<uint8_t *>(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<uint8_t *>(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<uint16_t *>(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<uint16_t *>(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<float *>(m_outputBlockTmp.data());
|
|
for (int i = 0; i < samplesPerFrameOut; ++i) {
|
|
p[offset + i] = m_outputBlockResampled[i];
|
|
}
|
|
} break;
|
|
}
|
|
|
|
++frameId;
|
|
offset += samplesPerFrameOut;
|
|
}
|
|
|
|
switch (m_sampleFormatOut) {
|
|
case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break;
|
|
case GGWAVE_SAMPLE_FORMAT_I16:
|
|
{
|
|
cbWaveformOut(m_outputBlockI16.data(), offset*m_sampleSizeBytesOut);
|
|
} break;
|
|
case GGWAVE_SAMPLE_FORMAT_U8:
|
|
case GGWAVE_SAMPLE_FORMAT_I8:
|
|
case GGWAVE_SAMPLE_FORMAT_U16:
|
|
case GGWAVE_SAMPLE_FORMAT_F32:
|
|
{
|
|
cbWaveformOut(m_outputBlockTmp.data(), offset*m_sampleSizeBytesOut);
|
|
} break;
|
|
}
|
|
|
|
m_txAmplitudeDataI16.resize(offset);
|
|
for (uint32_t i = 0; i < offset; ++i) {
|
|
m_txAmplitudeDataI16[i] = m_outputBlockI16[i];
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
void GGWave::decode(const CBWaveformInp & cbWaveformInp) {
|
|
while (m_hasNewTxData == false) {
|
|
// read capture data
|
|
float factor = m_sampleRateInp/m_sampleRate;
|
|
uint32_t nBytesNeeded = m_samplesNeeded*m_sampleSizeBytesInp;
|
|
|
|
if (m_sampleRateInp != m_sampleRate) {
|
|
// note : predict 4 extra samples just to make sure we have enough data
|
|
nBytesNeeded = (m_impl->resampler.resample(1.0f/factor, m_samplesNeeded, m_sampleAmplitudeResampled.data(), nullptr) + 4)*m_sampleSizeBytesInp;
|
|
}
|
|
|
|
uint32_t nBytesRecorded = 0;
|
|
|
|
switch (m_sampleFormatInp) {
|
|
case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break;
|
|
case GGWAVE_SAMPLE_FORMAT_U8:
|
|
case GGWAVE_SAMPLE_FORMAT_I8:
|
|
case GGWAVE_SAMPLE_FORMAT_U16:
|
|
case GGWAVE_SAMPLE_FORMAT_I16:
|
|
{
|
|
nBytesRecorded = cbWaveformInp(m_sampleAmplitudeTmp.data(), nBytesNeeded);
|
|
} break;
|
|
case GGWAVE_SAMPLE_FORMAT_F32:
|
|
{
|
|
nBytesRecorded = cbWaveformInp(m_sampleAmplitudeResampled.data(), nBytesNeeded);
|
|
} break;
|
|
}
|
|
|
|
if (nBytesRecorded % m_sampleSizeBytesInp != 0) {
|
|
ggprintf("Failure during capture - provided bytes (%d) are not multiple of sample size (%d)\n",
|
|
nBytesRecorded, m_sampleSizeBytesInp);
|
|
m_samplesNeeded = m_samplesPerFrame;
|
|
break;
|
|
}
|
|
|
|
if (nBytesRecorded > nBytesNeeded) {
|
|
ggprintf("Failure during capture - more samples were provided (%d) than requested (%d)\n",
|
|
nBytesRecorded/m_sampleSizeBytesInp, nBytesNeeded/m_sampleSizeBytesInp);
|
|
m_samplesNeeded = m_samplesPerFrame;
|
|
break;
|
|
}
|
|
|
|
// convert to 32-bit float
|
|
int nSamplesRecorded = nBytesRecorded/m_sampleSizeBytesInp;
|
|
switch (m_sampleFormatInp) {
|
|
case GGWAVE_SAMPLE_FORMAT_UNDEFINED: break;
|
|
case GGWAVE_SAMPLE_FORMAT_U8:
|
|
{
|
|
constexpr float scale = 1.0f/128;
|
|
auto p = reinterpret_cast<uint8_t *>(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<int8_t *>(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<uint16_t *>(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<int16_t *>(m_sampleAmplitudeTmp.data());
|
|
for (int i = 0; i < nSamplesRecorded; ++i) {
|
|
m_sampleAmplitudeResampled[i] = float(*(p + i))*scale;
|
|
}
|
|
} break;
|
|
case GGWAVE_SAMPLE_FORMAT_F32: break;
|
|
}
|
|
|
|
if (nSamplesRecorded == 0) {
|
|
break;
|
|
}
|
|
|
|
uint32_t offset = m_samplesPerFrame - m_samplesNeeded;
|
|
|
|
if (m_sampleRateInp != m_sampleRate) {
|
|
if (nSamplesRecorded <= 2*Resampler::kWidth) {
|
|
m_samplesNeeded = m_samplesPerFrame;
|
|
break;
|
|
}
|
|
|
|
// reset resampler state every minute
|
|
if (!m_receivingData && m_impl->resampler.nSamplesTotal() > 60.0f*factor*m_sampleRate) {
|
|
m_impl->resampler.reset();
|
|
}
|
|
|
|
int nSamplesResampled = offset + m_impl->resampler.resample(factor, nSamplesRecorded, m_sampleAmplitudeResampled.data(), m_sampleAmplitude.data() + offset);
|
|
nSamplesRecorded = nSamplesResampled;
|
|
} else {
|
|
for (int i = 0; i < nSamplesRecorded; ++i) {
|
|
m_sampleAmplitude[offset + i] = m_sampleAmplitudeResampled[i];
|
|
}
|
|
}
|
|
|
|
// we have enough bytes to do analysis
|
|
if (nSamplesRecorded >= m_samplesPerFrame) {
|
|
m_hasNewAmplitude = true;
|
|
|
|
if (m_isFixedPayloadLength) {
|
|
decode_fixed();
|
|
} else {
|
|
decode_variable();
|
|
}
|
|
|
|
int nExtraSamples = nSamplesRecorded - m_samplesPerFrame;
|
|
for (int i = 0; i < nExtraSamples; ++i) {
|
|
m_sampleAmplitude[i] = m_sampleAmplitude[m_samplesPerFrame + i];
|
|
}
|
|
|
|
m_samplesNeeded = m_samplesPerFrame - nExtraSamples;
|
|
} else {
|
|
m_samplesNeeded = m_samplesPerFrame - nSamplesRecorded;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool GGWave::takeTxAmplitudeI16(AmplitudeDataI16 & dst) {
|
|
if (m_txAmplitudeDataI16.size() == 0) return false;
|
|
|
|
dst = std::move(m_txAmplitudeDataI16);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool GGWave::stopReceiving() {
|
|
if (m_receivingData == false) {
|
|
return false;
|
|
}
|
|
|
|
m_receivingData = false;
|
|
|
|
return true;
|
|
}
|
|
|
|
int GGWave::takeRxData(TxRxData & dst) {
|
|
if (m_lastRxDataLength == 0) return 0;
|
|
|
|
auto res = m_lastRxDataLength;
|
|
m_lastRxDataLength = 0;
|
|
|
|
if (res != -1) {
|
|
dst = m_rxData;
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
bool GGWave::takeRxSpectrum(SpectrumData & dst) {
|
|
if (m_hasNewSpectrum == false) return false;
|
|
|
|
m_hasNewSpectrum = false;
|
|
dst = m_sampleSpectrum;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool GGWave::takeRxAmplitude(AmplitudeData & dst) {
|
|
if (m_hasNewAmplitude == false) return false;
|
|
|
|
m_hasNewAmplitude = false;
|
|
dst = m_sampleAmplitude;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool GGWave::computeFFTR(const float * src, float * dst, int N, float d) {
|
|
if (N > kMaxSamplesPerFrame) {
|
|
ggprintf("computeFFTR: N (%d) must be <= %d\n", N, GGWave::kMaxSamplesPerFrame);
|
|
return false;
|
|
}
|
|
|
|
FFT(src, dst, N, d);
|
|
|
|
return true;
|
|
}
|
|
|
|
//
|
|
// Variable payload length
|
|
//
|
|
|
|
void GGWave::decode_variable() {
|
|
m_sampleAmplitudeHistory[m_historyId] = m_sampleAmplitude;
|
|
|
|
if (++m_historyId >= kMaxSpectrumHistory) {
|
|
m_historyId = 0;
|
|
}
|
|
|
|
if (m_historyId == 0 || m_receivingData) {
|
|
m_hasNewSpectrum = true;
|
|
|
|
std::fill(m_sampleAmplitudeAverage.begin(), m_sampleAmplitudeAverage.end(), 0.0f);
|
|
for (auto & s : m_sampleAmplitudeHistory) {
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_sampleAmplitudeAverage[i] += s[i];
|
|
}
|
|
}
|
|
|
|
float norm = 1.0f/kMaxSpectrumHistory;
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_sampleAmplitudeAverage[i] *= norm;
|
|
}
|
|
|
|
// calculate spectrum
|
|
FFT(m_sampleAmplitudeAverage.data(), m_fftOut.data(), m_samplesPerFrame, 1.0);
|
|
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_sampleSpectrum[i] = (m_fftOut[2*i + 0]*m_fftOut[2*i + 0] + m_fftOut[2*i + 1]*m_fftOut[2*i + 1]);
|
|
}
|
|
for (int i = 1; i < m_samplesPerFrame/2; ++i) {
|
|
m_sampleSpectrum[i] += m_sampleSpectrum[m_samplesPerFrame - i];
|
|
}
|
|
}
|
|
|
|
if (m_framesLeftToRecord > 0) {
|
|
std::copy(m_sampleAmplitude.begin(),
|
|
m_sampleAmplitude.begin() + m_samplesPerFrame,
|
|
m_recordedAmplitude.data() + (m_framesToRecord - m_framesLeftToRecord)*m_samplesPerFrame);
|
|
|
|
if (--m_framesLeftToRecord <= 0) {
|
|
m_analyzingData = true;
|
|
}
|
|
}
|
|
|
|
if (m_analyzingData) {
|
|
ggprintf("Analyzing captured data ..\n");
|
|
auto tStart = std::chrono::high_resolution_clock::now();
|
|
|
|
const int stepsPerFrame = 16;
|
|
const int step = m_samplesPerFrame/stepsPerFrame;
|
|
|
|
bool isValid = false;
|
|
for (const auto & rxProtocolPair : m_rxProtocols) {
|
|
const auto & rxProtocolId = rxProtocolPair.first;
|
|
const auto & rxProtocol = rxProtocolPair.second;
|
|
|
|
// skip Rx protocol if start frequency is different from detected one
|
|
if (rxProtocol.freqStart != m_markerFreqStart) {
|
|
continue;
|
|
}
|
|
|
|
std::fill(m_sampleSpectrum.begin(), m_sampleSpectrum.end(), 0.0f);
|
|
|
|
m_framesToAnalyze = m_nMarkerFrames*stepsPerFrame;
|
|
m_framesLeftToAnalyze = m_framesToAnalyze;
|
|
|
|
// note : not sure if looping backwards here is more meaningful than looping forwards
|
|
for (int ii = m_nMarkerFrames*stepsPerFrame - 1; ii >= 0; --ii) {
|
|
bool knownLength = false;
|
|
|
|
int decodedLength = 0;
|
|
const int offsetStart = ii;
|
|
for (int itx = 0; itx < 1024; ++itx) {
|
|
int offsetTx = offsetStart + itx*rxProtocol.framesPerTx*stepsPerFrame;
|
|
if (offsetTx >= m_recvDuration_frames*stepsPerFrame || (itx + 1)*rxProtocol.bytesPerTx >= (int) m_dataEncoded.size()) {
|
|
break;
|
|
}
|
|
|
|
std::copy(
|
|
m_recordedAmplitude.begin() + offsetTx*step,
|
|
m_recordedAmplitude.begin() + offsetTx*step + m_samplesPerFrame, m_fftInp.data());
|
|
|
|
// note : should we skip the first and last frame here as they are amplitude-smoothed?
|
|
for (int k = 1; k < rxProtocol.framesPerTx; ++k) {
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_fftInp[i] += m_recordedAmplitude[(offsetTx + k*stepsPerFrame)*step + i];
|
|
}
|
|
}
|
|
|
|
FFT(m_fftInp.data(), m_fftOut.data(), m_samplesPerFrame, 1.0);
|
|
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_sampleSpectrum[i] = (m_fftOut[2*i + 0]*m_fftOut[2*i + 0] + m_fftOut[2*i + 1]*m_fftOut[2*i + 1]);
|
|
}
|
|
for (int i = 1; i < m_samplesPerFrame/2; ++i) {
|
|
m_sampleSpectrum[i] += m_sampleSpectrum[m_samplesPerFrame - i];
|
|
}
|
|
|
|
uint8_t curByte = 0;
|
|
for (int i = 0; i < 2*rxProtocol.bytesPerTx; ++i) {
|
|
double freq = m_hzPerSample*rxProtocol.freqStart;
|
|
int bin = std::round(freq*m_ihzPerSample) + 16*i;
|
|
|
|
int kmax = 0;
|
|
double amax = 0.0;
|
|
for (int k = 0; k < 16; ++k) {
|
|
if (m_sampleSpectrum[bin + k] > amax) {
|
|
kmax = k;
|
|
amax = m_sampleSpectrum[bin + k];
|
|
}
|
|
}
|
|
|
|
if (i%2) {
|
|
curByte += (kmax << 4);
|
|
m_dataEncoded[itx*rxProtocol.bytesPerTx + i/2] = curByte;
|
|
curByte = 0;
|
|
} else {
|
|
curByte = kmax;
|
|
}
|
|
}
|
|
|
|
if (itx*rxProtocol.bytesPerTx > m_encodedDataOffset && knownLength == false) {
|
|
RS::ReedSolomon rsLength(1, m_encodedDataOffset - 1);
|
|
if ((rsLength.Decode(m_dataEncoded.data(), m_rxData.data()) == 0) && (m_rxData[0] > 0 && m_rxData[0] <= 140)) {
|
|
knownLength = true;
|
|
decodedLength = m_rxData[0];
|
|
|
|
const int nTotalBytesExpected = m_encodedDataOffset + decodedLength + ::getECCBytesForLength(decodedLength);
|
|
const int nTotalFramesExpected = 2*m_nMarkerFrames + ((nTotalBytesExpected + rxProtocol.bytesPerTx - 1)/rxProtocol.bytesPerTx)*rxProtocol.framesPerTx;
|
|
if (m_recvDuration_frames > nTotalFramesExpected ||
|
|
m_recvDuration_frames < nTotalFramesExpected - 2*m_nMarkerFrames) {
|
|
knownLength = false;
|
|
break;
|
|
}
|
|
} else {
|
|
break;
|
|
}
|
|
}
|
|
|
|
{
|
|
const int nTotalBytesExpected = m_encodedDataOffset + decodedLength + ::getECCBytesForLength(decodedLength);
|
|
if (knownLength && itx*rxProtocol.bytesPerTx > nTotalBytesExpected + 1) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (knownLength) {
|
|
RS::ReedSolomon rsData(decodedLength, ::getECCBytesForLength(decodedLength));
|
|
|
|
if (rsData.Decode(m_dataEncoded.data() + m_encodedDataOffset, m_rxData.data()) == 0) {
|
|
if (m_rxData[0] != 0) {
|
|
std::string s((char *) m_rxData.data(), decodedLength);
|
|
|
|
ggprintf("Decoded length = %d, protocol = '%s' (%d)\n", decodedLength, rxProtocol.name, rxProtocolId);
|
|
ggprintf("Received sound data successfully: '%s'\n", s.c_str());
|
|
|
|
isValid = true;
|
|
m_hasNewRxData = true;
|
|
m_lastRxDataLength = decodedLength;
|
|
m_rxProtocol = rxProtocol;
|
|
m_rxProtocolId = TxProtocolId(rxProtocolId);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (isValid) {
|
|
break;
|
|
}
|
|
--m_framesLeftToAnalyze;
|
|
}
|
|
|
|
if (isValid) break;
|
|
}
|
|
|
|
m_framesToRecord = 0;
|
|
|
|
if (isValid == false) {
|
|
ggprintf("Failed to capture sound data. Please try again (length = %d)\n", m_rxData[0]);
|
|
m_lastRxDataLength = -1;
|
|
m_framesToRecord = -1;
|
|
}
|
|
|
|
m_receivingData = false;
|
|
m_analyzingData = false;
|
|
|
|
std::fill(m_sampleSpectrum.begin(), m_sampleSpectrum.end(), 0.0f);
|
|
|
|
m_framesToAnalyze = 0;
|
|
m_framesLeftToAnalyze = 0;
|
|
|
|
auto tEnd = std::chrono::high_resolution_clock::now();
|
|
ggprintf("Time to analyze: %g ms\n", getTime_ms(tStart, tEnd));
|
|
}
|
|
|
|
// check if receiving data
|
|
if (m_receivingData == false) {
|
|
bool isReceiving = false;
|
|
|
|
for (const auto & rxProtocol : getTxProtocols()) {
|
|
int nDetectedMarkerBits = m_nBitsInMarker;
|
|
|
|
for (int i = 0; i < m_nBitsInMarker; ++i) {
|
|
double freq = bitFreq(rxProtocol.second, i);
|
|
int bin = std::round(freq*m_ihzPerSample);
|
|
|
|
if (i%2 == 0) {
|
|
if (m_sampleSpectrum[bin] <= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) --nDetectedMarkerBits;
|
|
} else {
|
|
if (m_sampleSpectrum[bin] >= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) --nDetectedMarkerBits;
|
|
}
|
|
}
|
|
|
|
if (nDetectedMarkerBits == m_nBitsInMarker) {
|
|
m_markerFreqStart = rxProtocol.second.freqStart;
|
|
isReceiving = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (isReceiving) {
|
|
if (++m_nMarkersSuccess >= 1) {
|
|
} else {
|
|
isReceiving = false;
|
|
}
|
|
} else {
|
|
m_nMarkersSuccess = 0;
|
|
}
|
|
|
|
if (isReceiving) {
|
|
std::time_t timestamp = std::time(nullptr);
|
|
ggprintf("%sReceiving sound data ...\n", std::asctime(std::localtime(×tamp)));
|
|
|
|
m_receivingData = true;
|
|
std::fill(m_rxData.begin(), m_rxData.end(), 0);
|
|
|
|
// max recieve duration
|
|
m_recvDuration_frames =
|
|
2*m_nMarkerFrames +
|
|
maxFramesPerTx()*((kMaxLengthVarible + ::getECCBytesForLength(kMaxLengthVarible))/minBytesPerTx() + 1);
|
|
|
|
m_nMarkersSuccess = 0;
|
|
m_framesToRecord = m_recvDuration_frames;
|
|
m_framesLeftToRecord = m_recvDuration_frames;
|
|
}
|
|
} else {
|
|
bool isEnded = false;
|
|
|
|
for (const auto & rxProtocol : getTxProtocols()) {
|
|
int nDetectedMarkerBits = m_nBitsInMarker;
|
|
|
|
for (int i = 0; i < m_nBitsInMarker; ++i) {
|
|
double freq = bitFreq(rxProtocol.second, i);
|
|
int bin = std::round(freq*m_ihzPerSample);
|
|
|
|
if (i%2 == 0) {
|
|
if (m_sampleSpectrum[bin] >= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) nDetectedMarkerBits--;
|
|
} else {
|
|
if (m_sampleSpectrum[bin] <= m_soundMarkerThreshold*m_sampleSpectrum[bin + m_freqDelta_bin]) nDetectedMarkerBits--;
|
|
}
|
|
}
|
|
|
|
if (nDetectedMarkerBits == m_nBitsInMarker) {
|
|
isEnded = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (isEnded) {
|
|
if (++m_nMarkersSuccess >= 1) {
|
|
} else {
|
|
isEnded = false;
|
|
}
|
|
} else {
|
|
m_nMarkersSuccess = 0;
|
|
}
|
|
|
|
if (isEnded && m_framesToRecord > 1) {
|
|
std::time_t timestamp = std::time(nullptr);
|
|
m_recvDuration_frames -= m_framesLeftToRecord - 1;
|
|
ggprintf("%sReceived end marker. Frames left = %d, recorded = %d\n", std::asctime(std::localtime(×tamp)), m_framesLeftToRecord, m_recvDuration_frames);
|
|
m_nMarkersSuccess = 0;
|
|
m_framesLeftToRecord = 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
// Fixed payload length
|
|
|
|
void GGWave::decode_fixed() {
|
|
m_hasNewSpectrum = true;
|
|
|
|
// calculate spectrum
|
|
FFT(m_sampleAmplitude.data(), m_fftOut.data(), m_samplesPerFrame, 1.0);
|
|
|
|
for (int i = 0; i < m_samplesPerFrame; ++i) {
|
|
m_sampleSpectrum[i] = (m_fftOut[2*i + 0]*m_fftOut[2*i + 0] + m_fftOut[2*i + 1]*m_fftOut[2*i + 1]);
|
|
}
|
|
for (int i = 1; i < m_samplesPerFrame/2; ++i) {
|
|
m_sampleSpectrum[i] += m_sampleSpectrum[m_samplesPerFrame - i];
|
|
}
|
|
|
|
m_spectrumHistoryFixed[m_historyIdFixed] = m_sampleSpectrum;
|
|
|
|
if (++m_historyIdFixed >= (int) m_spectrumHistoryFixed.size()) {
|
|
m_historyIdFixed = 0;
|
|
}
|
|
|
|
bool isValid = false;
|
|
for (const auto & rxProtocolPair : m_rxProtocols) {
|
|
const auto & rxProtocolId = rxProtocolPair.first;
|
|
const auto & rxProtocol = rxProtocolPair.second;
|
|
|
|
const int binStart = rxProtocol.freqStart;
|
|
const int binDelta = 16;
|
|
|
|
if (binStart > m_samplesPerFrame) {
|
|
continue;
|
|
}
|
|
|
|
const int totalLength = m_payloadLength + getECCBytesForLength(m_payloadLength);
|
|
const int totalTxs = (totalLength + rxProtocol.bytesPerTx - 1)/rxProtocol.bytesPerTx;
|
|
|
|
int historyStartId = m_historyIdFixed - totalTxs*rxProtocol.framesPerTx;
|
|
if (historyStartId < 0) {
|
|
historyStartId += m_spectrumHistoryFixed.size();
|
|
}
|
|
|
|
const int nTones = 2*rxProtocol.bytesPerTx;
|
|
std::vector<int> detectedBins(2*totalLength);
|
|
|
|
struct ToneData {
|
|
int nMax[16];
|
|
};
|
|
|
|
std::vector<ToneData> tones(nTones);
|
|
|
|
bool detectedSignal = true;
|
|
int txDetectedTotal = 0;
|
|
int txNeededTotal = 0;
|
|
for (int k = 0; k < totalTxs; ++k) {
|
|
for (auto & tone : tones) {
|
|
std::fill(tone.nMax, tone.nMax + 16, 0);
|
|
}
|
|
|
|
for (int i = 0; i < rxProtocol.framesPerTx; ++i) {
|
|
int historyId = historyStartId + k*rxProtocol.framesPerTx + i;
|
|
if (historyId >= (int) m_spectrumHistoryFixed.size()) {
|
|
historyId -= m_spectrumHistoryFixed.size();
|
|
}
|
|
|
|
for (int j = 0; j < rxProtocol.bytesPerTx; ++j) {
|
|
int f0bin = -1;
|
|
int f1bin = -1;
|
|
|
|
double f0max = 0.0;
|
|
double f1max = 0.0;
|
|
|
|
for (int b = 0; b < 16; ++b) {
|
|
{
|
|
const auto & v = m_spectrumHistoryFixed[historyId][binStart + 2*j*binDelta + b];
|
|
|
|
if (f0max <= v) {
|
|
f0max = v;
|
|
f0bin = b;
|
|
}
|
|
}
|
|
|
|
{
|
|
const auto & v = m_spectrumHistoryFixed[historyId][binStart + 2*j*binDelta + binDelta + b];
|
|
|
|
if (f1max <= v) {
|
|
f1max = v;
|
|
f1bin = b;
|
|
}
|
|
}
|
|
}
|
|
|
|
tones[2*j + 0].nMax[f0bin]++;
|
|
tones[2*j + 1].nMax[f1bin]++;
|
|
}
|
|
}
|
|
|
|
int txDetected = 0;
|
|
int txNeeded = 0;
|
|
for (int j = 0; j < rxProtocol.bytesPerTx; ++j) {
|
|
if (k*rxProtocol.bytesPerTx + j >= totalLength) break;
|
|
txNeeded += 2;
|
|
for (int b = 0; b < 16; ++b) {
|
|
if (tones[2*j + 0].nMax[b] > rxProtocol.framesPerTx/2) {
|
|
detectedBins[2*(k*rxProtocol.bytesPerTx + j) + 0] = b;
|
|
txDetected++;
|
|
}
|
|
if (tones[2*j + 1].nMax[b] > rxProtocol.framesPerTx/2) {
|
|
detectedBins[2*(k*rxProtocol.bytesPerTx + j) + 1] = b;
|
|
txDetected++;
|
|
}
|
|
}
|
|
}
|
|
|
|
txDetectedTotal += txDetected;
|
|
txNeededTotal += txNeeded;
|
|
}
|
|
|
|
if (txDetectedTotal < 0.75*txNeededTotal) {
|
|
detectedSignal = false;
|
|
}
|
|
|
|
if (detectedSignal) {
|
|
RS::ReedSolomon rsData(m_payloadLength, getECCBytesForLength(m_payloadLength));
|
|
|
|
for (int j = 0; j < totalLength; ++j) {
|
|
m_dataEncoded[j] = (detectedBins[2*j + 1] << 4) + detectedBins[2*j + 0];
|
|
}
|
|
|
|
if (rsData.Decode(m_dataEncoded.data(), m_rxData.data()) == 0) {
|
|
if (m_rxData[0] != 0) {
|
|
ggprintf("Received sound data successfully: '%s'\n", m_rxData.data());
|
|
|
|
isValid = true;
|
|
m_hasNewRxData = true;
|
|
m_lastRxDataLength = m_payloadLength;
|
|
m_rxProtocol = rxProtocol;
|
|
m_rxProtocolId = TxProtocolId(rxProtocolId);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (isValid) {
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
int GGWave::maxFramesPerTx() const {
|
|
int res = 0;
|
|
for (const auto & protocol : getTxProtocols()) {
|
|
res = std::max(res, protocol.second.framesPerTx);
|
|
}
|
|
return res;
|
|
}
|
|
|
|
int GGWave::minBytesPerTx() const {
|
|
int res = getTxProtocols().begin()->second.bytesPerTx;
|
|
for (const auto & protocol : getTxProtocols()) {
|
|
res = std::min(res, protocol.second.bytesPerTx);
|
|
}
|
|
return res;
|
|
}
|
|
|