;=====================================================================================================================
; File name:       F243QEP2.ASM                     
;                    
; Originator:	Digital Control Systems Group
;			Texas Instruments
; Description:                                   
; This file contains source for the QEP theta calculation function.
;=====================================================================================
; History:
;-------------------------------------------------------------------------------------
; 9-15-2000	Release	Rev 1.0
;================================================================================
; Applicability: F240,F241,C242,F243,F24xx.  (Peripheral Independant).
;
;
;================================================================================
; Routine Name: Generic function.        Routine Type: C Callable
;
; Description:
;  
;  C prototype : int F243_qep_theta_calc(struct QEP *p)
;================================================================================
                
; Definition of the QEP structure is as follows:
; typedef struct QEP       { int theta_elec, theta_mech, QepDir;
;                            int dwn_cnt_offset,theta_raw,mech_scaler,
;                            int pole_pairs;
;                            int rev_counter;
;                            int QEP_pulse_cnt, 
;                            int index_sync_flg ;
;                            int (*calc)();
;                            int (*init)();
;                            int (*indexevent)();
;                          } ;
;
;
;                       
;
;
;
;
;================================================================================
; System Constants:

CAL_ANGLE       .set    0000h
ENCODER_MAX     .set    4000

;================================================================================

;================================================================================
; Include files
                        .include ..\include\x24x.h
;================================================================================

;================================================================================
; Global symbols:
                        .def        _F243_qep_theta_calc
;================================================================================

__F243_qep_theta_calc_framesize .set 0001h
;================================================================================
_F243_qep_theta_calc:
                     POPD        *+
                SAR        AR0,*+
                SAR        AR1,*
                LARK        AR0,__F243_qep_theta_calc_framesize
                LAR        AR0,*0+,AR0
        CLRC        SXM        
;================================================================================
        SBRK        #3              ; Point AR0 to the first argument.
;--------------------------------------------------------------------------------
        LAR     AR2,*                ; point AR2 to address of data structure.
;--------------------------------------------------------------------------------
        ADRK    #3              ; Move AR0 back to FR0
                                ; ARP=AR0. AR0->FR0 AR2->theta_elec.
;--------------------------------------------------------------------------------
        MAR     *,AR2           ; ARP=AR2. AR0->FR0 AR2->theta_elec.
;--------------------------------------------------------------------------------
        ADRK    #2              ; ARP=AR2. AR0->FR0 AR2->Qep_Dir.
;--------------------------------------------------------------------------------
        SPLK    #1,*+,AR3      ; Set QEP direction to Up-count.
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset.
;--------------------------------------------------------------------------------
        LAR     AR3,#GPTCON     ; Point AR3 to GPTCON
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->GPTCON.
;--------------------------------------------------------------------------------
        BIT        *,BIT14                        ; Check direction of rotation (count)
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->GPTCON.
;--------------------------------------------------------------------------------
        BCND        UP_COUNT,TC        ; If=1 upcount - no need to correct.
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->GPTCON.
;--------------------------------------------------------------------------------
DWN_COUNT:        
        LAR        AR3,#T2CNT      ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        MAR     *,AR3           ; ARP=AR3. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        LACC    *,AR2           ; Get current ccounter value in ACC.        
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        SUBS    *-              ; Subtract the down count offset.
                                ; ARP=AR2. AR0->FR0 AR2->QEP_dir
                                ; AR3->T2CNT.
;--------------------------------------------------------------------------------
        SPLK    #0ffffh,*+      ; Indicate down_count
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        B       UC_01           ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
                                ; On arrival
UP_COUNT:                       ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->GPTCON.

        LAR        AR3,#T2CNT      ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        MAR     *,AR3           ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        LACC    *,AR2           ; Get current ccounter value in ACC.
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
UC_01        ADD        #CAL_ANGLE        ; add offset amount
                                ; ARP=AR2. AR0->FR0 AR2->down_count_offset AR3->T2CNT.
;--------------------------------------------------------------------------------
        ADRK    #1              ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
        SACL    *               ; store raw angle.
                                ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
        SUB     #ENCODER_MAX    ; Check for wrap-around.
                                ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
        BCND        NO_WRAP, LEQ         ; If yes "wrap-around" the angle value
                                ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
WRAP:
        LACC        *               ; Get raw angle.
                                ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
        SUB        #ENCODER_MAX         ; New theta = theta + Cal_angle - ENCODER_MAX 
                                ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
        SACL        *
                                ; ARP=AR2. AR0->FR0 AR2->theta_raw AR3->T2CNT.
;--------------------------------------------------------------------------------
NO_WRAP:                        ; Calculate Mechanical angle
        LT        *+              ; Load "raw" Mech angle
                                ; ARP=AR2. AR0->FR0 AR2->mech_scaler AR3->T2CNT.
;--------------------------------------------------------------------------------
        MPY     *+              ; Scale Mech angle to Q15 (0-->7FFF)
                                ; ARP=AR2. AR0->FR0 AR2->pole_pairs AR3->T2CNT.
;--------------------------------------------------------------------------------
        PAC                     ; ARP=AR2. AR0->FR0 AR2->pole_pairs AR3->T2CNT.
;--------------------------------------------------------------------------------
        LT      *               ; ARP=AR2. AR0->FR0 AR2->pole_pairs AR3->T2CNT.
;--------------------------------------------------------------------------------
        SBRK    #5              ; ARP=AR2. AR0->FR0 AR2->theta_mech  AR3->T2CNT.
;--------------------------------------------------------------------------------
        SACH    *,5             ; Store mechanical angle
                                ; ARP=AR2. AR0->FR0 AR2->theta_mech  AR3->T2CNT.
;--------------------------------------------------------------------------------
        MPY     *-                ; pole_pairs*theta_mech.
                                ; ARP=AR2. AR0->FR0 AR2->theta_elec AR3->T2CNT.
;--------------------------------------------------------------------------------
        PAC                     ; ARP=AR2. AR0->FR0 AR2->theta_mech  AR3->T2CNT.
;--------------------------------------------------------------------------------
        AND     #7fffh          ;force to Q15 positive and allow wrap
                                ; ARP=AR2. AR0->FR0 AR2->theta_elec AR3->T2CNT.
;--------------------------------------------------------------------------------
        SACL        *,AR1

_F243_qep_theta_calc_end:

               ;;;; MAR     *,AR1   ; can be removed if this condition is met on
               ;;;;                 ; every path to this code.

                SBRK        #(__F243_qep_theta_calc_framesize+1)
                LAR        AR0,*-
                PSHD        *
                RET