Skip to content

Instantly share code, notes, and snippets.

@axpence

axpence/before_queues.c

Created February 11, 2014 16:59
Show Gist options
  • Save axpence/8939070 to your computer and use it in GitHub Desktop.
Save axpence/8939070 to your computer and use it in GitHub Desktop.
Download ZIP
BEFORE QUEUES STARTING QUEUES NOW.
Raw
before_queues.c
#include <stdio.h>
#include "platform.h"
#include "xspi.h"
#include "xtmrctr.h"
#include "xgpio.h"
#include "xscugic.h"
#include "queue.h"
#define SS_MASK 0x00000001
#define TIMER_DURATION 1000
#define PLAYER_1 20000
#define PLAYER_2 22000
#define PLAYER_3 24000
#define PLAYER_4 26000
#define PLAYER_5 28000
#define PLAYER_6 30000
#define PLAYER_7 32000
#define PLAYER_8 34000
#define PLAYER_9 36000
#define PLAYER_10 38000
#define ORDER 3 // This is for a third order butterworth filter
#define L 100 // Number of Samples/Second
#define THRESHOLD 10000 // This is the value the average must be above to count as being shot
#define FILTER_SIZE 7 // Order of bandpass filter (6th order lowpass becomes a 12th order bandpass)
#define FILTER_NUMBER 10 //The amount of filters to use ==> frequency values: 20, 22, 24, 26, 28, 30, 32, 34, 36, 38
//-----------------------------------------------
//---------- Function Prototypes ----------------
//-----------------------------------------------
void beginTransferADC();
unsigned char isTransferComplete();
u16 readADC();
void writeDAC(u16 value);
//----------------------------------------------- END FUNCTION PROTOTYPES
//-----------------------------------------------
//----------- Global Variables ------------------
//-----------------------------------------------
XSpi ADC_SPI;
XSpi DAC_SPI;
XTmrCtr Timer;
XScuGic InterruptController;
XScuGic_Config *GicConfig;
unsigned char transferFlag = 0; //used to say if the transfer is complete
volatile unsigned int timerFlag = 0;
const int player_freq_data[11][50] = //Rows represent the frequencies to display for each player
{
{0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240, 0, 1240},
{620, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30, 619, 1209, 984, 255, 30},
{620, 1229, 848, 96, 195, 984, 1180, 465, 1, 542, 1209, 918, 142, 142, 918, 1209, 542, 1, 465, 1180, 984, 195, 96, 848, 1229, 620, 10, 391, 1143, 1044, 255, 59, 774, 1238, 697, 30, 321, 1097, 1097, 321, 30, 697, 1238, 774, 59, 255, 1044, 1143, 391, 10},
{620, 1238, 697, 10, 465, 1209, 848, 59, 321, 1143, 984, 142, 195, 1044, 1097, 255, 96, 918, 1180, 391, 30, 774, 1229, 542, 1, 619, 1238, 697, 10, 465, 1209, 848, 59, 321, 1143, 984, 142, 195, 1044, 1097, 255, 96, 918, 1180, 391, 30, 774, 1229, 542, 1},
{620, 1238, 542, 10, 774, 1209, 391, 59, 918, 1143, 255, 142, 1044, 1044, 142, 255, 1143, 918, 59, 391, 1209, 774, 10, 542, 1238, 619, 1, 697, 1229, 465, 30, 848, 1180, 321, 96, 984, 1097, 195, 195, 1097, 984, 96, 321, 1180, 848, 30, 465, 1229, 697, 1},
{620, 1229, 391, 96, 1044, 984, 59, 465, 1238, 542, 30, 918, 1097, 142, 321, 1209, 697, 1, 774, 1180, 255, 195, 1143, 848, 10, 619, 1229, 391, 96, 1044, 984, 59, 465, 1238, 542, 30, 918, 1097, 142, 321, 1209, 697, 1, 774, 1180, 255, 195, 1143, 848, 10},
{620, 1209, 255, 255, 1209, 620, 30, 984, 984, 30, 619, 1209, 255, 255, 1209, 620, 30, 984, 984, 30, 619, 1209, 255, 255, 1209, 620, 30, 984, 984, 30, 619, 1209, 255, 255, 1209, 619, 30, 984, 984, 30, 619, 1209, 255, 255, 1209, 620, 30, 984, 984, 30},
{620, 1180, 142, 465, 1229, 255, 321, 1238, 391, 195, 1209, 542, 96, 1143, 697, 30, 1044, 848, 1, 918, 984, 10, 774, 1097, 59, 619, 1180, 142, 465, 1229, 255, 321, 1238, 391, 195, 1209, 542, 96, 1143, 697, 30, 1044, 848, 1, 918, 984, 10, 774, 1097, 59},
{620, 1143, 59, 697, 1097, 30, 774, 1044, 10, 848, 984, 1, 918, 918, 1, 984, 848, 10, 1044, 774, 30, 1097, 697, 59, 1143, 620, 96, 1180, 542, 142, 1209, 465, 195, 1229, 391, 255, 1238, 321, 321, 1238, 255, 391, 1229, 195, 465, 1209, 142, 542, 1180, 96},
{620, 1097, 10, 918, 848, 30, 1143, 542, 195, 1238, 255, 465, 1180, 59, 774, 984, 1, 1044, 697, 96, 1209, 391, 321, 1229, 142, 619, 1097, 10, 918, 848, 30, 1143, 542, 195, 1238, 255, 465, 1180, 59, 774, 984, 1, 1044, 697, 96, 1209, 391, 321, 1229, 142},
{620, 1044, 1, 1097, 542, 255, 1229, 96, 774, 918, 30, 1180, 391, 391, 1180, 30, 918, 774, 96, 1229, 255, 542, 1097, 1, 1044, 620, 195, 1238, 142, 697, 984, 10, 1143, 465, 321, 1209, 59, 848, 848, 59, 1209, 321, 465, 1143, 10, 984, 697, 142, 1238, 195}
};
u16 adcData;
// x is the output from the ADC
// float x[L];
double x[FILTER_SIZE];
// y is the output from the filter
// float y[FILTER_NUMBER][L];
double y[FILTER_NUMBER][FILTER_SIZE];
// y2 is the squred output from the filter
double y2[FILTER_NUMBER][L];
// mavg is the moving average for each of the ten filters
// float mavg_ARRAY[FILTER_NUMBER];
double mavg[FILTER_NUMBER];
// These variables store who the actual shooter
// float who_shot_ARRAY[FILTER_NUMBER]; //Holds 0 or 1 to show the values mavg above threshold
int frequency[FILTER_NUMBER]; //This just holds the frequencies 20, 22, 24, 26, 28, 30, 32, 34, 36, 38
int shooterFrequency; //This will be set to the frequency of the person shooting
int filter_flag;
// a and b are the coefficients calculated in MATLAB for the ten filters
// float a[10][FILTER_SIZE] = {{1.0000, -3.6332, 11.2578, -21.8757, 36.9524, -46.7559, 52.0292, -44.9020, 34.0802, -19.3754, 9.5756, -2.9677, 0.7844},
// {1.0000, -2.2031, 7.7798, -11.5619, 21.8538, -23.1906, 29.3984, -22.2711, 20.1553, -10.2405, 6.6174, -1.7996, 0.7844},
// {1.0000, -0.7382, 5.9844, -3.5798, 14.6911, -6.9093, 18.9502, -6.6354, 13.5493, -3.1706, 5.0902, -0.6030, 0.7844},
// { 1.0000, 0.7382, 5.9844, 3.5798, 14.6911, 6.9093, 18.9502, 6.6354, 13.5493, 3.1706, 5.0902, 0.6030, 0.7844},
// {1.0000, 2.2031, 7.7798, 11.5619, 21.8538, 23.1906, 29.3984, 22.2711, 20.1553, 10.2405, 6.6174, 1.7996, 0.7844},
// {1.0000, 3.6332, 11.2578, 21.8757, 36.9524, 46.7559, 52.0292, 44.9020, 34.0802, 19.3754, 9.5756, 2.9677, 0.7844},
// {1.0000, 5.0060, 16.1998, 35.6401, 61.1816, 82.0133, 89.7373, 78.7615, 56.4261, 31.5664, 13.7792, 4.0891, 0.7844},
// {1.0000, 6.2998, 22.2953, 53.3867, 95.5531, 132.3999, 145.5114, 127.1502, 88.1258, 47.2845, 18.9639, 5.1460, 0.7844},
// {1.0000, 7.4943, 29.1614, 74.9460, 140.2044, 199.5835, 221.0631, 191.6698, 129.3062, 66.3794, 24.8039, 6.1217, 0.7844},
// {1.0000, 8.5707, 36.3666, 99.4346, 193.8222, 282.6110, 315.4517, 271.4049, 178.7560, 88.0689, 30.9324, 7.0009, 0.7844}};
//
//
// float b[10][FILTER_SIZE] = {{0.00000000003, 0.0000, -0.00000000167, -0.00000000004, 0.00000001164, 0.00000000019, -0.00000002091, -0.00000000018, 0.00000001118, 0.00000000004, -0.00000000154, -0.0000, 0.00000000003},
// {0.00000000003, 0.0000, -0.00000000177, -0.00000000003, 0.00000001277, 0.00000000012, -0.00000002320, -0.00000000012, 0.00000001226, 0.00000000003, -0.00000000163, -0.0000, 0.00000000003},
// {0.00000000003, 0.0000, -0.00000000182, -0.00000000001, 0.00000001337, 0.00000000004, -0.00000002444, -0.00000000004, 0.00000001284, 0.00000000001, -0.00000000168, -0.0000, 0.00000000003},
// {0.00000000003, -0.0000, -0.00000000182, 0.00000000001, 0.00000001337, -0.00000000004, -0.00000002444, 0.00000000004, 0.00000001284, -0.00000000001, -0.00000000168, 0.0000, 0.00000000003},
// {0.00000000003, -0.0000, -0.00000000177, 0.00000000003, 0.00000001277, -0.00000000012, -0.00000002320, 0.00000000012, 0.00000001226, -0.00000000003, -0.00000000163, 0.0000, 0.00000000003},
// {0.00000000003, -0.0000, -0.00000000167, 0.00000000004, 0.00000001164, -0.00000000019, -0.00000002091, 0.00000000018, 0.00000001118, -0.00000000004, -0.00000000154, 0.0000, 0.00000000003},
// {0.00000000003, -0.0000, -0.00000000153, 0.00000000005, 0.00000001011, -0.00000000023, -0.00000001787, 0.00000000022, 0.00000000971, -0.00000000005, -0.00000000141, 0.0000, 0.00000000003},
// {0.00000000003, -0.0000, -0.00000000135, 0.00000000006, 0.0000000837, -0.00000000024, -0.00000001447, 0.00000000023, 0.00000000803, -0.00000000006, -0.00000000125, 0.0000, 0.00000000003},
// {0.00000000003, -0.0000, -0.00000000115, 0.00000000006, 0.00000000657, -0.00000000023, -0.00000001108, 0.00000000022, 0.00000000631, -0.00000000006, -0.00000000106, 0.0000, 0.00000000003},
// { 0.000000000028, -0.000000000003, -0.000000000946, 0.000000000058, 0.000000004892, -0.000000000200, -0.000000008000, 0.000000000195, 0.000000004698, -0.000000000054, -0.000000000872, 0.000000000002, 0.000000000025}};
double a[10][FILTER_SIZE] = {{ 1.0000, -1.8153, 3.9730, -3.7018, 3.8101, -1.6694, 0.8819},
{ 1.0000, -1.1007, 3.2783, -2.1597, 3.1439, -1.0123, 0.8819},
{ 1.0000, -0.3689, 2.9197, -0.7090, 2.8000, -0.3392, 0.8819},
{ 1.0000, 0.3689, 2.9197, 0.7090, 2.8000, 0.3392, 0.8819},
{ 1.0000, 1.1007, 3.2783, 2.1597, 3.1439, 1.0123, 0.8819},
{ 1.0000, 1.8153, 3.9730, 3.7018, 3.8101, 1.6694, 0.8819},
{1.0000, 2.5012, 4.9601, 5.3749, 4.7567, 2.3002, 0.8819},
{ 1.0000, 3.1476, 6.1776, 7.1901, 5.9243, 2.8947, 0.8819},
{ 1.0000, 3.7444, 7.5490, 9.1243, 7.2394, 3.4435, 0.8819},
{ 1.0000, 4.2822, 8.9882, 11.1197, 8.6195, 3.9381, 0.8819}};
double b[10][FILTER_SIZE] = {{ 1.0e-03 * 0.0075, 1.0e-03 * 0.0001, 1.0e-03 * -0.1019, 1.0e-03 * -0.0008, 1.0e-03 * 0.0998, 1.0e-03 * 0.0001, 1.0e-03 * -0.0071},
{ 1.0e-03 * 0.0075, 1.0e-03 * 0.0001, 1.0e-03 * -0.1073, 1.0e-03 * -0.0005, 1.0e-03 * 0.1050, 1.0e-03 * 0.0001, 1.0e-03 * -0.0071},
{ 1.0e-03 * 0.0075, 1.0e-03 * 0.0000, 1.0e-03 * -0.1100, 1.0e-03 * -0.0002, 1.0e-03 * 0.1077, 1.0e-03 * 0.0000, 1.0e-03 * -0.0071},
{ 1.0e-03 * 0.0075, 1.0e-03 * -0.0000, 1.0e-03 * -0.1100, 1.0e-03 * 0.0002, 1.0e-03 * 0.1077, 1.0e-03 * -0.0000, 1.0e-03 * -0.0071},
{ 1.0e-03 * 0.0075, 1.0e-03 * -0.0001, 1.0e-03 * -0.1073, 1.0e-03 * 0.0005, 1.0e-03 * 0.1050, 1.0e-03 * -0.0001, 1.0e-03 * -0.0071},
{ 1.0e-03 * 0.0075, 1.0e-03 * -0.0001, 1.0e-03 * -0.1019, 1.0e-03 * 0.0008, 1.0e-03 * 0.0998, 1.0e-03 * -0.0001, 1.0e-03 * -0.0071},
{ 1.0e-04 * 0.0751, 1.0e-04 * -0.0020, 1.0e-04 * -0.9435, 1.0e-04 * 0.0103, 1.0e-04 * 0.9240, 1.0e-04 * -0.0019, 1.0e-04 * -0.0706},
{ 1.0e-04 * 0.0751, 1.0e-04 * -0.0025, 1.0e-04 * -0.8501, 1.0e-04 * 0.0119, 1.0e-04 * 0.8324, 1.0e-04 * -0.0024, 1.0e-04 * -0.0706},
{ 1.0e-04 * 0.0751, 1.0e-04 * -0.0030, 1.0e-04 * -0.7448, 1.0e-04 * 0.0128, 1.0e-04 * 0.7294, 1.0e-04 * -0.0028, 1.0e-04 * -0.0706},
{ 1.0e-04 * 0.0751, 1.0e-04 * -0.0034, 1.0e-04 * -0.6343, 1.0e-04 * 0.0129, 1.0e-04 * 0.6212, 1.0e-04 * -0.0032, 1.0e-04 * -0.0706}};
//
// Queue Declarations
//
queue_t x_in;
queue_t * y_out[FILTER_NUMBER];//each queue should be FILTER_SIZE
queue_t * y_squared[FILTER_NUMBER];
queue_t moving_avg;
//----------------------------------------------- END VARIABLES
//-----------------------------------------------
//----------- TIMER/ISR Functions ---------------
//-----------------------------------------------
void TimerISR()
{
timerFlag = 1;
XTmrCtr_SetControlStatusReg(XPAR_AXI_TIMER_0_BASEADDR, 0, XTmrCtr_GetControlStatusReg(XPAR_AXI_TIMER_0_BASEADDR, 0));
}
int SetUpInterruptSystem(XScuGic *XScuGicInstancePtr)
{
/*
* Connect the interrupt controller interrupt handler to the hardware
* interrupt handling logic in the ARM processor.
*/
Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
(Xil_ExceptionHandler) XScuGic_InterruptHandler,
XScuGicInstancePtr);
/*
* Enable interrupts in the ARM
*/
Xil_ExceptionEnable();
return XST_SUCCESS;
}
int InitInterrupts()
{
int Status;
/*
* Initialize the interrupt controller driver so that it is ready to
* use.
*/
GicConfig = XScuGic_LookupConfig(XPAR_PS7_SCUGIC_0_DEVICE_ID);
if (NULL == GicConfig) {
return XST_FAILURE;
}
Status = XScuGic_CfgInitialize(&InterruptController, GicConfig,
GicConfig->CpuBaseAddress);
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
/*
* Setup the Interrupt System
*/
Status = SetUpInterruptSystem(&InterruptController);
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
/*
* Connect a device driver handler that will be called when an
* interrupt for the device occurs, the device driver handler performs
* the specific interrupt processing for the device
*/
Status = XScuGic_Connect(&InterruptController, XPAR_FABRIC_TMRCTR_0_VEC_ID,
(Xil_ExceptionHandler)TimerISR, &Timer);
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
// Enable the interrupt
XScuGic_Enable(&InterruptController, XPAR_FABRIC_TMRCTR_0_VEC_ID);
return XST_SUCCESS;
}
// -----------------------------------
// Begins an ADC transfer
// -----------------------------------
void beginTransferADC()
{
XSpi_SetSlaveSelectReg(&ADC_SPI, ~SS_MASK);
XSpi_WriteReg(ADC_SPI.BaseAddr, XSP_DTR_OFFSET,1);
}
// -----------------------------------
// Returns a non-zero value if an ADC transfer is complete.
// beginTransferADC() must be called before this.
// -----------------------------------
unsigned char isTransferComplete()
{
return (XSpi_GetStatusReg(&ADC_SPI) & XSP_SR_TX_EMPTY_MASK);
}
// -----------------------------------
// Reads the data received from by the ADC after a transfer has completed.
// beginTransferADC() and isTransferComplete() should be called before this.
// -----------------------------------
u16 readADC()
{
XSpi_SetSlaveSelectReg(&ADC_SPI, SS_MASK);
return XSpi_ReadReg(ADC_SPI.BaseAddr, XSP_DRR_OFFSET);
}
// -----------------------------------
// Writes a value to the DAC which controls LED1
// -----------------------------------
void writeDAC(u16 value)
{
XSpi_WriteReg(DAC_SPI.BaseAddr, XSP_DTR_OFFSET, value);
}
void printArray(float array[], int size) {
int p;
//xil_printf("ARRAY: ");
for(p = 0; p < size; p++) {
//xil_printf("%f , \n\r", array[p]);
}
}
int main()
{
init_platform();
int xStatus;
// -----------------------------------
// Initialize ADC SPI module
// -----------------------------------
xStatus = XSpi_Initialize(&ADC_SPI, XPAR_ADC_SPI_DEVICE_ID);
if(XST_SUCCESS != xStatus)
print("SPI INIT FAILED \n\r");
XSpi_SetControlReg(&ADC_SPI, XSP_CR_MANUAL_SS_MASK | XSP_CR_MASTER_MODE_MASK);
XSpi_Enable(&ADC_SPI);
// -----------------------------------
// Initialize DAC SPI module
// -----------------------------------
xStatus = XSpi_Initialize(&DAC_SPI, XPAR_DAC_SPI_DEVICE_ID);
if(XST_SUCCESS != xStatus)
print("SPI INIT FAILED \n\r");
XSpi_SetControlReg(&DAC_SPI, XSP_CR_CLK_PHASE_MASK | XSP_CR_LOOPBACK_MASK | XSP_CR_MASTER_MODE_MASK);
XSpi_SetSlaveSelectReg(&DAC_SPI, ~SS_MASK);
XSpi_Enable(&DAC_SPI);
//-----------------------------------------------
//------------- Timer Inits ---------------------
//-----------------------------------------------
xStatus = XTmrCtr_Initialize(&Timer,XPAR_AXI_TIMER_0_DEVICE_ID);
if(XST_SUCCESS != xStatus)
print("TIMER INIT FAILED \n\r");
XTmrCtr_SetResetValue(&Timer, XPAR_AXI_TIMER_0_DEVICE_ID, TIMER_DURATION);
XTmrCtr_SetOptions(&Timer, XPAR_AXI_TIMER_0_DEVICE_ID,
(XTC_INT_MODE_OPTION | XTC_AUTO_RELOAD_OPTION | XTC_DOWN_COUNT_OPTION));
xStatus = InitInterrupts();
if(XST_SUCCESS != xStatus)
print("INTERRUPT INIT FAILED \n\r");
XTmrCtr_Start(&Timer, XPAR_AXI_TIMER_0_DEVICE_ID);
// transfer flag - 0 indicates we need to start a transfer, 1 indicates we are waiting for one to finish
//unsigned char transferFlag = 0;
// time_t time_raw_format;
// int SinDataArray[8] = {0,1241,0,1241,0,1241,0,1241};
int dj = 0;
int i = 0;
int j = 0;
// Initialize the x array
for(i = 0; i < FILTER_SIZE; i++) {
x[i] = 0;
}
// Initialize the y array
for(i = 0; i < FILTER_NUMBER; i++) {
for(j = 0; j < FILTER_SIZE; j++) {
y[i][j] = 0;
}
}
// Initialize the y2 array
for(i = 0; i < FILTER_NUMBER; i++) {
for(j = 0; j < L; j++) {
y2[i][j] = 0;
}
}
// Initialize the mavg array
for(i = 0; i < FILTER_NUMBER; i++) {
mavg[i] = 0;
}
// Initialize the frequency array
for(i = 0, j = 20; i < FILTER_NUMBER; i++, j+=2) {
frequency[i] = j;
}
// Initialize all non array variables
shooterFrequency = 0;
filter_flag = 0;
//
// Queue Inits
//
queueInit(&x_in,FILTER_SIZE);
for(i=0; i<FILTER_NUMBER; i++){
queueInit(y_out[i], FILTER_SIZE);//note: &y is equivalent to y[i] in array notation!
}
for(i=0; i<FILTER_NUMBER;i++){
queueInit(y_squared[i], L);
}
queueInit(&moving_avg, FILTER_NUMBER);
while(1){
while(!timerFlag);
timerFlag = 0;
writeDAC(player_freq_data[10][dj]);
dj++;
if(dj >= 50){
dj=0;
//xil_printf("balls\n\r");
}
// adcData = readADC();
//xil_printf("ADC value %d \n\r", adcData);
frequency[0] = 20;
if(transferFlag)
{
if(isTransferComplete())
{
adcData = readADC();
//writeDAC(adcData);
//xil_printf("adcData %d \n\r ", adcData);
//adcDataArray[i] = adcData;
transferFlag = 0;
//filter_flag = 1;
}
}
else
{
beginTransferADC();
transferFlag = 1;
}
//----------------------------------------------------------------
// The functions to perform the filter
//----------------------------------------------------------------
// if(filter_flag == 1) {
//----------------------------------------------
//-------------- POPULATE X ARRAY --------------
//----------------------------------------------
// for(i = FILTER_SIZE - 1; i > 0; i--)
// {
// x[i] = x[i-1];
// }
// x[0] = adcData;
//put adc samples in x
if(queueFull(&x_in)){ queuePop(&x_in); }
queuePush(&x_in, adcData);
//---------------------------------------------- END POPULATE X ARRAY
//----------------------------------------------
//-------------- FILTER ALGORITHM --------------
//----------------------------------------------
//temp_y is the variable that will be put into the y array.
//temp_y2 is the variable that will go into the y2 array
// they aren't put directly in the arrays yet to allow the array to be shifted before putting it in
double temp_y2, temp_y, greatest, current;
//For loop to go through each frequency, 0 == 20 kHz, 1 == 22 kHz, ...
for(i = 0, temp_y2 = 0, temp_y = 0, greatest = 0; i < FILTER_NUMBER; i++) {
temp_y = b[i][0]*x[0] + b[i][1]*x[1] + b[i][2]*x[2] + b[i][3]*x[3] + b[i][4 ]*x[4] + b[i][5]*x[5] + b[i][6]*x[6];
temp_y -= a[i][1]*y[i][1] + a[i][2]*y[i][2] + a[i][3]*y[i][3] + a[i][4]*y[i][4] + a[i][5]*y[i][5] + a[i][6]*y[i][6];
temp_y2 = temp_y * temp_y; // Square the y value so that it is positive
mavg[i] = mavg[i] + temp_y2 - y2[i][L-1]; // Take the old moving average, add the new output, subtract the last past output
//----------------------------------------------
//--------------- SHIFT Y ARRAY ----------------
//----------------------------------------------
for(j = FILTER_SIZE - 1; j > 0; j--) {
y[i][j] = y[i][j-1];
}
y[i][0] = temp_y;
//----------------------------------------------- END SHIFT Y ARRAY
//----------------------------------------------
//--------- FOR LOOP - REPLACE WITH QUEUE-------
//----------------------------------------------
for(j = L - 1; j > 0; j--) { // L is the amount of samples to keep
y2[i][j] = y2[i][j-1]; // Shift everything in the y2 array over for the given frequency (i),
// this will drop off the last output which is what we want
}
y2[i][0] = temp_y2; // Add the value to the y2 array at spot 0, same frequency indexing as y array.
//---------------------------------------------- END QUEUE REPLACEMENT
//----------------------------------------------
//--------------- COMPARE OUTPUT ---------------
//----------------------------------------------
current = mavg[i];
if(current > greatest) { // Checks the current output against the greatest output
greatest = current;
shooterFrequency = frequency[i];
}
//----------------------------------------------- END COMPARE OUTPUT
}
//---------------------------------------------- END FILTER ALGORITHM
//printf("Shooter Frequency %d \n\r", shooterFrequency);
filter_flag = 0;
// }
//-------------------------------------------------------------------------- END FILTER
// writeDAC(player_freq_data[0][i]);
// i++;
// if(i == 50)
// i=0;
}
// xil_printf("\r\ndata array: \r\n");
// for(i=0;i<500;i++){
// xil_printf("%d ",adcDataArray[i]);
// }
return 0;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment