📄 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 + -