Localisation sonore par GCC
From Eric
Dernière version (préliminaire)
Le code de la version (préliminaire est donné ci-après) :
Fichier "main.c"
// (Eric)
// La fréquence d'horloge est fosc= 7.42MHz x 16 (PLL) =118.72MHz
// Le temps de cycle est fcy = fosc/4, soit :
#define FCY 29680000UL
#include <xc.h>
#include <libpic30.h>
#include <dsp.h>
#include <stdio.h>
#include <float.h>
#include "chuck_fft.h"
//-----------------------------------------------------------------------------
// Configuration du chip
//-----------------------------------------------------------------------------
// Horloge interne à 7.37Mhz, x16 par la PLL # 120Mhz
// soit environ 30 MIPS (il y a 4 tics par cycle instruction)
_FOSC(CSW_FSCM_OFF & FRC_PLL16);
_FWDT(WDT_OFF); /* Watch dog Timer OFF */
_FBORPOR(PBOR_OFF ); /* Brown Out OFF, MCLR Enabled */
_FGS(CODE_PROT_OFF); /* Code Protect OFF */
// Seuil à partir duquel on fait le calcul de position (puissance au carré)
#define PWR_THRESHOLD 0.0
// Nombre total d'échantillons.
// Attention : le buffer contient NB_SAMPLES valeurs réelles
// qui vont donner NB_SAMPLES / 2 valeurs complexes après FFT.
#define NB_SAMPLES 128
//-----------------------------------------------------------------------------
// Définition des fonctions locales
//-----------------------------------------------------------------------------
// Transmet une chaine de caractère sur le port série.
static void print (const char * msg);
// Démarre une séquence d'échantillonnage (63 valeurs))
void do_sampling( void );
// Effectue le calcul de corrélation
void do_gcc( void );
// Affiche le résultat de la FFT sous forme d'histogramme
void dump_data( int channel );
// Normalise dans [0,1] les valeurs complexes à partie imaginaire
// nulle.
void normalize( int channel );
// Retourne la puissance (carrée) maximale du signal complexe.
float maxpwr( int channel);
// Affiche la position par un "#".
// La position est comprise entre 0 et 70.
void show_pos ( int pos ) ;
//-----------------------------------------------------------------------------
// Définition des variables globales internes
//-----------------------------------------------------------------------------
// Compteur d'échantillons.
unsigned char nb_samples=0;
// Les délais calculés par la GCC doivent être filtrés.
// Nombre de délais pris en compte pour le filtrage par moyenne glissante.
// Doit être une puissance de 2.
#define MAX_DELAYS 4
// Variables utilisées pour le filtrage des délais
float delays[MAX_DELAYS]; // Les délais mesurés sur les n derniers cycles;
int delays_ind = 0; // L'index dans le buffer.
//-----------------------------------------------------------------------------
// Définition des variables globales externes
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
// Le signal.
//-----------------------------------------------------------------------------
// La variable "data" contient les NB_SAMPLES échantillons réels en entrée
// et les NB_SAMPLES/2 valeurs complexes en sortie de la FFT.
float data[2][NB_SAMPLES];
// Utilisés pour naviguer dans le tableau data.
static unsigned int * p_int_ch0 = (unsigned int *) & data[0][0];
static unsigned int * p_int_ch1 = (unsigned int *) & data[1][0];
//-----------------------------------------------------------------------------
// Définition des ports
//-----------------------------------------------------------------------------
// Attention : la modification d'un port de sortie doit utiliser LAT et non PORT
// car si on utilise PORT, le compilo. génère un READ-MODIFY-WRITE qui va
// entrainer la modification des bits AUTRES que celui que l'on veut modifier...
#define LED0 LATEbits.LATE0
#define LED1 LATEbits.LATE1
//-----------------------------------------------------------------------------
// Setup ports
//-----------------------------------------------------------------------------
// PORT B
// - D0 : ---
// - D1 : ---
// - D2 : <= SOUND 1 IN (Analog input)
// - D3 : <= SOUND 2 IN (Analog input)
// - D4 : ---
// - D5 : ---
// - D6 : ---
// - D7 : ---
// - D8 : ==
// PORT C
// - D0 : ---
// - D1 : ---
// - D2 : ---
// - D3 : ---
// - D4 : ---
// - D5 : ---
// - D6 : ---
// - D7 : ---
// PORT D
// - D0 : ---
// - D1 : ---
// - D2 : ---
// - D3 : ---
// - D4 : ---
// - D5 : ---
// - D6 : ---
// - D7 : ---
// PORT E
// - D0 : => LED 1
// - D1 : => LED 2
// - D2 : ---
// - D3 : ---
// - D4 : ---
// - D5 : ---
// - D6 : ---
// - D7 : ---
// - D8 : ---
// PORT F
// - D0 : ---
// - D1 : ---
// - D2 : ---
// - D3 : ---
// - D4 : U2RX
// - D5 : U2TX => Vers PC via convertisseur RS232 TTL <=> USB
// - D6 : ---
// - D7 : ---
// Attention : un bit à 1 dans TRIS<X> signifie "ENTREE"
void setup_ports()
{
// Tous les ports à 0
PORTB=0;
PORTC=0;
PORTD=0;
PORTE=0;
PORTF=0;
// Positionnement de la direction des ports (par défaut en entrée)
TRISB = 0xFFFF;
TRISC = 0xFFFF;
TRISD = 0xFFFF;
TRISE = 0xFFFF;
TRISEbits.TRISE0=0; // LED 0
TRISEbits.TRISE1=0; // LED 1
TRISF = 0xFFFF;
}
/*
* Configuration des lignes séries
*/
void setup_serials()
{
// Configuration 8 N 1
U2MODEbits.PDSEL = 0;
U2MODEbits.STSEL = 0;
// Baud Rate Generator (BRG) à 15
// soit :
// Fcy = 7.37e6 x 16 ) / 4
// Bds = Fcy / (16 x (BRG+1)) = 115156 # 115200 bds
U2BRG = 15;
// Activation du port no 2 (broches 28=RX, 27=TX)
U2MODEbits.UARTEN=1;
U2STAbits.UTXEN=1;
}
/*
* Configuration des timers
*/
void setup_timers()
{
// Configuration du timer de conversion de l'ADC
T3CONbits.TCKPS = 0b0; // Prescaler 1/1
T3CONbits.TCS = 0 ; // Internal clock = Tosc/4
TMR3 = 0;
PR3 = 295; // Cette valeur correspond à une fréquence de 100Khz :
// fCy/295 = 7.37e6 x 16 /4 / 295 = 100000
}
/*
* Configuration de l'ADC .
*/
void setup_adc()
{
// Inhibition de l'ADC
ADCON1bits.ADON = 0;
// Configuration en entrée analogique de AN2 et AN3
ADPCFGbits.PCFG2 = 0;
ADPCFGbits.PCFG3 = 0;
// Configuration des références de tension :
ADCON2bits.VCFG= 0b0; // References de tension AVDD / AVSS
// Configuration des temps d'échantillonnage
// TAD=TCYC/2(ADCS+1))
ADCON3bits.ADCS=63;
ADCON3bits.SAMC=31;
ADCON3bits.ADRC= 0; // Utilisation de l'horloge système.
ADCON1bits.SIMSAM = 0b01; // Simultaneous sampling
ADCON1bits.SSRC = 0b010; // Conversion trigger sur le timer 3
ADCON1bits.ASAM = 0b1; // L'échantillonnage est automatique après fin
// de la conversion
ADCON1bits.FORM = 0b0; // Représentation des valeurs en entier.
// Configuration des entrées des samplers (MUXA)
ADCHSbits.CH123NA = 0; // CH1, CH2, CH3 negative input is VREF-
ADCHSbits.CH123SA = 1 ; // CH1 positive input is AN3, CH2 positive input
// is AN4, CH3 positive input is AN5
ADCHSbits.CH0NA = 0; // CH0 negative input is VREF-
ADCHSbits.CH0SA = 2; // CH0 positive input is AN2
ADCSSL = 0b0;
ADCON2bits.CHPS = 0b01; // Convert channels 0 and 1
ADCON2bits.ALTS = 0; // MUX A only
ADCON2bits.CSCNA = 0; // Do not scan inputs
ADCON3bits.ADRC=0b0;
ADCON2bits.BUFM = 0; // 1 seul groupe de 16 buffers
ADCON2bits.SMPI = 1; // Interruption après deux conversions,
// Les résultats sont donc dans ADCBUF0..3 puisqu'on
// échantillonne ch0 et ch1.
// Activation de l'ADC
ADCON1bits.ADON = 1;
// Autoriser les interruptions sur ADC
IEC0bits.ADIE = 1;
}
/*
* Configuration des ITs externes
*/
void setup_it()
{
// Aucune it externe.
}
// Traitement de l'IT 1
void __attribute__((__interrupt__,auto_psv)) _INT1Interrupt(void)
{
// Effacement du bit d'interruption
IFS1bits.INT1IF = 0;
}
// Traitement de l'IT 0
void __attribute__((__interrupt__,auto_psv)) _INT0Interrupt(void)
{
// Effacement du bit d'interruption
IFS0bits.INT0IF = 0;
}
/*
* Handler de l'IT timer
*/
void __attribute__((__interrupt__, auto_psv)) _T1Interrupt( void )
{
// Effacement du bit d'interruption.
IFS0bits.T1IF = 0;
}
// Handler de l'IT ADC.
// Elle est générée toutes les deux phases de sampling.
// Le buffer contient donc 2x2 échantillons (2 par canal).
void __attribute__((interrupt, no_auto_psv)) _ADCInterrupt(void)
{
// On lit les valeurs et on les place dans les buffers.
// (Les valeurs sont placées dans des emplacements
// contigues, donc sur une valeur réelle puis sur une valeur
// complexes. Elles seront déplacées plus tard.)
nb_samples--;
*(p_int_ch0+nb_samples) = ADCBUF0;
*(p_int_ch1+nb_samples) = ADCBUF1;
nb_samples--;
*(p_int_ch0+nb_samples) = ADCBUF2;
*(p_int_ch1+nb_samples) = ADCBUF3;
LED1 = ~LED1;
// On arrête le timer lorsque tous les échantillons ont été obtenus.
if ( ! nb_samples )
T3CONbits.TON = 0;
// Acquittement de l'interruption.
IFS0bits.ADIF = 0;
}
//-----------------------------------------------------------------------------
// Programme principal
//-----------------------------------------------------------------------------
void setup_globals()
{
int i;
for ( i =0; i < MAX_DELAYS; i++)
delays[i]= 1.0;
}
//-----------------------------------------------------------------------------
// Programme principal
//-----------------------------------------------------------------------------
int main()
{
// Configuration des ports d'I/O
setup_ports();
// Configuration des lignes séries
setup_serials();
// Configuration de l'ADC
setup_adc();
// Configuration des interruptions
//setup_it();
// Configuration du timer "ligne"
setup_timers();
// Initialisation des variables globales (si si...)
setup_globals();
while(1) {
LED0 = ~LED0;
do_sampling();
while( nb_samples );
do_gcc();
// __delay_ms(100);
}
}
// Transmet une chaine de caractère sur le port série
void print ( const char * msg)
{
while (*msg)
{
// Attente de disponibilité du buffer
// 1 = buffer full
while ( U2STAbits.UTXBF );
// Envoi du caractère
U2TXREG = *msg;
msg++;
}
}
// Démarre une séquence d'échantillonnage (63 valeurs))
void do_sampling( void )
{
nb_samples = NB_SAMPLES;
T3CONbits.TON = 1; // Démarrer le timer de conversion
}
void do_gcc( void )
{
char msg[40];
int i;
float filtered_delay = 0.0;
// Conversion des échantillons en flottants entre 0 et 1.
for ( i=NB_SAMPLES-1; i>0 ; i--)
{
// Partie réelle.
data[0][i] = ((float) *(p_int_ch0+i)) / 1023.0;
data[1][i] = ((float) *(p_int_ch1+i)) / 1023.0;
}
// Calcul de la FFT sur des échantillons réels.
// data doit contenir 2*N valeurs flottantes.
// La fonction retourne N valeurs complexes.
rfft(&data[0][0], NB_SAMPLES/2, FFT_FORWARD);
rfft(&data[1][0], NB_SAMPLES/2, FFT_FORWARD);
// On ne poursuit le calcul que si la puissance est suffisante.
if (maxpwr(0) > PWR_THRESHOLD)
{
// Calcul du conjugué de fft(sig2)
for ( i=0;i<NB_SAMPLES/2; i++)
data[1][i*2+1] = -data[1][i*2+1];
// Calcul de la GCC : [fft(sig1)*.conj(fft(sig2))];
for ( i=0;i<NB_SAMPLES/2; i++)
{
float tmp1 = data[0][i*2]*data[1][i*2]-data[0][i*2+1]*data[1][i*2+1];
float tmp2 = data[0][i*2]*data[1][i*2+1]+data[1][i*2]*data[0][i*2+1];
data[0][i*2] = tmp1;
data[0][i*2+1] = tmp2;
}
// Calcul de la transformée inverse
cfft(&data[0][0], NB_SAMPLES/2,FFT_INVERSE);
// Normalisation dans [0.0, 1.0]
normalize(0);
// Filtrage
delays[delays_ind] = data[0][2];
delays_ind = ( delays_ind + 1) & (MAX_DELAYS-1);
for ( i = 0; i < MAX_DELAYS ; i++ )
filtered_delay += delays[i];
filtered_delay = filtered_delay / (float) MAX_DELAYS;
// Affichage de la courbe
// dump_data(0);
// Affichage du décalage (100 est au centre)
show_pos((int)(filtered_delay*70.0));
}
}
// Retourne la puissance (carrée) maximale du signal complexe.
float maxpwr( int ch )
{
int i;
// Calcul du max
float pwr, maxpwr = FLT_MIN;
for ( i = 1; i< (NB_SAMPLES/2); i++)
{
pwr=data[ch][i*2]*data[ch][i*2]+data[ch][i*2+1]*data[ch][i*2+1];
if ( pwr > maxpwr )
{
maxpwr = pwr;
}
}
return maxpwr;
}
// Affiche le résultat de la FFT sous forme d'histogramme
void dump_data( int channel )
{
int col, line;
print("\33[2J");
for ( line= 32; line>=0; line--)
{
for (col=1; col< NB_SAMPLES/2; col++)
{
float val = line / 32.0 ;
if ( data[channel][col<<1] >= val )
print("#");
else
print(" ");
}
print("\n\r");
}
}
// Retourne la position sous la forme d'une chaine
// pos est comprise entre 0 et 70.
// ------------<>----
void show_pos ( int pos )
{
char msg[74];
int i = 0;
for ( i = 0; i < pos ; i++ )
msg[i]=' ';
msg[pos]='#';
msg[pos+1]='\r';
msg[pos+2]='\n';
msg[pos+3]=0;
print(msg);
}
/* Normalise les valeurs (réelles) du canal */
void normalize( int channel )
{
int i, imax, imin = 0;
double vmax = FLT_MIN;
double vmin = FLT_MAX;
for ( i =0; i<NB_SAMPLES/2;i++)
{
if ( data[channel][i*2] > vmax )
{
imax = i;
vmax = data[channel][i*2];
}
else
if ( data[channel][i*2] < vmin )
{
imin = i;
vmin = data[channel][i*2];
}
}
if ( vmax != vmin )
for ( i =0; i<NB_SAMPLES/2;i++)
data[channel][i*2] = (data[channel][i*2]-vmin) / (vmax-vmin);
}
Fichier "chuck_fft.h"
//-----------------------------------------------------------------------------
// name: chuck_fft.h
// desc: fft impl - based on CARL distribution
//
// authors: code from San Diego CARL package
// Ge Wang (gewang@cs.princeton.edu)
// Perry R. Cook (prc@cs.princeton.edu)
// date: 11.27.2003
//-----------------------------------------------------------------------------
#ifndef __CHUCK_FFT_H__
#define __CHUCK_FFT_H__
// complex type
typedef struct { float re ; float im ; } complex;
// complex absolute value
#define cmp_abs(x) ( sqrt( (x).re * (x).re + (x).im * (x).im ) )
#define FFT_FORWARD 1
#define FFT_INVERSE 0
// real fft, N must be power of 2
void rfft( float * x, long N, unsigned int forward );
// complex fft, NC must be power of 2
void cfft( float * x, long NC, unsigned int forward );
#endif
Fichier "chuck_fft.c"
//-----------------------------------------------------------------------------
// name: chuck_fft.c
// desc: fft impl - based on CARL distribution
//
// authors: code from San Diego CARL package
// Ge Wang (gewang@cs.princeton.edu)
// Perry R. Cook (prc@cs.princeton.edu)
// date: 11.27.2003
//-----------------------------------------------------------------------------
#include "chuck_fft.h"
#include <stdlib.h>
#include <math.h>
static float PI ;
static float TWOPI ;
void bit_reverse( float * x, long N );
//-----------------------------------------------------------------------------
// name: rfft()
// desc: real value fft
//
// these routines from the CARL software, spect.c
// check out the CARL CMusic distribution for more source code
//
// if forward is true, rfft replaces 2*N real data points in x with N complex
// values representing the positive frequency half of their Fourier spectrum,
// with x[1] replaced with the real part of the Nyquist frequency value.
//
// if forward is false, rfft expects x to contain a positive frequency
// spectrum arranged as before, and replaces it with 2*N real values.
//
// N MUST be a power of 2.
//
//-----------------------------------------------------------------------------
void rfft( float * x, long N, unsigned int forward )
{
static int first = 1 ;
float c1, c2, h1r, h1i, h2r, h2i, wr, wi, wpr, wpi, temp, theta ;
float xr, xi ;
long i, i1, i2, i3, i4, N2p1 ;
if( first )
{
PI = (float) (4.*atan( 1. )) ;
TWOPI = (float) (8.*atan( 1. )) ;
first = 0 ;
}
theta = PI/N ;
wr = 1. ;
wi = 0. ;
c1 = 0.5 ;
if( forward )
{
c2 = -0.5 ;
cfft( x, N, forward ) ;
xr = x[0] ;
xi = x[1] ;
}
else
{
c2 = 0.5 ;
theta = -theta ;
xr = x[1] ;
xi = 0. ;
x[1] = 0. ;
}
wpr = (float) (-2.*pow( sin( 0.5*theta ), 2. )) ;
wpi = (float) sin( theta ) ;
N2p1 = (N<<1) + 1 ;
for( i = 0 ; i <= N>>1 ; i++ )
{
i1 = i<<1 ;
i2 = i1 + 1 ;
i3 = N2p1 - i2 ;
i4 = i3 + 1 ;
if( i == 0 )
{
h1r = c1*(x[i1] + xr ) ;
h1i = c1*(x[i2] - xi ) ;
h2r = -c2*(x[i2] + xi ) ;
h2i = c2*(x[i1] - xr ) ;
x[i1] = h1r + wr*h2r - wi*h2i ;
x[i2] = h1i + wr*h2i + wi*h2r ;
xr = h1r - wr*h2r + wi*h2i ;
xi = -h1i + wr*h2i + wi*h2r ;
}
else
{
h1r = c1*(x[i1] + x[i3] ) ;
h1i = c1*(x[i2] - x[i4] ) ;
h2r = -c2*(x[i2] + x[i4] ) ;
h2i = c2*(x[i1] - x[i3] ) ;
x[i1] = h1r + wr*h2r - wi*h2i ;
x[i2] = h1i + wr*h2i + wi*h2r ;
x[i3] = h1r - wr*h2r + wi*h2i ;
x[i4] = -h1i + wr*h2i + wi*h2r ;
}
wr = (temp = wr)*wpr - wi*wpi + wr ;
wi = wi*wpr + temp*wpi + wi ;
}
if( forward )
x[1] = xr ;
else
cfft( x, N, forward ) ;
}
//-----------------------------------------------------------------------------
// name: cfft()
// desc: complex value fft
//
// these routines from CARL software, spect.c
// check out the CARL CMusic distribution for more software
//
// cfft replaces float array x containing NC complex values (2*NC float
// values alternating real, imagininary, etc.) by its Fourier transform
// if forward is true, or by its inverse Fourier transform ifforward is
// false, using a recursive Fast Fourier transform method due to
// Danielson and Lanczos.
//
// NC MUST be a power of 2.
//
//-----------------------------------------------------------------------------
void cfft( float * x, long NC, unsigned int forward )
{
float wr, wi, wpr, wpi, theta, scale ;
long mmax, ND, m, i, j, delta ;
ND = NC<<1 ;
bit_reverse( x, ND ) ;
for( mmax = 2 ; mmax < ND ; mmax = delta )
{
delta = mmax<<1 ;
theta = TWOPI/( forward? mmax : -mmax ) ;
wpr = (float) (-2.*pow( sin( 0.5*theta ), 2. )) ;
wpi = (float) sin( theta ) ;
wr = 1. ;
wi = 0. ;
for( m = 0 ; m < mmax ; m += 2 )
{
register float rtemp, itemp ;
for( i = m ; i < ND ; i += delta )
{
j = i + mmax ;
rtemp = wr*x[j] - wi*x[j+1] ;
itemp = wr*x[j+1] + wi*x[j] ;
x[j] = x[i] - rtemp ;
x[j+1] = x[i+1] - itemp ;
x[i] += rtemp ;
x[i+1] += itemp ;
}
wr = (rtemp = wr)*wpr - wi*wpi + wr ;
wi = wi*wpr + rtemp*wpi + wi ;
}
}
// scale output
scale = (float)(forward ? 1./ND : 2.) ;
{
register float *xi=x, *xe=x+ND ;
while( xi < xe )
*xi++ *= scale ;
}
}
//-----------------------------------------------------------------------------
// name: bit_reverse()
// desc: bitreverse places float array x containing N/2 complex values
// into bit-reversed order
//-----------------------------------------------------------------------------
void bit_reverse( float * x, long N )
{
float rtemp, itemp ;
long i, j, m ;
for( i = j = 0 ; i < N ; i += 2, j += m )
{
if( j > i )
{
rtemp = x[j] ; itemp = x[j+1] ; /* complex exchange */
x[j] = x[i] ; x[j+1] = x[i+1] ;
x[i] = rtemp ; x[i+1] = itemp ;
}
for( m = N>>1 ; m >= 2 && j >= m ; m >>= 1 )
j -= m ;
}
}
