379 lines
12 KiB
NASM
379 lines
12 KiB
NASM
|
;;###########################################################################
|
||
|
|
;;
|
||
|
|
;; FILE: IQNcos.asm
|
||
|
|
;;
|
||
|
|
;; TITLE: C Callable IQ Math Function
|
||
|
|
;;
|
||
|
|
;;###########################################################################
|
||
|
|
;;
|
||
|
|
;; Ver | Date | Who | Description of changes
|
||
|
|
;; =====|=============|=======|==============================================
|
||
|
|
;; 1.2 | 11 Oct 2001 | A. T. | Original Release.
|
||
|
|
;; -----|-------------|-------|----------------------------------------------
|
||
|
|
;; 1.3 | 19 Nov 2001 | A. T. | No changes.
|
||
|
|
;; -----|-------------|-------|----------------------------------------------
|
||
|
|
;; 1.4 | 17 May 2002 | A. T. | Improved accuracy from 16 to about 29 bits
|
||
|
|
;; | | | (thanks to Settu for writing original code).
|
||
|
|
;; | | | Modified code so that calculation constants
|
||
|
|
;; | | | are embedded in code and not in table.
|
||
|
|
;; -----|-------------|-------|----------------------------------------------
|
||
|
|
;; | | |
|
||
|
|
;;
|
||
|
|
;;###########################################################################
|
||
|
|
;;#############################################################################
|
||
|
|
;;!
|
||
|
|
;;! Copyright: Copyright (C) 2023 Texas Instruments Incorporated -
|
||
|
|
;;! All rights reserved not granted herein.
|
||
|
|
;;! Limited License.
|
||
|
|
;;!
|
||
|
|
;;! Texas Instruments Incorporated grants a world-wide, royalty-free,
|
||
|
|
;;! non-exclusive license under copyrights and patents it now or hereafter
|
||
|
|
;;! owns or controls to make, have made, use, import, offer to sell and sell
|
||
|
|
;;! ("Utilize") this software subject to the terms herein. With respect to the
|
||
|
|
;;! foregoing patent license, such license is granted solely to the extent that
|
||
|
|
;;! any such patent is necessary to Utilize the software alone. The patent
|
||
|
|
;;! license shall not apply to any combinations which include this software,
|
||
|
|
;;! other than combinations with devices manufactured by or for TI
|
||
|
|
;;! ("TI Devices").
|
||
|
|
;;! No hardware patent is licensed hereunder.
|
||
|
|
;;!
|
||
|
|
;;! Redistributions must preserve existing copyright notices and reproduce this
|
||
|
|
;;! license (including the above copyright notice and the disclaimer and
|
||
|
|
;;! (if applicable) source code license limitations below) in the documentation
|
||
|
|
;;! and/or other materials provided with the distribution.
|
||
|
|
;;!
|
||
|
|
;;! Redistribution and use in binary form, without modification, are permitted
|
||
|
|
;;! provided that the following conditions are met:
|
||
|
|
;;!
|
||
|
|
;;! * No reverse engineering, decompilation, or disassembly of this software is
|
||
|
|
;;! permitted with respect to any software provided in binary form.
|
||
|
|
;;! * Any redistribution and use are licensed by TI for use only
|
||
|
|
;;! with TI Devices.
|
||
|
|
;;! * Nothing shall obligate TI to provide you with source code for the
|
||
|
|
;;! software licensed and provided to you in object code.
|
||
|
|
;;!
|
||
|
|
;;! If software source code is provided to you, modification and redistribution
|
||
|
|
;;! of the source code are permitted provided that the following conditions
|
||
|
|
;;! are met:
|
||
|
|
;;!
|
||
|
|
;;! * any redistribution and use of the source code, including any resulting
|
||
|
|
;;! derivative works, are licensed by TI for use only with TI Devices.
|
||
|
|
;;! * any redistribution and use of any object code compiled from the source
|
||
|
|
;;! code and any resulting derivative works, are licensed by TI for use
|
||
|
|
;;! only with TI Devices.
|
||
|
|
;;!
|
||
|
|
;;! Neither the name of Texas Instruments Incorporated nor the names of its
|
||
|
|
;;! suppliers may be used to endorse or promote products derived from this
|
||
|
|
;;! software without specific prior written permission.
|
||
|
|
;;#############################################################################
|
||
|
|
|
||
|
|
;;===========================================================================
|
||
|
|
;; Function: _IQNcos
|
||
|
|
;;===========================================================================
|
||
|
|
;;
|
||
|
|
;; C Usage: extern long _IQNcos(long Rad);
|
||
|
|
;;
|
||
|
|
;;---------------------------------------------------------------------------
|
||
|
|
;;
|
||
|
|
;; On Entry: ACC = Radian in IQ format
|
||
|
|
;;
|
||
|
|
;; Regs Used: XAR7, XAR6, XAR5, XAR4, XAR0
|
||
|
|
;; XT, P
|
||
|
|
;;
|
||
|
|
;; On Exit: ACC = cos(Radian) result in IQ format
|
||
|
|
;;
|
||
|
|
;; Q range: 30 to 1
|
||
|
|
;;
|
||
|
|
;;---------------------------------------------------------------------------
|
||
|
|
;; Algorithm: The "cos" value is calculated as follows:
|
||
|
|
;;
|
||
|
|
;; 1) The offset into a 512 word sin/cos table is calculated:
|
||
|
|
;;
|
||
|
|
;; k = 0x1FF & (int(Radian*512/(2*pi)))
|
||
|
|
;;
|
||
|
|
;;
|
||
|
|
;; 2) The fractional component between table samples is
|
||
|
|
;; calculated:
|
||
|
|
;;
|
||
|
|
;; x = fract(Radian*512/2*pi) * (2*pi)/512
|
||
|
|
;;
|
||
|
|
;; 3) The output sine value is calculated as follows:
|
||
|
|
;;
|
||
|
|
;; cos(Radian) = C(k) + x*(-S(k) + x*(-0.5*C(k) + 0.166*x*S(k)))
|
||
|
|
;;
|
||
|
|
;; where S(k) = Sin table value at offset "k"
|
||
|
|
;; C(k) = Cos table value at offset "k"
|
||
|
|
;;
|
||
|
|
;; Using the above method, with a 512x32 full wave sin/cos table,
|
||
|
|
;; will give an accuracy of approximately 29 bits.
|
||
|
|
;;
|
||
|
|
;;---------------------------------------------------------------------------
|
||
|
|
;; Benchmark:
|
||
|
|
;;
|
||
|
|
;; Assumptions: * Code executes from 0-wait SARAM block.
|
||
|
|
;; * _IQsin/cosTable located in seperate SARAM or ROM block.
|
||
|
|
;;
|
||
|
|
;; Cycles = 43 ( Q == 30, tables in 0-wait SARAM, includes LCR/LRETR)
|
||
|
|
;; = 44 (29 >= Q >= 25, tables in 0-wait SARAM, includes LCR/LRETR)
|
||
|
|
;; = 43 (24 >= Q >= 14, tables in 0-wait SARAM, includes LCR/LRETR)
|
||
|
|
;; = 44 (13 >= Q >= 0, tables in 0-wait SARAM, includes LCR/LRETR)
|
||
|
|
;;
|
||
|
|
;; Cycles = 45 ( Q == 30, tables in 1-wait ROM, includes LCR/LRETR)
|
||
|
|
;; = 46 (29 >= Q >= 25, tables in 1-wait ROM, includes LCR/LRETR)
|
||
|
|
;; = 45 (24 >= Q >= 14, tables in 1-wait ROM, includes LCR/LRETR)
|
||
|
|
;; = 46 (13 >= Q >= 0, tables in 1-wait ROM, includes LCR/LRETR)
|
||
|
|
;;
|
||
|
|
;;===========================================================================
|
||
|
|
|
||
|
|
|
||
|
|
K1 .set 02AAAh ; 1/6 in Q16
|
||
|
|
|
||
|
|
IQNcos .macro q_value
|
||
|
|
MOV *SP++,#0x3F6B ; (2*pi)/512 = 0x03243F6B (Q32)
|
||
|
|
MOV *SP++,#0x0324
|
||
|
|
MOV *SP++,#0xC1B7 ; 512/(2*pi) = 0x517CC1B7 (I8Q24)
|
||
|
|
MOV *SP++,#0x517C
|
||
|
|
MOVL XAR6,#_IQcosTable
|
||
|
|
MOVL XAR7,#_IQsinTable
|
||
|
|
|
||
|
|
SETC OVM ; turn overflow mode on
|
||
|
|
ABS ACC ; abs(Rad), TC = sign
|
||
|
|
|
||
|
|
MOVL XT,*--SP ; XT = 512/(2*pi) (I8Q24)
|
||
|
|
IMPYL P,XT,@ACC
|
||
|
|
QMPYL ACC,XT,@ACC ; ACC:P = Rad * 512 / (2pi)
|
||
|
|
|
||
|
|
.if q_value >= 25
|
||
|
|
ASR64 ACC:P,#16
|
||
|
|
ASR64 ACC:P,#(q_value - 24) ; ACC = Integer, P = fract
|
||
|
|
.endif
|
||
|
|
.if (q_value <= 24) & (q_value >= 9)
|
||
|
|
ASR64 ACC:P,#(q_value - 8) ; ACC = Integer, P = fract
|
||
|
|
.endif
|
||
|
|
.if q_value <= 7
|
||
|
|
LSL64 ACC:P,#(8 - q_value) ; ACC = Integer, P = fract
|
||
|
|
.endif
|
||
|
|
|
||
|
|
AND @AL,#0x1FF
|
||
|
|
LSL AL,#1
|
||
|
|
MOVZ AR0,@AL ; AR0 = Index into "sin/cos" table = k
|
||
|
|
|
||
|
|
MOVL XT,*--SP ; XT = (2*pi)/512 (Q32)
|
||
|
|
QMPYUL P,XT,@P ; P = x (Q32)
|
||
|
|
MOVB ACC,#0
|
||
|
|
MOVL XT,@P ; XT = x (Q32)
|
||
|
|
|
||
|
|
MOVL XAR4,*+XAR6[AR0] ; XAR4 = C(k) (Q30)
|
||
|
|
MOVL XAR5,*+XAR7[AR0] ; XAR5 = S(k) (Q30)
|
||
|
|
|
||
|
|
SUBL ACC,@XAR4 ; ACC = -C(k) (Q30)
|
||
|
|
ASR64 ACC:P,#1 ; ACC = -0.5*C(k) (Q30)
|
||
|
|
QMPYL P,XT,@XAR5 ; P = x*S(k) (Q30)
|
||
|
|
CLRC OVM ; Turn overflow mode off
|
||
|
|
MPY P,@PH,#K1 ; P = 0.166*x*S(k) (Q30)
|
||
|
|
ADDL ACC,P ; ACC = -0.5*C(k) + 0.166*x*S(k) (Q30)
|
||
|
|
|
||
|
|
QMPYL ACC,XT,@ACC ; ACC = x*(-0.5*C(k) + 0.166*x*S(k)) (Q30)
|
||
|
|
SUBL ACC,@XAR5 ; ACC = -S(k) + x*(-0.5*C(k) + 0.166*x*S(k)) (Q30)
|
||
|
|
QMPYL ACC,XT,@ACC ; ACC = x*(-S(k) + x*(-0.5*C(k) + 0.166*x*S(k))) (Q30)
|
||
|
|
ADDL ACC,@XAR4 ; ACC = C(k) + x*(-S(k) + x*(-0.5*C(k) + 0.166*x*S(k))) (Q30)
|
||
|
|
|
||
|
|
.if (q_value <= 29) & (q_value >= 14)
|
||
|
|
ASR64 ACC:P,#(30 - q_value) ; Scale back to required IQ value
|
||
|
|
.endif
|
||
|
|
.if q_value <= 13
|
||
|
|
ASR64 ACC:P,#16
|
||
|
|
ASR64 ACC:P,#(14 - q_value) ; Scale back to required IQ value
|
||
|
|
.endif
|
||
|
|
LRETR
|
||
|
|
.endm
|
||
|
|
|
||
|
|
;; The value "GLOBAL_Q" needs to be supplied by the assembler
|
||
|
|
;; using the "-dGLOBAL_Q=q_value" directive:
|
||
|
|
.sect "IQmath"
|
||
|
|
.ref _IQsinTable
|
||
|
|
.ref _IQcosTable
|
||
|
|
.if GLOBAL_Q == 30
|
||
|
|
.def __IQ30cos
|
||
|
|
__IQ30cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 29
|
||
|
|
.def __IQ29cos
|
||
|
|
__IQ29cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 28
|
||
|
|
.def __IQ28cos
|
||
|
|
__IQ28cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 27
|
||
|
|
.def __IQ27cos
|
||
|
|
__IQ27cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 26
|
||
|
|
.def __IQ26cos
|
||
|
|
__IQ26cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 25
|
||
|
|
.def __IQ25cos
|
||
|
|
__IQ25cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 24
|
||
|
|
.def __IQ24cos
|
||
|
|
__IQ24cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 23
|
||
|
|
.def __IQ23cos
|
||
|
|
__IQ23cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 22
|
||
|
|
.def __IQ22cos
|
||
|
|
__IQ22cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 21
|
||
|
|
.def __IQ21cos
|
||
|
|
__IQ21cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 20
|
||
|
|
.def __IQ20cos
|
||
|
|
__IQ20cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 19
|
||
|
|
.def __IQ19cos
|
||
|
|
__IQ19cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 18
|
||
|
|
.def __IQ18cos
|
||
|
|
__IQ18cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 17
|
||
|
|
.def __IQ17cos
|
||
|
|
__IQ17cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 16
|
||
|
|
.def __IQ16cos
|
||
|
|
__IQ16cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 15
|
||
|
|
.def __IQ15cos
|
||
|
|
__IQ15cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 14
|
||
|
|
.def __IQ14cos
|
||
|
|
__IQ14cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 13
|
||
|
|
.def __IQ13cos
|
||
|
|
__IQ13cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 12
|
||
|
|
.def __IQ12cos
|
||
|
|
__IQ12cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 11
|
||
|
|
.def __IQ11cos
|
||
|
|
__IQ11cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 10
|
||
|
|
.def __IQ10cos
|
||
|
|
__IQ10cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 9
|
||
|
|
.def __IQ9cos
|
||
|
|
__IQ9cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 8
|
||
|
|
.def __IQ8cos
|
||
|
|
__IQ8cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 7
|
||
|
|
.def __IQ7cos
|
||
|
|
__IQ7cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 6
|
||
|
|
.def __IQ6cos
|
||
|
|
__IQ6cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 5
|
||
|
|
.def __IQ5cos
|
||
|
|
__IQ5cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 4
|
||
|
|
.def __IQ4cos
|
||
|
|
__IQ4cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 3
|
||
|
|
.def __IQ3cos
|
||
|
|
__IQ3cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 2
|
||
|
|
.def __IQ2cos
|
||
|
|
__IQ2cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
.if GLOBAL_Q == 1
|
||
|
|
.def __IQ1cos
|
||
|
|
__IQ1cos:
|
||
|
|
IQNcos GLOBAL_Q
|
||
|
|
.endif
|
||
|
|
|
||
|
|
;;###########################################################################
|
||
|
|
;; No More.
|
||
|
|
;;###########################################################################
|