给个定点的,浮点就LS办法
;
public sin_cos
;计算正弦/余弦
public int_sin
public count_rad
public mul_1632
public mul_32
public _sqrt_int
;计算 R0~R3 中四字节的开方, 结果在R4~R7
math_data0 segment data
rseg math_data0
root: ds 4
sinx: ds 4
cosx: ds 4
rad_point: ds 6 ;累加 rad_step 后得到的当前计算采样点弧度.(经处理在 0~Pi/4 之间, 配合 octrant 描述在整圆内位置)
rad_step: ds 6 ;每个采样点间隔弧度
; xseg at 0x0200 - 32 * 6
;channela0_addition: ds 32
;channela1_addition: ds 32
;channelb0_addition: ds 32
;channelb1_addition: ds 32
;channelc0_addition: ds 32
;channelc1_addition: ds 32 ;0x0140 ~ 0x01ff
;累加结果缓存, +0~3 为简单累加和, +4~9 为平方和, +16~23 为正弦变换和, +24~31 为余弦变换和, +10~15 暂未使用
math_data1 segment data
rseg math_data1
mult: ds 11
octrant: ds 1 ;八分之一圆周.范围为 0~7
;rad_point: ds 6
;rad_step: ds 6
;----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
;octrant, | sin(x) = | cos(x) = | 累积计算方式 | 整圆位置 = 0 ~ 2 x Pi |rad_point 初始值为 0, octrant = 0
; | | | | | | | | | |
; 0 | sin (rad_point) | cos (rad_point) | rad_point += rad_step | 0 ~ Pi/4 |当 rad_point >= Pi/4 时 rad_point = Pi/2 - rad_point | octrant ++ (= 1)
; 1 | cos (rad_point) | sin (rad_point) | rad_point -= rad_step | Pi/4 ~ Pi/2 |当 rad_point < 0 即减法产生借位时 rad_point = 0 - rad_point | octrant ++ (= 2)
; 2 | cos (rad_point) | - sin (rad_point) | rad_point += rad_step | Pi/2 ~ Pix3/4 |当 rad_point >= Pi/4 时 rad_point = Pi/2 - rad_point | octrant ++ (= 3)
; 3 | sin (rad_point) | - cos (rad_point) | rad_point -= rad_step | Pix3/4 ~ Pi |当 rad_point < 0 即减法产生借位时 rad_point = 0 - rad_point | octrant ++ (= 4)
; 4 | - sin (rad_point) | - cos (rad_point) | rad_point += rad_step | Pi ~ Pix5/4 |当 rad_point >= Pi/4 时 rad_point = Pi/2 - rad_point | octrant ++ (= 5)
; 5 | - cos (rad_point) | - sin (rad_point) | rad_point -= rad_step | Pix5/4 ~ Pix3/2 |当 rad_point < 0 即减法产生借位时 rad_point = 0 - rad_point | octrant ++ (= 6)
; 6 | - cos (rad_point) | sin (rad_point) | rad_point += rad_step | Pix3/2 ~ Pix7/4 |当 rad_point >= Pi/4 时 rad_point = Pi/2 - rad_point | octrant ++ (= 7)
; 7 | - sin (rad_point) | cos (rad_point) | rad_point -= rad_step | Pix7/4 ~ Pix2 |当 rad_point < 0 即减法产生借位时 rad_point = 0 - rad_point | octrant = 0
;----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
;advance equ 1024
;sample_clock equ 98
;sample_clock equ 106
;sample_clock equ 162
;sample_time equ 1966080 + advance * sample_clock
;sample_time equ 2133334 + advance * sample_clock
;sample_time equ 3276800 + advance * sample_clock
;每98t@11.0592MHz/106t@12MHz/163t@18.432MHz 采样一点, 即使工频为55Hz时仍可以每周期采样1024点
;共采样 sample ;即使45Hz时仍可以采样16个周期
;其中 advance * sample_clock 为冗余, 而且可以剔除前若干点避免转换通道时的信号扰动
;捕捉测得16周期时间为TimeSample, 则每个采样点的弧度增加值 = 32Pi X sample_clock / TimeSample
math_bit segment bit
rseg math_bit
mathb0: dbit 1
rad_flag: dbit 1
math_code segment code
rseg math_code
;pi4: db 0x69, 0x21, 0xa2, 0xda, 0x0f, 0xc9 ;pi/4 = 3.141592653589793.../4
;pi2: db 0xd2, 0x42, 0x44, 0xb5, 0x1f, 0x92 ;pi/2 = 3.141592653589793.../2, 最高字节为0x01, 不参加运算
pi4_0 equ 0x69
pi4_1 equ 0x21
pi4_2 equ 0xa2
pi4_3 equ 0xda
pi4_4 equ 0x0f
pi4_5 equ 0xc9
pi2_0 equ 0xd2
pi2_1 equ 0x42
pi2_2 equ 0x44
pi2_3 equ 0xb5
pi2_4 equ 0x1f
pi2_5 equ 0x92
pi2_6 equ 0x01
T_data: db 0x26, 0x7c, 0x08, 0xd9, 0x08, 0x65, 0x0d, 0x2c ;11.0592MHz
;T_data: db 0x29, 0xa0, 0x48, 0x43, 0x90, 0xea, 0xb0, 0x35 ;12.000 MHz
;T_data: db 0x3f, 0x9e, 0x04, 0x2d, 0x4c, 0x92, 0x25, 0x73 ;18.432 MHz
;T_data = sample_clock x 16 x 2Pi x 2^48
fact_table: db 0xab, 0xaa, 0xaa, 0x2a ;1/3! = 1/6
db 0x22, 0x22, 0x22, 0x02 ;1/5! = 1/120
db 0xd0, 0x00, 0x0d, 0x00 ;1/7! = 1/5040
db 0x3c, 0x2e, 0x00, 0x00 ;1/9! = 1/362880
db 0x6c, 0x00, 0x00, 0x00 ;1/11! = 1/39916800 ;(pi/4)^11/11! = 0x00000008
db 0x00, 0x00, 0x00, 0x00
;--------------------------------------------------------------------------------------------------
TCR20 data 0xC8
TIFR2 data 0xE9
TPCR2H data 0xCB
TPCR2L data 0xCA
CCCRA data 0xEA
ICRAH data 0xAB ;捕捉结果寄存器, 本设计使用 A, 使用中必须先读取 ICRAL, 然后读 ICRAH.
ICRAL data 0xAA
OCRAL data 0xEE ;输出比较寄存器, 本设计不使用此功能, 将 CCCRA/B/C/D 相应关闭PWM功能后此八字节可做为内部 RAM 使用.
OCRAH data 0xEF
OCRBL data 0xFA
OCRBH data 0xFB
OCRCL data 0xFC
OCRCH data 0xFD
OCRDL data 0xFE
OCRDH data 0xFF
TL2 data 0xCC ;捕捉用定时寄存器
TH2 data 0xCD
TOR2L data 0xCE ;重装寄存器 = 0x00
TOR2H data 0xCF
Trig_Cnt equ OCRAL ;初值为 -3, 每次发生捕捉加一, 加为零时正式开始, 加到 16 时完成采样.
Trig_1 equ OCRCL ;正式开始时捕捉到的计数值
Trig_0 equ OCRAH
Trig_High equ OCRCH ;T2 溢出计数
Trig_Last equ OCRDL ;上次捕捉计数值, 两字节, 连 Trig_Last 一起减去 Trig_0 后即为前次捕捉时间.
Trig_Data equ OCRBL ;采样次数累计
;-------------------------------------------------------------------------------------------------------------
rseg math_code
_sqrt_int: ;[r4~7] 四字节开方 --> [r4~7], 速度优化版, 256 Bytes, <= 1829T
;已验证, 准确, 结果有效位30位.
;[r4~7] 为无符号小数, 小数点在最高位前一位
;破坏 [r0~3], root[0~3], Acc, B, PSW
mov r0, 4
mov r1, 5
mov r2, 6
mov r3, 7
clr a
mov r4, #1
mov r5, a
mov r6, a
mov r7, a
mov root, a
mov root + 1, a
mov root + 2, a
mov root + 3, a ;0 --> [r4~r7], root(0~3)
mov b, a
mov a, r3 ;r3 = 0
jz sqr_l1
sqr_l0: mov a, r3
rlc a
mov r3, a
mov a, root
rlc a
mov root, a
inc b
jb b.0, sqr_l0 ;r3 >> 2 --> root
subb a, r4
jc sqr1b ;root - r4 >= 0?
mov root, a ;Y.
inc r4
inc r4
sqr1b: dec r4
mov a, r4
setb c
rlc a
mov r4, a ;r4 >> 1
jnb b.3, sqr_l0 ;4 times = 1 byte
clr b.3
sqr_l1: mov a, r1
rlc a
mov r1, a
mov a, r2
rlc a
mov r2, a
mov a, root
rlc a
mov root, a
mov a, root + 1
rlc a
mov root + 1, a
inc b
jb b.0, sqr_l1 ;[r1r2] >> 2 --> root(0~1)
mov a, root
subb a, r4
mov a, root + 1
subb a, r5
jc sqr2b ;root(0~1) - r4r5 >= 0?
mov root + 1, a ;Y
mov a, root
subb a, r4
mov root, a
inc r4
inc r4
sqr2b: dec r4
mov a, r4
setb c
rlc a
mov r4, a
mov a, r5
rlc a
mov r5, a ;r4r5 >> 1
jnb b.4, sqr_l1 ;8 times
mov b, #(0x10 - 10) * 2
sqr_l2: clr c
mov a, r0
rlc a
mov r0, a
mov a, root
rlc a
mov root, a
mov a, root + 1
rlc a
mov root + 1, a
mov a, root + 2
rlc a
mov root + 2, a
inc b
jb b.0, sqr_l2
mov a, root
subb a, r4
mov a, root + 1
subb a, r5
mov a, root + 2
subb a, r6
jc sqr3b
mov root + 2, a
clr c
mov a, root
subb a, r4
mov root, a
mov a, root + 1
subb a, r5
mov root + 1, a
inc r4
inc r4
sqr3b: dec r4
setb c
mov a, r4
rlc a
mov r4, a
mov a, r5
rlc a
mov r5, a
mov a, r6
rlc a
mov r6, a
jnb b.5, sqr_l2 ;10 times
mov b, #0x10 - 2
sqr_l3: clr c
mov a, root
rlc a
mov root, a
mov a, root + 1
rlc a
mov root + 1, a
mov a, root + 2
rlc a
mov root + 2, a
mov a, root + 3
rlc a
mov root + 3, a
inc b
jb b.0, sqr_l3
clr c
mov a, root
subb a, r4
mov a, root + 1
subb a, r5
mov a, root + 2
subb a, r6
mov a, root + 3
subb a, r7
jc sqr4b
mov root + 3, a
mov a, root
subb a, r4
mov root, a
mov a, root + 1
subb a, r5
mov root + 1, a
mov a, root + 2
subb a, r6
mov root + 2, a
inc r4
inc r4
sqr4b: dec r4
jb b.5, sqrfinish
setb c ;根左移一位, 并加一
mov a, r4
rlc a
mov r4, a
mov a, r5
rlc a
mov r5, a
mov a, r6
rlc a
mov r6, a
mov a, r7
rlc a
mov r7, a
sjmp sqr_l3 ;9 times
sqrfinish:
mov a, r4 ;四舍五入 :-P
setb acc.1
mov r4, a
ret
mul_1632: ;r0r1 X mult[0~3] --> r2~r7; ;101 bytes; <= 108t
;未验证, 冒然使用后果自负!
;破坏 Acc, B, F0, PSW
mov a, mult ;a0 X b0
mov b, r0
mul ab
mov r2, a
mov r3, b
mov a, mult + 2 ;a2 X b0
mov b, r0
mul ab
mov r4, a
mov r5, b
mov a, mult + 3 ;a3 X b1 ;a0b0, a2b0, a3b1 --> r2~r7
mov b, r1
mul ab
mov r6, a
mov r7, b
mov a, mult ;a0 X b1
mov b, r1
mul ab
add a, r3
mov r3, a
mov a, b
addc a, r4
mov r4, a
mov f0, c
mov a, mult + 2 ;a2 X b1
mov b, r1
mul ab
mov c, f0
addc a, r5
mov r5, a
mov a, b
addc a, r6
mov r6, a
jnc $ + 3
inc r7 ;a0b0, a0b1, a2b0, a2b1, a3b1
mov a, mult + 1 ;a1 X b0
mov b, r0
mul ab
add a, r3
mov r3, a
mov a, b
addc a, r4
mov r4, a
mov f0, c
mov a, mult + 3 ;a3 X b0
mov b, r0
mul ab
mov c, f0
addc a, r5
mov r5, a
mov a, b
addc a, r6
mov r6, a
jnc $ + 3
inc r7 ;a0b0, a0b1, a1b0, a2b0, a2b1, a3b0, a3b1
mov a, mult + 1 ;a1 X b1 ;a0b0, a0b1, a1b0, a1b1, a2b0, a2b1, a3b0, a3b1
mov b, r1
mul ab
add a, r4
mov r4, a
mov a, b
mul_160:addc a, r5
mov r5, a
jnc $ + 7
inc r6
mov a, r6
jnz $ + 3
inc r7
ret
mul_32: ;mul[t0~3] X mult[4~7] --> r0~7; ;46 bytes, <= 267T
;不需要验证了吧 :-P
;破坏 [mult(4~9)], Acc, B, F0, PSW
mov r0, mult + 4
mov r1, mult + 5
lcall mul_1632
mov r0, mult + 6
mov r1, mult + 7
mov mult + 4, r2
mov mult + 5, r3
mov mult + 6, r4
mov mult + 7, r5
mov mult + 8, r6
mov mult + 9, r7
lcall mul_1632
mov r0, mult + 4
mov r1, mult + 5
mov a, mult + 6
add a, r2
mov r2, a
mov a, mult + 7
addc a, r3
mov r3, a
mov a, mult + 8
addc a, r4
mov r4, a
mov a, mult + 9
sjmp mul_160
;;-------------------------------------------------------------------------------------------------
int_sin: ; sin(x) = x - x^3/3! + x^5/5! - x^7/7! + x^9/9! - x^11/11! ..., x 单位为弧度(rad)
;为加快运算速度及减少代码量, 特经预处理使 x = (0 ~ Pi/4)
;原理是当 x = (Pi/4 ~ Pi/2) 时 sin(x) = cos(Pi/2 - x) 成立
;其他象限各区间也都有类似等式成立
;x = [r4~7] ; 205 bytes; <=3006T
;已验证, 准确, 结果有效位31位
mov a, r4
mov mult + 0, a
mov mult + 4, a
mov sinx + 0, a
mov root + 0, a
mov a, r5
mov mult + 1, a
mov mult + 5, a
mov sinx + 1, a
mov root + 1, a
mov a, r6
mov mult + 2, a
mov mult + 6, a
mov sinx + 2, a
mov root + 2, a
mov a, r7
mov mult + 3, a
mov mult + 7, a
mov sinx + 3, a
mov root + 3, a ; 种子 x --> sinx(0~3), mult(0~3), mult(4~7), root(0~3)
lcall mul_32 ; x^2
mov cosx + 0, r4
mov cosx + 1, r5
mov cosx + 2, r6
mov cosx + 3, r7 ; x^2 --> cosx(0~3)
mov mult + 10, #0
int_sin_loop:
mov mult + 0, root + 0
mov mult + 1, root + 1
mov mult + 2, root + 2
mov mult + 3, root + 3 ;x^n
mov mult + 4, cosx + 0
mov mult + 5, cosx + 1
mov mult + 6, cosx + 2
mov mult + 7, cosx + 3 ;x^2
lcall mul_32 ; x^n X x^2
mov a, r4
mov mult + 0, a
mov root + 0, a
mov a, r5
mov mult + 1, a
mov root + 1, a
mov a, r6
mov mult + 2, a
mov root + 2, a
mov a, r7
mov mult + 3, a
mov root + 3, a ; x^n --> root(0~3), mult(0~3)
mov dptr, #fact_table
mov a, mult + 10
mov r0, a
movc a, @a + dptr
mov mult + 4, a
inc r0
mov a, r0
movc a, @a + dptr
mov mult + 5, a
inc r0
mov a, r0
movc a, @a + dptr
mov mult + 6, a
inc r0
mov a, r0
movc a, @a + dptr
mov mult + 7, a ; 1/n! --> mult(4~7)
inc r0
mov a, r0
mov mult + 10, a
jnb acc.4, int_sin_xxxx
mov r0, mult + 4
mov r1, mult + 5
lcall mul_1632
mov r3, 5
mov r4, 6
mov r5, 7
clr a
mov r6, a
mov r7, a
sjmp $ + 5
int_sin_xxxx:
lcall mul_32 ; x^n/n!
mov a, r4
orl a, r5
orl a, r6
orl a, r7
jz int_sin_exit
mov a, r3
rlc a
mov a, mult + 10
jnb acc.2, add_xn
sub_xn: mov a, sinx + 0
subb a, r4
mov sinx + 0, a
mov a, sinx + 1
subb a, r5
mov sinx + 1, a
mov a, sinx + 2
subb a, r6
mov sinx + 2, a
mov a, sinx + 3
subb a, r7
mov sinx + 3, a ; x - x^n/n!
ljmp int_sin_loop
add_xn: mov a, sinx + 0
addc a, r4
mov sinx + 0, a
mov a, sinx + 1
addc a, r5
mov sinx + 1, a
mov a, sinx + 2
addc a, r6
mov sinx + 2, a
mov a, sinx + 3
addc a, r7
mov sinx + 3, a ; x + x^n/n!
ljmp int_sin_loop
int_sin_exit:
ret
sin_cos: ;|cos (x)| = sqrt [1 - sin (x) ^ 2] ;sin(x)^2 + cos(x)^2 = 1
lcall int_sin
mov a, sinx
mov mult, a
mov mult + 4, a
mov a, sinx + 1
mov mult + 1, a
mov mult + 5, a
mov a, sinx + 2
mov mult + 2, a
mov mult + 6, a
mov a, sinx + 3
mov mult + 3, a
mov mult + 7, a
lcall mul_32 ;sin (x) ^ 2
clr a
setb c
subb a, r4
mov r4, a
clr a
subb a, r5
mov r5, a
clr a
subb a, r6
mov r6, a
clr a
subb a, r7
mov r7, a ; 1 - sin (x) ^ 2
lcall _sqrt_int
mov cosx, r4
mov cosx + 1, r5
mov cosx + 2, r6
mov cosx + 3, r7 ;|cos (x)| = sqrt [1 - sin (x) ^ 2]
ret
end
;;;;;----------------------------------------------------------------------------------------------
;;;;;----------------------------------------------------------------------------------------------
/**************************************************************************************************
frequency: ;TimeSample = [r4 ~ 6], 计算频率, 结果为浮点数 --> [r4~7]. ;可以直接用C写.
mov mult + 1, r4
mov mult + 2, r5
mov mult + 3, r6
mul_32_1: ;mul[t0~3] X mult[4~7] --> r0~7 ; <= 229T, ;太长, 速度未快很多, 淘汰.
mov a, mult
mov b, mult + 4
mul ab
mov r0, a
mov r1, b
mov a, mult + 1
mov b, mult + 5
mul ab
mov r2, a
mov r3, b
mov a, mult + 2
mov b, mult + 6
mul ab
mov r4, a
mov r5, a
mov a, mult + 3
mov b, mult + 7
mul ab
mov r6, a
mov r7, b ;IV 36
mov a, mult
mov b, mult + 5
mul ab
add a, r1
mov r1, a
mov a, b
addc a, r2
mov r2, a
mov f0, c
mov a, mult + 1
mov b, mult + 6
mul ab
mov c, f0
addc a, r3
mov r3, a
mov a, b
addc a, r4
mov r4, a
mov f0, c
mov a, mult + 2
mov b, mult + 7
mul ab
mov c, f0
addc a, r5
mov r5, a
mov a, b
addc a, r6
mov r6, a
jnc $ + 3
inc r7 ;III 0 45
mov a, mult + 1
mov b, mult + 4
mul ab
add a, r1
mov r1, a
mov a, b
addc a, r2
mov r2, a
mov f0, c
mov a, mult + 2
mov b, mult + 5
mul ab
mov c, f0
addc a, r3
mov r3, a
mov a, b
addc a, r4
mov r4, a
mov f0, c
mov a, mult + 3
mov b, mult + 6
mul ab
mov c, f0
addc a, r5
mov r5, a
mov a, b
addc a, r6
mov r6, a
jnc $ + 3
inc r7 ;III 1 45
mov a, mult
mov b, mult + 6
mul ab
add a, r2
mov r2, a
mov a, b
addc a, r3
mov r3, a
mov f0, c
mov a, mult + 1
mov b, mult + 7
mul ab
mov c, f0
addc a, r4
mov r4, a
mov a, b
addc a, r5
mov r5, a
mov mathb0, c ;II 0 28
mov a, mult + 2
mov b, mult + 4
mul ab
add a, r2
mov r2, a
mov a, b
addc a, r3
mov r3, a
mov f0, c
mov a, mult + 3
mov b, mult + 5
mul ab
mov c, f0
addc a, r4
mov r4, a
mov a, b
addc a, r5
mov r5, a
clr a
rlc a
mov c, mathb0
addc a, r6
mov r6, a
jnc $ + 3
inc r7 ;II 1 36
mov a, mult
mov b, mult + 7
mul ab
add a, r3
mov r3, a
mov a, b
addc a, r4
mov f0, c ;I 0
mov a, mult + 3
mov b, mult + 4
mul ab
add a, r3
mov r3, a
mov a, b
addc a, r4
mov r4, a ;I 1
clr a
rlc a
mov c, f0
addc a, r5
mov r5, a
jnc $ + 7
inc r6
mov a, r6
jnz $ + 3
inc r7 ;I 37
ret
_sqrt_int: ;[r0~3] 四字节开方 --> [r4~7], 代码精简版, 113 Bytes, <= 2994T
;已验证, 准确, 结果有效位30位. 太慢, 淘汰.
clr a
mov r4, #0x01
mov r5, a
mov r6, a
mov r7, a
mov root, a
mov root + 1, a
mov root + 2, a
mov root + 3, a
mov b, #0x03
sqrdec0:clr c ;原数左移两位至余数
mov a, r0 ;原数
rlc a
mov r0, a
mov a, r1
rlc a
mov&n |
|
楼主
标题置顶
标题高亮
来自 2楼
