87 lines
2.3 KiB
C++
87 lines
2.3 KiB
C++
#ifndef FSK_MODULATOR_H
|
|
#define FSK_MODULATOR_H
|
|
|
|
#include <cmath>
|
|
#include <cstdint>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <vector>
|
|
|
|
#include "BitStreamReader.h"
|
|
|
|
class FSKModulatorConfig {
|
|
public:
|
|
int freq_lo;
|
|
int freq_hi;
|
|
int sample_rate;
|
|
int baud_rate;
|
|
std::shared_ptr<BitStreamReader> bitstreamreader;
|
|
};
|
|
|
|
namespace milstd {
|
|
class FSKModulator {
|
|
public:
|
|
FSKModulator(const FSKModulatorConfig& s) : freq_lo(s.freq_lo), freq_hi(s.freq_hi), sample_rate(s.sample_rate), baud_rate(s.baud_rate), bit_reader(s.bitstreamreader) {
|
|
omega[0] = (2.0 * M_PI * freq_lo) / sample_rate;
|
|
omega[1] = (2.0 * M_PI * freq_hi) / sample_rate;
|
|
baud_incr = static_cast<double>(baud_rate) / sample_rate;
|
|
phase = 0.0;
|
|
baud_frac = 0.0;
|
|
current_bit = 0;
|
|
}
|
|
|
|
std::vector<int16_t> modulate(unsigned int num_samples) {
|
|
std::vector<int16_t> samples;
|
|
samples.reserve(num_samples);
|
|
|
|
int bit = current_bit;
|
|
|
|
for (unsigned int i = 0; i < num_samples; i++) {
|
|
baud_frac += baud_incr;
|
|
if (baud_frac >= 1.0) {
|
|
baud_frac -= 1.0;
|
|
try
|
|
{
|
|
bit = bit_reader->getNextBit();
|
|
}
|
|
catch(const std::out_of_range&)
|
|
{
|
|
bit = 0;
|
|
}
|
|
if (bit != 0 && bit != 1)
|
|
bit = 0;
|
|
}
|
|
|
|
double sample = std::cos(phase);
|
|
int16_t sample_int = static_cast<int16_t>(sample * 32767);
|
|
samples.push_back(sample_int);
|
|
|
|
phase += omega[bit];
|
|
if (phase >= 2.0 * M_PI) {
|
|
phase -= 2.0 * M_PI;
|
|
}
|
|
}
|
|
|
|
current_bit = bit;
|
|
|
|
return samples;
|
|
}
|
|
|
|
private:
|
|
// parameters
|
|
int freq_lo, freq_hi;
|
|
int sample_rate;
|
|
int baud_rate;
|
|
std::shared_ptr<BitStreamReader> bit_reader;
|
|
|
|
// state variables
|
|
double phase;
|
|
double baud_frac;
|
|
double baud_incr;
|
|
std::array<double, 2> omega;
|
|
int current_bit;
|
|
};
|
|
|
|
} // namespace milstd
|
|
|
|
#endif /* FSK_MODULATOR_H */ |