acoustics.cc

机器人仿真平台,和stage配合运行

{

  player_audiodsp_config_t config;

  if( len != 1 )
  {
    PLAYER_ERROR2("config request len is invalid (%d != %d)",len,1);
    if(PutReply(client, PLAYER_MSGTYPE_RESP_NACK) != 0)
      PLAYER_ERROR("PutReply() failed");
    return 1;
  }

  config.sampleFormat = htons(this->sampleFormat);
  config.sampleRate = htons(this->sampleRate);
  config.channels = this->channels;

  if( PutReply(client, PLAYER_MSGTYPE_RESP_ACK, NULL, &config,
        sizeof(config)) != 0)
    PLAYER_ERROR("PutReply() failed");

  return 0;
}

int Acoustics::SetSampleFormat( int _format )
{
  int result=0;

  this->sampleFormat = _format;

  // Try to set the sample format
  if (ioctl(this->audioFD, SNDCTL_DSP_SETFMT, &this->sampleFormat) == -1 )
  {
    PLAYER_ERROR1("error setting sample format: %d",_format);
    this->sampleFormat=255;

    if(ioctl(this->audioFD,SNDCTL_DSP_SETFMT, &this->sampleFormat) == -1 )
      result = -1;
  }

  // Check if we were able to set the specified format
  if( !result && this->sampleFormat != _format )
  {
    PLAYER_WARN2("specified format %d set to %d",_format,this->sampleFormat);
  }

  // Get the bytes per sample
  switch( this->sampleFormat )
  {
    // Formats with 4 bits per sample
    case AFMT_IMA_ADPCM:
      this->bytesPerSample = 0.5;
      break;

    // Formats with 8 bits per sample
    case AFMT_MU_LAW:
    case AFMT_A_LAW:
    case AFMT_U8:
    case AFMT_S8:
      this->bytesPerSample = 1.0;
      break;

    // Formats with 16 bits per sample
    case AFMT_S16_LE:
    case AFMT_S16_BE:
    case AFMT_U16_LE:
    case AFMT_U16_BE:
      this->bytesPerSample = 2.0;
      break;
    default:
      this->bytesPerSample = 2.0;
      break;
  }

  return result;
}

int Acoustics::SetSampleRate( int _sampleRate )
{
  int result = 0;

  this->sampleRate = _sampleRate;

  // Try to set the sample rate
  if( ioctl(audioFD, SNDCTL_DSP_SPEED, &this->sampleRate) == -1 )
  {
    PLAYER_ERROR1("error setting sample rate:%d",this->sampleRate);
    result = 1;
  }

  // Check if the sample rate was set properly
  if( this->sampleRate != _sampleRate ) 
  {
    PLAYER_WARN2("specified rate:%d set to: %d",_sampleRate, this->sampleRate);
  }

  return result;
}

int Acoustics::SetChannels( unsigned short _channels )
{
  int result = 0;

  this->channels = _channels;

  // Try to set the number of channels
  if( ioctl(audioFD, SNDCTL_DSP_CHANNELS, &this->channels) == -1)
  {
    PLAYER_ERROR("error setting the number of channels");
    result = 1;
  }

  return result;
}

int Acoustics::SetBufferSize(int _size)
{
  int result=0;

  // If size==0, then calc. the proper size.
  if( _size <= 0 )
  {
    ioctl( audioFD, SNDCTL_DSP_GETBLKSIZE, &this->audioBuffSize);
    if( this->audioBuffSize < 1)
    {
      PLAYER_ERROR("failed to calculate audio buffer size");
      result = -1;
    }

  } else {
    this->audioBuffSize = _size;
  }

  if( !result && this->audioBuffSize > 0 )
  {
    if( this->audioBuffer )
      delete [] this->audioBuffer;

    this->audioBuffer = new char[this->audioBuffSize];
  } else
    result = -1;

  return result;
}


int Acoustics::ListenForTones()
{
  int result=0;

  if( !Record() )
  {
    int i,k;

    int frequency[this->N/2];
    int amplitude[this->N/2];

    // We will just do a Fourer transform over the first 1024 samples.
    double* fft = new double[this->N];
    if( this->bytesPerSample == 2 )
    {
      char* index = this->audioBuffer;
      for(i=0;i<this->N;i++)
      {
        switch( this->sampleFormat )
        {
          case AFMT_S16_LE:
          case AFMT_U16_LE:
            fft[i] = (short)( ( (*(index+1))&0xFF) << 8 | ( (*index)&0xFF ));
            break;

          case AFMT_S16_BE:
          case AFMT_U16_BE:
            fft[i] = (short)( ((*index)&0xFF) << 8 | (*(index+1))&0xFF);
            break;
        }

        index+=2;
      }

    } else {
      char* index = this->audioBuffer;
      for(i=0;i<this->N;i++)     
      {
        fft[i] = (*index);
        index++;
      }
    }
    
    gsl_fft_real_radix2_transform(fft,1,this->N);

    // Convert to power spectrum
    for (k = 1; k < (this->N+1)/2; ++k)  /* (k < N/2 rounded up) */
      frequency[k] = (int)((fft[k]*fft[k] + fft[this->N-k]*fft[this->N-k])/1000);
    if (this->N % 2 == 0) /* N is even,  Nyquist freq. */
      frequency[this->N/2] = (int)((fft[this->N/2]*fft[this->N/2])/1000);  

    // I think this does a bit of smoothing
    amplitude[0]=frequency[0]+frequency[1]/2;
    for (k = 1; k < (this->N-1)/2; ++k)  /* (k < N/2 rounded up) */ 
      amplitude[k] = (frequency[k-1]+frequency[k+1])/2+frequency[k];
    amplitude[(this->N-1)/2]=frequency[(this->N-3)/2]/2+frequency[(this->N-1)/2];

    // Initialize the peak data
    for (i=0; i<this->nHighestPeaks; i++) 
    {
      this->peakFreq[i]=0;
      this->peakAmp[i]=0;
    }

    for (i=MIN_FREQUENCY*this->N/this->sampleRate; i<(this->N-1)/2; i++) 
    {
      if(amplitude[i] > this->peakAmp[this->nHighestPeaks-1]) 
      {
        if((amplitude[i] >= amplitude[i-1]) && (amplitude[i]>amplitude[i+1])) 
        {
          InsertPeak(i,amplitude[i]);
        }
      }
    }

  } else {
    result = -1;
  }

  return result;
}

void Acoustics::InsertPeak(int f,int a) 
{
  int i=this->nHighestPeaks-1;
  int j;

  while (this->peakAmp[i-1]<a && i>0) {
    i--;
  }

  for (j=this->nHighestPeaks-1; j>i; j--) {
    this->peakAmp[j]=this->peakAmp[j-1];
    this->peakFreq[j]=this->peakFreq[j-1];
  }

  this->peakAmp[i]=(unsigned short)(a);
  this->peakFreq[i]=(unsigned short)(f);
}

int Acoustics::PlayBuffer(char* buffer,unsigned int size)
{
  int result;

  // Open the device for writing
  result = this->OpenDevice(O_WRONLY);

  // Write the audio data to the device
  if( !result && write(audioFD,buffer,size) != (int)size )
  {
    PLAYER_ERROR("played wrong number of bytes");
    result = -1;
  }

  // Wait for the sync to occur
  if( !result && ioctl(audioFD,SNDCTL_DSP_SYNC,0) == -1 )
  {
    PLAYER_ERROR("ioctl SNDCTL_DSP_SYNC failed");
    result = -1;
  }

  return result;
}

int Acoustics::Record()
{
  int result = this->OpenDevice(O_RDONLY);

  if( !result && read(audioFD, this->audioBuffer, this->audioBuffSize) != this->audioBuffSize)
  {
    PLAYER_WARN("did not read specified amount from audio device");
    result = -1;
  }

  return result;
}


void Acoustics::CreateChirp(unsigned char mseq[], unsigned short mseqSize, 
    unsigned short freq, unsigned short amp, unsigned int pulseTime, 
    char* buffer, unsigned int bufSize )
{
  unsigned int i;
  unsigned int pulseBufSize;
  char* oneBuffer = NULL;
  char* zeroBuffer = NULL;

  // Create the two buffers
  pulseBufSize = this->CalcBuffSize( pulseTime );
  oneBuffer = new char[pulseBufSize];
  zeroBuffer=new char[pulseBufSize];

  // Create one carrier pulse
  CreateSine(freq,amp,pulseTime,oneBuffer,pulseBufSize);

  // Make the zero buffer 180 degrees out of phase from the one buffer
  memcpy(zeroBuffer,oneBuffer,pulseBufSize);
  for(i=0;i<pulseBufSize;i++)
  {
    zeroBuffer[i]*=-1;
  }

  for(i=0;i<mseqSize;i++)
  {
    if(mseq[i])
    {
      memcpy(buffer+pulseBufSize*i,zeroBuffer,pulseBufSize);
    } else {
      memcpy(buffer+pulseBufSize*i,oneBuffer,pulseBufSize);
    }

  }

  delete [] oneBuffer;
  delete [] zeroBuffer;

  oneBuffer=NULL;
  zeroBuffer=NULL;
}

// This function might behave badly when the durations is too small....
void Acoustics::CreateSine(unsigned short freq, unsigned short amp, 
    unsigned int duration, char* buffer, unsigned int bufferSize)
{
  unsigned int i;

  double omega = (double)freq*2*M_PI/(double)this->sampleRate;
  double phase = 0;

  unsigned int numSamples = (unsigned int)((duration/1000.0)*this->sampleRate);

  short audio;

  // Calculate the first full wave
  for(phase=0,i=0;phase<2*M_PI&&i<numSamples;phase+=omega,i++)
  {
    audio=(short)(amp*sin(phase));

    if(this->channels==1)
    {
      buffer[2*i]=(audio>>8)&0xff;
      buffer[2*i+1]=audio&0xff;
    } else {
      buffer[4*i]=audio&0xff;
      buffer[4*i+1]=audio>>8;
      buffer[4*i+2]=audio&0xff;
      buffer[4*i+3]=audio>>8;
    }
  }

  // Get the rest of the data
  unsigned int bytesPerCopy = i*this->channels*2;
  for(unsigned int j=bytesPerCopy;j<bufferSize;j+=bytesPerCopy)
  {
    memcpy(buffer+j,buffer,bytesPerCopy);
  }

}

unsigned int Acoustics::CalcBuffSize( unsigned int duration )
{
  unsigned int numSamples = (unsigned int)((duration/1000.0)*this->sampleRate);
  return (unsigned int)(numSamples*this->bytesPerSample*this->channels); 
}