asigpro.c

F2812的TI框架编的小程序

/* ==============================================================================

File Name:  ASigPro.c

Description:    Primary System file for demonstrating the signal processing
                of tread abrasion of wheel
Originator:     HIT

Target dependency:  HIT203_F2812EVM

Description:
============

This software demonstrates the FIR Filter Usage

   The frame work, for demonstrating the FILTER is given below
   
   
           |------------|
           |            |            |------------|        |------------|
           |            |            |            |        |            | 
   input[N]|            | Output[N]  |  EXTREMUM  |        |  ABRASION  |
-----|---->|    FIR     |----------->|  DETECTION |------->|  POSITION  |
           |   FILTER   |            |            |        |  FLANGE    |
           |            |            |------------|        |            |
           |            |                                  |------------|    
           |------------|
                                                              
*/
//==============================================================================

#include "F2812reg.h"
#include "fir.h"
#include "ASigPro.h"

//===========================================================================
#define N 1052
#define SCALE_2 2
#define SCALE_3 3
#define SCALE_4 4
#define SCALE_5 5
#define SCALE_6 6
#define SCALE_7 7
#define SCALE_8 8
#define SCALE_9 9
#define SCALE_10 10
#define SCALE_20 20 // 2004/9/26

#define THRESHOLD1 5
#define THRESHOLD2 10
#define THRESHOLD3 50
#define THRESHOLD4 10
#define THRESHOLD5 35
#define THRESHOLD6 120 // 2004/9/26
//===========================================================================

/* Filter Symbolic Constants                        */
#define FIR_ORDER   50                                        
                                        
/* Create an Instance of FIRFILT_GEN module and place the object in "firfilt" section       */ 
#pragma DATA_SECTION(fir, "firfilt");
FIR16  fir= FIR16_DEFAULTS;
                                            
/* Define the Delay buffer for the 50th order filterfilter and place it in "firldb" section */  
#pragma DATA_SECTION(dbuffer,"firldb");
long dbuffer[(FIR_ORDER+2)/2];                                        
         
/* Define Constant Co-efficient Array  and place the
.constant section in ROM memory				*/ 

long const coeff3[(FIR_ORDER+2)/2]= FIR16_F3;

/* Finter Input and Output Variables
   and Define as Static 2004/10/6           */   
//static int ab_flag;
static int xn,yn;
static int y[N], d1y[N], d2y[N];
// static int ab_p[3];
int output3[N/2],output1[N/2],output2[N/2];
int counter1,counter2;
int divided_data[10][6];
//***************************************
//divided_data	1st		2ed		3rd		....10th
//0. preserved
//1. p_min1
//2. p_min2 
//3. p_d1max
//4. p_max
//5. max-min1
//===========================================================================
// Fir16Init:
// This function initializes the 16-bit fir filter
// Enable this function for using FirInit in the application 
//===========================================================================
void Fir16Init(void)
{
    int i;
/* FIR Generic Filter Initialisation    */ 
    fir.order=FIR_ORDER; 
    fir.dbuffer_ptr=dbuffer;
    fir.coeff_ptr=(long *)coeff3; 
    fir.init(&fir);
//    ab_flag = 0;
    xn = 0;
    yn = 0;
//    for (i=0; i<3; i++)
//      ab_p[i] = 0; 
    for (i=0; i<N; i++)
    {
      y[i] = 0;
      d1y[i] = 0;
    }
    for (i=0; i<N; i++)
      d2y[i] = 0;
    a_sp.ab_value = 0;
    a_sp.sp_status = 0;
    a_sp.flange = 0;

    for (i=0; i<N/2; i++)
    {
       d2y[i] = 0;
       output3[i] = 0;
       output1[i] = 0;
       output2[i] = 0;
    }
    for(i=0;i<5;i++)
    	for(counter1=0;counter1<10;counter1++)
    		divided_data[i][counter1]=0;
    counter1=0;
    counter2=0;        
}

//===========================================================================
// ASigPro:
// This function deals with the input data and detects the tread abrasion signal.
// Enable this function for using FirInit in the application 
//===========================================================================
void ASigPro(uint16 sam_num, int *input)
{
    
    int16 i, n, p_dmax,p_d1min,p_max,p_dmin,p_min2,p_d1max,p_min1, extremum[4];//p_dmin1,p_min, 
    int temp_num,  dmax, d1max,min1, min2,max, data_length,d1min,temp_num1,alpha3;// temp_num2,dmin,
    int *dinput;
    
    dinput = input;
    n = sam_num + FIR_ORDER/2 + 1;
    //----------------------------------------------------------------
    for(i=0;i<5;i++)
	for(counter1=0;counter1<10;counter1++)
		divided_data[i][counter1]=0;
    counter1=0;
    counter2=0; 
    for (i=0; i<n; i++)
    {
      if (i < sam_num)
      {
        xn = *(dinput+i);
        fir.input=xn;           
        fir.calc(&fir);
        yn=fir.output;
        y[i] = yn;
      }
      else
      {
        xn = 0;
        fir.input=xn;           
        fir.calc(&fir);
        yn=fir.output;
        y[i] = yn;        
      }
    }
    //----------------------------------------------------------------
    temp_num = n;
    a_sp.flange = y[temp_num];
    //----------------------------------------------------------------
    for (i=1; i<n; i++)
      d1y[i] = y[i] - y[i-1];
    for (i=1; i<n; i++)
      d2y[i] = d1y[i] - d1y[i-1];
    //----------------------------------------------------------------
      // max(), min()
     p_dmax = 0;
    p_dmin = 0;
    dmax = d1y[0];
   // dmin = d1y[0];
    //temp_num = THRESHOLD6; // 2004/9/26
    //temp_num = 620;
    for (i=0; i<n/3; i++)
    {
      if (dmax < d1y[i])
      {
        dmax = d1y[i];
        p_dmax = i;
      }
  //    if (dmin > d1y[i])
  //    {
  //      dmin = d1y[i];
  //      p_dmin = i;
  //    }
    }
    //FIND THE MAX OF D1Y AGAIN TO CONFIRM ITS VALIDITY!!!
    d1max = 0;
    p_d1max = 0;
    if(p_dmax>22)
	{ 
	    for(i=0; i<p_dmax-20;i++)
	    {
	    	if (d1max < d1y[i])
	      {
	        d1max = d1y[i];
	        p_d1max = i;
	      }
	      
	    }
	}
    if ((dmax-d1max)<10)
    {
    	dmax = d1max;
    	p_dmax = p_d1max;
    }
    
    //05.5.22
    //FIND THE MIN OF D1Y AGAIN TO OCNFIRM ITS VALIDITY!!!
     d1min = 0;
   	 p_d1min = 0;
  	for(i=(2*n/3); i<n;i++)
	{
		if(d1min > d1y[i])
		{
		//	dmin = d1y[i];
			p_dmin = i;
		}
	}

    
    //----------------------------------------------------------------
    temp_num = p_dmin - p_dmax;
    if (temp_num <= 0)
    {
      a_sp.ab_value = 0;
      a_sp.sp_status = 1; // Note 1: The indices are error!
      return;
    }
    //----------------------------------------------------------------
    extremum[3] = p_dmax + THRESHOLD2+THRESHOLD5;//05.5.21
    //extremum[3] = 100;
    //----------------------------------------------------------------
    n = 1;
    for (i=0; i<temp_num; i++)
    {
      if ((d2y[p_dmin-i-2] >= 0)&&(n == 1))
      {
        extremum[0] = p_dmin - i;
        n++;
        continue;
      }
      if ((d2y[p_dmin-i] <= 0)&&(n == 2))
      {
        extremum[1] = p_dmin - i + 1;
        break;
      }
    }
    p_dmin = extremum[1];
    //----------------------------------------------------------------
   /* temp_num = p_dmin - extremum[3] + 1;
    // max()
    p_dmax = 0;     // 2004.09.07
    dmax = d1y[extremum[3]];
    for (i=0; i<temp_num; i++)
    {
      if (dmax < d1y[i+extremum[3]])
      {
        dmax = d1y[i+extremum[3]];
        p_dmax = i;
      }
    }*/
    //++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
    p_d1max = 0;
    d1max = d1y[0];
	p_d1min=p_dmin;
    //d1min=dmin;
    data_length = p_d1min-extremum[3]-20;//05.5.21
    temp_num1 = data_length;		//05.5.28
    for(i=0;i<data_length;i++)
    {

    	output1[i] = y[i+extremum[3]];
    	output2[i] = d1y[i+extremum[3]];
    	output3[i] = d2y[i+extremum[3]];
        if (d1max < output2[i])
    	{
     		d1max = output2[i];
     		p_d1max = i;
   		}
   	}
	if(!counter2)//05.5.28
    {
	    for(i=0;i<data_length;i++)
	    {