fm.c

// Copyright (c) <2012> <Leif Asbrink>
//
// Permission is hereby granted, free of charge, to any person 
// obtaining a copy of this software and associated documentation 
// files (the "Software"), to deal in the Software without restriction, 
// including without limitation the rights to use, copy, modify, 
// merge, publish, distribute, sublicense, and/or sell copies of 
// the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be 
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 
// OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT 
// HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, 
// WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE 
// OR OTHER DEALINGS IN THE SOFTWARE.


#include "globdef.h"
#include "uidef.h"
#include "fft1def.h"
#include "fft3def.h"
#include "sigdef.h"
#include "seldef.h"
#include "screendef.h"
#include "options.h"


float fmpow[32];
float fmbuf[64];

int iniflag=0;

float fm_phase;
float fm_phasediff_avg;

float fm_lock_factor=0.9;

float fmfix(int p0, float *z, float *pha)
{
int i, nn;
int i0, i1, i2;
float t1,t2;
t2=pha[0];
if(fm_n==0)
  {
  t1=atan2(z[2*p0+1],z[2*p0]);
  }
else
  {  
  nn=(p0-fm_size+baseband_size)&baseband_mask;
  for(i=0; i<fm_size; i++)
    {
    fmbuf[2*i  ]=fm_win[i]*z[2*nn  ];
    fmbuf[2*i+1]=fm_win[i]*z[2*nn+1];
    nn=(nn+1)&baseband_mask;
    }
  fftforward(fm_size, fm_n, fmbuf, fm_tab, fm_permute, FALSE);
  t1=0;
  i1=0;
  for(i=0; i<fm_size; i++)
    {
    fmpow[i]=fmbuf[2*i]*fmbuf[2*i]+fmbuf[2*i+1]*fmbuf[2*i+1];
    if(t1 < fmpow[i])
      {
      t1=fmpow[i];
      i1=i;
      }
    }
  i0=(i1-1+fm_size)&(fm_size-1);    
  i2=(i1+1)&(fm_size-1);
  t1=atan2(  -fmpow[i0]*fmbuf[2*i0+1]+
              fmpow[i0]*fmbuf[2*i1+1]-
              fmpow[i0]*fmbuf[2*i2+1],
             -fmpow[i0]*fmbuf[2*i0  ]+
              fmpow[i0]*fmbuf[2*i1  ]-
              fmpow[i0]*fmbuf[2*i2  ]);
  t1+=(i0&1)*PI_L;           
  }
pha[0]=t1;
t1-=t2;
if(t1 > PI_L)t1-=2*PI_L;                
if(t1 < -PI_L)t1+=2*PI_L;
return t1;
}

void detect_fm(void)
{
float *zr;
float t1, t2, t3;
float pil2_sin, pil2_cos, pil2_phstep_sin, pil2_phstep_cos;
int p0, p1, last_point;
int use_audio_filter;
int i, mm, nn;
#if    NET_BASEBRAW_MODE == BASEBAND_IQ
// We do absolutely nothing when the user specified 
// that he wants the raw baseband signal.
#endif
if(genparm[DA_OUTPUT_SPEED]/2 > 1000*bg.fm_audio_bw)
  {
  use_audio_filter=TRUE;
  }
else
  {
  use_audio_filter=FALSE;
  }
if(bg_coherent == 0)
  {
  zr=baseb_raw_orthog;
  }
else
  {
  zr=baseb_carrier;
  }  
// ************************  Step 1 ***********************
// Make an FM detector by computing the phase with atan2.
// then take the derivative to get the instantaneous 
// frequency.
// The phase can also be determined from an FFT in fmfix()
// Return immediately if bg.fm_audio_bw equals 10 since the
// corresponding filter would not reduce the bandwidth.
// ********************************************************
p0=baseb_pa;
last_point=(baseb_pa+mix2.new_points)&baseband_mask;
if(baseb_channels == 1)
  {
  if(use_audio_filter)
    {
    while(p0 != last_point)
      {
      baseb_fm_demod[p0 ]=fmfix(p0,baseb_raw,&fm0_ph1);     
      if(bg.fm_subtract != 0)
        {
        baseb_fm_phase[p0]=fm0_ph1;
        }
      p0=(p0+1)&baseband_mask;
      }  
    }
  else
    {
    while(p0 != last_point)
      {
      baseb_out[2*p0  ]=fmfix(p0,baseb_raw,&fm0_ph1);
      baseb_out[2*p0+1]=fmfix(p0,baseb_raw,&fm0_ph1);
      p0=(p0+1)&baseband_mask;
      }
    return;
    }
  }
else
  {
  if(use_audio_filter)
    {
    while(p0 != last_point)
      {
      baseb_fm_demod[2*p0  ]=fmfix(p0,baseb_raw,&fm0_ph1);
      baseb_fm_demod[2*p0+1]=fmfix(p0,zr,&fm0_ph2);
      p0=(p0+1)&baseband_mask;
      }
    }
  else
    {
    while(p0 != last_point)
      {
      baseb_out[4*p0  ]=fmfix(p0,baseb_raw,&fm0_ph1);
      baseb_out[4*p0+1]=0;      
      baseb_out[4*p0+2]=fmfix(p0,zr,&fm0_ph2);
      baseb_out[4*p0+3]=0;      
      p0=(p0+1)&baseband_mask;
      }
    return;  
    }
  }
#if NET_BASEBRAW_MODE == WFM_FM_FULL_BANDWIDTH || \
    NET_BASEBRAW_MODE == WFM_AM_FULL_BANDWIDTH
if( (ui.network_flag & NET_RXOUT_BASEBRAW) != 0)return;
#endif
// **********************************************************
if(bg.fm_subtract == 2 ||
   fm_pilot_size == 0 || 
   bg_coherent != 0 ||
   baseb_channels != 1)goto nbfm;
// **********  Step 2  (for WFM stereo)  *************
// Apply low pass filters as specified by fmfil55fir and 
// fmfil70fir
// Put the lowest frequency range, 0 to 55 kHz in baseb_fm_demod_low[]
// Put the middle part, 55 to 70 kHz in baseb_fm_high[]
// ******************************************************

p1=(baseb_pa-fmfil55_points/2+baseband_mask)&baseband_mask;
p0=baseb_pa;
while(p0 != last_point)
  {
  nn=p1;
  t1=fmfil55_fir[fmfil55_points/2]*baseb_fm_demod[nn];
  mm=nn;
  for(i=fmfil55_points/2-1; i>=0; i--)
    {
    nn=(nn+baseband_mask)&baseband_mask;
    mm=(mm+1)&baseband_mask;
    t1+=fmfil55_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
    }
#if NET_BASEBRAW_MODE != WFM_STEREO_HIGH
  baseb_fm_demod_low[p0]=t1;
#endif
#if NET_BASEBRAW_MODE != WFM_STEREO_LOW
  nn=p1;
  t2=fmfil70_fir[fmfil70_points/2]*baseb_fm_demod[nn];
  mm=nn;
  for(i=fmfil70_points/2-1; i>=0; i--)
    {
    nn=(nn+baseband_mask)&baseband_mask;
    mm=(mm+1)&baseband_mask;
    t2+=fmfil70_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
    }
#if NET_BASEBRAW_MODE == WFM_STEREO_HIGH    
  baseb_fm_demod_low[p0]=t2-t1;
#else
  baseb_fm_demod_high[p0]=t2-t1;
#endif
#endif  
  p1=(p1+1)&baseband_mask;
  p0=(p0+1)&baseband_mask;
  }
#if NET_BASEBRAW_MODE == WFM_STEREO_LOW || \
    NET_BASEBRAW_MODE == WFM_STEREO_HIGH
if( (ui.network_flag & NET_RXOUT_BASEBRAW) != 0)return;
#endif    
// **********  Step 3  (for wideband stereo)  *************
// Look for the pilot tone. 
// ********************************************************
//  baseb_fm_demod[0]
//  baseb_fm_demod_low[fmfil55_points/2]
//  baseb_fm_demod_high[fmfil55_points/2]
  p1=last_point;
  t1=0;
  t2=0;
  for(i=0; i<fm_pilot_size; i++)
    {
    p1=(p1+baseband_mask)&baseband_mask;
    t1+=baseb_fm_demod_low[p1]*fm_pilot_tone[2*i  ];
    t2+=baseb_fm_demod_low[p1]*fm_pilot_tone[2*i+1];
    }
// **********  Step 4  (for wideband stereo)  *************
// Detect the stereo channel and the RDS channel.
// for development purposes, remember the pilot tone and its overtones.
// ********************************************************
  p0=(baseb_pa+baseband_mask)&baseband_mask;
  p1=last_point;
  t1=atan2(t1,t2);
  t2=2*PI_L*19000./baseband_sampling_speed;
  pil2_sin=sin(2*t1);
  pil2_cos=cos(2*t1);
  pil2_phstep_sin=sin(2*t2);
  pil2_phstep_cos=cos(2*t2);
  while(p0 != p1)
    {
    p1=(p1+baseband_mask)&baseband_mask;
    baseb_fm_pil2det[p1]=pil2_sin*baseb_fm_demod_low[p1];
    t3=pil2_cos;
    pil2_cos= pil2_cos*pil2_phstep_cos-pil2_sin*pil2_phstep_sin;
    pil2_sin= pil2_sin*pil2_phstep_cos+t3*pil2_phstep_sin;
//    baseb_fm_pil3[2*p1  ]=pil3_sin;
//    baseb_fm_pil3det[2*p1  ]=pil3_sin*baseb_fm_demod_high[p1];
//    baseb_fm_pil3[2*p1+1]=pil3_cos;
//    baseb_fm_pil3det[2*p1+1]=pil3_cos*baseb_fm_demod_high[p1];
// **************  probe the RDS signal ****************
//  baseb_out[2*p1  ]=baseb_fm_pil3det[2*p1  ];
//  baseb_out[2*p1+1]=baseb_fm_pil3det[2*p1+1];
//Ö    t3=pil3_cos;
//Ö    pil3_cos= pil3_cos*pil3_phstep_cos-pil3_sin*pil3_phstep_sin;
//Ö    pil3_sin= pil3_sin*pil3_phstep_cos+t3*pil3_phstep_sin;
    }
// **********  Step 5  (for wideband stereo)  *************
// Apply a low pass filter with cutoff frequency as specified
// by fm_audiofil_fir to the sound channels.
// ********************************************************
//  baseb_fm_demod[0]
//  baseb_fm_demod_low[fmfil55_points/2]
//  baseb_fm_demod_high[fmfil55_points/2]
//  baseb_fm_pil2[fmfil55_points/2]
//  baseb_fm_pil2det[fmfil55_points/2]
//  baseb_fm_pil3[fmfil55_points/2]
//  baseb_fm_pil3det[fmfil55_points/2]
p0=baseb_pa;
p1=(baseb_pa-fm_audiofil_points/2+baseband_mask)&baseband_mask;
while(p0 != last_point)
  {
  nn=p1;
  t1=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod_low[nn];
  t2=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_pil2det[nn];
  mm=nn;
  for(i=fm_audiofil_points/2-1; i>=0; i--)
    {
    nn=(nn+baseband_mask)&baseband_mask;
    mm=(mm+1)&baseband_mask;
    t1+=fm_audiofil_fir[i]*(baseb_fm_demod_low[nn]+baseb_fm_demod_low[mm]);
    t2+=fm_audiofil_fir[i]*(baseb_fm_pil2det[nn]+baseb_fm_pil2det[mm]);
    }
  baseb_out[2*p0  ]=t1+t2;
  baseb_out[2*p0+1]=t1-t2;
  p1=(p1+1)&baseband_mask;
  p0=(p0+1)&baseband_mask;
  }
return;

/*
// **********  Step 6  (for wideband stereo)  *************
// Low pass filter the RDS channels using fmfil_rds_fir.
// ********************************************************
//  baseb_fm_demod[0]
//  baseb_fm_demod_low[fmfil55_points/2]
//  baseb_fm_demod_high[fmfil55_points/2]
//  baseb_fm_pil2[fmfil55_points/2]
//  baseb_fm_pil2det[fmfil55_points/2]
//  baseb_fm_pil3[fmfil55_points/2]
//  baseb_fm_pil3det[fmfil55_points/2]
//  baseb_fm_sumchan[fmfil55_points/2+fmfil_points/2]
//  baseb_fm_diffchan[fmfil55_points/2+fmfil_points/2]
lir_fillbox(0,0,101,101,color_scale[1]);
p2=(baseb_pa-(fmfil_rds_points+fmfil55_points)/2+baseband_mask)&baseband_mask;
p1=(baseb_pa-fmfil_rds_points/2+baseband_mask)&baseband_mask;
p0=baseb_pa;
while(p0 != last_point)
  {
  nn=p1;
  r1=fmfil_rds_fir[fmfil_rds_points/2]*baseb_fm_pil3det[2*nn  ];
  r2=fmfil_rds_fir[fmfil_rds_points/2]*baseb_fm_pil3det[2*nn+1];
  mm=nn;
  for(i=fmfil_rds_points/2-1; i>=0; i--)
    {
    nn=(nn+baseband_mask)&baseband_mask;
    mm=(mm+1)&baseband_mask;
    r1+=fmfil_rds_fir[i]*(baseb_fm_pil3det[2*nn  ]+baseb_fm_pil3det[2*mm  ]);
    r2+=fmfil_rds_fir[i]*(baseb_fm_pil3det[2*nn+1]+baseb_fm_pil3det[2*mm+1]);
    }
//  baseb_fm_rdsraw[2*p0  ]=r1;
//  baseb_fm_rdsraw[2*p0+1]=r2;
//  *************  probe the RDS signal after the filter  ****************
//baseb_out[2*p0  ]=r1;
//baseb_out[2*p0+1]=r2;


  t2=r1*r1+r2*r2;
  if(t2 > 0.00000000001)
    {
    t1=atan2(r1,r2);
    }
  else
    {
    t1=0;
    }
  i=1;    
  if(t1-rds_phase<PI_L)rds_phase-=2*PI_L;
  if(t1-rds_phase>PI_L)rds_phase+=2*PI_L;
  if(t1-rds_phase<-0.5*PI_L)
    {
    t1+=PI_L;
    i=-1;
    }
  if(t1-rds_phase>0.5*PI_L)
    {
    t1-=PI_L;
    i=-1;
    }
    
   
  rds_power=rds_f1*rds_power+rds_f2*t2;
  if(rds_power > 0.000000001)
    {
    rds_phase=(1-(t2/rds_power))*rds_phase + 
               (t2/rds_power)*(rds_f1*rds_phase+rds_f2*t1);
    }
// Here it would be favourable to decode the rds signal, correct errors
// and to construct the correct rds signal with correct amplitude
// and rise times. For this test of principles, just use whatever
// amplitude that we have detected.
// the rds signal is i*sqrt(t2)  
// Here we could put it into a downsampled array.
//
// Start to build a cleaned up version of the detected composite
// signal. First we store the rds signal.

  baseb_fm_composite[p0]=(sin(rds_phase)*baseb_fm_pil3[2*p1  ]+
                   cos(rds_phase)*baseb_fm_pil3[2*p1+1])*rds_gain*i*sqrt(t2);
// here we should perhaps use the filtered signals and construct
// the ideal pilot tone. At good S/N it should not be needed.

  baseb_fm_composite[p0]+=baseb_fm_demod_low[p1];


fm_phase+=baseb_fm_composite[p0];
if(fm_phase > PI_L)fm_phase-=2*PI_L;
if(fm_phase < -PI_L)fm_phase+=2*PI_L;

//baseb_out[2*p0]=baseb_fm_composite[p0]-baseb_fm_demod[p2]; 
//baseb_out[2*p0+1]=0;

t1=fm_phase-baseb_fm_phase[p2];
if(t1 > PI_L)t1-=2*PI_L;
if(t1 < -PI_L)t1+=2*PI_L;

fm_phasediff_avg=fm_lock_factor*fm_phasediff_avg+(1-fm_lock_factor)*t1;

if(fabs(t1) > PI_L/2)
  {
  fm_phase=baseb_fm_phase[p2];
  }
  
fm_phase-=fm_phasediff_avg*(1-fm_lock_factor);  


t1=baseb_totpwr[p2]+baseb_totpwr[(p2+baseband_mask)&baseband_mask]+
                    baseb_totpwr[(p2+baseband_mask-1)&baseband_mask];
t1/=3;
baseb_out[2*p0  ]=-sqrt(t1)*cos(fm_phase);
baseb_out[2*p0+1]=-sqrt(t1)*sin(fm_phase);

//baseb_out[2*p0]+=baseb_raw[2*p2]; 
//baseb_out[2*p0+1]+=baseb_raw[2*p2+1];





  if(rx_daout_channels ==1)
    {
    baseb_out[2*p0  ]=r1;
    baseb_out[2*p0+1]=0;
    }
  else
    {
    baseb_out[2*p0  ]=r1;
    baseb_out[2*p0+1]=r2;
    }

if(p0%32==0)
{
mailbox[0]=50+1000*r1;
mailbox[1]=50+1000*r2;
if(mailbox[0]<0)mailbox[0]=0;
if(mailbox[0]>100)mailbox[0]=100;
if(mailbox[1]<0)mailbox[1]=0;
if(mailbox[1]>100)mailbox[1]=100;
lir_setpixel(mailbox[0],mailbox[1],15);
mailbox[2]=50+45*sin(rds_phase);
mailbox[3]=50+45*cos(rds_phase);
lir_setpixel(mailbox[2],mailbox[3],15);
}

  p2=(p2+1)&baseband_mask;
  p1=(p1+1)&baseband_mask;
  p0=(p0+1)&baseband_mask;
  }




return;

*/


// ********************  Step 2 for NBFM *************************
// Apply a low pass filter with cutoff frequency as specified
// by fm_audiofil_fir.
nbfm:;
p0=baseb_pa;
p1=(baseb_pa-fm_audiofil_points/2+baseband_mask)&baseband_mask;
if(baseb_channels == 1)
  {
  while(p0 != last_point)
    {
    nn=p1;
    t1=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod[nn];
    mm=nn;
    for(i=fm_audiofil_points/2-1; i>=0; i--)
      {
      nn=(nn+baseband_mask)&baseband_mask;
      mm=(mm+1)&baseband_mask;
      t1+=fm_audiofil_fir[i]*(baseb_fm_demod[nn]+baseb_fm_demod[mm]);
      }
    baseb_out[2*p0  ]=t1;
    baseb_out[2*p0+1]=t1;
    p1=(p1+1)&baseband_mask;
    p0=(p0+1)&baseband_mask;
    }
  }  
else
  {
  while(p0 != last_point)
    {
    nn=p1;
    t1=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod[2*nn  ];
    t2=fm_audiofil_fir[fm_audiofil_points/2]*baseb_fm_demod[2*nn+1];
    mm=nn;
    for(i=fm_audiofil_points/2-1; i>=0; i--)
      {
      nn=(nn+baseband_mask)&baseband_mask;
      mm=(mm+1)&baseband_mask;
      t1+=fm_audiofil_fir[i]*(baseb_fm_demod[2*nn  ]+baseb_fm_demod[2*mm  ]);
      t2+=fm_audiofil_fir[i]*(baseb_fm_demod[2*nn+1]+baseb_fm_demod[2*mm+1]);
      }
    baseb_out[2*p0  ]=t1;
    baseb_out[2*p0+1]=t2;
    p1=(p1+1)&baseband_mask;
    p0=(p0+1)&baseband_mask;
    }
  }


/*          
// *********************  Step 3 ******************************
// These locations correspond to the same moment in time:
// baseb_fm_fil1[p0]
// baseb_fm_raw[p0-len]
// baseb_fm_demod1[p0-len]
//
// The filtered data is more accurate than the unfiltered
// data. 
// Frequency modulate the original signal with the
// filtered demodulated signal. That should compress
// the bandwidth because we apply (nearly) the same
// FM modulation but in the opposite phase.
p0=baseb_pa;
p1=(baseb_pa-len+baseband_size)&baseband_mask;
t1=fm1_ph1;
if(baseb_channels == 1)
  {
  while(p0 != last_point)
    {
    t1+=baseb_fm_fil1[p0];
    if(t1>PI_L)t1-=2*PI_L;
    if(t1<-PI_L)t1+=2*PI_L;

    baseb_compressed_fm1[2*p0  ]= baseb_raw[2*p1  ]*cos(t1)+
                                  baseb_raw[2*p1+1]*sin(t1);
    baseb_compressed_fm1[2*p0+1]= baseb_raw[2*p1+1]*cos(t1)+
                                 -baseb_raw[2*p1  ]*sin(t1);                                 
//    baseb_compressed_fm1[2*p0  ]=baseb_raw[2*p1  ];
//    baseb_compressed_fm1[2*p0+1]=baseb_raw[2*p1+1];
    p0=(p0+1)&baseband_mask;
    p1=(p1+1)&baseband_mask;
    }
  }
fm1_ph1=t1;
*/

}