/*
* notch.c
*
* Created on: 2013-12-31
* Author: Beny.HEU
* Function: adaptive notch filter
* Useage:fixed out_data=notch(input)
*/
#include "msp430.h"
#include "notch.h"
#include "fixed.h"
#define GATE 0x0002 //Amp gate
#define GATE_TIME 500 //Time scale gate
// u = 0.5
#define u 0x3b92 //step=0.4654=2*pi*BW/(fs*tao)
#define point 9 //point = f/fs
const fixed wsin[point]={0x0000,0x5247,0x7e0e,0x6eda,0x2bc7,0xd439,0x9126,0x81f2,0xadb9};
const fixed wcos[point]={0x7fff,0x620e,0x163a,0xc000,0x87b8,0x87b8,0xc000,0x163a,0x620e};//Q15 format fixed-point,generated by MATLAB.f/fs=9
fixed notch(fixed t)
{
static fixed ws = 0;
static fixed wc = 0;
static fixed result,am;
static unsigned char i=0;
fixed err;
// calculate result = (int)(ws*wsin[i]+wc*wcos[i]);
MPY32CTL0 |= MPYFRAC; // Set fractional mode
MPYS = ws;
OP2 = wsin[i];
result = RESHI;
MPYS = wc;
OP2 = wcos[i];
result = result + RESHI;
// end of calculate
// calculate err = u*(ADC12MEM0 - result);
err = t - result; //ADC12MEM0 need to be transfered to Q15 format
MPYS = err;
OP2 = u;
err = RESHI;
//end of calculate
//calculate ws = ws + err*wsin[i];
MPYS = err;
OP2 = wsin[i];
ws = ws + RESHI;
//end
//calculate wc = wc + err*wcos[i];
MPYS = err;
OP2 = wcos[i];
wc = wc + RESHI;
//end
i++;
if (point == i)
i = 0;
//calculate am = ws*ws+wc*wc;
MPYS = ws;
OP2 = ws;
am = RESHI;
MPYS = wc;
OP2 = wc;
am = am + RESHI;
return am;
}