fract16 arg_fr16 ( complex_fract16 a )
{
// Phase = atan( a.imaginary / a.real) / 2*pi, phase = [0..1.0)
fract16 phase;
phase = atan2_fr16(a.im, a.re);
if( phase > 0 )
{
// atan2_fr16( [0..0.5) ) = [0x0..0x7fff]
// Expected phase : [0..0.5)
// => Need to divide output from atan2_fr16 by 2
phase = multr_fr1x16(phase,0x4000);
}
else if( phase < 0 )
{
// atan2_fr16( [0.5..1.0) ) = [0x8000..0x0)
// Expected phase : [0.5..1.0)
// => Need to scale by: (2.0 + output atan2_fr16) / 2
// = 1.0 + output/2
// = (1.0 - max fract16 + max fract16) + output/2
// = (max fract16 + output/2) + (1.0 - max fract16)
phase = (0x7fff + multr_fr1x16(phase,0x4000)) + 0x1;
}
return( phase );
}
void echo_generator(void)
{
echo_output = in_fir;
// Produce First echo
echo_output = add_fr1x16(echo_output, multr_fr1x16(0x1000, echo_delayed_input[echo_reader_first]));
// Produce Second echo
echo_output = add_fr1x16(echo_output, multr_fr1x16(0x2000, echo_delayed_input[echo_reader_second]));
// Produce Third echo
echo_output = add_fr1x16(echo_output, multr_fr1x16(0x4000, echo_delayed_input[echo_reader_third]));
echo_delayed_input[echo_writer] = in_fir; //Store the current input in the echo delay buffer
//Increment the echo buffer readers
echo_writer++;
echo_reader_first++;
echo_reader_second++;
echo_reader_third++;
if(echo_writer == echo_TAP_NUM)
{
echo_writer = 0;
}
if(echo_reader_first == echo_TAP_NUM)
{
echo_reader_first = 0;
}
if(echo_reader_second == echo_TAP_NUM)
{
echo_reader_second = 0;
}
if(echo_reader_third == echo_TAP_NUM)
{
echo_reader_third = 0;
}
}
void Vibrato_Function(void)
{
int j,k=0; // Increment d'échantillons
for (j=0;j<Size_acq;j++)
{
if(Toff==8000) {Toff=0;} // Si au bout de la periode, reprendre à 0
if(Toff<4001){
ShellFunction[j]=-multr_fr1x16(Toff,0.00000025r16); // Parabole de base (8000 , 1)
ShellFunction[j]=multr_fr1x16((ShellFunction[j]+0.001r16),Toff);} // Parabole convexe
else {
ShellFunction[j]= multr_fr1x16(Toff,0.00000025r16); // Parabole de base (14000,-1)
ShellFunction[j]=multr_fr1x16((ShellFunction[j]-0.003r16),Toff+8);} // Parabole concave
Toff=Toff+1; // Incrementation d'indice
ShellFunction[j]=multr_fr1x16(0.4r16,ShellFunction[j]); // Shell => [-1, 1]/( 1 + 1.5)
output_data[j] = multr_fr1x16(input_data[j],0.6r16); // OD = ID * ( 1.5 + Shell ) / ( 1 + 1.5 ) => OD = ID * 1.5/2.5
output_data[j] = output_data[j]+multr_fr1x16(input_data[j],ShellFunction[j]); // OD = ID * ( 1.5 + Shell ) / ( 1 + 1.5 ) => OD = OD + ID * Shell
}
}//end of function
