/*
 *  low_pass_filter.c
 *
 *  Single pole low pass filter
 *
 *  Calculation of scaled time constant - scaled_T (unsigned short int)
 *   
 *  T = 1/(2*3.141592654*F)
 *  scaled_T = (unsigned short int) 0xffff/(T*s + 1)
 *
 *  Where:
 *    F - cutoff frequency in Hz (float)
 *    T - time constant in seconds (float)
 *    s - sampling rate in samples per second (int)
 *
 *  Important Notes:
 *      
 *  1.  The scaled time constant must be:
 *        0 < 0xffff/(T*s + 1) < 0xffff
 *      This is because the sampling rate limits the upper bound
 *      and 32 bits limits the lower bound.  This is sufficient 
 *      for any practical cutoff frequency given the sampling rate.
 *      
 *  2.  The filter must be initialize so that the output (out_count)
 *      is equal to the input (in_count) to avoid a step response
 *      output from whatever the uninitialized output was (probably 0).
 *
 *  3.  At any time you can reset the filter history by making the
 *      output (out_count) equal the input (in_count).
 *
 *  4.  At any time you can dynamically change the cutoff frequency
 *      (scaled_T)
 */

#include "low_pass_filter.h"

unsigned short int low_pass_filter(const unsigned short int in_count,
				   struct lpf *lpfp)
{
	unsigned int dy;

	dy = in_count - lpfp->out_count;	/* 32 bit delta*/
	dy *= lpfp->scaled_T;	/* scaled and filter delta */
	dy += lpfp->remainder;	/* add last remainder back in */
	lpfp->remainder = dy & 0x0000ffff;	/* get new remainder */
        lpfp->out_count += dy >> 16;		/*  calc new output */

	return lpfp->out_count;
}