Localisation sonore par GCC
From Eric
Le code C de test de la GCC
Le code de la version (préliminaire est donné ci-après) :
// (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"
- include "fft_num_rec.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);
}
