mousealgorithm.s

陀螺仪程序

	.dbline 181
; 	if (motionTemp<0) {
	mov A,0
	jz L34
	.dbline 181
	.dbline 182
; 		motionTemp = -motionTemp;
	mov A,[_motionTemp+1]
	cpl A
	mov [__r1],A
	mov A,[_motionTemp]
	cpl A
	mov [__r0],A
	mov A,[__r1]
	add A,1
	mov [_motionTemp+1],A
	mov A,[__r0]
	adc A,0
	mov [_motionTemp],A
	.dbline 183
; 	}
L34:
	.dbline 184
; 	if (motionTemp>NO_MOTION) {
	mov A,3
	sub A,[_motionTemp+1]
	mov A,0
	sbb A,[_motionTemp]
	jnc L36
X16:
	.dbline 184
	.dbline 185
; 		motionDetected = 1;
	mov [_motionDetected],1
	.dbline 186
; 	}
L36:
	.dbline 187
; 	motionTemp = gyroZMax-gyroZMin;
	mov A,[_gyroZMax+1]
	sub A,[_gyroZMin+1]
	mov [_motionTemp+1],A
	mov A,[_gyroZMax]
	sbb A,[_gyroZMin]
	mov [_motionTemp],A
	.dbline 188
; 	if (motionTemp<0) {
	mov A,0
	jz L38
	.dbline 188
	.dbline 189
; 		motionTemp = -motionTemp;
	mov A,[_motionTemp+1]
	cpl A
	mov [__r1],A
	mov A,[_motionTemp]
	cpl A
	mov [__r0],A
	mov A,[__r1]
	add A,1
	mov [_motionTemp+1],A
	mov A,[__r0]
	adc A,0
	mov [_motionTemp],A
	.dbline 190
; 	}
L38:
	.dbline 191
; 	if (motionTemp>NO_MOTION) {
	mov A,3
	sub A,[_motionTemp+1]
	mov A,0
	sbb A,[_motionTemp]
	jnc L40
X17:
	.dbline 191
	.dbline 192
; 		motionDetected = 1;
	mov [_motionDetected],1
	.dbline 193
; 	}
L40:
	.dbline 194
; 	if (motionDetected) { 
	cmp [_motionDetected],0
	jz L42
	.dbline 194
	.dbline 195
; 		mode=6;
	mov [_mode],6
	.dbline 196
; 		biasCounter = 0;
	mov [_biasCounter+1],0
	mov [_biasCounter],0
	.dbline 197
; 		gyroYBiasAv = 0;
	mov [_gyroYBiasAv],0
	mov [_gyroYBiasAv+1],0
	mov [_gyroYBiasAv+2],0
	mov [_gyroYBiasAv+3],0
	.dbline 198
; 		gyroZBiasAv = 0;
	mov [_gyroZBiasAv],0
	mov [_gyroZBiasAv+1],0
	mov [_gyroZBiasAv+2],0
	mov [_gyroZBiasAv+3],0
	.dbline 199
; 		tempBiasAv = 0;
	mov [_tempBiasAv],0
	mov [_tempBiasAv+1],0
	mov [_tempBiasAv+2],0
	mov [_tempBiasAv+3],0
	.dbline 200
; 		gyroYMin = gyroYRaw;
	mov [_gyroYMin+1],[_gyroYRaw+1]
	mov [_gyroYMin],[_gyroYRaw]
	.dbline 201
; 		gyroYMax = gyroYRaw;
	mov [_gyroYMax+1],[_gyroYRaw+1]
	mov [_gyroYMax],[_gyroYRaw]
	.dbline 202
; 		gyroZMin = gyroZRaw;		
	mov [_gyroZMin+1],[_gyroZRaw+1]
	mov [_gyroZMin],[_gyroZRaw]
	.dbline 203
; 		gyroZMax = gyroZRaw;		
	mov [_gyroZMax+1],[_gyroZRaw+1]
	mov [_gyroZMax],[_gyroZRaw]
	.dbline 204
; 	}
L42:
	.dbline 206
; 	
; 	gyroY = (((signed long)gyroYRaw)<<8)-gyroYBias;
	mov [__r3],[_gyroYRaw+1]
	mov [__r2],[_gyroYRaw]
	mov [__r1],0
	mov [__r0],0
	mov A,8
X18:
	asl [__r3]
	rlc [__r2]
	rlc [__r1]
	rlc [__r0]
	dec A
	jnz X18
	mov A,[__r3]
	sub A,[_gyroYBias+3]
	mov [_gyroY+3],A
	mov A,[__r2]
	sbb A,[_gyroYBias+2]
	mov [_gyroY+2],A
	mov A,[__r1]
	sbb A,[_gyroYBias+1]
	mov [_gyroY+1],A
	mov A,[__r0]
	sbb A,[_gyroYBias]
	mov [_gyroY],A
	.dbline 207
; 	gyroZ = (((signed long)gyroZRaw)<<8)-gyroZBias;
	mov [__r3],[_gyroZRaw+1]
	mov [__r2],[_gyroZRaw]
	mov [__r1],0
	mov [__r0],0
	mov A,8
X19:
	asl [__r3]
	rlc [__r2]
	rlc [__r1]
	rlc [__r0]
	dec A
	jnz X19
	mov A,[__r3]
	sub A,[_gyroZBias+3]
	mov [_gyroZ+3],A
	mov A,[__r2]
	sbb A,[_gyroZBias+2]
	mov [_gyroZ+2],A
	mov A,[__r1]
	sbb A,[_gyroZBias+1]
	mov [_gyroZ+1],A
	mov A,[__r0]
	sbb A,[_gyroZBias]
	mov [_gyroZ],A
	.dbline 208
; 	if (calibrated==0) {
	cmp [_calibrated],0
	jnz L44
	.dbline 208
	.dbline 209
; 		gyroY = 0;
	mov [_gyroY],0
	mov [_gyroY+1],0
	mov [_gyroY+2],0
	mov [_gyroY+3],0
	.dbline 210
; 		gyroZ = 0;
	mov [_gyroZ],0
	mov [_gyroZ+1],0
	mov [_gyroZ+2],0
	mov [_gyroZ+3],0
	.dbline 211
; 	}
L44:
	.dbline 212
; 	temp = (((signed long)tempRaw)<<8)-tempBias;
	mov [__r3],[_tempRaw+1]
	mov [__r2],[_tempRaw]
	mov [__r1],0
	mov [__r0],0
	mov A,8
X20:
	asl [__r3]
	rlc [__r2]
	rlc [__r1]
	rlc [__r0]
	dec A
	jnz X20
	mov A,[__r3]
	sub A,[_tempBias+3]
	mov [_temp+3],A
	mov A,[__r2]
	sbb A,[_tempBias+2]
	mov [_temp+2],A
	mov A,[__r1]
	sbb A,[_tempBias+1]
	mov [_temp+1],A
	mov A,[__r0]
	sbb A,[_tempBias]
	mov [_temp],A
	.dbline 215
; 	
; 	//Drift Compensation
; 	driftComp(&gyroY, &gyroZ, &temp, &mode);
	mov A,>_mode
	push A
	mov A,<_mode
	push A
	mov A,>_temp
	push A
	mov A,<_temp
	push A
	mov A,>_gyroZ
	push A
	mov A,<_gyroZ
	push A
	mov A,>_gyroY
	push A
	mov A,<_gyroY
	push A
	xcall _driftComp
	add SP,-8
	.dbline 222
; 
; 	#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)) {
	mov A,[_gyroY+3]
	sub A,0
	mov A,[_gyroY+2]
	sbb A,6
	mov A,[_gyroY+1]
	sbb A,0
	mov A,[_gyroY]
	xor A,-128
	sbb A,(0 ^ 0x80)
	jnc L46
X21:
	mov A,0
	sub A,[_gyroY+3]
	mov A,-6
	sbb A,[_gyroY+2]
	mov A,-1
	sbb A,[_gyroY+1]
	mov A,[_gyroY]
	xor A,-128
	mov [__rX],A
	mov A,(255 ^ 0x80)
	sbb A,[__rX]
	jnc L46
X22:
	mov A,5
	cmp A,[_mode]
	jnc L46
X23:
	.dbline 222
	.dbline 223
; 		gyroY=0;
	mov [_gyroY],0
	mov [_gyroY+1],0
	mov [_gyroY+2],0
	mov [_gyroY+3],0
	.dbline 224
; 	}
L46:
	.dbline 225
; 	if (((gyroZ<(DEAD_ZONE)) && (gyroZ>(-DEAD_ZONE))) && (mode>5)) {
	mov A,[_gyroZ+3]
	sub A,0
	mov A,[_gyroZ+2]
	sbb A,6
	mov A,[_gyroZ+1]
	sbb A,0
	mov A,[_gyroZ]
	xor A,-128
	sbb A,(0 ^ 0x80)
	jnc L48
X24:
	mov A,0
	sub A,[_gyroZ+3]
	mov A,-6
	sbb A,[_gyroZ+2]
	mov A,-1
	sbb A,[_gyroZ+1]
	mov A,[_gyroZ]
	xor A,-128
	mov [__rX],A
	mov A,(255 ^ 0x80)
	sbb A,[__rX]
	jnc L48
X25:
	mov A,5
	cmp A,[_mode]
	jnc L48
X26:
	.dbline 225
	.dbline 226
; 		gyroZ=0;
	mov [_gyroZ],0
	mov [_gyroZ+1],0
	mov [_gyroZ+2],0
	mov [_gyroZ+3],0
	.dbline 227
; 	}
L48:
	.dbline 230
; 
; 	//Unless mode is OFF (on the desk), start in mode 6
; 	if (mode!=8) {mode = 6;}
	cmp [_mode],8
	jz L50
	.dbline 230
	.dbline 230
	mov [_mode],6
	.dbline 230
L50:
	.dbline 233
; 
;     //Determine if pointer should be in "STOP" mode
;     signY = 0;
	mov [_signY],0
	.dbline 234
;     signZ = 0;       
	mov [_signZ],0
	.dbline 235
; 	if (gyroY>0) {
	mov A,0
	sub A,[_gyroY+3]
	mov A,0
	sbb A,[_gyroY+2]
	mov A,0
	sbb A,[_gyroY+1]
	mov A,[_gyroY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L52
X27:
	.dbline 235
	.dbline 236
;         signY = 1;
	mov [_signY],1
	.dbline 237
; 	}
L52:
	.dbline 238
; 	if (gyroZ>0) {
	mov A,0
	sub A,[_gyroZ+3]
	mov A,0
	sbb A,[_gyroZ+2]
	mov A,0
	sbb A,[_gyroZ+1]
	mov A,[_gyroZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L54
X28:
	.dbline 238
	.dbline 239
;         signZ = 1;
	mov [_signZ],1
	.dbline 240
; 	}
L54:
	.dbline 241
; 	if (signY==lastSignY) {
	mov A,[_signY]
	cmp A,[_lastSignY]
	jnz L56
	.dbline 241
	.dbline 242
;         signYCount+=1;
	inc [_signYCount]
	.dbline 243
; 		if (signYCount==255) {
	cmp [_signYCount],-1
	jnz L57
	.dbline 243
	.dbline 244
; 			signYCount-=1;
	dec [_signYCount]
	.dbline 245
; 		}
	.dbline 246
	xjmp L57
L56:
	.dbline 246
; 	} else {
	.dbline 247
;         signYCount = 0;
	mov [_signYCount],0
	.dbline 248
; 	}
L57:
	.dbline 249
; 	if (signZ==lastSignZ) {
	mov A,[_signZ]
	cmp A,[_lastSignZ]
	jnz L60
	.dbline 249
	.dbline 250
;         signZCount+=1;
	inc [_signZCount]
	.dbline 251
; 		if (signZCount==255) {
	cmp [_signZCount],-1
	jnz L61
	.dbline 251
	.dbline 252
; 			signZCount-=1;
	dec [_signZCount]
	.dbline 253
; 		}
	.dbline 254
	xjmp L61
L60:
	.dbline 254
; 	} else {
	.dbline 255
;         signZCount = 0;
	mov [_signZCount],0
	.dbline 256
; 	}
L61:
	.dbline 257
;     lastSignY = signY;
	mov [_lastSignY],[_signY]
	.dbline 258
;     lastSignZ = signZ;
	mov [_lastSignZ],[_signZ]
	.dbline 260
; 
; 	if (mode!=8) {//Unless mode is "OFF" (on the desk)...
	cmp [_mode],8
	jz L64
	.dbline 260
	.dbline 261
; 		if ((signYCount<ENTER_STOP_MODE) && (signZCount<ENTER_STOP_MODE)) {//If sign changes are happening
	cmp [_signYCount],50
	jnc L66
X29:
	cmp [_signZCount],50
	jnc L66
X30:
	.dbline 261
	.dbline 263
; 			//at a high frequency, enter stop mode
; 			mode = 7;
	mov [_mode],7
	.dbline 264
; 		}
L66:
	.dbline 265
	mov A,64
	sub A,[_gyroTempY+3]
	mov A,31
	sbb A,[_gyroTempY+2]
	mov A,0
	sbb A,[_gyroTempY+1]
	mov A,[_gyroTempY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jc L70
X31:
	mov A,64
	sub A,[_gyroTempZ+3]
	mov A,31
	sbb A,[_gyroTempZ+2]
	mov A,0
	sbb A,[_gyroTempZ+1]
	mov A,[_gyroTempZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L68
X32:
L70:
	.dbline 265
; 		if ((gyroTempY>SPEED_5) || (gyroTempZ>SPEED_5)) {// Test for "MEDIUM" mode
	.dbline 266
; 			if (lastMode==7) {//Not allowed if coming straight from "stop mode"
	cmp [_lastMode],7
	jnz L71
	.dbline 266
	.dbline 267
; 				mode=6;
	mov [_mode],6
	.dbline 268
	xjmp L72
L71:
	.dbline 268
; 			} else {
	.dbline 269
; 				mode=5;
	mov [_mode],5
	.dbline 270
; 			}
L72:
	.dbline 271
; 		}
L68:
	.dbline 272
	mov A,10
	cmp A,[_signYCount]
	jc L75
X33:
	mov A,10
	cmp A,[_signZCount]
	jnc L73
X34:
L75:
	.dbline 272
; 		if ((signYCount>EXIT_STOP_MODE) || (signZCount>EXIT_STOP_MODE)) {// || (lastMode!=7)) {
	.dbline 273
; 			gyroTempY = gyroY;
	mov [_gyroTempY],[_gyroY]
	mov [_gyroTempY+1],[_gyroY+1]
	mov [_gyroTempY+2],[_gyroY+2]
	mov [_gyroTempY+3],[_gyroY+3]
	.dbline 274
; 			gyroTempZ = gyroZ;
	mov [_gyroTempZ],[_gyroZ]
	mov [_gyroTempZ+1],[_gyroZ+1]
	mov [_gyroTempZ+2],[_gyroZ+2]
	mov [_gyroTempZ+3],[_gyroZ+3]
	.dbline 275
; 			if (gyroTempY<0) {gyroTempY=-gyroTempY;}
	mov A,[_gyroTempY+3]
	sub A,0
	mov A,[_gyroTempY+2]
	sbb A,0
	mov A,[_gyroTempY+1]
	sbb A,0
	mov A,[_gyroTempY]
	xor A,-128
	sbb A,(0 ^ 0x80)
	jnc L76
X35:
	.dbline 275
	.dbline 275
	mov A,[_gyroTempY+3]
	cpl A
	sub A,-1
	mov [_gyroTempY+3],A
	mov A,[_gyroTempY+2]
	cpl A
	sbb A,-1
	mov [_gyroTempY+2],A
	mov A,[_gyroTempY+1]
	cpl A
	sbb A,-1
	mov [_gyroTempY+1],A
	mov A,[_gyroTempY]
	cpl A
	sbb A,-1
	mov [_gyroTempY],A
	.dbline 275
L76:
	.dbline 276
; 			if (gyroTempZ<0) {gyroTempZ=-gyroTempZ;}
	mov A,[_gyroTempZ+3]
	sub A,0
	mov A,[_gyroTempZ+2]
	sbb A,0
	mov A,[_gyroTempZ+1]
	sbb A,0
	mov A,[_gyroTempZ]
	xor A,-128
	sbb A,(0 ^ 0x80)
	jnc L78
X36:
	.dbline 276
	.dbline 276
	mov A,[_gyroTempZ+3]
	cpl A
	sub A,-1
	mov [_gyroTempZ+3],A
	mov A,[_gyroTempZ+2]
	cpl A
	sbb A,-1
	mov [_gyroTempZ+2],A
	mov A,[_gyroTempZ+1]
	cpl A
	sbb A,-1
	mov [_gyroTempZ+1],A
	mov A,[_gyroTempZ]
	cpl A
	sbb A,-1
	mov [_gyroTempZ],A
	.dbline 276
L78:
	.dbline 277
	mov A,96
	sub A,[_gyroTempY+3]
	mov A,109
	sbb A,[_gyroTempY+2]
	mov A,0
	sbb A,[_gyroTempY+1]
	mov A,[_gyroTempY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jc L82
X37:
	mov A,96
	sub A,[_gyroTempZ+3]
	mov A,109
	sbb A,[_gyroTempZ+2]
	mov A,0
	sbb A,[_gyroTempZ+1]
	mov A,[_gyroTempZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L80
X38:
L82:
	.dbline 277
; 			if ((gyroTempY>SPEED_0) || (gyroTempZ>SPEED_0)) {
	.dbline 278
; 				mode=0;
	mov [_mode],0
	.dbline 279
	xjmp L81
L80:
	.dbline 279
	mov A,-64
	sub A,[_gyroTempY+3]
	mov A,93
	sbb A,[_gyroTempY+2]
	mov A,0
	sbb A,[_gyroTempY+1]
	mov A,[_gyroTempY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jc L85
X39:
	mov A,-64
	sub A,[_gyroTempZ+3]
	mov A,93
	sbb A,[_gyroTempZ+2]
	mov A,0
	sbb A,[_gyroTempZ+1]
	mov A,[_gyroTempZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L83
X40:
L85:
	.dbline 279
; 			} else if ((gyroTempY>SPEED_1) || (gyroTempZ>SPEED_1)) {
	.dbline 280
; 				mode=1;
	mov [_mode],1
	.dbline 281
	xjmp L84
L83:
	.dbline 281
	mov A,32
	sub A,[_gyroTempY+3]
	mov A,78
	sbb A,[_gyroTempY+2]
	mov A,0
	sbb A,[_gyroTempY+1]
	mov A,[_gyroTempY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jc L88
X41:
	mov A,32
	sub A,[_gyroTempZ+3]
	mov A,78
	sbb A,[_gyroTempZ+2]
	mov A,0
	sbb A,[_gyroTempZ+1]
	mov A,[_gyroTempZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L86
X42:
L88:
	.dbline 281
; 			} else if ((gyroTempY>SPEED_2) || (gyroTempZ>SPEED_2)) {
	.dbline 282
; 				mode=2;
	mov [_mode],2
	.dbline 283
	xjmp L87
L86:
	.dbline 283
	mov A,-128
	sub A,[_gyroTempY+3]
	mov A,62
	sbb A,[_gyroTempY+2]
	mov A,0
	sbb A,[_gyroTempY+1]
	mov A,[_gyroTempY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jc L91
X43:
	mov A,-128
	sub A,[_gyroTempZ+3]
	mov A,62
	sbb A,[_gyroTempZ+2]
	mov A,0
	sbb A,[_gyroTempZ+1]
	mov A,[_gyroTempZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L89
X44:
L91:
	.dbline 283
; 			} else if ((gyroTempY>SPEED_3) || (gyroTempZ>SPEED_3)) {
	.dbline 284
; 				mode=3;
	mov [_mode],3
	.dbline 285
	xjmp L90
L89:
	.dbline 285
	mov A,-32
	sub A,[_gyroTempY+3]
	mov A,46
	sbb A,[_gyroTempY+2]
	mov A,0
	sbb A,[_gyroTempY+1]
	mov A,[_gyroTempY]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jc L94
X45:
	mov A,-32
	sub A,[_gyroTempZ+3]
	mov A,46
	sbb A,[_gyroTempZ+2]
	mov A,0
	sbb A,[_gyroTempZ+1]
	mov A,[_gyroTempZ]
	xor A,-128
	mov [__rX],A
	mov A,(0 ^ 0x80)
	sbb A,[__rX]
	jnc L92
X46:
L94:
	.dbline 285
; 			} else if ((gyroTempY>SPEED_4) || (gyroTempZ>SPEED_4)) {
	.dbline 286
; 				mode=4;
	mov [_mode],4
	.dbline 287
; 			} 
L92:
L90:
L87:
L84:
L81:
	.dbline 288
; 		}
L73:
	.dbline 289
; 		lastMode = mode;
	mov [_lastMode],[_mode]
	.dbline 290
; 	}
L64:
	.dbline 292
; 
; 	if (mode==0) {
	cmp [_mode],0
	jnz L95
	.dbline 292
	.dbline 298
; 
; 		#ifdef DEBUG_INFO
; 		printf("SPEED 0\n");
; 		#endif
; 
;         gyroY=gyroY-(gyroY>>2);