Problem with Tone Detection

[AlbertoFS]

I wanted to re-write in basic a famous project for a tone decoder with the PIC12F683 but without success.
So I have rewritten all the code with the PIC18F25K20 using Float variables. And curiously the code doesn't work. The same frequency is always detected, which is around 60 Hz, even if the parameters I set are for 600 or 200 HZ. I have verified everything many times and I do not see the error. Two years ago I had done a similar project with the same code and it worked quite well in simulation. Do I have an elf somewhere?
Using the PIC18F25K20 and Positron 4.0.3.8

Code Select Expand

'****************************************************************
'*  Name    : ToneDecoder_12F683.BAS                            *
'*  Author  : Original Code by [Alberto Freixanet]              *
'*  Notice  : Copyright (c) 2021 [Alberto Freixanet]            *
'*          : All Rights Reserved                               *
'*  Date    : 10/02/2024                                        *
'*  Version : 1.00                                              *
'*  Notes   : Tone Decoder written with Positron Compiler       *
'* Compiler : Version 4.0.3.8                                   *
'****************************************************************
'
    Device = 18F25K20
'    Declare Xtal = 32
'    Declare Xtal = 20
    Declare Xtal = 64   
    Declare Optimiser_Level = 2
    Declare Dead_Code_Remove = On
    Declare Float_Display_Type = Fast
    Declare Watchdog = On
'
    Declare Hserial_Baud = 9600                 ' Set the Baud rate
    Declare Hserial_Clear = 1                   ' Enable Error clearing on received characters
'
    Declare LCD_Type = Alphanumeric
    '/ Assigns the Port and Pins that the LCD's DT lines will attach to.
    Declare LCD_DTPin = PORTB.4
    Declare LCD_Interface = 4
    Declare LCD_Lines = 2
    Declare LCD_CommandUs = 2000
    Declare LCD_DataUs = 50
    '/ Assigns the Port and Pin that the LCD's EN line will attach to.
    Declare LCD_ENPin = PORTC.0
    '/ Assigns the Port and Pins that the LCD's RS line will attach to.     
    Declare LCD_RSPin = PORTC.1
'
'///////////////////////////////////////////////////////////////////////////////
(*
    The Goertzel parameters Maths Routines.
    The Goertzel Algorithm by Kevin Banks - August 28, 2002

    Precomputed constants:
    Once you've selected your sampling rate and block size, it's a simple five-step process to
    compute the constants you'll need during processing:
    K = 0,5 + (N * TragetFrequency / SampleRate)
   
    W = (2*PI/N)*k
    cosine = cos W
    sine = sin W
    coeff = 2 * cosine

    PI = 3.14159265
    For the per-sample processing you're going to need three variables. Let's call them Q0,
    Q1, and Q2.
    Q1 is just the value of Q0 last time. Q2 is just the value of Q0 two times ago
    (or Q1 last time).
    Q1 and Q2 must be initialized to zero at the beginning of each block of samples.

    For every sample, you need to run the following three equations:
    Q0 = coeff * Q1 - Q2 + sample
    Q2 = Q1
    Q1 = Q0

    After running the per-sample equations N times, it's time to see if the tone
    is present or not.
    Real = (Q1 - Q2 * cosine)
    imag = (Q2 * sine)
    magnitude2 = real2 + imag2

    A simple threshold test of the magnitude will tell you if the tone was present or not.
    Reset Q2 and Q1 to zero and start the next block.

    An optimized Goertzel:
    The optimized Goertzel requires less computation than the basic one, at the
    expense of phase information.
    The per-sample processing is the same, but the end of block processing is different.
    Instead of computing Real and imaginary components, and then converting those into
    relative magnitude squared, you directly compute the following:
    magnitude2 = Q1
    2 + Q2
    2-Q1*Q2*coeff

    This is the form of Goertzel I've used most often, and it was the first one I learned about.
    By Kevin Banks:

    The Goertzel Algorithm by Kevin Banks - August 28, 2002
    The third factor influencing your choice of N is the relationship between
    the sampling rate And the target frequencies. Ideally you want the frequencies
    To be centered in their respective bins. In other words, you want the target
    frequencies To be integer multiples of sample_rate/N.
    The good news is that, unlike the FFT, N doesn't have to be a power of two.

    FreqResolution = SamplingFreq/NbSamples
    TimeCapture = ((NbSamples*1000)/SamplingFreq)*1.0


    Goertzel algorithm implementation By Andrea Vitali
    The following function takes a vector x made of N samples, and computes the k-th
    frequency coefficient (k from 0 to N-1). The real part is returned in I (in-phase) and the
    imaginary part is returned in Q (quadrature-phase).
    function [I,Q] = goertzel(x,k,N)
    w = 2*pi*k/N;
    cw = cos(w); c = 2*cw;
    sw = sin(w);
    z1=0; z2=0; % init
    for n = 1 : N
    z0 = x(n) + c*z1 - z2;
    z2 = z1;
    z1 = z0;
    end;
    I = cw*z1 - z2;
    Q = sw*z1;
   
    Generalized Goertzel for non-integer k index
    The Goertzel algorithm can be generalized to handle the case where the k index is not an
    integer. The computation is the same as shown before. A correction step is added at the
    end to adjust the phase of the complex output. Note that the magnitude of the output is not
    changed.
    function [I,Q] = goertzelgen(x,k,N)
    w = 2*pi*k/N;
    cw = cos(w); c = 2*cw;
    sw = sin(w);
    z1=0; z2=0; % init

    for n = 1 : N
    z0 = x(n) + c*z1 - z2;   

    z2 = z1;
    z1 = z0;
    end;

    It = cw*z1 - z2;
    Qt = sw*z1;

    w2 = 2*pi*k;
    cw2 = cos(w2);
    sw2 = sin(w2);

    I = It*cw2 + Q*sw2;
    Q = -It*sw2 + Q*cw2;   
   
*)
'///////////////////////////////////////////////////////////////////////////////
'/ DEBUG CODE:
'    $define _TEST_TIMING        '/ To Test the TMR1 frequency: FreqTimer1 / 2
'    $define _MeasureLoopTime    '/ To test the Goertzel loop
'*******************************************************************************
'/ If you're trying to detect tones of short duration, you will have to use
'/ compatible values of N.
'/ The third factor influencing your choice of N is the relationship between the sampling
'/ rate And the target frequencies. Ideally you want the frequencies To be centered in
'/ their respective bins. In other words, you want the target frequencies To be integer
'/ multiples of sample_rate/N.
'/ The good news is that, unlike the FFT, N doesn't have to be a power of two.
'/ Sampling Frequency = 150 X NumberSamples
'/
'    NumberSamples = 128 => SamplingFrequency 19200 Hz
'    NumberSamples = 112 => SamplingFrequency 16800 Hz
'    NumberSamples = 96  => SamplingFrequency 14400 Hz
'    NumberSamples = 80  => SamplingFrequency 12000 Hz
'    NumberSamples = 64  => SamplingFrequency 9600 Hz
'    NumberSamples = 48  => SamplingFrequency 7200 Hz
'
'/******************************************************************************
    $define _EnableHammingWindow
'/ Choose one only Target Frequency
'
'    $define _TarjetFrequency 600
    $define _TarjetFrequency 200
'
    $define _SampleRate 7200
'    $define _SampleRate 12000
'    $define _SampleRate 18000
'
    $define _NumberSamples 120
'    $define _NumberSamples 80   
'    $define _NumberSamples 48
'*******************************************************************************

'*******************************************************************************
'/ The output Frequency Detected is normally 0 => NO Detection, 1 => Detection
'/ The output Frequency Detected is Inverted 1 => NO Detection, 0 => Detection (LM567)
'/ F_SIGN_INVERTED <= GPIO.3
'*******************************************************************************
'* Name    : WriteMSG                                                          *
'* Author  : [Alberto Freixanet]                                               *
'* Purpose : Preprocessor Macro To write a comment in the Asm file             *
'*******************************************************************************
$ifndef WriteMSG
$define WriteMSG(pMessage) '
Asm                        '
messg pMessage             '
EndAsm

$endif

'*******************************************************************************
'RAM VARIABLES SETUP
'*******************************************************************************
    $define True 1
    $define False 0
    $define CR 13
'/------------------------------------------------------------------------------
    Dim PP_BARG As Float System
    Dim PP_AARG As Float System
'
'*****Counter variables
    Dim MyLocalByte0 As Byte Access
    Dim VAR_AVG_CTR As Byte Access         '/ variable used to count through the number of A/D readings averaged
'
'/*****4 Variables for digital filter
'    Dim Count_A As Byte Access       
'    Dim Count_B As Byte Access
'    Dim CVA As Byte Access
'    Dim CSA As Byte Access
    Dim _CONT_A As Byte Access
    Dim _CONT_B As Byte Access     
    Dim _INPUT_BITS As Byte Access
    Dim _OUT_FILTER As Byte Access
'
'*****Variables for lookup tables
'    Dim VAR_TEXT_INDEX As Byte Access       '/ Used to pass the offset to the various lookup tables.  This variable is not altered by the lookup routine
    Dim VAR_TABLE_OFFSET As Byte Access '/ Used within the lookup routines to hold the offset
'
'*****Variables for math operations
    Dim VAR1_Float As Float Access
    Dim VAR2_Float As Float Access   
'   
    Dim AVERAGE_16BIT As Word
    Dim AVERAGE_16BIT_OLD As Word
    Dim AVERAGE_32BIT As Dword   
'
'    Dim VAR_TEMP1 As SWord Access
    Dim VAR_TEMP1 As Dword Access
    Dim VAR_TEMP2 As Dword Access
    Dim VAR_TEMP3 As Dword Access
'
'*****Variables used in the Goertzel algorithm
'/ These are the values computed during the Goertzel algorithm
    Dim VAR_Y0 As Float Access
    Dim VAR_Y1 As Float Access
    Dim VAR_Y2 As Float Access
    Dim VAR_X0 As Float Access
'
    Dim VAR_COS As Float                    '/ Stores the cosine coefficient used in the algorithm
    Dim VAR_SINE As Float              '/ Stores the cosine coefficient used in the algorithm
    Dim VAR_COEFF  As Float
    Dim VAR_COS2 As Float                   '/ Stores the cosine coefficient used in the algorithm
    Dim VAR_SINE2 As Float          '/ Stores the cosine coefficient used in the algorithm
'
    Dim VAR_SAMPLE_CTR As Byte Access       '/ Used for stepping through the samples in each scan
    Dim VAR_WINDOW_CTR As Byte Access '/ Counter variable used to lookup the correct Hamming window value
'
    Dim VAR_REAL As Float                   '/ Variables used to store the real and imaginary parts of the final result of the
    Dim VAR_IMAG As Float '/ algorithm.  These are then used to determine the magnitude
'     
    Dim VAR_MAGNITUDE As Dword              '/ Variable to store the magnitude of the algorithm output
'
    Dim VAR_DETECTION_THRD As Dword         '/ Stores the detection threshold value currently in use.  The actual value stored
                                            '/ will be equal to either the upper or lower threshold, depending on the previous
    Dim VAR_UPPER_THRD As Dword             '/ state of the detector output.  This is used for the hysteresis
                            '/Stores the upper detection threshold corresponding to the detection threshold input
    Dim VAR_LOWER_THRD As Dword             '/ Stores the lower detection threshold corresponding to the detection threshold input
'
    Dim VAR_MAG As Dword   
    Dim VAR_OLDMAG As Dword
    Dim MAX_MAG As Dword
'
    Dim ValueLoopDelay As Float
    Dim ADCResultSW As SWord Access
    Dim wADRESH As ADRESL.Word
    Dim wTMR1 As TMR1L.Word
'/==============================================================================
'*******************************************************************************
'*******************************************************************************
'/ Sampling Frequency = 150 X NbSamples
'/ 30000 Hz = 150 x 200
'/ 12000 Hz = 150 x 80

'    Symbol SAMPLE_DELAY $FE46           '/ Original Sample delay values for Timer 1, corresponding to 4400Hz sample rate. (11300 Hz??)
'    Symbol SAMPLE_DELAY $FE39           '/ Recalculated Sample delay values for Timer 1, corresponding to 11000Hz sample rate for Xtal 20Mhz.
'    Symbol SAMPLE_DELAY $FB90           '/ Recalculated Sample delay values for Timer 1, corresponding really to 4400Hz sample rate.
    Symbol SAMPLE_DELAY $FE5F           '/ Recalculated Sample delay values for Timer 1, corresponding really to 12000Hz sample rate.
'
'    Symbol NUMBER_OF_SAMPLES = _NumberSamples      '/ This is the number of samples taken and processed during the Goertzel algorithm
'    Symbol NUMBER_OF_SAMPLES = _NumberSamples       '/ This is the number of samples taken and processed during the Goertzel algorithm
'
'**********GPIO Assignments
    ' GP0=AN0 Analog Input for audio signal
    ' GP1=AN1 Detection threshold setting analog input
    ' GP2=Main Output
    ' GP3= Free
    ' GP4=XTAL
    ' GP5=XTAL
'
    Symbol F_SIGN_INVERTED = PORTA.3         '/ MCLR PIn is as Input for inverting the detected output.
                                            '/ 1 => NO inverted, 0 => Inverted
    Symbol TEST_PIN = PORTA.2                '/ Output pin that is used to test the Frequency Loop
    Symbol OUTPUT_PIN = PORTA.2              '/ Output pin that is set high if the programmed sequence is detected
'///////////////////////////////////////////////////////////////////////////////
'|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
PROGRAM_START:
    #ifdef Watchdog_req
    Clrwdt
    #endif
    WDTCON = %00001001          '/ As Reset
    INTCON = 0
'    IOC = 0
'    WPUB = 0
    PORTA = 0                    '/Clear the gpio port
    CM2CON1 = 0
    CM2CON0 = 0
'
    '/ Configure I/O ports as inputs or outputs
    '/ GPIO.0 => Analog0 => Analog Signal Input
    '/ GPIO.1 => Analog1 => Sensivity Adjustment
    '/ GPIO.2 => OutPin Digital => Frequency Detected   
    '/ GPIO.3 => MCLR
    '/ GPIO.4 => XTAL
    '/ GPIO.5 => XTAL
   
    '/
    TRISA = %00110011           '/ Configure GPIO I/O
    ANSEL = %00000011           '/ Configure AN0/AN1/ to analog   
    '/--------------------------------------------------------------------------
    '/ Configure the A/D converter for right justified results, reference voltage of Vdd (5 volts),
    '/ select analog channel 0, and turn the converter ON.
    ADCON0 = %00000001   
    ADCON1 = 0
'
'    ADCON2 = %10111110                  ' 20TAD, FOSC/64
    ADCON2 = %10101110                  ' 12TAD, FOSC/64
'    ADCON2 = %10111110                 ' 20TAD, FOSC/64
'    ADCON2 = %10101010                  ' 12TAD, FOSC/32
'    ADCON2 = %10101101                  ' 12TAD, FOSC/16
'    ADCON2 = %10111100                  ' 20TAD, FOSC/4   
'/------------------------------------------------------------------------------
    '/ Select analog channel 0
    ADCON0Bits_CHS0 = False     
    '/ Select analog channel 1
    'ADCON0Bits_CHS0 = True       
'
    '/ Turn off the comparator module
'    CMCON0 = 7
'
    '/ Clear digital filter variables
    'Count_A = 0   
    'Count_B = 0
    'CVA = 0
    'CSA = 0
    _CONT_A = 0
    _CONT_B = 0     
    _INPUT_BITS = 0
    _OUT_FILTER = 0
    VAR_MAGNITUDE = 0
    VAR_OLDMAG = 0
    MAX_MAG = 0
'
    '/ Output pin value at PowerOn
'    If F_SIGN_INVERTED = True Then
'        OUTPUT_PIN = False      '/ Physical Pin       
'        _OUT_FILTER.2 = False   '/ Image Pin
'    Else
'        OUTPUT_PIN = True       '/ Same as LM567
'        _OUT_FILTER.2 = True
'    EndIf
    Low LATA.2
    Cls
'*******************************************************************************
'END CONFIGURATION OF THE PROCESSOR
'*******************************************************************************
DelayMS 125
    HRSOut "TONE DECODER WITH PIC18F25K20",CR
'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
'Main Program
'+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    VAR_DETECTION_THRD = 80000     '/ Initialize the detection threshold to begin with
'///////////////////////////////////////////////////////////////////////////////
'///////////////////////////////////////////////////////////////////////////////
'    PIE1Bits_TMR1IE = False
    ' These values are used to establish the absolute minimum value of the threshold
    Symbol UPPER_THRESHOLD_FACTOR = 64 '/ 72  These values correspond to a hysterestis window width of 1/4 of the center value
    Symbol LOWER_THRESHOLD_FACTOR = 40 '/ 56 (1/8 of the window above and below the center"
'
    '/This value is used in the process of mapping the 10 bit A/D full scale range to
    '/The desired full scale range of values producted by the algorithm output
    Symbol DETECTION_THRD_SCALE_FACTOR = 8 
'   
    ' It cannot be zero, otherwise the detection will be triggered by noise
    Symbol DETECT_THRESHOLD_OFFSET = 39     
'
SENSIVITYADJUST:
    '/ Config the Average variables of potenciometer reading at power ON.
    AVERAGE_16BIT = 0
    AVERAGE_32BIT = 0
    VAR_AVG_CTR = 8
'
    '/ READ_ADC_FOR_AVERAGE:
'    ADCON0Bits_CHS1 = False
    ADCON0Bits_CHS0 = True  '/ Analog 1 => Pot sensitivity
    '/ Read the A/D input to get the setting of the detection threshold input
    Repeat
        DelayUS 500
        '/ Get sample from A/D
        ADC_READING()       '/ => ADCResultSW (Word)
        AVERAGE_16BIT = AVERAGE_16BIT + wADRESH '/ Variable Integer NO signed
        Dec VAR_AVG_CTR
    Until VAR_AVG_CTR = 0
'
    'Divide the total by 8 to get the average
    AVERAGE_16BIT = AVERAGE_16BIT / 8
    '/----------------------------------------------------------------------
    'Add the offset, used to establish the minimum allowable value of the detection threshold
    AVERAGE_16BIT = AVERAGE_16BIT + DETECT_THRESHOLD_OFFSET
'
    'Multiply the value by a scale factor
    'The value is now equal to the center value of the detection threshold. Store the value
    VAR_TEMP1 = AVERAGE_16BIT * DETECTION_THRD_SCALE_FACTOR       
'
    'Determine the upper threshold
    'Store the upper threshold
    VAR_UPPER_THRD = VAR_TEMP1 * UPPER_THRESHOLD_FACTOR
'
    'Determine the lower threshold
    'Store the lower threshold
    VAR_LOWER_THRD = VAR_TEMP1 * LOWER_THRESHOLD_FACTOR
'   
'/ Sensitivy Adjust completed.
'    HRSOut "VAR_LOWER_THRD: ",Dec VAR_LOWER_THRD,CR
'    HRSOut "VAR_UPPER_THRD: ",Dec VAR_UPPER_THRD,CR
   '/ Config the Average variables of potenciometer reading
'///////////////////////////////////////////////////////////////////////////////
'///////////////////////////////////////////////////////////////////////////////
'     6.283185 = TwoPI = 3.14159265 * 2.0
'     K = (INT) 0.5 + (_NumberSamples * _TarjetFrequency / _SampleRate)
'     W = TwoPI * K / _NumberSamples
'     Sine = Sin(W)
'     Cosine = Cos(W)
'     Coeff = 2 * Cos(W)
'
' With K = decimals => corrections
'    w2 = 2*pi*k;
'    cw2 = Cos(w2);
'    sw2 = Sin(w2);
'
    $if _TarjetFrequency = 600
    $if _SampleRate = 18000
    $if _NumberSamples = 120
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    ' INT K = 4  W = 0.2094395
    VAR_SINE = 0.0036554
    VAR_COS =  0.9999933
    VAR_COEFF = 1.999986
    '
    '/ K = 4.5 W = , K is not an integer
    '/ Disable calculations with K integer
    $undef _INT_K
    VAR_SINE2 = 0.4736937    '/ Sine coefficient for: 600 Hz / 18000Hz/120 samples
    VAR_COS2 =  0.9999658    '/ Cosine coefficient for: 600 Hz / 18000Hz/120 samples
    $endif   
    $endif
    $endif
'             
    $if _TarjetFrequency = 600
    $if _SampleRate = 7200
    $if _NumberSamples = 120
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    ' INT K = 10  W = 0,52359875
    VAR_SINE = 0.0091383
    VAR_COS =  0.9999582
    VAR_COEFF = 1.9999164
    '
    '/ K = 10.5 W = 65.9734425, K is not an integer
    '/ Disable calculations with K integer
    $undef _INT_K
    VAR_SINE2 = 0.0913356    '/ Sine coefficient for: 600 Hz / 7200Hz/120 samples
    VAR_COS2 =  0.9998729     '/ Cosine coefficient for: 600 Hz / 7200Hz/120 samples
    $endif   
    $endif
    $endif
'   
    $if _TarjetFrequency = 600
    $if _SampleRate = 7200   
    $if _NumberSamples = 48
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    '/ INT K = 4  W = 0.5235987
    VAR_SINE = 0.0091383        '/ Sine coefficient for: 600 Hz / 7200Hz/48 samples
    VAR_COS =  0.9999582        '/ Cosine coefficient for: 600 Hz / 7200Hz/48 samples
    VAR_COEFF = 1.9999164       '/ Coeff coefficient = 2*Cosine for: 600 Hz / 7200Hz/48 samples
    '
    '/ K = 4.5  W = 28.2743325 K is not an integer
    '/ Disable calculations with K integer
    $undef _INT_K
    VAR_SINE2 = 0.4736937
    VAR_COS2 = 0.9999658
    $endif
    $endif
    $endif
'
    $if _TarjetFrequency = 600
    $if _SampleRate = 12000   
    $if _NumberSamples = 80
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    '/ INT K = 4  W = 0.3141592  K is an integer
    VAR_SINE =  0.0054830    '/ Sine coefficient for: 600 Hz / 7200Hz/80 samples
    VAR_COS =   0.9999849    '/ Cosine coefficient for: 600 Hz / 7200Hz/80 samples
    VAR_COEFF = 1.9999618    '/ Coeff coefficient = 2*Cosine for: 600 Hz / 7200Hz/80 samples
    '
    '/ K =  W =
    '/ Enable calculations with K integer
    $ifndef _INT_K
    $define _INT_K
    $endif
    VAR_SINE2 = 0
    VAR_COS2 = 0
    $endif
    $endif
    $endif
'
    $if _TarjetFrequency = 200
    $if _SampleRate = 7200   
    $if _NumberSamples = 120
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    '/ INT K = 3.8  W = 0.1570796  K is not integer
    VAR_SINE =  0.0027415    '/ Sine coefficient for: 200 Hz / 7200Hz/80 samples
    VAR_COS =   0.9999849    '/ Cosine coefficient for: 200 Hz / 7200Hz/80 samples
    VAR_COEFF = 1.9999999    '/ Coeff coefficient = 2*Cosine for: 200 Hz / 7200Hz/80 samples
    '
    '/ K = 3.8 W = 23.876103
    '/ Disable calculations with K integer
    $undef _INT_K
    VAR_SINE2 = 0.9144228
    VAR_COS2 =  0.9998726
    $endif
    $endif
    $endif
'/------------------------------------------------------------------------------
    HRSOut "TARGET FREQ: ", Dec _TarjetFrequency," HZ",CR
    HRSOut "NB SAMPLES : ", Dec _NumberSamples,CR
    HRSOut "SAMPLE RATE: ", Dec _SampleRate," HZ",CR       
    $ifdef _INT_K
    HRSOut "CALCULATIONS WITH K INTEGER",CR   
    $else
    HRSOut "CALCULATIONS WITH K NOT INTEGER",CR     
    $endif
'///////////////////////////////////////////////////////////////////////////////
    InitSampleRate()
'/==============================================================================
'    $ifdef _MeasureLoopTime
'    ConfigTimer1()
'    $endif
    VAR_Y0 = 0
    VAR_Y1 = 0
    VAR_Y2 = 0
    AVERAGE_16BIT = 0
    VAR_AVG_CTR = 8
'/==============================================================================
START_FILTER:        '/ BEGIN MAIN LOOP
    #ifdef Watchdog_req
    Clrwdt
    #endif 
'/**************Determine the Upper and Lower Detection Thresholds at power up**
AVG_ANALOGREADING:              '/ For Pot sensivity adjust
    '/ READ_ADC_FOR_AVERAGE:
    '/ The Detection of the potenciomenter takes 8 loops
'    ADCON0Bits_CHS1 = False
    ADCON0Bits_CHS0 = True      '/ Analog 1
'
    '/ Read the A/D input to get the setting of the detection threshold input
    ADC_READING()               '/ Get sample from A/D => ADCResultSW (SWord)
'
    AVERAGE_16BIT = AVERAGE_16BIT + wADRESH
'
    Dec VAR_AVG_CTR
    If VAR_AVG_CTR = 0 Then
'
        'Divide the total by 8 to get the average
        AVERAGE_16BIT = AVERAGE_16BIT / 8
        '/----------------------------------------------------------------------
        'Add the offset, used to establish the minimum allowable value of the detection threshold
        AVERAGE_16BIT = AVERAGE_16BIT + DETECT_THRESHOLD_OFFSET
'
        'Multiply the value by a scale factor
        'The value is now equal to the center value of the detection threshold. Store the value
        VAR_TEMP1 = AVERAGE_16BIT * DETECTION_THRD_SCALE_FACTOR         
'
        'Determine the upper threshold
        'Store the upper threshold
         VAR_UPPER_THRD = VAR_TEMP1 * UPPER_THRESHOLD_FACTOR
'
        'Determine the lower threshold
        'Store the lower threshold
        VAR_LOWER_THRD = VAR_TEMP1 * LOWER_THRESHOLD_FACTOR   
'
        VAR_AVG_CTR = 8     '/ Load number of average for next calculation
        AVERAGE_16BIT = 0   '/ Clear Average for next calculation.
    EndIf
'///////////////////////////////////////////////////////////////////////////////
'**********Run the Goertzel Routine to determine if the target frequency is present
'/******************************************************************************
'*******************************************************************************
' Code GOERTZEL_SAMPLE
' This subroutine is the Goertzel routine itself.  It returns the magnitude of the
' response in the VAR_MAGNITUDE variable.
'*******************************************************************************
    'Set the A/D converter to read from AN0 (audio input)
'    ADCON0Bits_CHS1 = False
    ADCON0Bits_CHS0 = False       
'
    T2CONBits_TMR2ON = True     '/ Start the Timer2
'///////////////////////////////////////////////////////////////////////////////
    ' Load sample counter with number of samples
    VAR_SAMPLE_CTR = NUMBER_OF_SAMPLES
'
    ' Clear window counter
    VAR_WINDOW_CTR = 0
'
    $ifdef _MeasureLoopTime
    ConfigTimer1()
    $endif
'
    VAR_Y2 = 0    '/ Clear values to start again.
    VAR_Y1 = 0 
'/==============================================================================
    '/--------------------------------------------------------------------------
    WriteMSG("CODE: GOERTZEL_SAMPLE : START")
    '/--------------------------------------------------------------------------
GOERTZEL_LOOP:
    Repeat
        While PIR1Bits_TMR2IF = 0 : Wend    '/ Waiting the Timer1 interrupt.
        PIR1Bits_TMR2IF = 0                 '/ Then Clear the interrupt flag for next loop.
        '/----------------------------------------------------------------------
        $ifdef _MeasureLoopTime
        T1CONBits_TMR1ON = True         '/ Start Timer1, counting...
        $endif
        #ifdef Watchdog_req
        Clrwdt
        #endif   
'
        $ifdef _TEST_TIMING
        Toggle TEST_PIN         '/ Using the same Pin as Frequency Detected. => Timing Frequency / 2
'        High LATC.0
        $endif
'
        '/----------------------------------------------------------------------
CALC_VAR_X0:
        '
        '/ Get sample from A/D
        'wADRESH = 0
        ADCON0Bits_GO_DONE = True
        'Poll the go done bit.  The conversion is complete when the bit is cleared
        Repeat : Until ADCON0Bits_GO_DONE = False
        '/----------------------------------------------------------------------
        '/ Look up the window value for multiplication
        $ifdef _EnableHammingWindow
        MyLocalByte0 = CRead8 HAMMING_WINDOW_TABLE2[VAR_WINDOW_CTR]
'       
        '/ Multiply the A/D reading by the window value
        PP_BARG = wADRESH - 511             '/ Center the samples around value (-, 0, +)
        VAR_X0 = PP_BARG * MyLocalByte0     '/ X Hamming Window Factor & Store the X0 value for later
        Inc VAR_WINDOW_CTR                  '/ Next value in the Hamming Table
        $else
        VAR_X0 = wADRESH - 511              '/ Center the samples around value (-, 0, +)       
        VAR_X0 = VAR_X0 * 64
        $endif
'
'///////////////////////////////////////////////////////////////////////////////
'****Calculate Y0***************************************************************
CALC_Y0:
'/ Calculations for every sample.
'/ Formula: Y0 = (2*COS_COEFF * Y1) - Y2 + X0
'
'/ Calculations for every sample.
'/ nQ0 = (Coeff * nQ1) - nQ2 + AudioSample
'
    ' The value in VAR_1 is now 2*COS_COEFF*Y1 - Y2.  Next we will add X0
'        VAR2_Float = VAR_Y1 * VAR_COEFF
'        VAR2_Float = VAR2_Float - VAR_Y2
'        VAR_Y0 = VAR2_Float + VAR_X0
'
        PP_AARG = VAR_Y1 * VAR_COEFF
        PP_BARG = PP_AARG - VAR_Y2
        VAR_Y0 = PP_BARG + VAR_X0
'
        VAR_Y2 = VAR_Y1             '/ Copy Y1A to Y2A
        VAR_Y1 = VAR_Y0             '/ Copy Y0A to Y1A
'
        '/ Decrement sample counter, goto end if 0 is reached. Otherwise, go to start of sampling loop
        Dec VAR_SAMPLE_CTR
'LATC.0  = 0
    Until VAR_SAMPLE_CTR = 0
    '/--------------------------------------------------------------------------
    WriteMSG("CODE: GOERTZEL_SAMPLE : END")
    '/--------------------------------------------------------------------------
    '/ Debug code to measure the GOERTZEL loop time.
    $ifdef _MeasureLoopTime
STOPTTimer1:
    Nop
    StopTimer1()                    '/ Stop Timer1 and read the eeprom values
    $endif
'///////////////////////////////////////////////////////////////////////////////
'***********Calculate the real and imaginary values
CALC_REAL:
'*****COMPUTE REAL PART
'/ The real part is REAL = Y1 - (Y2*COS_COEFF)
'/ Real = (Q1 - Q2 * Cosine)
'    VAR2_Float = VAR_Y2 * VAR_COS       'First, compute Y2*COS_COEFF
'    VAR_REAL = VAR_Y1 - VAR2_Float      'Now subtract the result from Y1 'Store the Real Part to VAR_REAL
'
    PP_BARG = VAR_Y2 * VAR_COS
    VAR_REAL = VAR_Y1 - PP_BARG
'
'*****COMPUTE IMAGINARY PART
CAL_IMAG:
'/ The imaginary part is IMAG = Y2 * SIN_COEFF
    VAR_IMAG = VAR_Y2 * VAR_SINE         ' Store the Imaginary Part to VAR_IMAG
'
'*******************************************************************************
CAL_K:
' Code MAGNITUDE: If K = Float
'/ Corrections of REAL & IMAG when K is NOT an integer.
'/ K = with DECIMALS
'/    w2 = 2*pi*k;
'/    cw2 = Cos(w2);
'/    sw2 = Sin(w2);
'/
'/    I = It*cw2 + Q*sw2;
'/    Q = -It*sw2 + Q*cw2;   
'/
    $ifndef _INT_K
    VAR1_Float = VAR_IMAG * VAR_COS2
    PP_AARG = VAR_REAL * VAR_SINE2
    VAR_IMAG = VAR1_Float + PP_AARG

    VAR1_Float = VAR_IMAG * VAR_SINE2
    PP_AARG = VAR_REAL * VAR_COS2
    VAR_REAL = PP_AARG - VAR1_Float
    $endif
'/==============================================================================
' This subroutine is used to provide an approximation of the magnitude of a vector.
' This is the (max + 1/4*min) OR the (Max + 3Min/8) version.
' It is used to avoid the more complicated math of the pythagorean theorem to
' determine the magnitude of a vector.
' It is used in this application to estimate the magnitude of the algorithm output
' from the real and imaginary parts.
'*******************************************************************************
' Calculate the magnitude. Move the results into the variables VAR_TEMP2 & VAR_TEMP3
CALC_MAG:
'/ These formulas dot not modify the values.
    PP_AARG = fAbs(VAR_IMAG)        '/ VAR_IMAG: Take the absolute value of Imaginary
    VAR_TEMP3 = fRound(PP_AARG)
'   
    PP_AARG = fAbs(VAR_REAL)        '/ VAR_REAL: Take the absolute value of Real
    VAR_TEMP2 = fRound(PP_AARG)
'   
'/==============================================================================
'/ Code simulates the square root of magnitude.
'/ Magnitude:   
'/                      Largest error(%) Largest error(dB) Average error(%) Average error(dB) Max |V| (%
'/ (Max + 3Min/8)         6.8%            0.57 dB          3.97%             0.34 dB            93.6%
'/ (15*(Max + Min/2)/16) –6.25%          –0.56 dB          1.79%             0.15 dB            95.4%
'/------------------------------------------------------------------------------
'/ Magnitude: (Max + Min/4)
'    If VAR_TEMP2 > VAR_TEMP3 Then
'        VAR_MAG = (VAR_TEMP3 / 4)
'        VAR_MAG = VAR_MAG + VAR_TEMP2
'    EndIf
'    If  VAR_TEMP3 > VAR_TEMP2 Then
'        VAR_MAG = (VAR_TEMP2 / 4)
'        VAR_MAG = VAR_MAG  + VAR_TEMP3
'    EndIf
'/------------------------------------------------------------------------------
'/ Magnitude: (Max + 3*Min/8)
    If VAR_TEMP2 > VAR_TEMP3 Then
        VAR_MAG = VAR_TEMP3 * 3
        VAR_MAG = VAR_MAG / 8
        VAR_MAG = VAR_MAG + VAR_TEMP2
    EndIf
    If  VAR_TEMP3 > VAR_TEMP2 Then
        VAR_MAG = VAR_TEMP2 * 3
        VAR_MAG = VAR_MAG / 8
        VAR_MAG = VAR_MAG  + VAR_TEMP3
    EndIf
'/------------------------------------------------------------------------------
'/ Magnitude: Sqr(VAR_REAL^2 + VAR_IMAG^2)
'
'    VAR_REAL = VAR_REAL * VAR_REAL
'    VAR_IMAG = VAR_IMAG * VAR_IMAG
'    VAR_MAG = Sqr(VAR_REAL + VAR_IMAG)
'/------------------------------------------------------------------------------
CALC_MEDIAN:
'/ It is necessary to make a median for better stability.
'/ Median/2:
    '/ val = (val + sample) / 2
'    VAR_MAGNITUDE = VAR_MAGNITUDE + VAR_MAG   
'    VAR_MAGNITUDE = VAR_MAGNITUDE / 2

'/ Median/3:
    '/ val = (val + old_sample + sample) / 3
    '/ old_sample = sample
    VAR_MAGNITUDE = VAR_MAGNITUDE + VAR_OLDMAG
    VAR_MAGNITUDE = VAR_MAGNITUDE + VAR_MAG
    VAR_MAGNITUDE = VAR_MAGNITUDE / 3
    VAR_OLDMAG = VAR_MAG       
'/------------------------------------------------------------------------------
    VAR_Y2 = 0    '/ Clear values to start again.
    VAR_Y1 = 0   
'/------------------------------------------------------------------------------
'END of Routine GOERTZEL
'/******************************************************************************
'/******************************************************************************
' VAR_MAGNITUDE for comparison to threshold
'/------------------------------------------------------------------------------
' Compare magnitude to threshold to decide whether frequency is present.
' If the result is less than the current threshold, set the upper threshold as
' the current threshold for the next time the algorithm is run.
' This is for the hysteresis:
' If the result is greater than the current threshold, set the lower threshold
' as the current threshold for the next time the algorithm is run.
'
    If VAR_MAGNITUDE > VAR_DETECTION_THRD Then
        '/ Frequency detected
        $ifndef _TEST_TIMING
        High LATA.2        '/ Image of output pin
        $endif
        VAR_DETECTION_THRD = VAR_LOWER_THRD
    EndIf
    If VAR_MAGNITUDE < VAR_DETECTION_THRD Then
        '/ Frequency NOT detected
        $ifndef _TEST_TIMING
        Low LATA.2
        MAX_MAG = 0
        $endif
        VAR_DETECTION_THRD = VAR_UPPER_THRD
    EndIf
'/------------------------------------------------------------------------------
'/ Compare magnitude to the Upper threshold to decide whether frequency is present
'/
'    If VAR_MAGNITUDE > VAR_UPPER_THRD Then
'        '/ Frequency detected
'        $ifndef _TEST_TIMING
'        High LATA.2        '/ Image of output pin
'        $endif
'    EndIf
'    If VAR_MAGNITUDE < VAR_LOWER_THRD Then
'        '/ Frequency NOT detected
'        $ifndef _TEST_TIMING
'        Low LATA.2
'        MAX_MAG = 0
'        $endif
'    EndIf
'/------------------------------------------------------------------------------
'/ PRINT some results to the LCD:
    $ifndef _TEST_TIMING
    If VAR_MAGNITUDE > MAX_MAG Then
        MAX_MAG = VAR_MAGNITUDE
    EndIf   
    DelayMS 50
    Print At 1,1,"                "
    DelayMS 5
    Print At 1,1,Dec VAR_MAGNITUDE      '/ Print the magnitude result
    Print At 1,9,Dec MAX_MAG            '/ Print the MAX Value of Magnitude
    DelayMS 5
    Print At 2,1,"                "   
    DelayMS 5
    Print At 2,1,"L", Dec VAR_LOWER_THRD   
    Print " U", Dec VAR_UPPER_THRD       
    $endif
'/******************************************************************************
'/******************************************************************************
' The purpose of this routine is to debounce, i.e. digitally low pass filter
' inputs. The algorithm handles upto 8 bits at a time. An input is considered
' filtered if it has not changed states in the last 4 samples.
'
' 2-bit cyclic vertical counters count the 4 samples. As long as there is no
' change, the counters are held in the reset state of 00b. When a change is detected
' between the current sample and the filtered or debounced sample, the counters
' are incremented. The counting sequence is 00,01,10,11,00... When the counters
' roll over from 11b to 00b, the debounced state is updated. If the input changes
' back to the filtered state while the counters are counting, then the counters
' are re-initialized to the reset state and the filtered state is unaffected.
' In other words, a glitch or transient input has been filtered.
'
' The filter delay Time is (1/Timing Frequency Hz) * 4
'/ For 4400 Hz timing then the pulse filtered is < 909 US 1100 Hz
'
' The algorithm will return in W the state of those inputs that have just
' been filtered.
' 13 instructions
' 14 cycles
' Inputs:
' csa - The current sample Outputs
'
' cva - The current value (filtered version of csa)
' W - Bits that have just been filtered.
'
' RAM used
' Count_A,
' Count_B - State variables for the 8 2-bit counters
' CSA
' CVA
'/------------------------------------------------------------------------------
(*
DIGITALFILTER:
    Bcf STATUS,5
    ' Increment the vertical counter
    Movf Count_B,W
    Xorwf Count_A,F
    Comf Count_B,F
    ' See if any changes occurred
    Movf CSA,W
    Xorwf CVA,W
    ' Reset the counter if no change has occurred
    Andwf Count_B,F
    Andwf Count_A,F
    ' If there is a pending change and the count has
    ' rolled over to 0, then the change has been filtered
    Xorlw 0xFF          ' Invert the changes
    Iorwf Count_A,W     ' If count is 0, both A and B
    Iorwf Count_B,W     ' bits are 0
    ' Any bit in W that is clear at this point means that the input
    ' has changed and the count has rolled over.
    Xorlw 0xFF
    ' Now W holds the state of inputs that have just been filtered
    ' to a new state. Update the changes:
    Xorwf CVA,F
    ' leave with W holding the state of all filtered bits that have
    ' change state
    $ifndef _TEST_TIMING                                   
'    Andlw 4
    Movwf PORTA                  '/ Value filtered, send to pin Port   
    $endif
*)   
DIGITALFILTER:
    ' This code is written near the assembler method. (more efficient)
    ' The current sample is _INPUT_BITS           
    ' Increment the vertical counter
'    #ifdef Watchdog_req
'    Clrwdt
'    #endif
(*
    WREG = _CONT_B
    _CONT_A = _CONT_A ^ WREG
    _CONT_B = ~_CONT_B
    'See if any changes occurred
    WREG = _INPUT_BITS
    WREG = _OUT_FILTER ^ WREG
    _CONT_B = _CONT_B & WREG
    _CONT_A = _CONT_A & WREG       
    'Determine the counter's state
    WREG = _CONT_B
    WREG = _CONT_A | WREG   
    'Clear all bits that are filtered-or more accurately,
    'save the state of those that are being filtered
    _OUT_FILTER = _OUT_FILTER & WREG
    'Xorlw   255
    WREG = WREG ^ 255       'Invert the changes   
    'Re-write the bits that haven't changed.
    WREG = _INPUT_BITS & WREG     
    _OUT_FILTER = _OUT_FILTER | WREG
    'The 8 filtered bits are in _OUT_FILTER Variable       
'    $ifdef _TEST_TIMING
'    LATA = _OUT_FILTER 
'    $endif   
    LATA.2 = _OUT_FILTER.2
*)   
'===============================================================================
'===============================================================================
    GoTo START_FILTER   '/ END LOOP
'///////////////////////////////////////////////////////////////////////////////
'///////////////////////////////////////////////////////////////////////////////
'/ PROCEDURES:
'===============================================================================
Proc InitSampleRate()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: InitSampleRate : START")
    '/--------------------------------------------------------------------------
' Set the Timer2 for Sample timing (GOERTZEL_LOOP) => (18000 Hz)
$if _SampleRate = 18000       
    '/ For Xtal = 64Mhz
    '/Timer2
    '/Prescaler 1:1; Postscaler 1:4; TMR2 Preload = 221; Actual Interrupt Time : 55,5 us
    $if _xtal = 64
    PIE1Bits_TMR2IE = False
    T2CON = %00011000     
    PR2 = 221
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2   
    $endif
'
    '/ For Xtal = 32Mhz
    'Timer2
    'Prescaler 1:1; Postscaler 1:3; TMR2 Preload = 221; Actual Interrupt Time : 83,25 us
    $if _xtal = 32
    PIE1Bits_TMR2IE = False
    T2CON = %00010000           '/ T2CON = OFF, 1:2 Postscaler, 00 = Prescaler is 1   
    PR2 = 221
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2   
    $endif
'
    '/ For Xtal = 20Mhz Timing = 12048 Hz
    $if _xtal = 20
    PIE1Bits_TMR2IE = False
    T2CON = %00001000       '/ T2CON = OFF, 1:2 Postscaler, 00 = Prescaler is 1   
    PR2 = 207
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2
    $endif
$endif
'
' Set the Timer2 for Sample timing (GOERTZEL_LOOP) => (12048 Hz)
$if _SampleRate = 12000       
'/ Number Samples = 80 Sample Rate = 12048 Hz       
    '/ For Xtal = 64Mhz
    $if _xtal = 64
    PIE1Bits_TMR2IE = False
    T2CON = %00101000       '/ T2CON = OFF, 1:2 Postscaler, 00 = Prescaler is 1   
    PR2 = 221
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2   
    $endif
'
    '/ For Xtal = 32Mhz
    'Timer2
    'Prescaler 1:1; Postscaler 1:3; TMR2 Preload = 221; Actual Interrupt Time : 83,25 us
    $if _xtal = 32
    PIE1Bits_TMR2IE = False
    T2CON = %00010000           '/ T2CON = OFF, 1:2 Postscaler, 00 = Prescaler is 1   
    PR2 = 221
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2   
    $endif
'
    '/ For Xtal = 20Mhz Timing = 12048 Hz
    $if _xtal = 20
    PIE1Bits_TMR2IE = False
    T2CON = %00001000       '/ T2CON = OFF, 1:2 Postscaler, 00 = Prescaler is 1   
    PR2 = 207
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2
    $endif
$endif
'
$if _SampleRate = 7200 
'/ Number Samples = 48 Sample Rate = 7200 Hz (7220 Hz)
    '/ For Xtal = 64Mhz
    '//Timer2
    '//Prescaler 1:1; Postscaler 1:9; TMR2 Preload = 246; Actual Interrupt Time : 138,9375 us
    $if _xtal = 64
    PIE1Bits_TMR2IE = False
    T2CON = %01000000
    PR2 = 246
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2   
    $endif   
'
    '//Timer2
    '//Prescaler 1:1; Postscaler 1:5; TMR2 Preload = 221; Actual Interrupt Time : 138,75 us
    '/ For Xtal = 32Mhz
    $if _xtal = 32
    PIE1Bits_TMR2IE = False
    T2CON = %00100000       '/ T2CON = OFF, //Prescaler 1:1; Postscaler 1:3; TMR2 Preload = 230;   
    PR2 = 221
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2
    $endif
'
'/ Number Samples = 48 Sample Rate = 7200 Hz (7220 Hz)
    '/ For Xtal = 20Mhz
    $if _xtal = 20
    PIE1Bits_TMR2IE = False
    T2CON = %00010000       '/ T2CON = OFF, //Prescaler 1:1; Postscaler 1:3; TMR2 Preload = 230;   
    PR2 = 230
    TMR2 = 0
    'T2CONBits_TMR2ON = True '/ Start the Timer2
    $endif
$endif   
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: InitSampleRate : END")
    '/--------------------------------------------------------------------------
EndProc
'*******************************************************************************
' Subroutine GOERTZEL_SAMPLE
' This subroutine is the Goertzel routine itself.  It returns the magnitude of the
' response in the VAR_MAGNITUDE variable.
'*******************************************************************************
(*
Proc GOERTZEL_SAMPLE()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: GOERTZEL_SAMPLE : START")
    '/--------------------------------------------------------------------------
    'Clear Y0, Y1, and Y2
    VAR_Y0 = 0
    VAR_Y1 = 0
    VAR_Y2 = 0
'
    'Load sample counter with number of samples
    VAR_SAMPLE_CTR = NUMBER_OF_SAMPLES
'
    'Clear window counter
    VAR_WINDOW_CTR = 0
'
    'Set the A/D converter to read from AN0 (audio input)
    ADCON0Bits_CHS1 = False
    ADCON0Bits_CHS0 = False       
'///////////////////////////////////////////////////////////////////////////////
GOERTZEL_LOOP:
    T1CON = 0                       'Turn timer1 OFF, select 1:1 prescaler, use system clock, Internal clock (FOSC/4)
'
    '/ Sample delay values for Timer 1, corresponding to 4400Hz sampl3 rate. 11300 Hz??
    '/ Load the sample delay values into TMR1 high and low bytes
    TMR1H = SAMPLE_DELAY.HighByte   '/ 11300 Hz??   $FB90 => 4400 Hz
    Nop
    TMR1L = SAMPLE_DELAY.LowByte    '/ $FE46 for 4400 Hz ???
'
    PIR1Bits_TMR1IF = False         'Make sure the overflow flag is cleared to begin
    T1CONBits_TMR1ON = True         'Turn on timer 1 by setting the TMR1ON bit in T1CON
    '/==========================================================================
    '/ Calculations for every sample.
    '/ Y0 is given by the following formula: Y0 = X0 + (2*COS_COEFF*Y1) - Y2

'    VAR_Y2 = VAR_Y1             '/ Copy Y1A to Y2A
'    VAR_Y1 = VAR_Y0             '/ Copy Y0A to Y1A

    ADC_READING()                   '/ Get sample from A/D     
'   
    '/ Look up the window value for multiplication
    MyLocalByte0 = CRead8 HAMMING_WINDOW_TABLE2[VAR_WINDOW_CTR]   
'
    VAR_X0 = ADCResultSW * MyLocalByte0 '/ Multiply the A/D reading by the window value
    VAR_X0 = VAR_X0 / 64 
'
   Inc VAR_WINDOW_CTR
    '/ Store the X0 value for later
'///////////////////////////////////////////////////////////////////////////////
'****Calculate Y0***************************************************************
'
    ' Y0 is given by the following formula: Y0 = X0 + (2*COS_COEFF*Y1) - Y2
    ' First, take care of the second term, 2*COS_COEFF*Y1.
    VAR1_Float = VAR_Y1 * VAR_COS
'
    ' Multiply the the result in VAR1 by 2 (shift to the left)
    VAR1_Float = VAR1_Float * 2
'
    ' The 2*COS_COEFF*Y1 value is now in VAR_1.  Next we will subtract Y2
    VAR1_Float = VAR1_Float - VAR_Y2
'
    ' The value in VAR_1 is now 2*COS_COEFF*Y1-Y2.  Next we will add X0
    ' The result is transfered to Y0
    VAR_Y0 = VAR1_Float + VAR_X0
'
    ' Poll timer 1 overflow to determine if delay is complete
    ' WAIT_FOR_DELAY_COMPLETE:
    Repeat
        #ifdef Watchdog_req
        Clrwdt
        #endif     
    Until PIR1Bits_TMR1IF = True
'
    VAR_Y2 = VAR_Y1             '/ Copy Y1A to Y2A
    VAR_Y1 = VAR_Y0             '/ Copy Y0A to Y1A
'
    ' Decrement sample counter, goto end if 0 is reached. Otherwise, go to start of sampling loop
    Decfsz VAR_SAMPLE_CTR,F
    GoTo GOERTZEL_LOOP
'///////////////////////////////////////////////////////////////////////////////
'***********Calculate the real and imaginary values
'
'*****COMPUTE REAL PART
' The real part is REAL = Y1 - (Y2*COS_COEFF)
'
    ' First, compute Y2*COS_COEFF
    'VAR1_Float = VAR_Y2 * VAR_COS
    VAR2_Float = VAR_Y2 * VAR_COS      'First, compute Y2*COS_COEFF
'    VAR1_Float = VAR2_Float / 32
'
    ' Now subtract the result from Y1
    ' Store the Real Part to VAR_REAL
    VAR_REAL = VAR_Y1 - VAR2_Float
'
'*****COMPUTE IMAGINARY PART
' The imaginary part is IMAG = Y2 * SIN_COEFF
' Store the Imaginary Part to VAR_IMAG
    'VAR_IMAG = VAR_Y2 * VAR_SINE
    VAR2_Float = VAR_Y2 * VAR_SINE      ' Store the Imaginary Part to VAR_IMAG
    VAR_IMAG = VAR2_Float / 10000
'
'*******************************************************************************
' Subroutine MATH_16BIT_MAGNITUDE
' This subroutine is used to provide an approximation of the magnitude of a vector.
' This is the (max + 1/4*min version.
' It is used to avoid the more complicated math of the pythagorean therorm to
' determine the magnitude of a vector.
' It is used in this application to estimate the magnitude of the algorithm output
' from the real and imaginary parts.
'*******************************************************************************
' Calculate the magnitude. Move the results into the variable VAR_MAGNITUDE
' Magnitude Code
    VAR_TEMP2 = Abs(VAR_REAL)   '/ Take the absolute value of Real
    VAR_TEMP3 = Abs(VAR_IMAG)   '/ Take the absolute value of Imaginary
'
    If VAR_TEMP2 > VAR_TEMP3 Then
        VAR_MAGNITUDE = (VAR_TEMP3 / 4) + VAR_TEMP2
    Else
        VAR_MAGNITUDE = (VAR_TEMP2 / 4) + VAR_TEMP3
    EndIf
'
'Return from Subroutine GOERTZEL_SAMPLE
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: GOERTZEL_SAMPLE : END")
    '/--------------------------------------------------------------------------
EndProc
*)
'*******************************************************************************
' Subroutine MATH_16BIT_MAGNITUDE
' This subroutine is used to provide an approximation of the magnitude of a vector.
' This is the max + 1/4*min version.
' It is used to avoid the more complicated math of the pythagorean therorm to
' determine the magnitude of a vector.
' It is used in this application to estimate the magnitude of the algorithm output
' from the real and imaginary parts.
'*******************************************************************************
''MATH_16BIT_MAGNITUDE:
Proc MATH_16BIT_MAGNITUDE()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: MATH_16BIT_MAGNITUDE : START")
    '/--------------------------------------------------------------------------
    VAR_TEMP2 = Abs(VAR_REAL)
    VAR_TEMP3 = Abs(VAR_IMAG)

    If VAR_TEMP2 > VAR_TEMP3 Then
        VAR_MAGNITUDE = (VAR_TEMP3 / 4) + VAR_TEMP2
    Else
        VAR_MAGNITUDE = (VAR_TEMP2 / 4) + VAR_TEMP3
    EndIf
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: MATH_16BIT_MAGNITUDE : END")
    '/--------------------------------------------------------------------------
EndProc
'*******************************************************************************
'       END of Subroutine MATH_16BIT_MAGNITUDE
'*******************************************************************************

'*******************************************************************************
'Set A/D conversion clock to Fosc/16, and configure GPIO #0 as an analog input.
'Note: The conversion process takes 11 Tad, and Tad will be 2usec for an 8MHz oscillator and these settings, so the conversion
'time will be 22usec or 44 instruction cycles.
'
' Subroutine ADC_READING
'This subroutine initiates an analog to digital conversion.  The correct analog
' channel must be selected before calling the
'subroutine.  The value is returned via the VAR_1 variable.
'Total execution time = 58 instruction cycles, including call and return.
'*******************************************************************************
Proc ADC_READING()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: ADC_READING : START")
    '/--------------------------------------------------------------------------
'Start conversion by setting the GO/DONE bit in ADCON0 register
    ADCON0Bits_GO_DONE = True
'
    'Poll the go done bit.  The conversion is complete when the bit is cleared
    Repeat
        #ifdef Watchdog_req
        Clrwdt
        #endif     
    Until ADCON0Bits_GO_DONE = False
    'ADCResultSW = wADRESH - 511
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: ADC_READING : END")
    '/--------------------------------------------------------------------------
EndProc 'Return from ADC_READING
'*******************************************************************************
'       END of Subroutine ADC_READING
'*******************************************************************************

'*******************************************************************************
'Subroutine AVG_ANALOG_READING
'This routine reads the analog input (selected before the routine is called) 8 times
' and returns the average via the VAR_1 variable
' Variables Integer only
' Output : AVERAGE_16BIT (Integer)
'*******************************************************************************
Proc AVG_ANALOG_READING()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: AVG_ANALOG_READING : START")
    '/--------------------------------------------------------------------------
    AVERAGE_16BIT = 0
    VAR_AVG_CTR = 8

'READ_ADC_FOR_AVERAGE:
    Repeat
        DelayUS 100         '/ DelayUS 500
        'Read the A/D input to get the setting of the frequency select input
        'Get sample from A/D
        ADCON0Bits_CHS1 = False
        ADCON0Bits_CHS0 = True  '/ Analog 1 
        ADC_READING()       '/ => ADCResultSW (SWord)
        AVERAGE_16BIT = AVERAGE_16BIT + Abs(ADCResultSW + 511)
        Dec VAR_AVG_CTR
    Until VAR_AVG_CTR = 0
    ADCON0Bits_CHS1 = False
    ADCON0Bits_CHS0 = False  '/ Analog 0 
    'Divide the total by 8 to get the average
    AVERAGE_16BIT = AVERAGE_16BIT / 8
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: AVG_ANALOG_READING : END")
    '/--------------------------------------------------------------------------
EndProc 'Return from AVG_ANALOG_READING
'*******************************************************************************
'       END of Subroutine AVG_ANALOG_READING
'*******************************************************************************
'///////////////////////////////////////////////////////////////////////////////
'/ Procedure : ConfigTimer1()
'/ Purpose   : Config & start Timer1
'/ Input     : None
'/ OutPut    : Timer1
'/ Notes     : to count the code time
'/
Proc ConfigTimer1()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: ConfigTimer1 : START")
    '/--------------------------------------------------------------------------
    $if _xtal = 64
    T1CON = %10110000   '/ Timer1 = OFF. Count FOSC/4 / 8 => 2000000 Hz = 0,5uS
    $endif
    $if _xtal = 32
    T1CON = %10110000   '/ Timer1 = OFF. Count FOSC/4 / 8 => 1000000 Hz = 1,0uS
    $endif
    $if _xtal = 20
    T1CON = %10110000   '/ Timer1 = OFF. Count FOSC/4 / 8 => 625000 Hz = 1,6uS
    $endif
'   
    TMR1H = 0
    Nop
    TMR1L = 0
    PIE1Bits_TMR1IE = False
    PIR1Bits_TMR1IF = False
'    T1CONBits_TMR1ON = True     '/ Timer1 = ON.
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: ConfigTimer1 : END")
    '/--------------------------------------------------------------------------
EndProc
'///////////////////////////////////////////////////////////////////////////////
'/ Procedure : ConfigTimer1()
'/ Purpose   : Stop Timer1 to count the code time
'/ Input     : None
'/ OutPut    : Timer1
'/ Notes     : NOW read the eeprom memory to know how many uS used for the Calculations loop
'/           : Address 1 & 2 of the eeprom. addres 0 is not used.
'/
Proc StopTimer1()
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: StopTimer1 : START")
    '/--------------------------------------------------------------------------
    T1CONBits_TMR1ON = False
    'EWrite 1,[TMR1H]
    'EWrite 2,[TMR1L]
    '/ Stop the Programm to read the eeprom values with the programmmer
    $if _xtal = 64
    ValueLoopDelay = wTMR1 * 0.5    '/ value max 32,767mS
    $endif
    $if _xtal = 32
    ValueLoopDelay = wTMR1 * 1.0    '/ Value nmax 65,535 mS
    $endif
    $if _xtal = 20
    ValueLoopDelay = wTMR1 * 1.6    '/ Value max 104.564 mS
    $endif
'
    If PIR1Bits_TMR1IF = True Then
        HRSOut "Time Loop: TMR1 OVERFLOWED",CR   
    Else
        HRSOut "Time Loop: ", Dec ValueLoopDelay," MicroSec",CR
    EndIf
    Stop
    '/--------------------------------------------------------------------------
    WriteMSG("PROCEDURE: StopTimer1 : START")
    '/--------------------------------------------------------------------------
EndProc
'*******************************************************************************
' Subroutine HAMMING_WINDOW_TABLE
' These tables are called by the Cread8 command. 
'*******************************************************************************
'HAMMING_WINDOW_TABLE0: 200 values
$if _NumberSamples = 200
Dim HAMMING_WINDOW_TABLE2 As Flash8 = 5,5,5,5,5,5,6,6,6,6,7,7,7,8,8,8,9,9,10,10,11,11,_
    12,12,13,14,14,15,16,17,17,18,19,20,20,21,22,23,24,25,25,26,27,28,29,30,31,32,_
    33,34,35,35,36,37,38,39,40,41,42,43,44,45,45,46,47,48,49,50,50,51,52,53,53,54,_
    55,55,56,57,57,58,58,59,59,60,60,61,61,62,62,62,63,63,63,63,63,64,64,64,64,64,_
    64,64,64,64,64,64,63,63,63,63,63,62,62,62,61,61,60,60,59,59,58,58,57,57,56,55,_
    55,54,53,53,52,51,50,50,49,48,47,46,45,45,44,43,42,41,40,39,38,37,36,35,35,34,_   
33,32,31,30,29,28,27,26,25,25,24,23,22,21,20,20,19,18,17,17,16,15,14,14,13,12,_
12,11,11,10,10,9,9,8,8,8,7,7,7,6,6,6,6,5,5,5,5,5
$endif
'Dim HAMMING_WINDOW_TABLE2 As Flash8 = 5,5,5,5,6,7,7,9,10,11,13,14,16,19,20,_
'    22,25,27,29,33,35,36,40,42,44,47,49,50,53,55,56,58,59,60,61,62,63,63,_
'    64,64,64,64,64,63,63,63,61,60,59,57,56,55,52,50,49,45,44,42,38,36,35,_
'    31,29,27,24,22,20,17,16,14,12,11,10,8,7,7,6,5,5,5

'/ HAMMING_WINDOW for SAMPLES = 80
'Dim HAMMING_WINDOW_TABLE2 As Flash8 = 61,61,61,61,61,61,61,61,61,61,60,60,60,_
'    60,59,59,59,58,58,58,57,57,57,56,56,55,55,54,54,53,52,52,51,51,50,49,49,48,47,_
'    46,46,45,44,43,42,42,41,40,39,38,37,36,35,34,33,32,31,30,29,28,27,25,24,23,22,_
'    21,19,18,17,15,14,13,11,10,9,7,6,4,3,1
'
'$if _NumberSamples = 80   
'Dim HAMMING_WINDOW_TABLE2 As Flash8 = 64,64,64,64,64,64,64,64,64,64,63,63,63,_
'    63,62,62,62,61,61,61,60,60,60,59,59,58,58,57,57,56,55,55,54,54,53,52,52,51,50,_
'    49,49,48,47,46,45,45,44,43,42,41,40,39,38,37,36,35,34,33,32,31,30,28,27,26,25,_
'    24,22,21,20,18,17,16,14,13,12,10,9,7,6,4
'$endif
$if _NumberSamples = 80   
Dim HAMMING_WINDOW_TABLE2 As Flash8 = 6,9,12,14,17,20,22,25,27,30,32,34,36,38,_
40,42,44,45,47,49,50,52,53,54,55,57,58,59,60,60,61,62,62,63,63,64,64,64,64,_
64,64,64,64,64,64,64,63,63,62,62,61,60,60,59,58,57,55,54,53,52,50,49,47,45,44,42,_
40,38,36,34,32,30,27,25,22,20,17,14,12,9,6
$endif
'
'$if _NumberSamples = 48   
'Dim HAMMING_WINDOW_TABLE2 As Flash8 = 64,64,64,64,64,63,63,63,62,61,61,60,59,_
'    59,58,57,56,55,54,52,51,50,48,47,45,44,42,41,39,37,35,33,32,30,28,26,24,_
'    22,19,17,14,12,9,7,4,3,1,1
'$endif
$if _NumberSamples = 48   
Dim HAMMING_WINDOW_TABLE2 As Flash8 = 1,4,9,14,19,24,28,32,35,39,42,45,48,51,_
    54,56,58,59,61,62,63,64,64,64,64,64,64,63,62,61,59,58,56,54,51,48,45,42,_
    39,35,32,28,24,19,14,9,4,1
$endif
$if _NumberSamples = 120   
Dim HAMMING_WINDOW_TABLE2 As Flash8 = 3,5,7,9,11,13,15,17,18,20,22,24,25,27,29,_
30,32,33,34,36,37,39,40,41,42,43,45,46,47,48,49,50,51,52,53,54,54,55,56,57,57,_
58,59,59,60,60,61,61,62,62,63,63,63,64,64,64,64,64,64,64,64,64,64,64,64,64,64,64,_
63,63,62,62,61,61,60,60,59,59,58,57,57,56,55,54,54,53,52,51,50,49,48,47,46,45,_
43,42,41,40,39,37,36,34,33,32,30,29,27,25,24,22,20,18,17,15,13,11,9,7,5,3
$endif
'*******************************************************************************
'       END of Subroutine HAMMING_WINDOW_TABLE
'*******************************************************************************

'/==============================================================================
' Setup the fuses for the 4xPLL
'
Config_Start
    FOSC = HSPLL        ' HS oscillator, PLL enabled and under software control
    Debug = Off         ' Background debugger disabled' RB6 and RB7 configured as general purpose I/O pins
    XINST = Off         ' Instruction set extension and Indexed Addressing mode disabled (Legacy mode)
    STVREN = Off        ' Reset on stack overflow/underflow disabled
    WDTEN = On          ' WDT is always disabled
    FCMEN = Off         ' Fail-Safe Clock Monitor disabled
    IESO = Off          ' Two-Speed Start-up disabled
    WDTPS = 128         ' Watchdog is 1:128
    BOREN = Off         ' Brown-out Reset disabled in hardware and software
    BORV = 18           ' VBOR set to 1.8 V nominal
    MCLRE = On          ' MCLR pin enabled, RE3 input pin disabled
    HFOFST = Off        ' The system clock is held Off until the HF-INTOSC is stable.
    LPT1OSC = Off       ' T1 operates in standard power mode
    PBADEN = Off        ' PORTB<4:0> pins are configured as digital I/O on Reset
    CCP2MX = PORTC      ' CCP2 input/output is multiplexed with RC1
    LVP = Off           ' Single-Supply ICSP disabled
    Cp0 = Off           ' Block 0 (000800-001FFFh) not code-protected
    CP1 = Off           ' Block 1 (002000-003FFFh) not code-protected
    CPB = Off           ' Boot block (000000-0007FFh) not code-protected
    CPD = Off           ' Data eeprom not code-protected
    WRT0 = Off          ' Block 0 (000800-001FFFh) not write-protected
    WRT1 = Off          ' Block 1 (002000-003FFFh) not write-protected
    WRTB = Off          ' Boot block (000000-0007FFh) not write-protected
    WRTC = Off          ' Configuration registers (300000-3000FFh) not write-protected
    WRTD = Off          ' Data eeprom not write-protected
    EBTR0 = Off         ' Block 0 (000800-001FFFh) not protected from table reads executed in other blocks
    EBTR1 = Off         ' Block 1 (002000-003FFFh) not protected from table reads executed in other blocks
    EBTRB = Off         ' Boot block (000000-0007FFh) not protected from table reads executed in other blocks
Config_End
'/==============================================================================


[AlbertoFS]

This is a bit strange. The parameters do not work.

Code Select Expand

    $if _TarjetFrequency = 600
    $if _SampleRate = 4320   
    $if _NumberSamples = 120
    $define _EnableHammingWindow
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    '/ INT K = 17.1320  W = 0.890117875  K is not integer
    VAR_SINE =   0.01553486   
    VAR_COSINE = 0.99987932   
    VAR_COEFF =  1.99975865   
    '
    '/ K = 17.1320 W = 107.6435254
    '/ Disable calculations with K integer
    $undef _INT_K
    VAR_SINE2 = 0.95296069
    VAR_COS2 = -0.30309390
    $endif
    $endif
    $endifThe code detects 30Hz only.

But if I multiply the values by some coefficient, it works.
Could someone explain to me why?

Code Select Expand

    $if _TarjetFrequency = 600
    $if _SampleRate = 4320   
    $if _NumberSamples = 120
    $define _EnableHammingWindow
    Symbol NUMBER_OF_SAMPLES = _NumberSamples
    '/ INT K = 17.1320  W = 0.890117875  K is not integer
    VAR_SINE =   0.01553486 * 0.35   
    VAR_COSINE = 0.99987932 * 0.35   
    VAR_COEFF =  1.99975865 * 0.35 
    '
    '/ K = 17.1320 W = 107.6435254
    '/ Disable calculations with K integer
    $undef _INT_K
    VAR_SINE2 = 0.95296069 * 0.35
    VAR_COS2 = -0.30309390 * 0.35
    $endif
    $endif
    $endif

[AlbertoFS]

Could someone explain to me the problem?
Thank you
Alberto

[top204]

Loading a variable with a single constant value or using a constant expression, do the same thing in the underlying assembler code and load the variable with a constant value.

Have you tried pre-calculating the expression and loading the variables with that value?

For example: 0.01553486 * 0.35 is 0.005437201

So...

VAR_SINE = 0.005437201

I do not know how the filter works, but a lot of filters require different values depending on the speed of the samples taken and the speed of the actual filter operating, and with the 18F devices operating a lot faster than the 14-bit core devices, both in actual frequency and internally with mnemonics, the filter may need different values.

Is it always 0.35 that is needed?

[AlbertoFS]

Hi Les,
For different detection frequencies, different coefficients are needed.
I see the passband detection is very narrow.
Alberto

[AlbertoFS]

I have applied it directly by calculating with the coefficients and the result is the same detection. The band pass is +-10Hz more or less.

[gtvpic]

Glad to hear you've applied the coefficients directly and got the same detection result. A band pass of +/-10Hz difference sounds pretty reasonable considering real-world variations.