?? rd_usedlocate_lib.s
字號:
sts _g_fLastAngle,R2
sts _g_fLastAngle+2+1,R5
sts _g_fLastAngle+2,R4
.dbline 182
; }
.dbline 184
; //計算位置微元
; {
.dbline 185
; INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
lds R2,L10
lds R3,L10+1
lds R10,_g_lCounterLImage
lds R11,_g_lCounterLImage+1
sub R10,R2
sbc R11,R3
.dbline 186
; float fR = (((float)nDeltaCounterL * (float)K_L) * (1.0 / fDeltaAngle)
ldi R16,<L4
ldi R17,>L4
rcall lpm32
movw R2,R16
movw R4,R18
movw R16,R10
rcall int2fp
st -y,R19
st -y,R18
st -y,R17
st -y,R16
movw R16,R2
movw R18,R4
rcall fpmule2
movw R2,R16
movw R4,R18
ldi R16,<L15
ldi R17,>L15
rcall lpm32
movw R24,R28
adiw R24,8
st -y,R25
st -y,R24
rcall fpdiv1x
movw R16,R2
movw R18,R4
rcall fpmule2
movw R2,R16
movw R4,R18
ldi R16,<L16
ldi R17,>L16
rcall lpm32
st -y,R19
st -y,R18
st -y,R17
st -y,R16
movw R16,R2
movw R18,R4
rcall fpadd2
std y+16,R16
std y+17,R17
std y+18,R18
std y+19,R19
.dbline 189
; + (float)D_BTW_WHEEL * 0.5);
;
; fR = 2.0 * fR * sin(fDeltaAngle * 0.5); //fR->l
ldd R16,y+4
ldd R17,y+5
ldd R18,y+6
ldd R19,y+7
rcall _sinf
movw R2,R16
movw R4,R18
ldi R16,<L17
ldi R17,>L17
rcall lpm32
movw R24,R28
adiw R24,16
st -y,R25
st -y,R24
rcall fpmule1
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall fpmule2
std y+20,R16
std y+21,R17
std y+22,R18
std y+23,R19
.dbline 191
;
; g_fX += fR * cos(fAbsoluteAngle);
ldd R16,y+12
ldd R17,y+13
ldd R18,y+14
ldd R19,y+15
rcall _cosf
movw R2,R16
movw R4,R18
lds R8,_g_fX+2
lds R9,_g_fX+2+1
lds R6,_g_fX
lds R7,_g_fX+1
ldd R16,y+20
ldd R17,y+21
ldd R18,y+22
ldd R19,y+23
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall fpmule2x
movw R16,R6
movw R18,R8
rcall fpadd2
sts _g_fX+1,R17
sts _g_fX,R16
sts _g_fX+2+1,R19
sts _g_fX+2,R18
.dbline 192
; g_fY += fR * sin(fAbsoluteAngle);
ldd R16,y+12
ldd R17,y+13
ldd R18,y+14
ldd R19,y+15
rcall _sinf
movw R2,R16
movw R4,R18
lds R8,_g_fY+2
lds R9,_g_fY+2+1
lds R6,_g_fY
lds R7,_g_fY+1
ldd R16,y+20
ldd R17,y+21
ldd R18,y+22
ldd R19,y+23
st -y,R5
st -y,R4
st -y,R3
st -y,R2
rcall fpmule2x
movw R16,R6
movw R18,R8
rcall fpadd2
sts _g_fY+1,R17
sts _g_fY,R16
sts _g_fY+2+1,R19
sts _g_fY+2,R18
.dbline 193
; }
.dbline 194
; }
L13:
.dbline 197
;
; //數據更新
; s_lLastCounterL = g_lCounterLImage;
lds R4,_g_lCounterLImage+2
lds R5,_g_lCounterLImage+2+1
lds R2,_g_lCounterLImage
lds R3,_g_lCounterLImage+1
sts L10+1,R3
sts L10,R2
sts L10+2+1,R5
sts L10+2,R4
.dbline 198
; s_lLastCounterR = g_lCounterRImage;
lds R4,_g_lCounterRImage+2
lds R5,_g_lCounterRImage+2+1
lds R2,_g_lCounterRImage
lds R3,_g_lCounterRImage+1
sts L11+1,R3
sts L11,R2
sts L11+2+1,R5
sts L11+2,R4
.dbline 199
; g_bRunFlagB = TRUE;
ldi R24,1
sts _g_bRunFlagB,R24
.dbline 201
;
; return FALSE;
clr R16
.dbline -2
L9:
.dbline 0 ; func end
adiw R28,24
rjmp pop_xgsetF00C
.dbsym l fR1 20 D
.dbsym l fR0 16 D
.dbsym r nDeltaCounterL0 10 I
.dbsym l fTempAngle0 16 D
.dbsym l fAbsoluteAngle0 12 D
.dbsym l fDeltaAngle0 8 D
.dbsym l fR0 8 D
.dbsym r nDeltaCounterL0 10 I
.dbsym l Reg8 4 D
.dbend
.dbfunc e fMod _fMod fD
; fNumberB -> y+4
; fNumberA -> y+0
.even
_fMod::
rcall push_arg4
.dbline -1
.dbline 211
; }
;
; /***********************************************************
; * 函數說明：實數求模函數 *
; * 輸入： 被除數，除數 *
; * 輸出： 取模結果 *
; * 調用函數：無 *
; ***********************************************************/
; float fMod(float fNumberA,float fNumberB)
; {
.dbline 212
; if (fNumberB < 0)
ldd R2,y+4
ldd R3,y+5
ldd R4,y+6
ldd R5,y+7
ldi R16,<L8
ldi R17,>L8
rcall lpm32
st -y,R19
st -y,R18
st -y,R17
st -y,R16
movw R16,R2
movw R18,R4
rcall fpcmp2
brlt X13
rjmp L19
X13:
X3:
.dbline 213
; {
.dbline 214
; if (fNumberA < 0)
ldd R2,y+0
ldd R3,y+1
ldd R4,y+2
ldd R5,y+3
ldi R16,<L8
ldi R17,>L8
rcall lpm32
st -y,R19
st -y,R18
st -y,R17
st -y,R16
movw R16,R2
movw R18,R4
rcall fpcmp2
brge L21
X4:
.dbline 215
; {
rjmp L24
L23:
.dbline 217
; while(fNumberA < fNumberB)
; {
.dbline 218
; fNumberA -= fNumberB;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpsub1
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 219
; }
L24:
.dbline 216
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpcmp1
brlt L23
X5:
.dbline 221
;
; return fNumberA;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
rjmp L18
L21:
.dbline 223
; }
; else if (fNumberA > 0)
movw R24,R28
ldi R16,<L8
ldi R17,>L8
rcall lpm32
st -y,R25
st -y,R24
rcall fpcmp1
brge L26
X6:
.dbline 224
; {
rjmp L29
L28:
.dbline 226
; while((fNumberA + fNumberB) > fNumberB)
; {
.dbline 227
; fNumberA += fNumberB;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpadd1
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 228
; }
L29:
.dbline 225
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpadd1x
; stack offset 4
ldd R16,y+8
ldd R17,y+9
ldd R18,y+10
ldd R19,y+11
rcall fpcmp2
brlt L28
X7:
.dbline 230
;
; return fNumberA;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
rjmp L18
L26:
.dbline 233
; }
; else
; {
.dbline 234
; return 0.0;
ldi R16,<L8
ldi R17,>L8
rcall lpm32
rjmp L18
L19:
.dbline 237
; }
; }
; else if (fNumberB > 0)
movw R24,R28
adiw R24,4
ldi R16,<L8
ldi R17,>L8
rcall lpm32
st -y,R25
st -y,R24
rcall fpcmp1
brlt X14
rjmp L31
X14:
X8:
.dbline 238
; {
.dbline 239
; if (fNumberA < 0)
ldd R2,y+0
ldd R3,y+1
ldd R4,y+2
ldd R5,y+3
ldi R16,<L8
ldi R17,>L8
rcall lpm32
st -y,R19
st -y,R18
st -y,R17
st -y,R16
movw R16,R2
movw R18,R4
rcall fpcmp2
brge L33
X9:
.dbline 240
; {
rjmp L36
L35:
.dbline 242
; while((fNumberA + fNumberB) < fNumberB)
; {
.dbline 243
; fNumberA += fNumberB;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpadd1
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 244
; }
L36:
.dbline 241
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpadd1
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpcmp1
brlt L35
X10:
.dbline 246
;
; return fNumberA;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
rjmp L18
L33:
.dbline 248
; }
; else if (fNumberA > 0)
movw R24,R28
ldi R16,<L8
ldi R17,>L8
rcall lpm32
st -y,R25
st -y,R24
rcall fpcmp1
brge L38
X11:
.dbline 249
; {
rjmp L41
L40:
.dbline 251
; while(fNumberA > fNumberB)
; {
.dbline 252
; fNumberA -= fNumberB;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
movw R24,R28
adiw R24,4
st -y,R25
st -y,R24
rcall fpsub1
std y+0,R16
std y+1,R17
std y+2,R18
std y+3,R19
.dbline 253
; }
L41:
.dbline 250
movw R24,R28
ldd R16,y+4
ldd R17,y+5
ldd R18,y+6
ldd R19,y+7
st -y,R25
st -y,R24
rcall fpcmp1
brlt L40
X12:
.dbline 255
;
; return fNumberA;
ldd R16,y+0
ldd R17,y+1
ldd R18,y+2
ldd R19,y+3
rjmp L18
L38:
.dbline 258
; }
; else
; {
.dbline 259
; return 0.0;
ldi R16,<L8
ldi R17,>L8
rcall lpm32
rjmp L18
L31:
.dbline 263
; }
; }
; else
; {
.dbline 264
; return 0.0;
ldi R16,<L8
ldi R17,>L8
rcall lpm32
.dbline -2
L18:
.dbline 0 ; func end
adiw R28,4
ret
.dbsym l fNumberB 4 D
.dbsym l fNumberA 0 D
.dbend
.area lit(rom, con, rel)
L17:
.word 0x0,0x4000
L16:
.word 0x0,0x42ba
L15:
.word 0x0,0x3f80
L14:
.word 0x0,0x3f00
L8:
.word 0x0,0x0
L4:
.word 0x1323,0x3dc3
L3:
.word 0x4fb,0x3dc1
L2:
.word 0x2c0b,0x3bb0
; }
; }
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -