/****************************************************************************/
/* Copyright 1993 to 1995 MBARI                                             */
/****************************************************************************/
/* Summary  : IBC Filter Routine                                            */
/* Filename : filter.c                                                      */
/* Author   : Andrew Pearce                                                 */
/* Project  : New ROV IBC Based Data Concentrator                           */
/* Version  : 1.0                                                           */
/* Created  : 03/03/93                                                      */
/* Modified : 05/31/95                                                      */
/* Archived :                                                               */
/****************************************************************************/
/* Modification History:                                                    */
/* $Header: /usr/tiburon/.cvsroot/micro/ibc/filter/filter.c,v 1.1.1.1 1997/05/02 18:51:43 pean Exp $
 * $Log: filter.c,v $
 * Revision 1.1.1.1  1997/05/02 18:51:43  pean
 * Initial check in of IBC microcontroller software
 *
 * Revision 1.1  93/07/02  09:19:56  09:19:56  pean (Andrew Pearce)
 * Initial revision
 *
*/
/****************************************************************************/
/*
 * This filter is a simple IIR filter. The implemention is based on Michael
 * Matthews work on digital filtering. The following algorithm is used:
 *
 *      Y(k) = 1.25 * Y(k-1) - 0.5 * Y(k-2)
 *           + 0.0625 U(k) + 0.125 * U(k-1) + 0.0625 * U(k-2)
 *
 *      where U are the raw values and Y are the filtered values
 *
 ****************************************************************************/

#pragma model(196)
#pragma nosignedchar

#include "C:/C96/include/stddef.h"      /* C Standard Definitions           */
#include "const.h"                      /* Misc. constants - TRUE/FALSE etc */
#include "types.h"                      /* MBARI style guide declarations   */
#include "filter.h"                     /* General Purpose Filter Functions */

/****************************************************************************/
/* Function    : initIIRFilter                                              */
/* Purpose     : Initialize IIR filter data structure with zeros            */
/* Inputs      : Pointer to IIR filter data structure                       */
/* Outputs     : None                                                       */
/****************************************************************************/
    Void
initIIRFilter(filterParam *filterData, Int16 maxValue)
{
    Int16 i;                                    /* Array index counter      */

    filterData->sampleCount = 0;

    for (i = 0; i < IIR_RAW_SIZE; i++)
        filterData->raw[i] = 0;                 /* Clear raw data values    */

    for (i = 0; i < IIR_FILT_SIZE; i++)
        filterData->filtered[i] = 0;            /* Clear filtered data      */

    if (maxValue == 0)                          /* If max valus is zero then*/
        filterData->filterGain = 0;             /* use unity gain filter    */
    else
    {                                           /* calculate gain based on  */
                                                /* maximum value power of 2 */
        for (i = IIR_FILT_MAX_BITS; i >= 0; i--)
            if (maxValue >= (1 << i))
            {
                filterData->filterGain = IIR_FILT_MAX_BITS - i;
                return;
            } /* if */
    } /* else */
} /* initIIRFilter() */

/****************************************************************************/
/* Function    : IIRfilter                                                  */
/* Purpose     : General purpose IIR filter function                        */
/* Inputs      : New sample value and pointer to filter data structure      */
/* Outputs     : Returns OK or ERROR and filtered value                     */
/****************************************************************************/
    Int16
IIRfilter(Int16 newRaw, Int16 *newFiltered, filterParam *filter)
{
    Int32 filtValue;
    Int32 scaledRaw;

    scaledRaw = newRaw << filter->filterGain;   /* Apply gain to new value  */
                                                /* to improve dynamic range */

                                                /* 1.2500 * Y(k-1)          */
    filtValue = ((filter->filtered[0] >> 2) + filter->filtered[0] );

    filtValue -= (filter->filtered[1] >> 1);    /* 0.5000 * Y(k-1)          */
    filtValue += (scaledRaw >> 4);              /* 0.0625 * U(k)            */
    filtValue += (filter->raw[0] >> 3);         /* 0.1250 * U(k-1)          */
    filtValue +=  filter->raw[1] >> 4;          /* 0.0625 * U(k-2)          */

    filter->raw[1] = filter->raw[0];            /* Shuffle Raw Data Value   */
    filter->raw[0] = scaledRaw;

    filter->filtered[1] = filter->filtered[0];  /* Shuffle Filtered Data    */
    filter->filtered[0] = filtValue;

    filter->sampleCount = min(filter->sampleCount + 1, IIR_SAMPLE_MAX);
    if (filter->sampleCount != IIR_SAMPLE_MAX)
        return (ERROR);
                                                /* Apply gain to filt value */
    *newFiltered = (Int16) (filtValue >> filter->filterGain);
    return (OK);
} /* IIRfilter() */