mirror of
https://github.com/ggerganov/ggwave.git
synced 2026-02-07 01:11:22 +08:00
arduino-rx : first working example on RP2040 Connect !
This commit is contained in:
Georgi Gerganov
@@ -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
|
||||
|
||||
|
||||
127
examples/arduino-rx/arduino-rx.ino
Normal file
127
examples/arduino-rx/arduino-rx.ino
Normal file
@@ -0,0 +1,127 @@
|
||||
#include "ggwave/ggwave.h"
|
||||
|
||||
#include <PDM.h>
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
1436
examples/arduino-rx/ggwave.cpp
Normal file
1436
examples/arduino-rx/ggwave.cpp
Normal file
File diff suppressed because it is too large
Load Diff
574
examples/arduino-rx/ggwave/ggwave.h
Normal file
574
examples/arduino-rx/ggwave/ggwave.h
Normal file
@@ -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 <cstdint>
|
||||
#include <functional>
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
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<TxProtocolId, TxProtocol>;
|
||||
using RxProtocols = std::map<RxProtocolId, RxProtocol>;
|
||||
|
||||
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<ToneData>;
|
||||
using WaveformTones = std::vector<Tones>;
|
||||
|
||||
using AmplitudeData = std::vector<float>;
|
||||
using AmplitudeDataI16 = std::vector<int16_t>;
|
||||
using SpectrumData = std::vector<float>;
|
||||
using RecordedData = std::vector<float>;
|
||||
using TxRxData = std::vector<std::uint8_t>;
|
||||
|
||||
using CBWaveformOut = std::function<void(const void * data, uint32_t nBytes)>;
|
||||
using CBWaveformInp = std::function<uint32_t(void * data, uint32_t nMaxBytes)>;
|
||||
|
||||
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<float> m_fftInp; // real
|
||||
std::vector<float> 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<AmplitudeData> m_sampleAmplitudeHistory;
|
||||
|
||||
RecordedData m_recordedAmplitude;
|
||||
|
||||
int m_historyIdFixed;
|
||||
std::vector<SpectrumData> 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<Impl> m_impl;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
21
examples/arduino-rx/reed-solomon/LICENSE
Normal file
21
examples/arduino-rx/reed-solomon/LICENSE
Normal file
@@ -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.
|
||||
235
examples/arduino-rx/reed-solomon/gf.hpp
Normal file
235
examples/arduino-rx/reed-solomon/gf.hpp
Normal file
@@ -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 <stdint.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
|
||||
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
|
||||
|
||||
94
examples/arduino-rx/reed-solomon/poly.hpp
Normal file
94
examples/arduino-rx/reed-solomon/poly.hpp
Normal file
@@ -0,0 +1,94 @@
|
||||
/* Author: Mike Lubinets (aka mersinvald)
|
||||
* Date: 29.12.15
|
||||
*
|
||||
* See LICENSE */
|
||||
|
||||
#ifndef POLY_H
|
||||
#define POLY_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <assert.h>
|
||||
|
||||
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
|
||||
538
examples/arduino-rx/reed-solomon/rs.hpp
Normal file
538
examples/arduino-rx/reed-solomon/rs.hpp
Normal file
@@ -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 <assert.h>
|
||||
#include <string.h>
|
||||
#include <stdint.h>
|
||||
#include <vector>
|
||||
|
||||
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<uint8_t> 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<uint8_t> 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
|
||||
|
||||
161
examples/arduino-rx/resampler.cpp
Normal file
161
examples/arduino-rx/resampler.cpp
Normal file
@@ -0,0 +1,161 @@
|
||||
#include "resampler.h"
|
||||
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <cstdio>
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
49
examples/arduino-rx/resampler.h
Normal file
49
examples/arduino-rx/resampler.h
Normal file
@@ -0,0 +1,49 @@
|
||||
#pragma once
|
||||
|
||||
#include <vector>
|
||||
#include <cstdint>
|
||||
|
||||
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<float> m_sincTable;
|
||||
std::vector<float> m_delayBuffer;
|
||||
std::vector<float> m_edgeSamples;
|
||||
std::vector<float> m_samplesInp;
|
||||
|
||||
struct State {
|
||||
int nSamplesTotal = 0;
|
||||
int timeInt = 0;
|
||||
int timeLast = 0;
|
||||
double timeNow = 0.0;
|
||||
};
|
||||
|
||||
State m_state;
|
||||
};
|
||||
Reference in New Issue
Block a user