mousealgorithm.c

来自「陀螺仪程序」· C语言 代码 · 共 411 行

/**************************************************************************************
;* Copyright 2007 InvenSense, Inc. All rights reserved.
;* GyroMouse v. 1.1
;**************************************************************************************/
// MouseAlgorithm.cpp : Mouse update function
//

#include "driftcompensation.h"

//Thresholds
#define DEAD_ZONE 1536
#define NO_MOTION 3
#define ENTER_STOP_MODE 50
#define EXIT_STOP_MODE 10
#define SPEED_0 28000
#define SPEED_1 24000
#define SPEED_2 20000
#define SPEED_3 16000
#define SPEED_4 12000
#define SPEED_5 8000

//Variables

//Pointer speed mode variables
unsigned char mode=0;
unsigned char lastMode=0;

//Raw gyro data
unsigned short gyroYRaw=0;
unsigned short gyroZRaw=0;
unsigned short tempRaw =0;

//Buffered raw gyro data for median filter
//Used for filtering out button clicks
unsigned short gyroYRaw1=0;
unsigned short gyroZRaw1=0;
unsigned short gyroYRaw2=0;
unsigned short gyroZRaw2=0;
unsigned short gyroYRaw3=0;
unsigned short gyroZRaw3=0;

//Temporary storage for computing "speed" metric
signed long gyroTempY = 0;
signed long gyroTempZ = 0;

//Bias calculation variables
signed long gyroZBiasAv = 0;
signed long gyroYBiasAv = 0;
signed long tempBiasAv = 0;
unsigned short gyroZMin=0;
unsigned short gyroZMax=0;
unsigned short gyroYMin=0;
unsigned short gyroYMax=0;
signed long gyroYBias = 2048;
signed long gyroZBias = 2048;
signed long tempBias = 2048;
unsigned short motionTemp;
unsigned short biasCounter = 0;
unsigned char motionDetected = 0;

//Variables for detecting "STOP" mode
unsigned char lastSignY = 0;
unsigned char lastSignZ = 0;
unsigned char signYCount = 0;
unsigned char signZCount = 0;
unsigned char signY = 0;
unsigned char signZ = 0;

//Processed gyro data
signed long gyroY = 0;
signed long gyroZ = 0;
signed long temp = 0;

//Integrated gyro data
signed long gyroYInt = 0;
signed long gyroZInt = 0;

//Button information
//Used for freezing gyro data temporarily when button is pressed or released
unsigned char lastButton = 48;
unsigned char buttonCounter = 0;

//Initial Calibration
unsigned char calibrated = 0;


void updateMouse(unsigned short *tempData, unsigned short *gyroYData, unsigned short *gyroZData, signed short *pixelYShift, signed short *pixelZShift, unsigned char *button){

	//Compute median filter to filter out button clicks
	gyroYRaw3 = gyroYRaw2;
	gyroZRaw3 = gyroZRaw2;

	gyroYRaw2 = gyroYRaw1;
	gyroZRaw2 = gyroZRaw1;

	gyroYRaw = *gyroYData;
	gyroZRaw = *gyroZData;
	tempRaw = *tempData;

	gyroYRaw1 = gyroYRaw;
	gyroZRaw1 = gyroZRaw;

	if (gyroYRaw3>gyroYRaw2) {
		if (gyroYRaw2>gyroYRaw) {
			gyroYRaw=gyroYRaw2;
		} else if (gyroYRaw3<gyroYRaw) {
			gyroYRaw=gyroYRaw3;
		}
	} else {
		if (gyroYRaw2<gyroYRaw) {
			gyroYRaw=gyroYRaw2;
		} else if (gyroYRaw3>gyroYRaw) {
			gyroYRaw=gyroYRaw3;
		}
	}

	if (gyroZRaw3>gyroZRaw2) {
		if (gyroZRaw2>gyroZRaw) {
			gyroZRaw=gyroZRaw2;
		} else if (gyroZRaw3<gyroZRaw) {
			gyroZRaw=gyroZRaw3;
		}
	} else {
		if (gyroZRaw2<gyroZRaw) {
			gyroZRaw=gyroZRaw2;
		} else if (gyroZRaw3>gyroZRaw) {
			gyroZRaw=gyroZRaw3;
		}
	}

	//Bias Calibration
	gyroYBiasAv+=(signed long)gyroYRaw;
	tempBiasAv+=(signed long)tempRaw;

	if (gyroYRaw<gyroYMin) {
		gyroYMin = gyroYRaw;
	}
	if (gyroYRaw>gyroYMax) {
		gyroYMax = gyroYRaw;
	}

	gyroZBiasAv+=(signed long)gyroZRaw;
	if (gyroZRaw<gyroZMin) {
		gyroZMin = gyroZRaw;
	}
	if (gyroZRaw>gyroZMax) {
		gyroZMax = gyroZRaw;
	}

	biasCounter+=1;
	if (biasCounter>511) {
		biasCounter = 0;
	}
	if (biasCounter==0) {
		
		#ifdef DEBUG_INFO
		printf("RESETTING BIAS\n");
		#endif

		mode = 8;
		gyroYBias = gyroYBiasAv>>1;
		gyroZBias = gyroZBiasAv>>1;
		tempBias = tempBiasAv>>1;
		calibrated = 1;

		gyroYBiasAv = 0;
		gyroZBiasAv = 0;
		tempBiasAv = 0;
		
		gyroYMin = gyroYRaw;
		gyroYMax = gyroYRaw;
		gyroZMin = gyroZRaw;
		gyroZMax = gyroZRaw;		
		gyroYInt = 0;
		gyroZInt = 0;
	} 
	
	motionDetected = 0;

	motionTemp = gyroYMax-gyroYMin;
	if (motionTemp<0) {
		motionTemp = -motionTemp;
	}
	if (motionTemp>NO_MOTION) {
		motionDetected = 1;
	}
	motionTemp = gyroZMax-gyroZMin;
	if (motionTemp<0) {
		motionTemp = -motionTemp;
	}
	if (motionTemp>NO_MOTION) {
		motionDetected = 1;
	}
	if (motionDetected) { 
		mode=6;
		biasCounter = 0;
		gyroYBiasAv = 0;
		gyroZBiasAv = 0;
		tempBiasAv = 0;
		gyroYMin = gyroYRaw;
		gyroYMax = gyroYRaw;
		gyroZMin = gyroZRaw;		
		gyroZMax = gyroZRaw;		
	}
	
	gyroY = (((signed long)gyroYRaw)<<8)-gyroYBias;
	gyroZ = (((signed long)gyroZRaw)<<8)-gyroZBias;
	if (calibrated==0) {
		gyroY = 0;
		gyroZ = 0;
	}
	temp = (((signed long)tempRaw)<<8)-tempBias;
	
	//Drift Compensation
	driftComp(&gyroY, &gyroZ, &temp, &mode);

	#ifdef TEMP_DEBUG_INFO
	printf("%*d %*d %*d\n", 10, gyroY>>8, 10, gyroZ>>8, 10, temp>>8);
	#endif
	
	//Dead zone
	if (((gyroY<(DEAD_ZONE)) && (gyroY>(-DEAD_ZONE))) && (mode>5)) {
		gyroY=0;
	}
	if (((gyroZ<(DEAD_ZONE)) && (gyroZ>(-DEAD_ZONE))) && (mode>5)) {
		gyroZ=0;
	}

	//Unless mode is OFF (on the desk), start in mode 6
	if (mode!=8) {mode = 6;}

    //Determine if pointer should be in "STOP" mode
    signY = 0;
    signZ = 0;       
	if (gyroY>0) {
        signY = 1;
	}
	if (gyroZ>0) {
        signZ = 1;
	}
	if (signY==lastSignY) {
        signYCount+=1;
		if (signYCount==255) {
			signYCount-=1;
		}
	} else {
        signYCount = 0;
	}
	if (signZ==lastSignZ) {
        signZCount+=1;
		if (signZCount==255) {
			signZCount-=1;
		}
	} else {
        signZCount = 0;
	}
    lastSignY = signY;
    lastSignZ = signZ;

	if (mode!=8) {//Unless mode is "OFF" (on the desk)...
		if ((signYCount<ENTER_STOP_MODE) && (signZCount<ENTER_STOP_MODE)) {//If sign changes are happening
			//at a high frequency, enter stop mode
			mode = 7;
		}
		if ((gyroTempY>SPEED_5) || (gyroTempZ>SPEED_5)) {// Test for "MEDIUM" mode
			if (lastMode==7) {//Not allowed if coming straight from "stop mode"
				mode=6;
			} else {
				mode=5;
			}
		}
		if ((signYCount>EXIT_STOP_MODE) || (signZCount>EXIT_STOP_MODE)) {// || (lastMode!=7)) {
			gyroTempY = gyroY;
			gyroTempZ = gyroZ;
			if (gyroTempY<0) {gyroTempY=-gyroTempY;}
			if (gyroTempZ<0) {gyroTempZ=-gyroTempZ;}
			if ((gyroTempY>SPEED_0) || (gyroTempZ>SPEED_0)) {
				mode=0;
			} else if ((gyroTempY>SPEED_1) || (gyroTempZ>SPEED_1)) {
				mode=1;
			} else if ((gyroTempY>SPEED_2) || (gyroTempZ>SPEED_2)) {
				mode=2;
			} else if ((gyroTempY>SPEED_3) || (gyroTempZ>SPEED_3)) {
				mode=3;
			} else if ((gyroTempY>SPEED_4) || (gyroTempZ>SPEED_4)) {
				mode=4;
			} 
		}
		lastMode = mode;
	}

	if (mode==0) {

		#ifdef DEBUG_INFO
		printf("SPEED 0\n");
		#endif

        gyroY=gyroY-(gyroY>>2);
        gyroZ=gyroZ-(gyroZ>>2);
	}
	if (mode==1) {

		#ifdef DEBUG_INFO
		printf("SPEED 1\n");
		#endif

        gyroY=gyroY-(gyroY>>2)-(gyroY>>3);
        gyroZ=gyroZ-(gyroZ>>2)-(gyroZ>>3);
	}
	if (mode==2) {

		#ifdef DEBUG_INFO
		printf("SPEED 2\n");
		#endif

        gyroY=(gyroY>>1);
        gyroZ=(gyroZ>>1);
	}
	if (mode==3) {

		#ifdef DEBUG_INFO
		printf("SPEED 3\n");
		#endif

        gyroY=(gyroY>>1)-(gyroY>>3);
        gyroZ=(gyroZ>>1)-(gyroZ>>3);
	}
	if (mode==4) {

		#ifdef DEBUG_INFO
		printf("SPEED 4\n");
		#endif

        gyroY=(gyroY>>1)-(gyroY>>2);
        gyroZ=(gyroZ>>1)-(gyroZ>>2);
	}
	if (mode==5) {

		#ifdef DEBUG_INFO
		printf("SPEED 5\n");
		#endif

        gyroY=(gyroY>>2);
        gyroZ=(gyroZ>>2);
	}
	if (mode==6) {

		#ifdef DEBUG_INFO
		printf("SPEED 6\n");
		#endif

        gyroY=gyroY>>3;
        gyroZ=gyroZ>>3;
	}
	//"STOP MODE", when gyro pointer is staying within a small area
	if (mode==7) {

		#ifdef DEBUG_INFO
		printf("STOP\n");
		#endif

		gyroY=gyroY>>5;
		gyroZ=gyroZ>>5;
		if (gyroY>25) {gyroY=25;}
		if (gyroY<-25) {gyroY=-25;}
		if (gyroZ>25) {gyroZ=25;}
		if (gyroZ<-25) {gyroZ=-25;}
	}
	//"OFF" Mode, when the gyro is on a desk, and only sensor noise is detected
	if (mode==8) {

		#ifdef DEBUG_INFO
		printf("OFF\n");
		#endif

		gyroY=0;	
		gyroZ=0;
	}
	//When button is detected, temporarily freeze the gyro data
	if (buttonCounter<25) {

		#ifdef DEBUG_INFO
		printf("CLICK DETECTED\n");
		#endif

		buttonCounter+=1;
		gyroY = 0;
		gyroZ = 0;		
	}
	if ((*button!=lastButton)){
		buttonCounter=0;
	}
	lastButton = *button;

	//Integrate the gyro data
	gyroYInt += gyroY;
	gyroZInt -= gyroZ;

	//Round to the nearest pixel, and adjust the gyro integral
	*pixelYShift = (signed short)(gyroYInt>>10);
	*pixelZShift = (signed short)(gyroZInt>>10);
	gyroYInt-=(((signed long)*pixelYShift)<<10);
	gyroZInt-=(((signed long)*pixelZShift)<<10);
	if (calibrated==0) {
		*pixelYShift = 0;
		*pixelZShift = 0;
	}	
	
}