Raspberry Pi 3上で151タップのFIRフィルタを実現する短いCプログラムです。オーディオ信号はIC-7410からUSBオーディオインターフェース経由で入力され、低域通過フィルタ処理されて、ボード上のアナログオーディオ端子から出力されます。
ウォーターフォール画面の上半分と下半分とは、それぞれ低域通過フィルタ前後の信号を示しています。
/* (c) JH1OOD, Mike */
/* % gcc -Wall -std=c99 test.c -o test -lm -lasound -lfftw3 */
#define NO_DEBUG
#define NO_MARKER
#define NFFT 4096
#define WINDOW_XSIZE 1320
#define WINDOW_YSIZE 500
#define AREA1_XSIZE 99
#define AREA1_YSIZE 50
#define WATERFALL_XSIZE 512
#define WATERFALL_YSIZE 768
#define WAVEFORM_LEN 128
#define BAUDRATE B19200
#define TIMEOUT_VALUE 100
#define END_OF_COMMAND 0xfd
#define M_PI 3.141592653589793
#include "asoundlib.h"
#include <fftw3.h>
#include <alloca.h>
#include <complex.h>
#include <ctype.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <math.h>
#include <sched.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <termios.h>
#include <unistd.h>
static char myrig [256] = "/dev/ttyUSB0"; /* IC-7410 control */
static char device [256] = "hw:1,0"; /* capture device (IC-7410) */
static char device2[256] = "hw:0,0"; /* playback device (Raspberry) */
static unsigned int myrate [2] = { 32000, 32000}; /* stream rate */
static unsigned int mychannels [2] = { 1, 2}; /* count of channels */
static unsigned int mybuffer_time[2] = {512000, 512000}; /* ring buffer length in us */
static unsigned int myperiod_time[2] = {128000, 128000}; /* ring buffer length in us */
static snd_pcm_sframes_t mybuffer_size[2];
static snd_pcm_sframes_t myperiod_size[2];
static int byte_per_sample = 2; /* 16 bit format */
static double audio_signal [NFFT];
static double audio_signal_ffted[NFFT];
static double fft_window [NFFT];
static int nsamples;
static double bin_size;
static double amax = 14.0, amin = 7.0;
int ntap = 151;
double lpf[151] = {
1.247414986406200e-19 ,
4.334705494590657e-05 ,
8.850060712564477e-05 ,
1.361006393933032e-04 ,
1.866984580479588e-04 ,
2.406936513109861e-04 ,
2.982732904863955e-04 ,
3.593558051438401e-04 ,
4.235419118958102e-04 ,
4.900748511120966e-04 ,
5.578119888415469e-04 ,
6.252095759297119e-04 ,
6.903221284279323e-04 ,
7.508175103284513e-04 ,
8.040083705582066e-04 ,
8.469001216194019e-04 ,
8.762551590938831e-04 ,
8.886725221742926e-04 ,
8.806816987992936e-04 ,
8.488487984711850e-04 ,
7.898928649533107e-04 ,
7.008096928689311e-04 ,
5.790001590430827e-04 ,
4.223997924009963e-04 ,
2.296060950694531e-04 ,
1.269465717218602e-18 ,
-2.661421870477758e-04 ,
-5.675499571190165e-04 ,
-9.018763238075437e-04 ,
-1.265635303898488e-03 ,
-1.654162014855776e-03 ,
-2.061597828909119e-03 ,
-2.480902523911547e-03 ,
-2.903894804281316e-03 ,
-3.321321943257440e-03 ,
-3.722958634097960e-03 ,
-4.097734447801007e-03 ,
-4.433888594504296e-03 ,
-4.719149991620855e-03 ,
-4.940939970926038e-03 ,
-5.086594325967014e-03 ,
-5.143600826465778e-03 ,
-5.099847822969537e-03 ,
-4.943879146617302e-03 ,
-4.665150187537139e-03 ,
-4.254279820994538e-03 ,
-3.703292750564303e-03 ,
-3.005846857317347e-03 ,
-2.157440285595270e-03 ,
-1.155593258862735e-03 ,
-3.153189039542980e-18 ,
1.307353376967917e-03 ,
2.762108541214490e-03 ,
4.357495036116219e-03 ,
6.084332872893948e-03 ,
7.931078886295701e-03 ,
9.883917240592958e-03 ,
1.192689357713280e-02 ,
1.404209139127653e-02 ,
1.620984833667827e-02 ,
1.840900929837332e-02 ,
2.061721227239471e-02 ,
2.281120235698237e-02 ,
2.496716851584792e-02 ,
2.706109723255228e-02 ,
2.906913674980633e-02 ,
3.096796528888811e-02 ,
3.273515648009884e-02 ,
3.434953521002414e-02 ,
3.579151720701521e-02 ,
3.704342594131541e-02 ,
3.808978080607253e-02 ,
3.891755106250746e-02 ,
3.951637066630648e-02 ,
3.987870982977506e-02 ,
4.000000000000000e-02 ,
3.987870982977506e-02 ,
3.951637066630648e-02 ,
3.891755106250746e-02 ,
3.808978080607253e-02 ,
3.704342594131541e-02 ,
3.579151720701521e-02 ,
3.434953521002414e-02 ,
3.273515648009884e-02 ,
3.096796528888811e-02 ,
2.906913674980633e-02 ,
2.706109723255228e-02 ,
2.496716851584792e-02 ,
2.281120235698237e-02 ,
2.061721227239471e-02 ,
1.840900929837332e-02 ,
1.620984833667827e-02 ,
1.404209139127653e-02 ,
1.192689357713280e-02 ,
9.883917240592958e-03 ,
7.931078886295701e-03 ,
6.084332872893948e-03 ,
4.357495036116219e-03 ,
2.762108541214490e-03 ,
1.307353376967917e-03 ,
-3.153189039542980e-18 ,
-1.155593258862735e-03 ,
-2.157440285595270e-03 ,
-3.005846857317347e-03 ,
-3.703292750564303e-03 ,
-4.254279820994538e-03 ,
-4.665150187537139e-03 ,
-4.943879146617302e-03 ,
-5.099847822969537e-03 ,
-5.143600826465778e-03 ,
-5.086594325967014e-03 ,
-4.940939970926038e-03 ,
-4.719149991620855e-03 ,
-4.433888594504296e-03 ,
-4.097734447801007e-03 ,
-3.722958634097960e-03 ,
-3.321321943257440e-03 ,
-2.903894804281316e-03 ,
-2.480902523911547e-03 ,
-2.061597828909119e-03 ,
-1.654162014855776e-03 ,
-1.265635303898488e-03 ,
-9.018763238075437e-04 ,
-5.675499571190165e-04 ,
-2.661421870477758e-04 ,
1.269465717218602e-18 ,
2.296060950694531e-04 ,
4.223997924009963e-04 ,
5.790001590430827e-04 ,
7.008096928689311e-04 ,
7.898928649533107e-04 ,
8.488487984711850e-04 ,
8.806816987992936e-04 ,
8.886725221742926e-04 ,
8.762551590938831e-04 ,
8.469001216194019e-04 ,
8.040083705582066e-04 ,
7.508175103284513e-04 ,
6.903221284279323e-04 ,
6.252095759297119e-04 ,
5.578119888415469e-04 ,
4.900748511120966e-04 ,
4.235419118958102e-04 ,
3.593558051438401e-04 ,
2.982732904863955e-04 ,
2.406936513109861e-04 ,
1.866984580479588e-04 ,
1.361006393933032e-04 ,
8.850060712564477e-05 ,
4.334705494590657e-05 ,
1.247414986406200e-19
};
int fd = -1;
double *in;
fftw_complex *out;
fftw_plan p;
static signed short ringbuffer[16384]; // 0x4000
static int wpnt = 0; // 0x0000
static int rpnt = 16384 / 2; // 0x2000
// ---------------------------------------------------------------------
static int myset_hwparams(snd_pcm_t *handle, snd_pcm_hw_params_t *params, int id) {
snd_pcm_uframes_t size;
int err, dir;
fprintf(stderr, "myset_hwparams: id = %2d \n", id);
/* choose all parameters */
err = snd_pcm_hw_params_any(handle, params);
if (err < 0) {
printf("Broken configuration for playback: no configurations available: %s\n",
snd_strerror(err));
return err;
}
/* set hardware resampling */
err = snd_pcm_hw_params_set_rate_resample(handle, params, 0); // 0: no resampling
if (err < 0) {
printf("Resampling setup failed for playback: %s\n", snd_strerror(err));
return err;
}
/* set the interleaved read/write format */
err = snd_pcm_hw_params_set_access(handle, params, SND_PCM_ACCESS_RW_INTERLEAVED);
if (err < 0) {
printf("Access type not available for playback: %s\n", snd_strerror(err));
return err;
}
/* set the sample format */
err = snd_pcm_hw_params_set_format(handle, params, SND_PCM_FORMAT_S16);
if (err < 0) {
printf("Sample format not available for playback: %s\n", snd_strerror(err));
return err;
}
/* set the count of channels */
err = snd_pcm_hw_params_set_channels(handle, params, mychannels[id]);
if (err < 0) {
printf("Channels count (%i) not available for the device: %s\n", mychannels[id],
snd_strerror(err));
return err;
}
/* set the stream rate */
unsigned int rrate;
rrate = myrate[id];
err = snd_pcm_hw_params_set_rate_near(handle, params, &rrate, 0);
fprintf(stderr, "rate = %d, rrate = %d \n", myrate[id], rrate);
if (err < 0) {
printf("Rate %iHz not available for playback: %s\n", myrate[id], snd_strerror(err));
return err;
}
if (rrate != myrate[id]) {
printf("set_hwparams2: Rate doesn't match (requested %iHz, get %iHz)\n", myrate[id], err);
return -EINVAL;
}
/* set the buffer time */
err = snd_pcm_hw_params_set_buffer_time_near(handle, params, &mybuffer_time[id], &dir);
if (err < 0) {
printf("Unable to set buffer time %i for the device: %s\n", mybuffer_time[id], snd_strerror(err));
return err;
}
err = snd_pcm_hw_params_get_buffer_size(params, &size);
if (err < 0) {
printf("Unable to get buffer size for playback: %s\n", snd_strerror(err));
return err;
}
mybuffer_size[id] = size;
/* set the period time */
err = snd_pcm_hw_params_set_period_time_near(handle, params, &myperiod_time[id], &dir);
if (err < 0) {
printf("Unable to set period time %i for playback: %s\n", myperiod_time[id], snd_strerror(err));
return err;
}
err = snd_pcm_hw_params_get_period_size(params, &size, &dir);
if (err < 0) {
printf("Unable to get period size for playback: %s\n", snd_strerror(err));
return err;
}
myperiod_size[id] = size;
/* write the parameters to device */
err = snd_pcm_hw_params(handle, params);
if (err < 0) {
printf("Unable to set hw params for playback: %s\n", snd_strerror(err));
return err;
}
return 0;
}
// ---------------------------------------------------------------------
static int myset_swparams(snd_pcm_t *handle, snd_pcm_sw_params_t *swparams, int id) {
int err;
fprintf(stderr, "myset_swparams: id = %2d \n", id);
/* get the current swparams */
err = snd_pcm_sw_params_current(handle, swparams);
if (err < 0) {
printf("Unable to determine current swparams for playback: %s\n",
snd_strerror(err));
return err;
}
/* start the transfer when the buffer is almost full: */
err = snd_pcm_sw_params_set_start_threshold(
handle, swparams, (mybuffer_size[id] / myperiod_size[id]) * myperiod_size[id]);
if (err < 0) {
printf("Unable to set start threshold mode for playback: %s\n",
snd_strerror(err));
return err;
}
/* allow the transfer when at least period_size samples can be processed */
err = snd_pcm_sw_params_set_avail_min(handle, swparams, myperiod_size[id]);
if (err < 0) {
printf("Unable to set avail min for playback: %s\n", snd_strerror(err));
return err;
}
/* write the parameters to the playback device */
err = snd_pcm_sw_params(handle, swparams);
if (err < 0) {
printf("Unable to set sw params for playback: %s\n", snd_strerror(err));
return err;
}
return 0;
}
// ---------------------------------------------------------------------
static void async_callback2(snd_async_handler_t *ahandler) {
snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
signed short *samples = snd_async_handler_get_callback_private(ahandler);
snd_pcm_sframes_t avail;
int err;
static int icount = 0;
avail = snd_pcm_avail_update(handle);
while (avail >= myperiod_size[1]) {
err = snd_pcm_writei(handle, samples, myperiod_size[1]);
if (err < 0) {
printf("Write error: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
if (err != myperiod_size[1]) {
printf("Write error: written %i expected %li\n", err, myperiod_size[1]);
exit(EXIT_FAILURE);
}
fprintf( stderr, "async_callback2: icount = %8d, avail = %12ld, period_size = %12ld \n",
icount++, avail, myperiod_size[1]);
for (int i = 0; i < 4096; i++) {
double val = 0.0;
int index = 0;
for (int i=0;i<ntap;i++) {
index = rpnt - i;
if (index < 0) index += 16384;
val += ringbuffer[index] * lpf[i];
}
samples[2 * i] = (signed short) val;
samples[2 * i + 1] = (signed short) val;
rpnt++;
rpnt &= 0x3fff;
}
avail = snd_pcm_avail_update(handle);
}
}
// ---------------------------------------------------------------------
static int async_loop2(snd_pcm_t *handle, signed short *samples) {
snd_async_handler_t *ahandler;
int err, count;
err = snd_async_add_pcm_handler(&ahandler, handle, async_callback2, samples);
if (err < 0) {
printf("Unable to register async handler\n");
exit(EXIT_FAILURE);
}
for (count = 0; count < 2; count++) {
err = snd_pcm_writei(handle, samples, myperiod_size[1]);
if (err < 0) {
printf("Initial write error: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
if (err != myperiod_size[1]) {
printf("Initial write error: written %i expected %li\n", err,
myperiod_size[1]);
exit(EXIT_FAILURE);
}
}
if (snd_pcm_state(handle) == SND_PCM_STATE_PREPARED) {
err = snd_pcm_start(handle);
if (err < 0) {
printf("Start error: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
}
return 0;
}
// ---------------------------------------------------------------------
static void async_callback(snd_async_handler_t *ahandler) {
snd_pcm_t *handle = snd_async_handler_get_pcm(ahandler);
signed short *samples = snd_async_handler_get_callback_private(ahandler);
snd_pcm_sframes_t avail;
int err;
static int icount = 0;
avail = snd_pcm_avail_update(handle);
while (avail >= myperiod_size[0]) {
err = snd_pcm_readi(handle, samples, myperiod_size[0]);
if (err < 0) {
fprintf(stderr, "Write error: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
if (err != myperiod_size[0]) {
fprintf(stderr, "Write error: written %i expected %li\n", err,
myperiod_size[0]);
exit(EXIT_FAILURE);
}
fprintf( stderr, "async_callback : icount = %8d, avail = %12ld, period_size = %12ld \n",
icount++, avail, myperiod_size[0]);
for (int i = 0; i < 4096; i++) {
ringbuffer[wpnt++] = samples[i];
wpnt &= 0x3fff;
}
for (int i = 0; i < NFFT; i++) { /* NFFT=period_size */
audio_signal[i] = samples[i];
}
/* audio signal FFT */
for (int i = 0; i < NFFT; i++) {
in[i] = fft_window[i] * audio_signal[i];
}
fftw_execute(p);
/* log10 and normalize */
for (int i = 0; i < NFFT / 4; i++) {
double val;
val = out[i][0] * out[i][0] + out[i][1] * out[i][1];
if (val < pow(10.0, amin)) {
audio_signal_ffted[i] = 0.0;
} else if (val > pow(10.0, amax)) {
audio_signal_ffted[i] = 1.0;
} else {
audio_signal_ffted[i] = (log10(val) - amin) / (amax - amin);
}
// if(i<512) fprintf(stdout, "%1d", (int) (10.0*audio_signal_ffted[i])
// ); // apple
}
// fprintf(stdout, "\n"); // apple
// fflush(stdout); // apple
avail = snd_pcm_avail_update(handle);
}
}
// ---------------------------------------------------------------------
static int async_loop(snd_pcm_t *handle, signed short *samples) {
snd_async_handler_t *ahandler;
int err;
fprintf(stderr, "async_loop: period_size = %8ld, nsamples = %d \n",
myperiod_size[0], nsamples);
err = snd_async_add_pcm_handler(&ahandler, handle, async_callback, samples);
if (err < 0) {
fprintf(stderr, "Unable to register async handler\n");
exit(EXIT_FAILURE);
}
if (snd_pcm_state(handle) == SND_PCM_STATE_PREPARED) {
err = snd_pcm_start(handle);
if (err < 0) {
fprintf(stderr, "Start error: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
}
return 0;
}
// ---------------------------------------------------------------------
void serial_init(void) {
struct termios tio;
memset(&tio, 0, sizeof(tio));
tio.c_cflag = CS8 | CLOCAL | CREAD;
tio.c_cc[VEOL] = 0xfd; /* IC-7410 postamble */
tio.c_lflag = 0; /* non canonical mode */
tio.c_cc[VTIME] = 0; /* non canonical mode */
tio.c_cc[VMIN] = 1; /* non canonical mode */
tio.c_iflag = IGNPAR | ICRNL;
cfsetispeed(&tio, BAUDRATE);
cfsetospeed(&tio, BAUDRATE);
tcsetattr(fd, TCSANOW, &tio);
}
// ---------------------------------------------------------------------
int main(int argc, char *argv[]) {
struct termios oldtio;
snd_pcm_t *handle;
snd_pcm_t *handle2;
snd_pcm_hw_params_t *hwparams;
snd_pcm_hw_params_t *hwparams2;
snd_pcm_sw_params_t *swparams;
snd_pcm_sw_params_t *swparams2;
signed short *samples;
signed short *mysamples;
int err;
int id; /* 0: capture device, 1: playback device */
for(int i=0;i<ntap;i++) fprintf(stderr, "%20.10f \n", lpf[i]);
setvbuf(stdout, NULL, _IOFBF, 0);
if (argc != 4) {
fprintf(stderr, "Usage %s /dev/ttyUSB0 hw:1,0 hw:0,0 \n", argv[0]);
fprintf(stderr,
" try ls -l /dev/ttyUSB*; arecord -l; aplay -l to know "
"these parameters.\n");
return -1;
}
strcpy(myrig , argv[1]);
strcpy(device , argv[2]);
strcpy(device2, argv[3]);
fprintf(stderr, "serial device is [%s] \n", myrig);
fprintf(stderr, "audio capture device is [%s] \n", device);
fprintf(stderr, "audio output device is [%s] \n", device2);
fprintf(stderr, "byte_per_sample = %d \n", byte_per_sample);
bin_size = myrate[0] / (double)NFFT;
for (int i = 0; i < NFFT; i++) {
fft_window[i] = 0.54 - 0.46 * cos(2.0 * M_PI * i / (double)NFFT);
}
in = malloc(sizeof(double) * NFFT);
out = (fftw_complex *) fftw_malloc(sizeof(fftw_complex) * (NFFT / 2 + 1));
p = fftw_plan_dft_r2c_1d(NFFT, in, out, FFTW_MEASURE);
snd_pcm_hw_params_alloca(&hwparams);
snd_pcm_hw_params_alloca(&hwparams2);
snd_pcm_sw_params_alloca(&swparams);
snd_pcm_sw_params_alloca(&swparams2);
if ((err = snd_pcm_open(&handle, device, SND_PCM_STREAM_CAPTURE, 0)) < 0) {
fprintf(stderr, "Audio capture devic3 open error: %s\n", snd_strerror(err));
return 0;
}
if ((err = snd_pcm_open(&handle2, device2, SND_PCM_STREAM_PLAYBACK, 0)) < 0) {
fprintf(stderr, "Audio output devic open error: %s\n", snd_strerror(err));
return 0;
}
id = 0; /* capture device */
if ((err = myset_hwparams(handle, hwparams, id)) < 0) {
fprintf(stderr, "Setting of hwparams failed: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
if ((err = myset_swparams(handle, swparams, id)) < 0) {
fprintf(stderr, "Setting of swparams failed: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
id = 1; /* playback device */
if ((err = myset_hwparams(handle2, hwparams2, id)) < 0) {
printf("Setting of hwparams failed: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
if ((err = myset_swparams(handle, swparams, id)) < 0) {
fprintf(stderr, "Setting of swparams failed: %s\n", snd_strerror(err));
exit(EXIT_FAILURE);
}
nsamples = myperiod_size[0] * mychannels[0] * byte_per_sample;
fprintf(stderr, "main: period_size = %8ld, nsamples = %d \n", myperiod_size[0],
nsamples);
samples = malloc(nsamples);
if (samples == NULL) {
fprintf(stderr, "cannot malloc samples \n");
exit(EXIT_FAILURE);
}
mysamples = malloc(32768);
if (mysamples == NULL) {
fprintf(stderr, "cannot malloc mysamples \n");
exit(EXIT_FAILURE);
}
for (int i = 0; i < 4096; i++) {
mysamples[2 * i + 1] = 8096.0 * sin(i * 2.0 * 3.14 / 128.0);
mysamples[2 * i + 0] = 8096.0 * sin(i * 2.0 * 3.14 / 128.0);
}
if ((err = async_loop(handle, samples)) < 0) {
fprintf(stderr, "async_loop set error: %s\n", snd_strerror(err));
}
if ((err = async_loop2(handle2, mysamples)) < 0) {
fprintf(stderr, "async_loop2 set error: %s\n", snd_strerror(err));
}
fd = open(myrig, O_RDWR | O_NOCTTY);
if (fd < 0) {
fprintf(stderr, "Error: can not open %s \n", myrig);
return (-1);
}
tcgetattr(fd, &oldtio);
serial_init();
while (1) {
sleep(10000);
}
fftw_destroy_plan(p);
fftw_free(in);
fftw_free(out);
return 0;
}