dg.c

老外开发的机器人的底层单片机代码。比较有参考价值哦！

//  Directional gyro & yaw rate gyro     DG.c		// Rev 9/1/05

//Copyright (C) 2005 Alex Brown	[e-mail]
//This program is free software; See license at the end of this file for details.


#include <hcs12dp256.h>		    //for registers
#include <stdio.h>				//for i/o prototypes
#include <stdlib.h>				//for abs()

extern int Compass;				//compass heading from I2Ccompass.c
extern int time;  //temp for test
extern long Xcoord; //temp for test

/* Rate gyro null calculator
     This function is meant to be called many times as fast as possible to 
	 calculate an average null to high precision.  It will take more
	 time than a real time interrupt (16 msec).  Hence, it should be called 
	 from the initialization section in main(), or from background.
	 
	 Counting 5000 values seems to give a 2 sigma error of about 4 counts.
	 5000 values takes about 3.15 seconds to execute in main() initialization.
	 100000 samples takes about 15 seconds.
	 Don't exceed 100,000 samples.  Might overflow the sum.
	 
	 The for j loop count of 220 should cause enough delay that each sample is
	 a new one from the ATD.  If not, it will still get a valid answer, just 
	 not based on as many samples.  
	
	 
	 PS.   the first run is usually way off due to some bad 
	 initial values.  So, reads 10000 samples before starting average.
	 
	 I have a fudge factor added to the null. The null calculation run during
	 initialization gets a value 10 units lower than a similar value calculated
	 from data read during the realtime interrupt.   I don't know why???
	 
*/ 
	int NullAvg;
long bins[16];	//test
int tempDG;		//test
long ratetemp;	//test
long hdgLtest;  //test
int hdgdegtest;
int hdgcorr;

int RGnull(void)
  { long NullSumL; 		      //sums sample values
    long cnt;	   		  	  //counts number of sample values
	int j;		   		  	  //counter for a short delay
int temp;
	for (cnt = 1; cnt<10000; cnt++)  //first few samples seem off
	 {  NullSumL = ATD0DR2<<5;		//  so dump a bunch first
	    for (j=0;j<220;j++);	 //short delay to avoid reading same ATD sample
	 } 
    NullSumL = 0;
	   
    for (cnt = 1; cnt<=100000; cnt++)
	  { 
	     temp= ATD0DR2<<5;	 //rate * 32
	    NullSumL += temp;
		for (j=0;j<220;j++);	 //short delay to avoid reading same ATD sample
//printf("%6ld %10ld %6d\n",cnt,NullSumL,(int)(NullSumL/cnt));
	    NullAvg = (int)(NullSumL/cnt);// + 10;// fudge factor ???
      }	
//printf("Null = %d\n",NullAvg);
	return NullAvg;
  }
  
// Directional Gyro  DGyro
/*
  Notes:  ATD output is 0.3906 deg/second per lsb.  Hence is 0.012207 
  deg/second after scaling B5 (*32).
  Since calculation will be at 61 hz, an integration will provide 0.774463 
  deg/second per lsb  (0.012207 * 61).

*/
int  YRatedps;	 //yaw rate scaled to degrees/second * 10
int  YRatemrad;  //yaw rate scaled to milliradians
int hdgdeg;		 //heading in degrees * 10
int hdgmrad;	 //heading in milliradians
long hdgL;	 	 //heading 

void DGyro(void)
  { 
    long rateL;	 	 //yaw rate input scaled B5
// 	static int hdgcorr;	 //correction of inertial heading to compass
	int delhdg;		 //difference between compass and inertial heading
    //calculate yaw rate
    rateL = ATD0DR2<<5;	 		   	  	//rate analog signal * 32
//printf("%10ld",rateL);
	rateL -= NullAvg;	 		  		//subtract null offset
//printf("%10ld",rateL);
	rateL = (rateL * /*874*/866)/1000;  		//fudge factor for gain tolerance
	YRatedps = (rateL*12207)/100000;    //convert to degrees/second *10
	YRatemrad = (rateL*21304)/100000;   //convert to milliradians/second
	
	//calculate heading
	if((time == 100) && (Compass != 9999))
  	{
     		hdgL = ((long)Compass*4997)/10;
	 	hdgLtest = hdgL;
  	}
	if(1) //add logic here to determine when to freeze integration
	  { 
	   	 hdgL += (rateL + hdgcorr);    //integral of yaw rate and correction
		if(hdgL >  (long) 1798920) hdgL -=  1798920; //limit to 0 to 360 deg
		if(hdgL <               0) hdgL +=  1798920;		   
		hdgdeg = hdgL*10/4997;		//convert to deg *10
		hdgLtest += rateL;
		if(hdgLtest >  (long) 1798920) hdgLtest -=  1798920; //limit to 0 to 360 deg
		if(hdgLtest <               0) hdgLtest +=  1798920;		   
		hdgdegtest = hdgLtest*10/4997;
	//printf("%10d\n",hdgdeg);		hdgmrad = hdgL*10/8721;  	//convert to milliradians
	      }
		  
	//calculate correction to compass
	if((abs(YRatedps) < 50)&&(Compass != 9999))  //add more logic here for when to correct
	  { delhdg = Compass - hdgdeg;
	    if(delhdg > 1800) delhdg -= 3600; //limit to range +/- 180 degrees
		hdgcorr = 0;
		if(delhdg >0) hdgcorr =  10;  //correction rate 
		if(delhdg <0) hdgcorr = -10;  // 10 =  7.3 degrees/minute
	  }
	else
	  { hdgcorr = 0;
	  }    
//printf("DG  %6d  %8d  %8ld   %8d  %8ld  %8d   %8ld  \n",time,NullAvg,rateL,YRatedps,hdgL,hdgdeg,Xcoord);
    return;
  }  
  
// Set heading integrator 

void hdgset(int hdgnew)
  {
    	hdgL = ((long)hdgnew * 4997) /10;
	if(hdgL >  (long) 899460) hdgL -= 	1798920; //limit to +/- 180 deg
	if(hdgL < -(long) 899460) hdgL +=  1798920;		   
	hdgdeg = hdgL*10/4997;		//convert to deg *10
	hdgmrad = hdgL*10/8721;  	//convert to milliradians
	return;
  }  
  
//  OPEN SOURCE SOFTWARE LICENSE
/* Permission is hereby granted, free of charge, to any person obtaining a copy 
of this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to use, 
copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the 
Software, and to permit persons to whom the Software is furnished to do so, 
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all 
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 
FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 
COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 
IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 
CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/