kopia lustrzana https://github.com/F5OEO/PiFmRds
Functional rds_wav that generates stereo multiplex signals
rodzic
e1defb48e6
commit
e758ee63a0
201
src/fm_mpx.c
201
src/fm_mpx.c
|
@ -19,10 +19,14 @@
|
||||||
|
|
||||||
You should have received a copy of the GNU General Public License
|
You should have received a copy of the GNU General Public License
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
|
||||||
|
fm_mpx.c: generates an FM multiplex signal containing RDS plus possibly
|
||||||
|
monaural or stereo audio.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <sndfile.h>
|
#include <sndfile.h>
|
||||||
#include <stdlib.h>
|
#include <stdlib.h>
|
||||||
|
#include <strings.h>
|
||||||
#include <math.h>
|
#include <math.h>
|
||||||
|
|
||||||
#include "rds.h"
|
#include "rds.h"
|
||||||
|
@ -34,8 +38,8 @@
|
||||||
#define FIR_HALF_SIZE 30
|
#define FIR_HALF_SIZE 30
|
||||||
#define FIR_SIZE (2*FIR_HALF_SIZE-1)
|
#define FIR_SIZE (2*FIR_HALF_SIZE-1)
|
||||||
|
|
||||||
#define LENGTH 114000
|
|
||||||
// TODO: remove constant
|
size_t length;
|
||||||
|
|
||||||
// coefficients of the low-pass FIR filter
|
// coefficients of the low-pass FIR filter
|
||||||
float low_pass_fir[FIR_HALF_SIZE];
|
float low_pass_fir[FIR_HALF_SIZE];
|
||||||
|
@ -52,81 +56,112 @@ int phase_19 = 0;
|
||||||
float downsample_factor;
|
float downsample_factor;
|
||||||
|
|
||||||
|
|
||||||
float rds_buffer[LENGTH] = {0};
|
float *audio_buffer;
|
||||||
float audio_buffer[LENGTH] = {0};
|
|
||||||
int audio_index = 0;
|
int audio_index = 0;
|
||||||
int audio_len = 0;
|
int audio_len = 0;
|
||||||
float audio_pos;
|
float audio_pos;
|
||||||
float out = 0;
|
|
||||||
float alpha = .03;
|
|
||||||
|
|
||||||
float fir_buffer[FIR_SIZE] = {0};
|
float fir_buffer_mono[FIR_SIZE] = {0};
|
||||||
|
float fir_buffer_stereo[FIR_SIZE] = {0};
|
||||||
int fir_index = 0;
|
int fir_index = 0;
|
||||||
|
int channels;
|
||||||
|
|
||||||
SNDFILE *inf;
|
SNDFILE *inf;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
float *alloc_empty_buffer(size_t length) {
|
||||||
|
float *p = malloc(length * sizeof(float));
|
||||||
|
if(p == NULL) return NULL;
|
||||||
|
|
||||||
|
bzero(p, length * sizeof(float));
|
||||||
|
|
||||||
|
return p;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
int fm_mpx_open(char *filename) {
|
int fm_mpx_open(char *filename, size_t len) {
|
||||||
// Open the input file
|
length = len;
|
||||||
SF_INFO sfinfo;
|
|
||||||
if(! (inf = sf_open(filename, SFM_READ, &sfinfo))) {
|
|
||||||
fprintf(stderr, "Error: could not open input file %s.\n", filename) ;
|
|
||||||
return EXIT_FAILURE; // TODO better error code
|
|
||||||
}
|
|
||||||
|
|
||||||
int in_samplerate = sfinfo.samplerate;
|
|
||||||
downsample_factor = 228000. / in_samplerate;
|
|
||||||
|
|
||||||
printf("Input: %d Hz, upsampling factor: %.2f\n", in_samplerate, downsample_factor);
|
|
||||||
|
|
||||||
|
|
||||||
// Create the low-pass FIR filter
|
|
||||||
float cutoff_freq = 15000 * .8;
|
|
||||||
if(in_samplerate/2 < cutoff_freq) cutoff_freq = in_samplerate/2 * .8;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
low_pass_fir[FIR_HALF_SIZE-1] = 2 * cutoff_freq / 228000 /2;
|
|
||||||
// Here we divide this coefficient by two because it will be counted twice
|
|
||||||
// when applying the filter
|
|
||||||
|
|
||||||
// Only store half of the filter since it is symmetric
|
if(filename != NULL) {
|
||||||
for(int i=1; i<FIR_HALF_SIZE; i++) {
|
// Open the input file
|
||||||
low_pass_fir[FIR_HALF_SIZE-1-i] =
|
SF_INFO sfinfo;
|
||||||
sin(2 * PI * cutoff_freq * i / 228000) / (PI * i) // sinc
|
if(! (inf = sf_open(filename, SFM_READ, &sfinfo))) {
|
||||||
* (.54 - .46 * cos(2*PI * (i+FIR_HALF_SIZE) / (2*FIR_HALF_SIZE)));
|
fprintf(stderr, "Error: could not open input file %s.\n", filename) ;
|
||||||
// Hamming window
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
int in_samplerate = sfinfo.samplerate;
|
||||||
|
downsample_factor = 228000. / in_samplerate;
|
||||||
|
|
||||||
|
printf("Input: %d Hz, upsampling factor: %.2f\n", in_samplerate, downsample_factor);
|
||||||
|
|
||||||
|
channels = sfinfo.channels;
|
||||||
|
if(channels > 1) {
|
||||||
|
printf("%d channels, generating stereo multiplex.\n", channels);
|
||||||
|
} else {
|
||||||
|
printf("1 channel, monophonic operation.\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
// Create the low-pass FIR filter
|
||||||
|
float cutoff_freq = 15000 * .8;
|
||||||
|
if(in_samplerate/2 < cutoff_freq) cutoff_freq = in_samplerate/2 * .8;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
low_pass_fir[FIR_HALF_SIZE-1] = 2 * cutoff_freq / 228000 /2;
|
||||||
|
// Here we divide this coefficient by two because it will be counted twice
|
||||||
|
// when applying the filter
|
||||||
|
|
||||||
|
// Only store half of the filter since it is symmetric
|
||||||
|
for(int i=1; i<FIR_HALF_SIZE; i++) {
|
||||||
|
low_pass_fir[FIR_HALF_SIZE-1-i] =
|
||||||
|
sin(2 * PI * cutoff_freq * i / 228000) / (PI * i) // sinc
|
||||||
|
* (.54 - .46 * cos(2*PI * (i+FIR_HALF_SIZE) / (2*FIR_HALF_SIZE)));
|
||||||
|
// Hamming window
|
||||||
|
}
|
||||||
|
printf("Created low-pass FIR filter for audio channels, with cutoff at %.1f Hz\n", cutoff_freq);
|
||||||
|
|
||||||
|
/*
|
||||||
|
for(int i=0; i<FIR_HALF_SIZE; i++) {
|
||||||
|
printf("%.5f ", low_pass_fir[i]);
|
||||||
|
}
|
||||||
|
printf("\n");
|
||||||
|
*/
|
||||||
|
|
||||||
|
audio_pos = downsample_factor;
|
||||||
|
audio_buffer = alloc_empty_buffer(length * channels);
|
||||||
|
if(audio_buffer == NULL) return -1;
|
||||||
|
|
||||||
|
} // end if(filename != NULL)
|
||||||
|
else {
|
||||||
|
inf = NULL;
|
||||||
|
// inf == NULL indicates that there is no audio
|
||||||
}
|
}
|
||||||
printf("Created low-pass FIR filter for audio channels, with cutoff at %.1f Hz\n", cutoff_freq);
|
|
||||||
|
|
||||||
/*
|
return 0;
|
||||||
for(int i=0; i<FIR_HALF_SIZE; i++) {
|
|
||||||
printf("%.5f ", low_pass_fir[i]);
|
|
||||||
}
|
|
||||||
printf("\n");
|
|
||||||
*/
|
|
||||||
|
|
||||||
audio_pos = downsample_factor;
|
|
||||||
|
|
||||||
return 0; // TODO
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
int fm_mpx_get_samples(float *mpx_buffer) { // TODO accept length in argument
|
// samples provided by this function are in 0..10: they need to be divided by
|
||||||
get_rds_samples(rds_buffer, LENGTH);
|
// 10 after.
|
||||||
|
int fm_mpx_get_samples(float *mpx_buffer) {
|
||||||
|
get_rds_samples(mpx_buffer, length);
|
||||||
|
|
||||||
for(int i=0; i<LENGTH; i++) {
|
if(inf == NULL) return 0; // if there is no audio, stop here
|
||||||
|
|
||||||
|
for(int i=0; i<length; i++) {
|
||||||
if(audio_pos >= downsample_factor) {
|
if(audio_pos >= downsample_factor) {
|
||||||
audio_pos -= downsample_factor;
|
audio_pos -= downsample_factor;
|
||||||
|
|
||||||
if(audio_len == 0) {
|
if(audio_len == 0) {
|
||||||
for(int j=0; j<2; j++) { // one retry
|
for(int j=0; j<2; j++) { // one retry
|
||||||
audio_len = sf_read_float(inf, audio_buffer, LENGTH);
|
audio_len = sf_read_float(inf, audio_buffer, length);
|
||||||
if (audio_len < 0) {
|
if (audio_len < 0) {
|
||||||
fprintf(stderr, "Error reading audio\n");
|
fprintf(stderr, "Error reading audio\n");
|
||||||
exit(EXIT_FAILURE);
|
return -1;
|
||||||
}
|
}
|
||||||
if(audio_len == 0) {
|
if(audio_len == 0) {
|
||||||
sf_seek(inf, 0, SEEK_SET);
|
sf_seek(inf, 0, SEEK_SET);
|
||||||
|
@ -136,49 +171,72 @@ int fm_mpx_get_samples(float *mpx_buffer) { // TODO accept length in argument
|
||||||
}
|
}
|
||||||
audio_index = 0;
|
audio_index = 0;
|
||||||
} else {
|
} else {
|
||||||
audio_index++;
|
audio_index += channels;
|
||||||
audio_len--;
|
audio_len -= channels;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// Apply FIR low-pass filter
|
|
||||||
fir_buffer[fir_index] = audio_buffer[audio_index];
|
// First store the current sample(s) into the FIR filter's ring buffer
|
||||||
|
if(channels == 0) {
|
||||||
|
fir_buffer_mono[fir_index] = audio_buffer[audio_index];
|
||||||
|
} else {
|
||||||
|
// In stereo operation, generate sum and difference signals
|
||||||
|
fir_buffer_mono[fir_index] =
|
||||||
|
audio_buffer[audio_index] + audio_buffer[audio_index+1];
|
||||||
|
fir_buffer_stereo[fir_index] =
|
||||||
|
audio_buffer[audio_index] - audio_buffer[audio_index+1];
|
||||||
|
}
|
||||||
fir_index++;
|
fir_index++;
|
||||||
if(fir_index >= FIR_SIZE) fir_index = 0;
|
if(fir_index >= FIR_SIZE) fir_index = 0;
|
||||||
|
|
||||||
|
// Now apply the FIR low-pass filter
|
||||||
|
|
||||||
/* As the FIR filter is symmetric, we do not multiply all
|
/* As the FIR filter is symmetric, we do not multiply all
|
||||||
the coefficients independently, but two-by-two, thus reducing
|
the coefficients independently, but two-by-two, thus reducing
|
||||||
the total number of multiplications by a factor of two
|
the total number of multiplications by a factor of two
|
||||||
*/
|
*/
|
||||||
out = 0;
|
float out_mono = 0;
|
||||||
|
float out_stereo = 0;
|
||||||
int ifbi = fir_index; // ifbi = increasing FIR Buffer Index
|
int ifbi = fir_index; // ifbi = increasing FIR Buffer Index
|
||||||
int dfbi = fir_index; // dfbi = decreasing FIR Buffer Index
|
int dfbi = fir_index; // dfbi = decreasing FIR Buffer Index
|
||||||
for(int fi=0; fi<FIR_HALF_SIZE; fi++) { // fi = Filter Index
|
for(int fi=0; fi<FIR_HALF_SIZE; fi++) { // fi = Filter Index
|
||||||
dfbi--;
|
dfbi--;
|
||||||
if(dfbi < 0) dfbi = FIR_SIZE-1;
|
if(dfbi < 0) dfbi = FIR_SIZE-1;
|
||||||
out += low_pass_fir[fi] * (fir_buffer[ifbi] + fir_buffer[dfbi]);
|
out_mono +=
|
||||||
|
low_pass_fir[fi] *
|
||||||
|
(fir_buffer_mono[ifbi] + fir_buffer_mono[dfbi]);
|
||||||
|
if(channels > 1) {
|
||||||
|
out_stereo +=
|
||||||
|
low_pass_fir[fi] *
|
||||||
|
(fir_buffer_stereo[ifbi] + fir_buffer_stereo[dfbi]);
|
||||||
|
}
|
||||||
ifbi++;
|
ifbi++;
|
||||||
if(ifbi >= FIR_SIZE) ifbi = 0;
|
if(ifbi >= FIR_SIZE) ifbi = 0;
|
||||||
}
|
}
|
||||||
// End of FIR filter
|
// End of FIR filter
|
||||||
|
|
||||||
|
|
||||||
mpx_buffer[i] = (rds_buffer[i] +
|
mpx_buffer[i] =
|
||||||
4*out /* +
|
mpx_buffer[i] + // RDS data samples are currently in mpx_buffer
|
||||||
2 * carrier_38[phase_38] * out +
|
4.05*out_mono; // Unmodulated monophonic (or stereo-sum) signal
|
||||||
.1*carrier_19[phase_19]*/) / 10;
|
|
||||||
|
if(channels>1) {
|
||||||
phase_19++;
|
mpx_buffer[i] +=
|
||||||
phase_38++;
|
4.05 * carrier_38[phase_38] * out_stereo + // Stereo difference signal
|
||||||
if(phase_19 >= 12) phase_19 = 0;
|
.09*carrier_19[phase_19]; // Stereo pilot tone
|
||||||
if(phase_38 >= 6) phase_38 = 0;
|
|
||||||
|
phase_19++;
|
||||||
|
phase_38++;
|
||||||
|
if(phase_19 >= 12) phase_19 = 0;
|
||||||
|
if(phase_38 >= 6) phase_38 = 0;
|
||||||
|
}
|
||||||
|
|
||||||
audio_pos++;
|
audio_pos++;
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
return 0; // TODO
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
@ -186,5 +244,8 @@ int fm_mpx_close() {
|
||||||
if(sf_close(inf) ) {
|
if(sf_close(inf) ) {
|
||||||
fprintf(stderr, "Error closing audio file");
|
fprintf(stderr, "Error closing audio file");
|
||||||
}
|
}
|
||||||
return 0; // TODO
|
|
||||||
|
free(audio_buffer);
|
||||||
|
|
||||||
|
return 0;
|
||||||
}
|
}
|
|
@ -21,6 +21,6 @@
|
||||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
extern int fm_mpx_open(char *filename);
|
extern int fm_mpx_open(char *filename, size_t len);
|
||||||
extern int fm_mpx_get_samples(float *mpx_buffer);
|
extern int fm_mpx_get_samples(float *mpx_buffer);
|
||||||
extern int fm_mpx_close();
|
extern int fm_mpx_close();
|
|
@ -36,17 +36,20 @@
|
||||||
|
|
||||||
/* Simple test program */
|
/* Simple test program */
|
||||||
int main(int argc, char **argv) {
|
int main(int argc, char **argv) {
|
||||||
if(argc < 3) {
|
if(argc < 4) {
|
||||||
fprintf(stderr, "Error: missing argument.\n");
|
fprintf(stderr, "Error: missing argument.\n");
|
||||||
fprintf(stderr, "Syntax: rds_wav <out.wav> <text>\n");
|
fprintf(stderr, "Syntax: rds_wav <in_file> <out.wav> <text>\n");
|
||||||
return EXIT_FAILURE;
|
return EXIT_FAILURE;
|
||||||
}
|
}
|
||||||
|
|
||||||
set_rds_pi(0x1234);
|
set_rds_pi(0x1234);
|
||||||
set_rds_ps(argv[2]);
|
set_rds_ps(argv[3]);
|
||||||
set_rds_rt(argv[2]);
|
set_rds_rt(argv[3]);
|
||||||
|
|
||||||
fm_mpx_open("sound_22050.wav");
|
if(fm_mpx_open(argv[1], LENGTH) != 0) {
|
||||||
|
printf("Could not setup FM mulitplex generator.\n");
|
||||||
|
return EXIT_FAILURE;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
@ -62,8 +65,8 @@ int main(int argc, char **argv) {
|
||||||
sfinfo.seekable = 0;
|
sfinfo.seekable = 0;
|
||||||
|
|
||||||
// Open the output file
|
// Open the output file
|
||||||
if (! (outf = sf_open(argv[1], SFM_WRITE, &sfinfo))) {
|
if (! (outf = sf_open(argv[2], SFM_WRITE, &sfinfo))) {
|
||||||
fprintf(stderr, "Error: could not open output file %s.\n", argv[1]) ;
|
fprintf(stderr, "Error: could not open output file %s.\n", argv[2]) ;
|
||||||
return EXIT_FAILURE;
|
return EXIT_FAILURE;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -71,6 +74,11 @@ int main(int argc, char **argv) {
|
||||||
|
|
||||||
for(int j=0; j<40; j++) {
|
for(int j=0; j<40; j++) {
|
||||||
fm_mpx_get_samples(mpx_buffer);
|
fm_mpx_get_samples(mpx_buffer);
|
||||||
|
|
||||||
|
// scale samples
|
||||||
|
for(int i=0; i<LENGTH; i++) {
|
||||||
|
mpx_buffer[i] /= 10.;
|
||||||
|
}
|
||||||
|
|
||||||
if(sf_write_float(outf, mpx_buffer, LENGTH) != LENGTH) {
|
if(sf_write_float(outf, mpx_buffer, LENGTH) != LENGTH) {
|
||||||
fprintf(stderr, "Error: writing to file %s.\n", argv[1]);
|
fprintf(stderr, "Error: writing to file %s.\n", argv[1]);
|
||||||
|
|
Plik binarny nie jest wyświetlany.
Ładowanie…
Reference in New Issue