localisation_audio/sound-source-loc_v.1.0.3.cpp

226 lines
4.9 KiB
C++

#include <iostream>
using namespace std;
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#include <alsa/asoundlib.h>
#include "/usr/include/alsa/asoundlib.h"
//#define SAMPLE_TYPE float
//#define SAMPLE_TYPE_ALSA SND_PCM_FORMAT_FLOAT_LE
#define SAMPLE_TYPE short
#define SAMPLE_TYPE_ALSA SND_PCM_FORMAT_S16_LE
class RunningAverage {
int _nbValuesForAverage;
int _nbValues;
float _mean;
public:
RunningAverage(int nbValuesForAverage) {
_nbValuesForAverage = nbValuesForAverage;
_mean = 0;
_nbValues = 0;
}
void newValue(SAMPLE_TYPE v) {
if (_nbValues < _nbValuesForAverage)
_nbValues++;
_mean = ((_mean * (_nbValues - 1)) + v) / (float)_nbValues;
}
SAMPLE_TYPE getMean() {
return (SAMPLE_TYPE) _mean;
}
};
class SoundSourceLoc {
static const int _nbSamplesMaxDiff = 13;
static const int _bufferSize = 4096;
static constexpr float _minLevelFactorForValidLoc = 1.05f;
static constexpr float _soundSpeed = 344;
/**
* sound sampling rate in Hz
*/
unsigned int _soundSamplingRate;
static constexpr float _distanceBetweenMicrophones = 0.1f;
RunningAverage* _averageSoundLevel;
snd_pcm_t* _capture_handle;
SAMPLE_TYPE _rightBuffer[_bufferSize];
SAMPLE_TYPE _leftBuffer[_bufferSize];
public:
SoundSourceLoc() {
_averageSoundLevel = new RunningAverage(50);
_soundSamplingRate = 44100;
int err;
snd_pcm_hw_params_t* hw_params;
// ideally use "hw:0,0" for embedded, to limit processing. But check if card support our needs...
const char* device = "plughw:1,0";
if ((err = snd_pcm_open(&_capture_handle, device,
SND_PCM_STREAM_CAPTURE, 0)) < 0) {
fprintf(stderr, "cannot open audio device %s (%s)\n", device,
snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params_malloc(&hw_params)) < 0) {
fprintf(stderr,
"cannot allocate hardware parameter structure (%s)\n",
snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params_any(_capture_handle, hw_params)) < 0) {
fprintf(stderr,
"cannot initialize hardware parameter structure (%s)\n",
snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params_set_access(_capture_handle, hw_params,
SND_PCM_ACCESS_RW_NONINTERLEAVED)) < 0) {
fprintf(stderr, "cannot set access type (%s)\n", snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params_set_format(_capture_handle, hw_params,
SAMPLE_TYPE_ALSA)) < 0) {
fprintf(stderr, "cannot set sample format (%s)\n",
snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params_set_rate_near(_capture_handle, hw_params,
&_soundSamplingRate, 0)) < 0) {
fprintf(stderr, "cannot set sample rate (%s)\n", snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params_set_channels(_capture_handle, hw_params, 2))
< 0) {
fprintf(stderr, "cannot set channel count (%s)\n",
snd_strerror(err));
exit(1);
}
if ((err = snd_pcm_hw_params(_capture_handle, hw_params)) < 0) {
fprintf(stderr, "cannot set parameters (%s)\n", snd_strerror(err));
exit(1);
}
snd_pcm_hw_params_free(hw_params);
if ((err = snd_pcm_prepare(_capture_handle)) < 0) {
fprintf(stderr, "cannot prepare audio interface for use (%s)\n",
snd_strerror(err));
exit(1);
}
}
/** Clean exit */
~SoundSourceLoc() {
snd_pcm_close(_capture_handle);
delete _averageSoundLevel;
}
void run() {
while (true) {
processNextSoundBlock();
}
}
private:
void processNextSoundBlock() {
SAMPLE_TYPE* bufs[2];
bufs[0] = _rightBuffer;
bufs[1] = _leftBuffer;
int err;
if ((err = snd_pcm_readn(_capture_handle, (void**) bufs, _bufferSize))
!= _bufferSize) {
fprintf(stderr, "read from audio interface failed (%s)\n",
snd_strerror(err));
exit(1);
}
SAMPLE_TYPE level = computeLevel(_rightBuffer, _leftBuffer);
_averageSoundLevel->newValue(level);
float relativeLevel = (float) level
/ (float) _averageSoundLevel->getMean();
int minDiff = INT_MAX;
int minDiffTime = -1;
for (int t = -_nbSamplesMaxDiff; t < _nbSamplesMaxDiff; t++) {
int diff = 0;
for (int i = _nbSamplesMaxDiff;
i < _bufferSize - _nbSamplesMaxDiff - 1; i++) {
diff += abs(_leftBuffer[i] - _rightBuffer[i + t]);
}
if (diff < minDiff) {
minDiff = diff;
minDiffTime = t;
}
}
if ((relativeLevel > _minLevelFactorForValidLoc)
&& (minDiffTime > -_nbSamplesMaxDiff)
&& (minDiffTime < _nbSamplesMaxDiff)) {
float angle =
-(float) asin(
(minDiffTime * _soundSpeed)
/ (_soundSamplingRate
* _distanceBetweenMicrophones));
cout << angle << ";" << relativeLevel << endl;
}
}
SAMPLE_TYPE computeLevel(SAMPLE_TYPE right[], SAMPLE_TYPE left[]) {
float level = 0;
for (int i = 0; i < _bufferSize; i++) {
float s = (left[i] + right[i]) / 2;
level += (s * s);
}
level /= _bufferSize;
level = sqrt(level);
return (SAMPLE_TYPE) level;
}
};
int main(int argc, char *argv[]) {
SoundSourceLoc soundLoc;
soundLoc.run();
exit(0);
}