54 lines
1.7 KiB
C++
54 lines
1.7 KiB
C++
#include "bfsk.hpp"
|
|
#include <cmath>
|
|
#include <cstdlib>
|
|
|
|
std::vector<float> bfskModulate(const std::vector<uint8_t> &data){
|
|
int spb=(int)(SAMPLE_RATE/BFSK_BAUD);
|
|
size_t tb=data.size()*8;
|
|
size_t ts=tb*spb;
|
|
std::vector<float> out(ts*2,0.0f);
|
|
double phase0=0.0,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 idx=0;
|
|
for(auto byteVal:data){
|
|
for(int b=0;b<8;b++){
|
|
int bit=(byteVal>>b)&1;
|
|
for(int s=0;s<spb;s++){
|
|
float val=bit? sinf((float)phase1): sinf((float)phase0);
|
|
if(bit) phase1+=inc1; else phase0+=inc0;
|
|
out[idx*2+0]=val*0.3f;
|
|
out[idx*2+1]=val*0.3f;
|
|
idx++;
|
|
}
|
|
}
|
|
}
|
|
return out;
|
|
}
|
|
|
|
std::vector<uint8_t> bfskDemodulate(const std::vector<float> &monoData){
|
|
int spb=(int)(SAMPLE_RATE/BFSK_BAUD);
|
|
size_t tb=monoData.size()/spb;
|
|
size_t tbytes=tb/8;
|
|
std::vector<uint8_t> res(tbytes,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 bitIdx=0;
|
|
for(size_t b=0;b<tb;b++){
|
|
double sum0=0.0,sum1=0.0;
|
|
double ph0=0.0,ph1=0.0;
|
|
size_t st=b*spb;
|
|
for(int s=0;s<spb;s++){
|
|
float sample=monoData[st+s];
|
|
float r0=sinf((float)ph0), r1=sinf((float)ph1);
|
|
sum0+=sample*r0; sum1+=sample*r1;
|
|
ph0+=inc0; ph1+=inc1;
|
|
}
|
|
int bit=(std::fabs(sum1)>std::fabs(sum0))?1:0;
|
|
size_t bytePos=bitIdx/8; int bitPos=bitIdx%8;
|
|
if(bytePos<res.size()) res[bytePos]|=(bit<<bitPos);
|
|
bitIdx++;
|
|
}
|
|
return res;
|
|
}
|