cohsub.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 "osnum.h"
#include "globdef.h"
#include <string.h>
#include "uidef.h"
#include "fft1def.h"
#include "sigdef.h"
#include "screendef.h"
#include "options.h"

// float[2] baseb_out=data to send to loudspeaker
// float[2] baseb=complex amplitude of first level coherent data.
// float[2] baseb_raw=complex amplitude of baseband signal. Raw data, pol. adapted.
// float[2] baseb_raw_orthog=complex amplitude of pol. orthog signal. Raw data.
// float[2] baseb_carrier=phase of carrier. Complex data, cosines and sines.
// float[1] baseb_carrier_ampl=amplitude of carrier
// float[1] baseb_totpwr=total power of baseb_raw
// float[2] baseb_envelope=complex amplitude from fitted dots and dashes.
// float[1] baseb_upthreshold=forward peak detector for power
// float[1] baseb_threshold=combined forward and backward peak detector at -6dB
// float[2] baseb_fit=fitted dots and dashes.
// float[2] baseb_tmp=array for intermediate data in complex format
// float[1] baseb_agc_level=used only when AGC is enabled.
// float[2] baseb_wb_raw=complex amplitude like baseb_raw at a larger bandwidth.
// float[2] baseb_wb=complex amplitude of coherent detect like baseb, at larger bw.
// short int[1] baseb_ramp=indicator for time of power above/below threshold.
// short_int[1] baseb_clock=CW code clock
// float[2] baseb_tmp=for debug during development

// baseb_pa  The starting point of the latest mix2.size/2 points.
// baseb_pb  The point up to which thresholds exist.
// baseb_pc  The point up to which cw speed statistics and ramp is collected.
// baseb_pd  A key up region close before baseb_pc
// baseb_pe  The point up to which we have run first detect. 
// baseb_pf
// baseb_px  The oldest point that contains valid data.


// ************************************************************
// ************************************************************
// These defines allow a lot of information to be written to dmp
#if DUMPFILE == TRUE
#define PR00 0
#define PR01 0
#define PR02 0
#define PR03 0
#define PR04 0
#define PR05 0
#else
#define PR00 0
#define PR01 0
#define PR02 0
#define PR03 0
#define PR04 0
#define PR05 0
#endif

#define PRT00 if(PR00 != 0)DEB
#define PRT01 if(PR01 != 0)DEB
#define PRT02 if(PR02 != 0)DEB
#define PRT03 if(PR03 != 0)DEB
#define PRT04 if(PR04 != 0)DEB
#define PRT05 if(PR05 != 0)DEB
// ************************************************************
// ************************************************************

#define ZZ 0.0000000001
#define MAX_CW_IDEAL_RISE 16
float cw_ideal_rise[MAX_CW_IDEAL_RISE];


void fit_dash(void)
{
int i,k,m,ss,pp,n1,n2,n3,trials;
float t1,t2,t3,r1,r2;
float im_1,im_2,im_3;
float re_1,re_2,re_3;
float p1,p2,p3,old_re,old_im;
float midpoint[4],offset[4],ampl[4];
int ref[4],shift[4];
// Compare the current waveform to the waveform of a dash.
// Set cg_wave_start to the position giving the best fit.
// Set cg_wave_midpoint to the center of the waveform.
// Return similarity of waveforms.
m=0;
trials=0;
old_re=0;
old_im=0;
try_fit:;
pp=cg_wave_start;
ref[trials]=pp;
ss=(cg_wave_start+dash_pts)&baseband_mask;
re_1=0;
re_2=0;
re_3=0;
im_1=0;
im_2=0;
im_3=0;
p2=0;
n2=pp;
k=dash_pts/12;
if(k<3)k=3;
n1=(pp-k+baseband_size)&baseband_mask;
n3=(pp+k)&baseband_mask;
i=0;
while(n2 != ss)
  {
  p2+=baseb_totpwr[n2];
  re_1+=baseb[2*n1  ]*dash_waveform[2*i  ]+
      baseb[2*n1+1]*dash_waveform[2*i+1];
  im_1+=baseb[2*n1  ]*dash_waveform[2*i+1]-
      baseb[2*n1+1]*dash_waveform[2*i  ];
  re_2+=baseb[2*n2  ]*dash_waveform[2*i  ]+
      baseb[2*n2+1]*dash_waveform[2*i+1];
  im_2+=baseb[2*n2  ]*dash_waveform[2*i+1]-
      baseb[2*n2+1]*dash_waveform[2*i  ];
  re_3+=baseb[2*n3  ]*dash_waveform[2*i  ]+
      baseb[2*n3+1]*dash_waveform[2*i+1];
  im_3+=baseb[2*n3  ]*dash_waveform[2*i+1]-
      baseb[2*n3+1]*dash_waveform[2*i  ];
  i++;
  n1=(n1+1)&baseband_mask;
  n2=(n2+1)&baseband_mask;
  n3=(n3+1)&baseband_mask;
  }
p1=p2;
p3=p2;
n1=(pp-k+baseband_size)&baseband_mask;
n3=(pp+k)&baseband_mask;
while(n1 != pp)
  {
  p1+=baseb_totpwr[n1];
  n1=(n1+1)&baseband_mask;
  }
while(n1 != n3)
  {
  p3-=baseb_totpwr[n1];
  n1=(n1+1)&baseband_mask;
  }
n1=(ss-k+baseband_size)&baseband_mask;
n3=(ss+k)&baseband_mask;
while(n1 != ss)
  {
  p1-=baseb_totpwr[n1];
  n1=(n1+1)&baseband_mask;
  }
while(n1 != n3)
  {
  p3+=baseb_totpwr[n1];
  n1=(n1+1)&baseband_mask;
  }
t1=sqrt((re_1*re_1+im_1*im_1)/(p1*dash_sumsq));
t2=sqrt((re_2*re_2+im_2*im_2)/(p2*dash_sumsq));
t3=sqrt((re_3*re_3+im_3*im_3)/(p3*dash_sumsq));
// t1,t2 and t3 reflect how well the current waveform fits to the reference
// when shifted by -k,0 and +k.
// These points should have a maximum close to t2.
if( (t1<0.33 && t2<0.33 && t3<0.33) )
  {
// If all points are really small, return zero to indicate an error.  
skip:;
  cg_wave_fit=0;
  return;
  }
parabolic_fit(&r2,&r1,t1,t2,t3);
r1*=k;
offset[trials]=r1;
ampl[trials]=r2;
cg_wave_midpoint=cg_wave_start+0.5*dash_pts+r1;
if(cg_wave_midpoint >= baseband_size)cg_wave_midpoint-=baseband_size;
midpoint[trials]=cg_wave_midpoint;
if(trials >= 3)goto skip;  
m=(fabs(r1)+0.5)*r1/fabs(r1);
if(m==0)goto pos_ok;
if(trials != 0)
  {
  if(m*shift[trials-1] <0)
    {
    m=(m-shift[trials-1])/2;
    }
  for(i=0; i<=trials; i++)
    {
    if(ref[i] == ((cg_wave_start+m)&baseband_mask) )m/=2;
    }
  if(abs(m) <= 1)goto pos_ok;
  }  
shift[trials]=m;
cg_wave_start=(cg_wave_start+m)&baseband_mask;
trials++;
old_re=re_2;
old_im=im_2;
goto try_fit;
pos_ok:;
cg_wave_coh_re=re_2;
cg_wave_coh_im=im_2;
if(trials != 0)
  {
  if(shift[trials]*shift[trials-1] < 0)
    {
    if(fabs(offset[trials]) > 2*fabs(offset[trials-1]) )
      {
      if(fabs(offset[trials]) > 0.5*fabs(offset[trials-1]) )
        {
// The previous fit was much better. Use it !!
        cg_wave_coh_re=old_re;
        cg_wave_coh_im=old_im;
        trials--;
        }
      else
        {
// We have two results, one from each side. 
// None is much better than the other.
// Use the weighted average.        
        t1=midpoint[trials];
        t2=midpoint[trials-1];
        if(t1-t2 >  (baseband_size >>1))t1=t1-baseband_size;                    
        if(t1-t2 < -(baseband_size >>1))t1=t1+baseband_size;                    
        p1=offset[trials];
        p2=offset[trials-1];
        p1=1/(0.2+fabs(p1));
        p2=1/(0.2+fabs(p2));
        p1=p1*p1;
        p2=p2*p2;
        t1=(p1*t1+p2*t2)/(p1+p2);
        if(t1 >= baseband_size)t1-=baseband_size;
        if(t1<0)t1+=baseband_size;
        midpoint[trials]=t1;
        t1=ampl[trials];
        t2=ampl[trials-1];
        ampl[trials]=(p1*t1+p2*t2)/(p1+p2);
        cg_wave_coh_re=(p1*re_2+p2*old_re)/(p1+p2);
        cg_wave_coh_im=(p1*im_2+p2*old_im)/(p1+p2);
        }
      }
    }
  }  
cg_wave_midpoint=midpoint[trials];
cg_wave_fit=ampl[trials];
cg_wave_coh_re/=dash_sumsq;
cg_wave_coh_im/=dash_sumsq;
}


int store_symmetry_adapted_dash(int ideal)
{
unsigned int i, j, k, ih;
int ia, ib, ic, im;
float t1, t2, t3, r1, r2, r3, der1, der2, phmax, phslope;
float maxpow, afac;
// Dashes contain a lot of energy. The AFC has placed the
// center of gravity of the carrier at freq=0 and the 
// coherent detect procedure should ensure that the
// average phase over the duration of a dash should be close to zero.
// The amplitude should be constant while the phase should
// be zero at the midpoint.
// Knowing that the curve shape should be symmetrical around it's
// midpoint, fold the signal over and form sums and differences.

if(PR00 != 0)
  {
  for(j=0; j<cw_avg_points; j++)
    {
    DEB"\nAVG in: %d  %f  ",j,  fftn_tmp[2*j  ]);
    }
  }        
ia=0;
ib=2*(cw_avg_points-1);

while (ia < ib)
  {
  t1=fftn_tmp[ib];
  t2=fftn_tmp[ia];
PRT00"\nre: [%d]=%f  [%d]=%f     %f,%f",ia/2, ZZ*t1, ib/2,ZZ*t2,ZZ*(t1+t2),ZZ*(t1-t2));
  fftn_tmp[ia]=t1+t2;
  fftn_tmp[ib]=t1-t2;

  t1=fftn_tmp[ia+1];
  t2=fftn_tmp[ib+1];
PRT00"\nim: [%d]=%f  [%d]=%f     %f,%f",ia/2, ZZ*t1, ib/2,ZZ*t2,ZZ*(t1-t2),ZZ*(t1+t2));
  fftn_tmp[ia+1]=t1-t2;
  fftn_tmp[ib+1]=t1+t2;
  ia+=2;
  ib-=2;
  }
// Step from the center until we reach the 6dB point.
// Get it as the summed powers of the symmetric real part and 
// the antisymmetric imaginary part.
// Collect the total power.
// Also collect the noise as the deviation from symmetry.
maxpow=0;
r1=0;
r2=0;
r3=0;
t1=0;
t3=0;
while(ia >= 0)
  {
  ia-=2;
  ib+=2;
  t1=fftn_tmp[ia]*fftn_tmp[ia]+fftn_tmp[ia+1]*fftn_tmp[ia+1];
PRT00"\nA: %d  %d  %f",ia/2,ib/2,ZZ*ZZ*t1);
  if(maxpow < t1)maxpow=t1;
  if(maxpow > 4*t1)goto collect_power_x;
  t3=t1;
  r1+=t1;
  r2+=fftn_tmp[ib  ]*fftn_tmp[ib  ];
  r3+=fftn_tmp[ib+1]*fftn_tmp[ib+1];
  }
collect_power_x:;
PRT02"\nTest for error: r1 %f ia %d ib %d  %.1f < %d < %.1f",
               ZZ*ZZ*r1,ia/2,ib/2,4.5*cwbit_pts,(ib-ia), 7.5*cwbit_pts);
if(r1 == 0 || ib-ia < 4.5*cwbit_pts || ib-ia > 7.5*cwbit_pts )
  {
  PRT01"Waveform error 1");    
  return FALSE;
  }
r2/=r1;
r3/=r1;
PRT02"\nTest for error: r2(<.05) %f  r3(<.03) %f",r2,r3);
if(r2 > 0.05 || r3>0.03)
  { 
  PRT01"Waveform error 2");    
  return FALSE;
  }
ih=ia;
t2=0.05*maxpow;
while(ia > 0 && t1 < t3 && t1 > t2 && fftn_tmp[ia]>0)
  {
  t3=t1;
  ia-=2;
  t1=fftn_tmp[ia]*fftn_tmp[ia]+fftn_tmp[ia+1]*fftn_tmp[ia+1];
  }
PRT00"\nia%d,ih%d",ia/2,ih/2);


// The phase angle should vary linearly with time and go
// through zero at the midpoint.
// Normalize for a peak amplitude=1
// Store the power and the phase angle.
afac=1/sqrt(maxpow);
k=cw_avg_points+2;
for(i=ia; i<cw_avg_points; i+=2)
  {
  fftn_tmp[i]*=afac;
  fftn_tmp[i+1]*=afac;
  fftn_tmp[k+i  ]=fftn_tmp[i]*fftn_tmp[i]+fftn_tmp[i+1]*fftn_tmp[i+1];
  fftn_tmp[k+i+1]=atan2(fftn_tmp[i+1],fftn_tmp[i]);
  }
if(PR00 != 0)
  {
  for(j=ia/2; j<cw_avg_points/2; j++)
    {
    DEB"\nB: %d  %f %f ",j,  fftn_tmp[2*j  ],  fftn_tmp[2*j+1  ]);
    }
  }        
// The phase angle has to be zero halfway between ib and the next
// point ib+2 since we work in the symmetry adapted function.
// Collect the power weighted average slope.
r1=0;
phslope=0;
for(i=ia; i<cw_avg_points; i+=2)
  {
  r1+=fftn_tmp[k+i];
  phslope+=fftn_tmp[k+i]*fftn_tmp[k+i+1]/(cw_avg_points-i-1);
  }
phslope/=r1;
// Use the average slope to construct the phase function 
// The real part is cos(x), make the imaginary part sin(x) 
// while collecting the rms error and total power.
t1=0;
t2=0;
for(i=ia; i<cw_avg_points; i+=2)
  {
  t1+=fftn_tmp[i]*fftn_tmp[i];
  r1=fftn_tmp[i]*tan((cw_avg_points-i-1)*phslope);
  t2+=(fftn_tmp[i+1]-r1)*(fftn_tmp[i+1]-r1);
  fftn_tmp[i+1]=r1;
  }
PRT02"\nTest for error: t2/t1 (<.05) %f ",t2/t1);
if(t2/t1 > 0.05)
  {
  PRT01"Waveform error 3");    
  return FALSE;
  }
phmax=phslope*(cw_avg_points-ih+1);
cg_chirpx=cg_x0-1.5*cg_size*cos(phmax);
cg_chirpy=cg_y0-1.5*cg_size*sin(phmax);
// ih is the point of half amplitude or nearest below.
// Our data may be corrupted by noise so it is perhaps not
// very reliable at low amplitudes.
// Remember the beginning of the curve in case we have an ideal
// waveform as input.
// We will use it for the beginning of the dot waveform
// on the averaged data that is less accurate at low levels.

if(PR00 != 0)
  {
  for(j=ia/2; j<cw_avg_points/2; j++)
    {
    DEB"\nC: %d  %f %f ",j,  fftn_tmp[2*j  ],  fftn_tmp[2*j+1  ]);
    }
  }        

if(ideal == TRUE)
  {
  im=ia;
  while(im>0 && fftn_tmp[im-2]*afac > 0.01)
    {
    im-=2;
    fftn_tmp[im]*=afac;
    }
  if(im > 0)im-=2;  
  ib=im;
  while(ib >=0 )
    {
    fftn_tmp[ib]=0;
    ib-=2;
    }


if(PR00 != 0)
  {
  DEB"\nim%d,ia%d,ih%d",im/2,ia/2,ih/2);
  for(j=ia/2; j<cw_avg_points/2; j++)
    {
    DEB"\nD: %d  %f %f ",j,  fftn_tmp[2*j  ],  fftn_tmp[2*j+1  ]);
    }
  }        
  i=0;  
  ib=im;
  if(fftn_tmp[ib] != 0)
    {
    cw_ideal_rise[0]=0;
    i=1;
    }
  while( i< MAX_CW_IDEAL_RISE)
    {
    cw_ideal_rise[i]=fftn_tmp[ib];
    ib+=2;
    i++;
    }

  if(PR00 != 0)
    {
    for(j=0; j<i; j++)
      {
      DEB"\nE: %d  %f",j,cw_ideal_rise[j]);
      }
    }  
  for(i=0; i<cw_avg_points; i++)fftn_tmp[2*ib]=0;
  }
else
  {
// Replace the first part of the rising edge by
// the stored ideal waveform.
// Point im to the point that comes nearest the -6dB point.
  im=ia;
  while(fabs(fftn_tmp[im]-0.5) > fabs(fftn_tmp[im+2]-0.5))
    {
    im+=2;
    }
  t1=fftn_tmp[im];
  ih=im;
  PRT00"\n im= %d  %f",im/2,t1);
// Point i to the first point in the ideal curve with a value
// higher than the value on our average at im.
  i=0;
  while(cw_ideal_rise[i] < t1)
    {
    i++;
    }
// Fit a parabola to the three points i-2,i-1 and i.
  der2=0;
  der1=cw_ideal_rise[i+1]-cw_ideal_rise[i];
next_ideal:;
  r3=cw_ideal_rise[i];
  if(i > 1)
    {
    r2=cw_ideal_rise[i-1];
    r1=cw_ideal_rise[i-2];
    der2=r3+r1-2*r2;
    der1=r3-r1;
    }
  else
    {
    if(i > 0)
      {
      r2=cw_ideal_rise[i-1];
      der2=0;
      der1=r3-r1;
      }
    }  
  PRT00"\nder1 %f   der2 %f",der1,der2);    
  im-=2;
  i--; 
  PRT02"\nTest for error im(>0) %d    i(<1) %d",im,i);
  if(im < 0 || i< 1)
    {
    PRT01"Waveform error 8");    
    return FALSE;
    }
  t1-=der1-0.5*der2;
  fftn_tmp[im]=t1;
  PRT00"\nH: %d  %f  i=%d",im/2,  fftn_tmp[im],i);
  if(t1 > 0)goto next_ideal;
  fftn_tmp[im]=0;
// Put the imaginary part in place.
  for(i=im; i<=ih; i+=2)
    {
    fftn_tmp[i+1]=r1=fftn_tmp[i]*tan((cw_avg_points-i-1)*phslope);
    }
  }
// Unfold the waveform to get the symmetry adapted original waveform back.
ia=im;
ib=2*(cw_avg_points-1)-im;
while (ia < ib)
  {
  fftn_tmp[ib]=fftn_tmp[ia];
  fftn_tmp[ib+1]=-fftn_tmp[ia+1];
  ia+=2;
  ib-=2;
  }
ic=im;
while(fftn_tmp[ic]==0)ic+=2;
dash_pts=1;
dash_waveform[0]=0;
dash_waveform[1]=0;
while(fftn_tmp[ic] != 0 )
  {
  dash_waveform[2*dash_pts  ]=fftn_tmp[ic  ];
  dash_waveform[2*dash_pts+1]=fftn_tmp[ic+1];
  ic+=2;
  dash_pts++;
  if(dash_pts > cw_waveform_max-2)
    {
    PRT01"Waveform error 4");    
    return FALSE;
    }
  } 
PRT02"\nTest for error %d < %d ",dash_pts, cw_waveform_max-2);     
dash_waveform[2*dash_pts  ]=0;
dash_waveform[2*dash_pts+1]=0;
dash_pts++;
dash_sumsq=0;
t1=0;
for(ia=0; ia<dash_pts; ia++)
  {
PRT00"\nDASH WAVEFORM %d %f %f",ia,dash_waveform[2*ia  ],dash_waveform[2*ia+1]);
  
  t2=dash_waveform[2*ia  ]*dash_waveform[2*ia  ]+
     dash_waveform[2*ia+1]*dash_waveform[2*ia+1];
  dash_sumsq+=t2;
  if(t1<t2)t1=t2;
  }
t1/=6;
cwbit_pts=0;
for(ia=0; ia<dash_pts; ia++)
  {
  t2=dash_waveform[2*ia  ]*dash_waveform[2*ia  ]+
     dash_waveform[2*ia+1]*dash_waveform[2*ia+1];
  if(t2>t1)cwbit_pts++;
  }
cwbit_pts/=3;
return TRUE;
}



void clear_coherent(void)
{
int i,n,nn,k;
float t1;
// ********   Clear graphics data *********
n=cg_size*cg_size;
for(i=0; i<n; i++)cg_map[i]=0;
if(genparm[CW_DECODE_ENABLE] != 0)
  {
  clear_cg_traces();
// *********** Clear data for cw speed determination *********
  for(i=0; i<keying_spectrum_size; i++)keying_spectrum[i]=0;
// The keying spectra arrive at a rate of 2*baseband_sampling_speed/mix2.size.
// The baseband bandwidth (user selected filter) is keying_spectrum_size/2
// so the frequency of a string of morse code dots that fit the
// filter would be keying_spectrum_size/4.
// Accumulate power spectra in a leaky integrator that has a life time
// of 150 morse code dots.
  t1=((150*4)/keying_spectrum_size);
  keying_spectrum_max=0.5+t1/3;
  keying_spectrum_f1=pow(0.25,1/t1);
  keying_spectrum_cnt=0;
  no_of_cwdat=0;
  correlate_no_of_cwdat=0;
  cw_detect_flag=CWDETECT_CLEARED;
  }
// ********* Clear old data so the oscilloscope will not 
// display incorrect information. *********
nn=(baseb_ps-cg_osc_offset+cg_osc_points-cg_oscw+baseband_size)&baseband_mask;
k=(baseb_pa-nn+baseband_size)&baseband_mask;
if(k+nn <= baseband_size)
  { 
  clear_baseb_arrays(nn,k);
  }
else
  {
  clear_baseb_arrays(nn,baseband_size-nn);
  clear_baseb_arrays(0,k-baseband_size+nn);
  }  
// ***********  Clear all the baseb pointers ********
baseb_pb=baseb_pa;
baseb_pc=baseb_pa;
baseb_pd=baseb_pa;
baseb_pe=baseb_pa;
baseb_pf=baseb_pa;
baseb_ps=baseb_pa;
baseb_pm=baseb_pa;
baseb_px=baseb_pa;
nn=0.5+daout_pa/(rx_daout_channels*rx_daout_bytes*da_resample_ratio);
baseb_fx=(baseb_pa-2-nn+baseband_size)&baseband_mask;
// *********** Clear basblock. Used for S-meter averaging ************ 
basblock_pa=2*baseb_pa/mix2.size;
nn=basblock_pa;
for(i=0; i<basblock_hold_points; i++)
  {
  basblock_maxpower[nn]=0;
  basblock_avgpower[nn]=0;
  nn=(nn+basblock_mask)&basblock_mask;
  }
cg_chirpx=cg_x0;
cg_chirpy=cg_y0;
s_meter_peak_hold=0;
cg_maxavg=0;
if(s_meter_avgnum > 0)
  {
  s_meter_avg=0;
  s_meter_avgnum=0;
  }
}