calculate.c

一个带CANBUS的倒车雷达源码！希望大家能用的上！

#include "config.h"

void Calculate( void )
{
	uchar channel;
	uchar MiniDistance,MiniChannel;	
	uint temp;
		
	for( channel = 0; channel < 6; channel ++ )
	{
		if( ( TimeTemp[channel][0] + 8 < TimeTemp[channel][1] ) || ( TimeTemp[channel][0] - 8 > TimeTemp[channel][1] ) )
		  LastDist[channel] = 0xFF;
		else
		{
			temp = TimeTemp[channel][0];
			temp = temp + TimeTemp[channel][1] ;
		  LastDist[channel] = temp /2;
		}
	}
	
  for( channel = 0; channel < 6; channel ++ )
	  {
     if( channel > 1 )
      {
	      if( LastDist[channel] < 0x0F )
	      {
		      if( ( channel == 0 ) || ( channel == 1 ) )
		      {
		        SensorAbnormal.Bit.BackMiddle = 1;
		      } 
		      else if( channel == 2 )
		      {
		        SensorAbnormal.Bit.BackRight = 1;
		      }
		      else if( channel == 3 )
		      {
		        SensorAbnormal.Bit.BackLeft = 1;
		      }
		      else if( channel == 4 )
		      {
		        SensorAbnormal.Bit.FrontRight = 1;
		      }
		      else if( channel == 5 ) 
		      {
		        SensorAbnormal.Bit.FrontLeft = 1; 
		      }
		    }
  	    else if( LastDist[channel] <= 0x48 )
  	    {
  		    if( LastDist[channel] <= 0x12 ) LastDist[channel] = 0x01;
  		    else if( LastDist[channel] <= 0x24 ) LastDist[channel] = 0x02;
  		    else if( LastDist[channel] <= 0x36 ) LastDist[channel] = 0x03;
  		    else LastDist[channel] = 0x04;
  		    //LastDist[channel] = 0x02;
  	    }
  	    else if( LastDist[channel] <= 0x6D )
  	    {
  		    if( LastDist[channel] <= 0x5A ) LastDist[channel] = 0x05;
  		    else  LastDist[channel] = 0x06;
  		    //LastDist[channel] = 0x05;
  	    }
  	    else LastDist[channel] = 0x0F;
  	  }
  	 else
 	   {
	 	    if( LastDist[channel] < 0x0F ) 
	 	    {
		 	    if( channel == 4 )
		      {
			        SensorAbnormal.Bit.FrontRight = 1;
			    }
		      else if( channel == 5 ) 
		      {
		        SensorAbnormal.Bit.FrontLeft = 1; 
	        }
	      }
  		  else if( LastDist[channel] <= 0x5A )
  		  {
  			  if( LastDist[channel] <= 0x12 ) LastDist[channel] = 0x01;
  		    else if( LastDist[channel] <= 0x24 ) LastDist[channel] = 0x02;
  		    else if( LastDist[channel] <= 0x36 ) LastDist[channel] = 0x03;
  		    else if( LastDist[channel] <= 0x48 ) LastDist[channel]= 0x04;
  		    else LastDist[channel] = 0x05;
  		    //LastDist[channel] = 0x03;
  	    }
  	    else if( LastDist[channel] <= 0x91 )
  	    {
  		    if( LastDist[channel] <= 0x6D ) LastDist[channel] = 0x06;
  		    else if( LastDist[channel] <= 0x7F ) LastDist[channel] = 0x07;
  		    else LastDist[channel] = 0x08;
  		    //LastDist[channel] = 0x07;
  	    }
  	    else if( LastDist[channel] <= 0xDA )
  	    {
  		    if( LastDist[channel] <= 0xA3 ) LastDist[channel] = 0x09;
  		    else if( LastDist[channel] <= 0xB5  ) LastDist[channel] = 0x0A;
  		    else if( LastDist[channel] <= 0xC7 ) LastDist[channel] = 0x0B;
  		    else LastDist[channel] = 0x0C;
  		    LastDist[channel] = 0x0A;
  	    }
  	    else LastDist[channel] = 0x0F;
  	  }
  	}
  	
	for( channel = 5; channel > 0; channel = channel-2 )
	{
		switch( channel )
		{
		  case 5:
		           TXB0D0 = LastDist[channel];
		           temp = LastDist[channel - 1];
		           temp <<= 4;
		           TXB0D0 |= temp;
		           break;
		  case 3:
		           TXB0D1 = LastDist[channel];
		           temp = LastDist[channel - 1];
		           temp <<= 4;
		           TXB0D1 |= temp;
		           break;
		  case 1:
		           TXB0D2 = LastDist[channel];
		           temp = LastDist[channel - 1];
		           temp <<= 4;
		           TXB0D2 |= temp;
		           channel = 2;
		           break;
		  default:
		           break;
		}
	}

}