mousealgorithm.lis

二轴陀螺仪IDG300源程序

 012C           ;               gyroYMax = gyroYRaw;
 012C 5F0101            mov [_gyroYMax+1],[_gyroYRaw+1]
 012F 5F0000            mov [_gyroYMax],[_gyroYRaw]
 0132                   .dbline 140
 0132           ;       }
 0132           L24:
 0132                   .dbline 142
 0132           ; 
 0132           ;       gyroZBiasAv+=(signed long)gyroZRaw;
 0132 5F0001            mov [__r3],[_gyroZRaw+1]
 0135 5F0000            mov [__r2],[_gyroZRaw]
 0138 5100              mov A,[__r3]
 013A 0403              add [_gyroZBiasAv+3],A
 013C 5100              mov A,[__r2]
 013E 0C02              adc [_gyroZBiasAv+2],A
 0140 0E0100            adc [_gyroZBiasAv+1],0
 0143 0E0000            adc [_gyroZBiasAv],0
 0146                   .dbline 143
 0146           ;       if (gyroZRaw<gyroZMin) {
 0146 5101              mov A,[_gyroZRaw+1]
 0148 1201              sub A,[_gyroZMin+1]
 014A 5100              mov A,[_gyroZRaw]
 014C 1A00              sbb A,[_gyroZMin]
 014E D007              jnc L26
 0150           X12:
 0150                   .dbline 143
 0150                   .dbline 144
 0150           ;               gyroZMin = gyroZRaw;
 0150 5F0101            mov [_gyroZMin+1],[_gyroZRaw+1]
 0153 5F0000            mov [_gyroZMin],[_gyroZRaw]
 0156                   .dbline 145
 0156           ;       }
 0156           L26:
 0156                   .dbline 146
 0156           ;       if (gyroZRaw>gyroZMax) {
 0156 5101              mov A,[_gyroZMax+1]
 0158 1201              sub A,[_gyroZRaw+1]
 015A 5100              mov A,[_gyroZMax]
 015C 1A00              sbb A,[_gyroZRaw]
 015E D007              jnc L28
 0160           X13:
 0160                   .dbline 146
 0160                   .dbline 147
 0160           ;               gyroZMax = gyroZRaw;
 0160 5F0101            mov [_gyroZMax+1],[_gyroZRaw+1]
 0163 5F0000            mov [_gyroZMax],[_gyroZRaw]
 0166                   .dbline 148
 0166           ;       }
 0166           L28:
 0166                   .dbline 150
 0166           ; 
 0166           ;       biasCounter+=1;
 0166 7601              inc [_biasCounter+1]
 0168 0E0000            adc [_biasCounter],0
 016B                   .dbline 151
 016B           ;       if (biasCounter>511) {
 016B 50FF              mov A,-1
 016D 1201              sub A,[_biasCounter+1]
 016F 5001              mov A,1
 0171 1A00              sbb A,[_biasCounter]
 0173 D007              jnc L30
 0175           X14:
 0175                   .dbline 151
 0175                   .dbline 152
 0175           ;               biasCounter = 0;
 0175 550100            mov [_biasCounter+1],0
 0178 550000            mov [_biasCounter],0
 017B                   .dbline 153
 017B           ;       }
 017B           L30:
 017B                   .dbline 154
 017B           ;       if (biasCounter==0) {
 017B 3C0000            cmp [_biasCounter],0
 017E B0C0              jnz L32
 0180 3C0100            cmp [_biasCounter+1],0
 0183 B0BB              jnz L32
 0185           X15:
 0185                   .dbline 154
 0185                   .dbline 160
 0185           ;               
 0185           ;               #ifdef DEBUG_INFO
 0185           ;               printf("RESETTING BIAS\n");
 0185           ;               #endif
 0185           ; 
 0185           ;               mode = 8;
 0185 550008            mov [_mode],8
 0188                   .dbline 161
 0188           ;               gyroYBias = gyroYBiasAv>>1;
 0188 5F0000            mov [__r0],[_gyroYBiasAv]
 018B 5F0001            mov [__r1],[_gyroYBiasAv+1]
 018E 5F0002            mov [__r2],[_gyroYBiasAv+2]
 0191 5F0003            mov [__r3],[_gyroYBiasAv+3]
 0194 6800              asr [__r0]
 0196 6E00              rrc [__r1]
 0198 6E00              rrc [__r2]
 019A 6E00              rrc [__r3]
 019C 5F0000            mov [_gyroYBias],[__r0]
 019F 5F0100            mov [_gyroYBias+1],[__r1]
 01A2 5F0200            mov [_gyroYBias+2],[__r2]
 01A5 5F0300            mov [_gyroYBias+3],[__r3]
 01A8                   .dbline 162
 01A8           ;               gyroZBias = gyroZBiasAv>>1;
 01A8 5F0000            mov [__r0],[_gyroZBiasAv]
 01AB 5F0001            mov [__r1],[_gyroZBiasAv+1]
 01AE 5F0002            mov [__r2],[_gyroZBiasAv+2]
 01B1 5F0003            mov [__r3],[_gyroZBiasAv+3]
 01B4 6800              asr [__r0]
 01B6 6E00              rrc [__r1]
 01B8 6E00              rrc [__r2]
 01BA 6E00              rrc [__r3]
 01BC 5F0000            mov [_gyroZBias],[__r0]
 01BF 5F0100            mov [_gyroZBias+1],[__r1]
 01C2 5F0200            mov [_gyroZBias+2],[__r2]
 01C5 5F0300            mov [_gyroZBias+3],[__r3]
 01C8                   .dbline 163
 01C8           ;               tempBias = tempBiasAv>>1;
 01C8 5F0000            mov [__r0],[_tempBiasAv]
 01CB 5F0001            mov [__r1],[_tempBiasAv+1]
 01CE 5F0002            mov [__r2],[_tempBiasAv+2]
 01D1 5F0003            mov [__r3],[_tempBiasAv+3]
 01D4 6800              asr [__r0]
 01D6 6E00              rrc [__r1]
 01D8 6E00              rrc [__r2]
 01DA 6E00              rrc [__r3]
 01DC 5F0000            mov [_tempBias],[__r0]
 01DF 5F0100            mov [_tempBias+1],[__r1]
 01E2 5F0200            mov [_tempBias+2],[__r2]
 01E5 5F0300            mov [_tempBias+3],[__r3]
 01E8                   .dbline 164
 01E8           ;               calibrated = 1;
 01E8 550001            mov [_calibrated],1
 01EB                   .dbline 166
 01EB           ; 
 01EB           ;               gyroYBiasAv = 0;
 01EB 550000            mov [_gyroYBiasAv],0
 01EE 550100            mov [_gyroYBiasAv+1],0
 01F1 550200            mov [_gyroYBiasAv+2],0
 01F4 550300            mov [_gyroYBiasAv+3],0
 01F7                   .dbline 167
 01F7           ;               gyroZBiasAv = 0;
 01F7 550000            mov [_gyroZBiasAv],0
 01FA 550100            mov [_gyroZBiasAv+1],0
 01FD 550200            mov [_gyroZBiasAv+2],0
 0200 550300            mov [_gyroZBiasAv+3],0
 0203                   .dbline 168
 0203           ;               tempBiasAv = 0;
 0203 550000            mov [_tempBiasAv],0
 0206 550100            mov [_tempBiasAv+1],0
 0209 550200            mov [_tempBiasAv+2],0
 020C 550300            mov [_tempBiasAv+3],0
 020F                   .dbline 170
 020F           ;               
 020F           ;               gyroYMin = gyroYRaw;
 020F 5F0101            mov [_gyroYMin+1],[_gyroYRaw+1]
 0212 5F0000            mov [_gyroYMin],[_gyroYRaw]
 0215                   .dbline 171
 0215           ;               gyroYMax = gyroYRaw;
 0215 5F0101            mov [_gyroYMax+1],[_gyroYRaw+1]
 0218 5F0000            mov [_gyroYMax],[_gyroYRaw]
 021B                   .dbline 172
 021B           ;               gyroZMin = gyroZRaw;
 021B 5F0101            mov [_gyroZMin+1],[_gyroZRaw+1]
 021E 5F0000            mov [_gyroZMin],[_gyroZRaw]
 0221                   .dbline 173
 0221           ;               gyroZMax = gyroZRaw;            
 0221 5F0101            mov [_gyroZMax+1],[_gyroZRaw+1]
 0224 5F0000            mov [_gyroZMax],[_gyroZRaw]
 0227                   .dbline 174
 0227           ;               gyroYInt = 0;
 0227 550000            mov [_gyroYInt],0
 022A 550100            mov [_gyroYInt+1],0
 022D 550200            mov [_gyroYInt+2],0
 0230 550300            mov [_gyroYInt+3],0
 0233                   .dbline 175
 0233           ;               gyroZInt = 0;
 0233 550000            mov [_gyroZInt],0
 0236 550100            mov [_gyroZInt+1],0
 0239 550200            mov [_gyroZInt+2],0
 023C 550300            mov [_gyroZInt+3],0
 023F                   .dbline 176
 023F           ;       } 
 023F           L32:
 023F                   .dbline 178
 023F           ;       
 023F           ;       motionDetected = 0;
 023F 550000            mov [_motionDetected],0
 0242                   .dbline 180
 0242           ; 
 0242           ;       motionTemp = gyroYMax-gyroYMin;
 0242 5101              mov A,[_gyroYMax+1]
 0244 1201              sub A,[_gyroYMin+1]
 0246 5301              mov [_motionTemp+1],A
 0248 5100              mov A,[_gyroYMax]
 024A 1A00              sbb A,[_gyroYMin]
 024C 5300              mov [_motionTemp],A
 024E                   .dbline 181
 024E           ;       if (motionTemp<0) {
 024E 5000              mov A,0
 0250 A017              jz L34
 0252                   .dbline 181
 0252                   .dbline 182
 0252           ;               motionTemp = -motionTemp;
 0252 5101              mov A,[_motionTemp+1]
 0254 73                cpl A
 0255 5300              mov [__r1],A
 0257 5100              mov A,[_motionTemp]
 0259 73                cpl A
 025A 5300              mov [__r0],A
 025C 5100              mov A,[__r1]
 025E 0101              add A,1
 0260 5301              mov [_motionTemp+1],A
 0262 5100              mov A,[__r0]
 0264 0900              adc A,0
 0266 5300              mov [_motionTemp],A
 0268                   .dbline 183
 0268           ;       }
 0268           L34:
 0268                   .dbline 184
 0268           ;       if (motionTemp>NO_MOTION) {
 0268 5003              mov A,3
 026A 1201              sub A,[_motionTemp+1]
 026C 5000              mov A,0
 026E 1A00              sbb A,[_motionTemp]
 0270 D004              jnc L36
 0272           X16:
 0272                   .dbline 184
 0272                   .dbline 185
 0272           ;               motionDetected = 1;
 0272 550001            mov [_motionDetected],1
 0275                   .dbline 186
 0275           ;       }
 0275           L36:
 0275                   .dbline 187
 0275           ;       motionTemp = gyroZMax-gyroZMin;
 0275 5101              mov A,[_gyroZMax+1]
 0277 1201              sub A,[_gyroZMin+1]
 0279 5301              mov [_motionTemp+1],A
 027B 5100              mov A,[_gyroZMax]
 027D 1A00              sbb A,[_gyroZMin]
 027F 5300              mov [_motionTemp],A
 0281                   .dbline 188
 0281           ;       if (motionTemp<0) {
 0281 5000              mov A,0
 0283 A017              jz L38
 0285                   .dbline 188
 0285                   .dbline 189
 0285           ;               motionTemp = -motionTemp;
 0285 5101              mov A,[_motionTemp+1]
 0287 73                cpl A
 0288 5300              mov [__r1],A
 028A 5100              mov A,[_motionTemp]
 028C 73                cpl A
 028D 5300              mov [__r0],A
 028F 5100              mov A,[__r1]
 0291 0101              add A,1
 0293 5301              mov [_motionTemp+1],A
 0295 5100              mov A,[__r0]
 0297 0900              adc A,0
 0299 5300              mov [_motionTemp],A
 029B                   .dbline 190
 029B           ;       }
 029B           L38:
 029B                   .dbline 191
 029B           ;       if (motionTemp>NO_MOTION) {
 029B 5003              mov A,3
 029D 1201              sub A,[_motionTemp+1]
 029F 5000              mov A,0
 02A1 1A00              sbb A,[_motionTemp]
 02A3 D004              jnc L40
 02A5           X17:
 02A5                   .dbline 191
 02A5                   .dbline 192
 02A5           ;               motionDetected = 1;
 02A5 550001            mov [_motionDetected],1
 02A8                   .dbline 193
 02A8           ;       }
 02A8           L40:
 02A8                   .dbline 194
 02A8           ;       if (motionDetected) { 
 02A8 3C0000            cmp [_motionDetected],0
 02AB A046              jz L42
 02AD                   .dbline 194
 02AD                   .dbline 195
 02AD           ;               mode=6;
 02AD 550006            mov [_mode],6
 02B0                   .dbline 196
 02B0           ;               biasCounter = 0;
 02B0 550100            mov [_biasCounter+1],0
 02B3 550000            mov [_biasCounter],0
 02B6                   .dbline 197
 02B6           ;               gyroYBiasAv = 0;
 02B6 550000            mov [_gyroYBiasAv],0
 02B9 550100            mov [_gyroYBiasAv+1],0
 02BC 550200            mov [_gyroYBiasAv+2],0
 02BF 550300            mov [_gyroYBiasAv+3],0
 02C2                   .dbline 198
 02C2           ;               gyroZBiasAv = 0;
 02C2 550000            mov [_gyroZBiasAv],0
 02C5 550100            mov [_gyroZBiasAv+1],0
 02C8 550200            mov [_gyroZBiasAv+2],0
 02CB 550300            mov [_gyroZBiasAv+3],0
 02CE                   .dbline 199
 02CE           ;               tempBiasAv = 0;
 02CE 550000            mov [_tempBiasAv],0
 02D1 550100            mov [_tempBiasAv+1],0
 02D4 550200            mov [_tempBiasAv+2],0
 02D7 550300            mov [_tempBiasAv+3],0
 02DA                   .dbline 200
 02DA           ;               gyroYMin = gyroYRaw;
 02DA 5F0101            mov [_gyroYMin+1],[_gyroYRaw+1]
 02DD 5F0000            mov [_gyroYMin],[_gyroYRaw]
 02E0                   .dbline 201
 02E0           ;               gyroYMax = gyroYRaw;
 02E0 5F0101            mov [_gyroYMax+1],[_gyroYRaw+1]
 02E3 5F0000            mov [_gyroYMax],[_gyroYRaw]
 02E6                   .dbline 202
 02E6           ;               gyroZMin = gyroZRaw;            
 02E6 5F0101            mov [_gyroZMin+1],[_gyroZRaw+1]
 02E9 5F0000            mov [_gyroZMin],[_gyroZRaw]
 02EC                   .dbline 203
 02EC           ;               gyroZMax = gyroZRaw;