79 lines
2.4 KiB
C++
79 lines
2.4 KiB
C++
#include "bfsk.hpp"
|
|
#include <cmath>
|
|
#include <cstdlib>
|
|
#include <iostream>
|
|
|
|
std::vector<float> bfskModulate(const std::vector<unsigned char> &data) {
|
|
int samplesPerBit = (int)((double)SAMPLE_RATE / BFSK_BAUD);
|
|
size_t totalBits = data.size() * 8;
|
|
size_t totalSamples = totalBits * samplesPerBit;
|
|
std::vector<float> out(totalSamples * 2, 0.0f);
|
|
|
|
double phase0 = 0.0;
|
|
double phase1 = 0.0;
|
|
double inc0 = 2.0 * M_PI * BFSK_FREQ0 / (double)SAMPLE_RATE;
|
|
double inc1 = 2.0 * M_PI * BFSK_FREQ1 / (double)SAMPLE_RATE;
|
|
|
|
size_t sampleIndex = 0;
|
|
|
|
for (auto byteVal : data) {
|
|
for (int b = 0; b < 8; b++) {
|
|
int bit = (byteVal >> b) & 1;
|
|
for (int s = 0; s < samplesPerBit; s++) {
|
|
float val;
|
|
if (bit == 0) {
|
|
val = sinf((float)phase0);
|
|
phase0 += inc0;
|
|
} else {
|
|
val = sinf((float)phase1);
|
|
phase1 += inc1;
|
|
}
|
|
out[sampleIndex*2 + 0] = val * 0.3f;
|
|
out[sampleIndex*2 + 1] = val * 0.3f;
|
|
sampleIndex++;
|
|
}
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
std::vector<unsigned char> bfskDemodulate(const std::vector<float> &monoData) {
|
|
int samplesPerBit = (int)((double)SAMPLE_RATE / BFSK_BAUD);
|
|
size_t totalBits = monoData.size() / samplesPerBit;
|
|
size_t totalBytes = totalBits / 8;
|
|
|
|
double inc0 = 2.0 * M_PI * BFSK_FREQ0 / (double)SAMPLE_RATE;
|
|
double inc1 = 2.0 * M_PI * BFSK_FREQ1 / (double)SAMPLE_RATE;
|
|
|
|
std::vector<unsigned char> result(totalBytes, 0);
|
|
|
|
size_t bitIndex = 0;
|
|
for (size_t b = 0; b < totalBits; b++) {
|
|
double sum0 = 0.0;
|
|
double sum1 = 0.0;
|
|
double phase0 = 0.0;
|
|
double phase1 = 0.0;
|
|
|
|
size_t startSample = b * samplesPerBit;
|
|
for (int s = 0; s < samplesPerBit; s++) {
|
|
float sample = monoData[startSample + s];
|
|
float ref0 = sinf((float)phase0);
|
|
float ref1 = sinf((float)phase1);
|
|
sum0 += sample * ref0;
|
|
sum1 += sample * ref1;
|
|
phase0 += inc0;
|
|
phase1 += inc1;
|
|
}
|
|
|
|
int bit = (std::fabs(sum1) > std::fabs(sum0)) ? 1 : 0;
|
|
size_t bytePos = bitIndex / 8;
|
|
int bitPos = bitIndex % 8;
|
|
if (bytePos < result.size()) {
|
|
result[bytePos] |= (bit << bitPos);
|
|
}
|
|
bitIndex++;
|
|
}
|
|
|
|
return result;
|
|
}
|