mousealgorithm.lis

二轴陀螺仪IDG300源程序

 0002           ; //Buffered raw gyro data for median filter
 0002           ; //Used for filtering out button clicks
 0002           ; unsigned short gyroYRaw1=0;
 0002           ; unsigned short gyroZRaw1=0;
 0002           ; unsigned short gyroYRaw2=0;
 0002           ; unsigned short gyroZRaw2=0;
 0002           ; unsigned short gyroYRaw3=0;
 0002           ; unsigned short gyroZRaw3=0;
 0002           ; 
 0002           ; //Temporary storage for computing "speed" metric
 0002           ; signed long gyroTempY = 0;
 0002           ; signed long gyroTempZ = 0;
 0002           ; 
 0002           ; //Bias calculation variables
 0002           ; signed long gyroZBiasAv = 0;
 0002           ; signed long gyroYBiasAv = 0;
 0002           ; signed long tempBiasAv = 0;
 0002           ; unsigned short gyroZMin=0;
 0002           ; unsigned short gyroZMax=0;
 0002           ; unsigned short gyroYMin=0;
 0002           ; unsigned short gyroYMax=0;
 0002           ; signed long gyroYBias = 2048;
 0002           ; signed long gyroZBias = 2048;
 0002           ; signed long tempBias = 2048;
 0002           ; unsigned short motionTemp;
 0002           ; unsigned short biasCounter = 0;
 0002           ; unsigned char motionDetected = 0;
 0002           ; 
 0002           ; //Variables for detecting "STOP" mode
 0002           ; unsigned char lastSignY = 0;
 0002           ; unsigned char lastSignZ = 0;
 0002           ; unsigned char signYCount = 0;
 0002           ; unsigned char signZCount = 0;
 0002           ; unsigned char signY = 0;
 0002           ; unsigned char signZ = 0;
 0002           ; 
 0002           ; //Processed gyro data
 0002           ; signed long gyroY = 0;
 0002           ; signed long gyroZ = 0;
 0002           ; signed long temp = 0;
 0002           ; 
 0002           ; //Integrated gyro data
 0002           ; signed long gyroYInt = 0;
 0002           ; signed long gyroZInt = 0;
 0002           ; 
 0002           ; //Button information
 0002           ; //Used for freezing gyro data temporarily when button is pressed or released
 0002           ; unsigned char lastButton = 48;
 0002           ; unsigned char buttonCounter = 0;
 0002           ; 
 0002           ; //Initial Calibration
 0002           ; unsigned char calibrated = 0;
 0002           ; 
 0002           ; 
 0002           ; void updateMouse(unsigned short *tempData, unsigned short *gyroYData, unsigned short *gyroZData, signed short *pixelYShift, signed short *pixelZShift, unsigned char *button){
 0002                   .dbline 90
 0002           ; 
 0002           ;       //Compute median filter to filter out button clicks
 0002           ;       gyroYRaw3 = gyroYRaw2;
 0002 5F0101            mov [_gyroYRaw3+1],[_gyroYRaw2+1]
 0005 5F0000            mov [_gyroYRaw3],[_gyroYRaw2]
 0008                   .dbline 91
 0008           ;       gyroZRaw3 = gyroZRaw2;
 0008 5F0101            mov [_gyroZRaw3+1],[_gyroZRaw2+1]
 000B 5F0000            mov [_gyroZRaw3],[_gyroZRaw2]
 000E                   .dbline 93
 000E           ; 
 000E           ;       gyroYRaw2 = gyroYRaw1;
 000E 5F0101            mov [_gyroYRaw2+1],[_gyroYRaw1+1]
 0011 5F0000            mov [_gyroYRaw2],[_gyroYRaw1]
 0014                   .dbline 94
 0014           ;       gyroZRaw2 = gyroZRaw1;
 0014 5F0101            mov [_gyroZRaw2+1],[_gyroZRaw1+1]
 0017 5F0000            mov [_gyroZRaw2],[_gyroZRaw1]
 001A                   .dbline 96
 001A           ; 
 001A           ;       gyroYRaw = *gyroYData;
 001A 52FA              mov A,[X-6]
 001C 5300              mov [__r1],A
 001E 3E00              mvi A,[__r1]
 0020 5300              mov [_gyroYRaw],A
 0022 3E00              mvi A,[__r1]
 0024 5301              mov [_gyroYRaw+1],A
 0026                   .dbline 97
 0026           ;       gyroZRaw = *gyroZData;
 0026 52F8              mov A,[X-8]
 0028 5300              mov [__r1],A
 002A 3E00              mvi A,[__r1]
 002C 5300              mov [_gyroZRaw],A
 002E 3E00              mvi A,[__r1]
 0030 5301              mov [_gyroZRaw+1],A
 0032                   .dbline 98
 0032           ;       tempRaw = *tempData;
 0032 52FC              mov A,[X-4]
 0034 5300              mov [__r1],A
 0036 3E00              mvi A,[__r1]
 0038 5300              mov [_tempRaw],A
 003A 3E00              mvi A,[__r1]
 003C 5301              mov [_tempRaw+1],A
 003E                   .dbline 100
 003E           ; 
 003E           ;       gyroYRaw1 = gyroYRaw;
 003E 5F0101            mov [_gyroYRaw1+1],[_gyroYRaw+1]
 0041 5F0000            mov [_gyroYRaw1],[_gyroYRaw]
 0044                   .dbline 101
 0044           ;       gyroZRaw1 = gyroZRaw;
 0044 5F0101            mov [_gyroZRaw1+1],[_gyroZRaw+1]
 0047 5F0000            mov [_gyroZRaw1],[_gyroZRaw]
 004A                   .dbline 103
 004A           ; 
 004A           ;       if (gyroYRaw3>gyroYRaw2) {
 004A 5101              mov A,[_gyroYRaw2+1]
 004C 1201              sub A,[_gyroYRaw3+1]
 004E 5100              mov A,[_gyroYRaw2]
 0050 1A00              sbb A,[_gyroYRaw3]
 0052 D025              jnc L2
 0054           X0:
 0054                   .dbline 103
 0054                   .dbline 104
 0054           ;               if (gyroYRaw2>gyroYRaw) {
 0054 5101              mov A,[_gyroYRaw+1]
 0056 1201              sub A,[_gyroYRaw2+1]
 0058 5100              mov A,[_gyroYRaw]
 005A 1A00              sbb A,[_gyroYRaw2]
 005C D009              jnc L4
 005E           X1:
 005E                   .dbline 104
 005E                   .dbline 105
 005E           ;                       gyroYRaw=gyroYRaw2;
 005E 5F0101            mov [_gyroYRaw+1],[_gyroYRaw2+1]
 0061 5F0000            mov [_gyroYRaw],[_gyroYRaw2]
 0064                   .dbline 106
 0064 8035              xjmp L3
 0066           L4:
 0066                   .dbline 106
 0066           ;               } else if (gyroYRaw3<gyroYRaw) {
 0066 5101              mov A,[_gyroYRaw3+1]
 0068 1201              sub A,[_gyroYRaw+1]
 006A 5100              mov A,[_gyroYRaw3]
 006C 1A00              sbb A,[_gyroYRaw]
 006E D02B              jnc L3
 0070           X2:
 0070                   .dbline 106
 0070                   .dbline 107
 0070           ;                       gyroYRaw=gyroYRaw3;
 0070 5F0101            mov [_gyroYRaw+1],[_gyroYRaw3+1]
 0073 5F0000            mov [_gyroYRaw],[_gyroYRaw3]
 0076                   .dbline 108
 0076           ;               }
 0076                   .dbline 109
 0076 8023              xjmp L3
 0078           L2:
 0078                   .dbline 109
 0078           ;       } else {
 0078                   .dbline 110
 0078           ;               if (gyroYRaw2<gyroYRaw) {
 0078 5101              mov A,[_gyroYRaw2+1]
 007A 1201              sub A,[_gyroYRaw+1]
 007C 5100              mov A,[_gyroYRaw2]
 007E 1A00              sbb A,[_gyroYRaw]
 0080 D009              jnc L8
 0082           X3:
 0082                   .dbline 110
 0082                   .dbline 111
 0082           ;                       gyroYRaw=gyroYRaw2;
 0082 5F0101            mov [_gyroYRaw+1],[_gyroYRaw2+1]
 0085 5F0000            mov [_gyroYRaw],[_gyroYRaw2]
 0088                   .dbline 112
 0088 8011              xjmp L9
 008A           L8:
 008A                   .dbline 112
 008A           ;               } else if (gyroYRaw3>gyroYRaw) {
 008A 5101              mov A,[_gyroYRaw+1]
 008C 1201              sub A,[_gyroYRaw3+1]
 008E 5100              mov A,[_gyroYRaw]
 0090 1A00              sbb A,[_gyroYRaw3]
 0092 D007              jnc L10
 0094           X4:
 0094                   .dbline 112
 0094                   .dbline 113
 0094           ;                       gyroYRaw=gyroYRaw3;
 0094 5F0101            mov [_gyroYRaw+1],[_gyroYRaw3+1]
 0097 5F0000            mov [_gyroYRaw],[_gyroYRaw3]
 009A                   .dbline 114
 009A           ;               }
 009A           L10:
 009A           L9:
 009A                   .dbline 115
 009A           ;       }
 009A           L3:
 009A                   .dbline 117
 009A           ; 
 009A           ;       if (gyroZRaw3>gyroZRaw2) {
 009A 5101              mov A,[_gyroZRaw2+1]
 009C 1201              sub A,[_gyroZRaw3+1]
 009E 5100              mov A,[_gyroZRaw2]
 00A0 1A00              sbb A,[_gyroZRaw3]
 00A2 D025              jnc L12
 00A4           X5:
 00A4                   .dbline 117
 00A4                   .dbline 118
 00A4           ;               if (gyroZRaw2>gyroZRaw) {
 00A4 5101              mov A,[_gyroZRaw+1]
 00A6 1201              sub A,[_gyroZRaw2+1]
 00A8 5100              mov A,[_gyroZRaw]
 00AA 1A00              sbb A,[_gyroZRaw2]
 00AC D009              jnc L14
 00AE           X6:
 00AE                   .dbline 118
 00AE                   .dbline 119
 00AE           ;                       gyroZRaw=gyroZRaw2;
 00AE 5F0101            mov [_gyroZRaw+1],[_gyroZRaw2+1]
 00B1 5F0000            mov [_gyroZRaw],[_gyroZRaw2]
 00B4                   .dbline 120
 00B4 8035              xjmp L13
 00B6           L14:
 00B6                   .dbline 120
 00B6           ;               } else if (gyroZRaw3<gyroZRaw) {
 00B6 5101              mov A,[_gyroZRaw3+1]
 00B8 1201              sub A,[_gyroZRaw+1]
 00BA 5100              mov A,[_gyroZRaw3]
 00BC 1A00              sbb A,[_gyroZRaw]
 00BE D02B              jnc L13
 00C0           X7:
 00C0                   .dbline 120
 00C0                   .dbline 121
 00C0           ;                       gyroZRaw=gyroZRaw3;
 00C0 5F0101            mov [_gyroZRaw+1],[_gyroZRaw3+1]
 00C3 5F0000            mov [_gyroZRaw],[_gyroZRaw3]
 00C6                   .dbline 122
 00C6           ;               }
 00C6                   .dbline 123
 00C6 8023              xjmp L13
 00C8           L12:
 00C8                   .dbline 123
 00C8           ;       } else {
 00C8                   .dbline 124
 00C8           ;               if (gyroZRaw2<gyroZRaw) {
 00C8 5101              mov A,[_gyroZRaw2+1]
 00CA 1201              sub A,[_gyroZRaw+1]
 00CC 5100              mov A,[_gyroZRaw2]
 00CE 1A00              sbb A,[_gyroZRaw]
 00D0 D009              jnc L18
 00D2           X8:
 00D2                   .dbline 124
 00D2                   .dbline 125
 00D2           ;                       gyroZRaw=gyroZRaw2;
 00D2 5F0101            mov [_gyroZRaw+1],[_gyroZRaw2+1]
 00D5 5F0000            mov [_gyroZRaw],[_gyroZRaw2]
 00D8                   .dbline 126
 00D8 8011              xjmp L19
 00DA           L18:
 00DA                   .dbline 126
 00DA           ;               } else if (gyroZRaw3>gyroZRaw) {
 00DA 5101              mov A,[_gyroZRaw+1]
 00DC 1201              sub A,[_gyroZRaw3+1]
 00DE 5100              mov A,[_gyroZRaw]
 00E0 1A00              sbb A,[_gyroZRaw3]
 00E2 D007              jnc L20
 00E4           X9:
 00E4                   .dbline 126
 00E4                   .dbline 127
 00E4           ;                       gyroZRaw=gyroZRaw3;
 00E4 5F0101            mov [_gyroZRaw+1],[_gyroZRaw3+1]
 00E7 5F0000            mov [_gyroZRaw],[_gyroZRaw3]
 00EA                   .dbline 128
 00EA           ;               }
 00EA           L20:
 00EA           L19:
 00EA                   .dbline 129
 00EA           ;       }
 00EA           L13:
 00EA                   .dbline 132
 00EA           ; 
 00EA           ;       //Bias Calibration
 00EA           ;       gyroYBiasAv+=(signed long)gyroYRaw;
 00EA 5F0001            mov [__r3],[_gyroYRaw+1]
 00ED 5F0000            mov [__r2],[_gyroYRaw]
 00F0 5100              mov A,[__r3]
 00F2 0403              add [_gyroYBiasAv+3],A
 00F4 5100              mov A,[__r2]
 00F6 0C02              adc [_gyroYBiasAv+2],A
 00F8 0E0100            adc [_gyroYBiasAv+1],0
 00FB 0E0000            adc [_gyroYBiasAv],0
 00FE                   .dbline 133
 00FE           ;       tempBiasAv+=(signed long)tempRaw;
 00FE 5F0001            mov [__r3],[_tempRaw+1]
 0101 5F0000            mov [__r2],[_tempRaw]
 0104 5100              mov A,[__r3]
 0106 0403              add [_tempBiasAv+3],A
 0108 5100              mov A,[__r2]
 010A 0C02              adc [_tempBiasAv+2],A
 010C 0E0100            adc [_tempBiasAv+1],0
 010F 0E0000            adc [_tempBiasAv],0
 0112                   .dbline 135
 0112           ; 
 0112           ;       if (gyroYRaw<gyroYMin) {
 0112 5101              mov A,[_gyroYRaw+1]
 0114 1201              sub A,[_gyroYMin+1]
 0116 5100              mov A,[_gyroYRaw]
 0118 1A00              sbb A,[_gyroYMin]
 011A D007              jnc L22
 011C           X10:
 011C                   .dbline 135
 011C                   .dbline 136
 011C           ;               gyroYMin = gyroYRaw;
 011C 5F0101            mov [_gyroYMin+1],[_gyroYRaw+1]
 011F 5F0000            mov [_gyroYMin],[_gyroYRaw]
 0122                   .dbline 137
 0122           ;       }
 0122           L22:
 0122                   .dbline 138
 0122           ;       if (gyroYRaw>gyroYMax) {
 0122 5101              mov A,[_gyroYMax+1]
 0124 1201              sub A,[_gyroYRaw+1]
 0126 5100              mov A,[_gyroYMax]
 0128 1A00              sbb A,[_gyroYRaw]
 012A D007              jnc L24
 012C           X11:
 012C                   .dbline 138
 012C                   .dbline 139