mousealgorithm.s

陀螺仪程序

	.module mousealgorithm.c
	.area data(ram, con, rel)
_mode::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e mode _mode c
_lastMode::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e lastMode _lastMode c
_gyroYRaw::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYRaw _gyroYRaw s
_gyroZRaw::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZRaw _gyroZRaw s
_tempRaw::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e tempRaw _tempRaw s
_gyroYRaw1::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYRaw1 _gyroYRaw1 s
_gyroZRaw1::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZRaw1 _gyroZRaw1 s
_gyroYRaw2::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYRaw2 _gyroYRaw2 s
_gyroZRaw2::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZRaw2 _gyroZRaw2 s
_gyroYRaw3::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYRaw3 _gyroYRaw3 s
_gyroZRaw3::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZRaw3 _gyroZRaw3 s
_gyroTempY::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroTempY _gyroTempY L
_gyroTempZ::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroTempZ _gyroTempZ L
_gyroZBiasAv::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZBiasAv _gyroZBiasAv L
_gyroYBiasAv::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYBiasAv _gyroYBiasAv L
_tempBiasAv::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e tempBiasAv _tempBiasAv L
_gyroZMin::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZMin _gyroZMin s
_gyroZMax::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZMax _gyroZMax s
_gyroYMin::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYMin _gyroYMin s
_gyroYMax::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYMax _gyroYMax s
_gyroYBias::
	.blkb 4
	.area idata
	.word 0,2048
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYBias _gyroYBias L
_gyroZBias::
	.blkb 4
	.area idata
	.word 0,2048
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZBias _gyroZBias L
_tempBias::
	.blkb 4
	.area idata
	.word 0,2048
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e tempBias _tempBias L
_biasCounter::
	.blkb 2
	.area idata
	.word 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e biasCounter _biasCounter s
_motionDetected::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e motionDetected _motionDetected c
_lastSignY::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e lastSignY _lastSignY c
_lastSignZ::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e lastSignZ _lastSignZ c
_signYCount::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e signYCount _signYCount c
_signZCount::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e signZCount _signZCount c
_signY::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e signY _signY c
_signZ::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e signZ _signZ c
_gyroY::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroY _gyroY L
_gyroZ::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZ _gyroZ L
_temp::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e temp _temp L
_gyroYInt::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroYInt _gyroYInt L
_gyroZInt::
	.blkb 4
	.area idata
	.word 0,0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e gyroZInt _gyroZInt L
_lastButton::
	.blkb 1
	.area idata
	.byte 48
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e lastButton _lastButton c
_buttonCounter::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e buttonCounter _buttonCounter c
_calibrated::
	.blkb 1
	.area idata
	.byte 0
	.area data(ram, con, rel)
	.dbfile ./mousealgorithm.c
	.dbsym e calibrated _calibrated c
	.area text(rom, con, rel)
	.dbfile ./mousealgorithm.c
	.dbfunc e updateMouse _updateMouse fV
;         button -> X-15
;    pixelZShift -> X-13
;    pixelYShift -> X-11
;      gyroZData -> X-9
;      gyroYData -> X-7
;       tempData -> X-5
_updateMouse::
	.dbline -1
	push X
	mov X,SP
	.dbline 87
; /**************************************************************************************
; ;* 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){
	.dbline 90
; 
; 	//Compute median filter to filter out button clicks
; 	gyroYRaw3 = gyroYRaw2;
	mov [_gyroYRaw3+1],[_gyroYRaw2+1]
	mov [_gyroYRaw3],[_gyroYRaw2]
	.dbline 91
; 	gyroZRaw3 = gyroZRaw2;
	mov [_gyroZRaw3+1],[_gyroZRaw2+1]
	mov [_gyroZRaw3],[_gyroZRaw2]
	.dbline 93
; 
; 	gyroYRaw2 = gyroYRaw1;
	mov [_gyroYRaw2+1],[_gyroYRaw1+1]
	mov [_gyroYRaw2],[_gyroYRaw1]
	.dbline 94
; 	gyroZRaw2 = gyroZRaw1;
	mov [_gyroZRaw2+1],[_gyroZRaw1+1]
	mov [_gyroZRaw2],[_gyroZRaw1]
	.dbline 96
; 
; 	gyroYRaw = *gyroYData;
	mov A,[X-6]
	mov [__r1],A
	mvi A,[__r1]
	mov [_gyroYRaw],A
	mvi A,[__r1]
	mov [_gyroYRaw+1],A
	.dbline 97
; 	gyroZRaw = *gyroZData;
	mov A,[X-8]
	mov [__r1],A
	mvi A,[__r1]
	mov [_gyroZRaw],A
	mvi A,[__r1]
	mov [_gyroZRaw+1],A
	.dbline 98
; 	tempRaw = *tempData;
	mov A,[X-4]
	mov [__r1],A
	mvi A,[__r1]
	mov [_tempRaw],A
	mvi A,[__r1]
	mov [_tempRaw+1],A
	.dbline 100
; 
; 	gyroYRaw1 = gyroYRaw;
	mov [_gyroYRaw1+1],[_gyroYRaw+1]
	mov [_gyroYRaw1],[_gyroYRaw]
	.dbline 101
; 	gyroZRaw1 = gyroZRaw;
	mov [_gyroZRaw1+1],[_gyroZRaw+1]
	mov [_gyroZRaw1],[_gyroZRaw]
	.dbline 103
; 
; 	if (gyroYRaw3>gyroYRaw2) {
	mov A,[_gyroYRaw2+1]
	sub A,[_gyroYRaw3+1]
	mov A,[_gyroYRaw2]
	sbb A,[_gyroYRaw3]
	jnc L2
X0:
	.dbline 103
	.dbline 104
; 		if (gyroYRaw2>gyroYRaw) {
	mov A,[_gyroYRaw+1]
	sub A,[_gyroYRaw2+1]
	mov A,[_gyroYRaw]
	sbb A,[_gyroYRaw2]
	jnc L4
X1:
	.dbline 104
	.dbline 105
; 			gyroYRaw=gyroYRaw2;
	mov [_gyroYRaw+1],[_gyroYRaw2+1]
	mov [_gyroYRaw],[_gyroYRaw2]
	.dbline 106
	xjmp L3
L4:
	.dbline 106
; 		} else if (gyroYRaw3<gyroYRaw) {
	mov A,[_gyroYRaw3+1]
	sub A,[_gyroYRaw+1]
	mov A,[_gyroYRaw3]
	sbb A,[_gyroYRaw]
	jnc L3
X2:
	.dbline 106
	.dbline 107
; 			gyroYRaw=gyroYRaw3;
	mov [_gyroYRaw+1],[_gyroYRaw3+1]
	mov [_gyroYRaw],[_gyroYRaw3]
	.dbline 108
; 		}
	.dbline 109
	xjmp L3
L2:
	.dbline 109
; 	} else {
	.dbline 110
; 		if (gyroYRaw2<gyroYRaw) {
	mov A,[_gyroYRaw2+1]
	sub A,[_gyroYRaw+1]
	mov A,[_gyroYRaw2]
	sbb A,[_gyroYRaw]
	jnc L8
X3:
	.dbline 110
	.dbline 111
; 			gyroYRaw=gyroYRaw2;
	mov [_gyroYRaw+1],[_gyroYRaw2+1]
	mov [_gyroYRaw],[_gyroYRaw2]
	.dbline 112
	xjmp L9
L8:
	.dbline 112
; 		} else if (gyroYRaw3>gyroYRaw) {
	mov A,[_gyroYRaw+1]
	sub A,[_gyroYRaw3+1]
	mov A,[_gyroYRaw]
	sbb A,[_gyroYRaw3]
	jnc L10
X4:
	.dbline 112
	.dbline 113
; 			gyroYRaw=gyroYRaw3;
	mov [_gyroYRaw+1],[_gyroYRaw3+1]
	mov [_gyroYRaw],[_gyroYRaw3]
	.dbline 114
; 		}
L10:
L9:
	.dbline 115
; 	}
L3:
	.dbline 117
; 
; 	if (gyroZRaw3>gyroZRaw2) {
	mov A,[_gyroZRaw2+1]
	sub A,[_gyroZRaw3+1]
	mov A,[_gyroZRaw2]
	sbb A,[_gyroZRaw3]
	jnc L12
X5:
	.dbline 117
	.dbline 118
; 		if (gyroZRaw2>gyroZRaw) {
	mov A,[_gyroZRaw+1]
	sub A,[_gyroZRaw2+1]
	mov A,[_gyroZRaw]
	sbb A,[_gyroZRaw2]
	jnc L14
X6:
	.dbline 118
	.dbline 119
; 			gyroZRaw=gyroZRaw2;
	mov [_gyroZRaw+1],[_gyroZRaw2+1]
	mov [_gyroZRaw],[_gyroZRaw2]
	.dbline 120
	xjmp L13
L14:
	.dbline 120
; 		} else if (gyroZRaw3<gyroZRaw) {
	mov A,[_gyroZRaw3+1]
	sub A,[_gyroZRaw+1]
	mov A,[_gyroZRaw3]
	sbb A,[_gyroZRaw]
	jnc L13
X7:
	.dbline 120
	.dbline 121
; 			gyroZRaw=gyroZRaw3;
	mov [_gyroZRaw+1],[_gyroZRaw3+1]
	mov [_gyroZRaw],[_gyroZRaw3]
	.dbline 122
; 		}
	.dbline 123
	xjmp L13
L12:
	.dbline 123
; 	} else {
	.dbline 124
; 		if (gyroZRaw2<gyroZRaw) {
	mov A,[_gyroZRaw2+1]
	sub A,[_gyroZRaw+1]
	mov A,[_gyroZRaw2]
	sbb A,[_gyroZRaw]
	jnc L18
X8:
	.dbline 124
	.dbline 125
; 			gyroZRaw=gyroZRaw2;
	mov [_gyroZRaw+1],[_gyroZRaw2+1]
	mov [_gyroZRaw],[_gyroZRaw2]
	.dbline 126
	xjmp L19
L18:
	.dbline 126
; 		} else if (gyroZRaw3>gyroZRaw) {
	mov A,[_gyroZRaw+1]
	sub A,[_gyroZRaw3+1]
	mov A,[_gyroZRaw]
	sbb A,[_gyroZRaw3]
	jnc L20
X9:
	.dbline 126
	.dbline 127
; 			gyroZRaw=gyroZRaw3;
	mov [_gyroZRaw+1],[_gyroZRaw3+1]
	mov [_gyroZRaw],[_gyroZRaw3]
	.dbline 128
; 		}
L20:
L19:
	.dbline 129
; 	}
L13:
	.dbline 132
; 
; 	//Bias Calibration
; 	gyroYBiasAv+=(signed long)gyroYRaw;
	mov [__r3],[_gyroYRaw+1]
	mov [__r2],[_gyroYRaw]
	mov A,[__r3]
	add [_gyroYBiasAv+3],A
	mov A,[__r2]
	adc [_gyroYBiasAv+2],A
	adc [_gyroYBiasAv+1],0
	adc [_gyroYBiasAv],0
	.dbline 133
; 	tempBiasAv+=(signed long)tempRaw;
	mov [__r3],[_tempRaw+1]
	mov [__r2],[_tempRaw]
	mov A,[__r3]
	add [_tempBiasAv+3],A
	mov A,[__r2]
	adc [_tempBiasAv+2],A
	adc [_tempBiasAv+1],0
	adc [_tempBiasAv],0
	.dbline 135
; 
; 	if (gyroYRaw<gyroYMin) {
	mov A,[_gyroYRaw+1]
	sub A,[_gyroYMin+1]
	mov A,[_gyroYRaw]
	sbb A,[_gyroYMin]
	jnc L22
X10:
	.dbline 135
	.dbline 136
; 		gyroYMin = gyroYRaw;
	mov [_gyroYMin+1],[_gyroYRaw+1]
	mov [_gyroYMin],[_gyroYRaw]
	.dbline 137
; 	}
L22:
	.dbline 138
; 	if (gyroYRaw>gyroYMax) {
	mov A,[_gyroYMax+1]
	sub A,[_gyroYRaw+1]
	mov A,[_gyroYMax]
	sbb A,[_gyroYRaw]
	jnc L24
X11:
	.dbline 138
	.dbline 139
; 		gyroYMax = gyroYRaw;
	mov [_gyroYMax+1],[_gyroYRaw+1]
	mov [_gyroYMax],[_gyroYRaw]
	.dbline 140
; 	}
L24:
	.dbline 142
; 
; 	gyroZBiasAv+=(signed long)gyroZRaw;
	mov [__r3],[_gyroZRaw+1]
	mov [__r2],[_gyroZRaw]
	mov A,[__r3]
	add [_gyroZBiasAv+3],A
	mov A,[__r2]
	adc [_gyroZBiasAv+2],A
	adc [_gyroZBiasAv+1],0
	adc [_gyroZBiasAv],0
	.dbline 143
; 	if (gyroZRaw<gyroZMin) {
	mov A,[_gyroZRaw+1]
	sub A,[_gyroZMin+1]
	mov A,[_gyroZRaw]
	sbb A,[_gyroZMin]
	jnc L26
X12:
	.dbline 143
	.dbline 144
; 		gyroZMin = gyroZRaw;
	mov [_gyroZMin+1],[_gyroZRaw+1]
	mov [_gyroZMin],[_gyroZRaw]
	.dbline 145
; 	}
L26:
	.dbline 146
; 	if (gyroZRaw>gyroZMax) {
	mov A,[_gyroZMax+1]
	sub A,[_gyroZRaw+1]
	mov A,[_gyroZMax]
	sbb A,[_gyroZRaw]
	jnc L28
X13:
	.dbline 146
	.dbline 147
; 		gyroZMax = gyroZRaw;
	mov [_gyroZMax+1],[_gyroZRaw+1]
	mov [_gyroZMax],[_gyroZRaw]
	.dbline 148
; 	}
L28:
	.dbline 150
; 
; 	biasCounter+=1;
	inc [_biasCounter+1]
	adc [_biasCounter],0
	.dbline 151
; 	if (biasCounter>511) {
	mov A,-1
	sub A,[_biasCounter+1]
	mov A,1
	sbb A,[_biasCounter]
	jnc L30
X14:
	.dbline 151
	.dbline 152
; 		biasCounter = 0;
	mov [_biasCounter+1],0
	mov [_biasCounter],0
	.dbline 153
; 	}
L30:
	.dbline 154
; 	if (biasCounter==0) {
	cmp [_biasCounter],0
	jnz L32
	cmp [_biasCounter+1],0
	jnz L32
X15:
	.dbline 154
	.dbline 160
; 		
; 		#ifdef DEBUG_INFO
; 		printf("RESETTING BIAS\n");
; 		#endif
; 
; 		mode = 8;
	mov [_mode],8
	.dbline 161
; 		gyroYBias = gyroYBiasAv>>1;
	mov [__r0],[_gyroYBiasAv]
	mov [__r1],[_gyroYBiasAv+1]
	mov [__r2],[_gyroYBiasAv+2]
	mov [__r3],[_gyroYBiasAv+3]
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	mov [_gyroYBias],[__r0]
	mov [_gyroYBias+1],[__r1]
	mov [_gyroYBias+2],[__r2]
	mov [_gyroYBias+3],[__r3]
	.dbline 162
; 		gyroZBias = gyroZBiasAv>>1;
	mov [__r0],[_gyroZBiasAv]
	mov [__r1],[_gyroZBiasAv+1]
	mov [__r2],[_gyroZBiasAv+2]
	mov [__r3],[_gyroZBiasAv+3]
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	mov [_gyroZBias],[__r0]
	mov [_gyroZBias+1],[__r1]
	mov [_gyroZBias+2],[__r2]
	mov [_gyroZBias+3],[__r3]
	.dbline 163
; 		tempBias = tempBiasAv>>1;
	mov [__r0],[_tempBiasAv]
	mov [__r1],[_tempBiasAv+1]
	mov [__r2],[_tempBiasAv+2]
	mov [__r3],[_tempBiasAv+3]
	asr [__r0]
	rrc [__r1]
	rrc [__r2]
	rrc [__r3]
	mov [_tempBias],[__r0]
	mov [_tempBias+1],[__r1]
	mov [_tempBias+2],[__r2]
	mov [_tempBias+3],[__r3]
	.dbline 164
; 		calibrated = 1;
	mov [_calibrated],1
	.dbline 166
; 
; 		gyroYBiasAv = 0;
	mov [_gyroYBiasAv],0
	mov [_gyroYBiasAv+1],0
	mov [_gyroYBiasAv+2],0
	mov [_gyroYBiasAv+3],0
	.dbline 167
; 		gyroZBiasAv = 0;
	mov [_gyroZBiasAv],0
	mov [_gyroZBiasAv+1],0
	mov [_gyroZBiasAv+2],0
	mov [_gyroZBiasAv+3],0
	.dbline 168
; 		tempBiasAv = 0;
	mov [_tempBiasAv],0
	mov [_tempBiasAv+1],0
	mov [_tempBiasAv+2],0
	mov [_tempBiasAv+3],0
	.dbline 170
; 		
; 		gyroYMin = gyroYRaw;
	mov [_gyroYMin+1],[_gyroYRaw+1]
	mov [_gyroYMin],[_gyroYRaw]
	.dbline 171
; 		gyroYMax = gyroYRaw;
	mov [_gyroYMax+1],[_gyroYRaw+1]
	mov [_gyroYMax],[_gyroYRaw]
	.dbline 172
; 		gyroZMin = gyroZRaw;
	mov [_gyroZMin+1],[_gyroZRaw+1]
	mov [_gyroZMin],[_gyroZRaw]
	.dbline 173
; 		gyroZMax = gyroZRaw;		
	mov [_gyroZMax+1],[_gyroZRaw+1]
	mov [_gyroZMax],[_gyroZRaw]
	.dbline 174
; 		gyroYInt = 0;
	mov [_gyroYInt],0
	mov [_gyroYInt+1],0
	mov [_gyroYInt+2],0
	mov [_gyroYInt+3],0
	.dbline 175
; 		gyroZInt = 0;
	mov [_gyroZInt],0
	mov [_gyroZInt+1],0
	mov [_gyroZInt+2],0
	mov [_gyroZInt+3],0
	.dbline 176
; 	} 
L32:
	.dbline 178
; 	
; 	motionDetected = 0;
	mov [_motionDetected],0
	.dbline 180
; 
; 	motionTemp = gyroYMax-gyroYMin;
	mov A,[_gyroYMax+1]
	sub A,[_gyroYMin+1]
	mov [_motionTemp+1],A
	mov A,[_gyroYMax]
	sbb A,[_gyroYMin]
	mov [_motionTemp],A