csdhl.asm

PSoC(可编程片上系统)是Cypress半导体公司生产的包含有8位微处理器核和数字与模拟混合的信号阵列芯片

;    of this function.  The same is true for all RAM page pointer registers in
;    the Large Memory Model.  When necessary, it is the calling function's
;    responsibility to perserve their values across calls to fastcall16 
;    functions.
;
 CSD_wGetCentroidPos:
_CSD_wGetCentroidPos:
; Legacy function name labels below, do not use for new designs
 CSD_bGetCentroidPos:
_CSD_bGetCentroidPos:
 CSD_iGetCentroidPos:
_CSD_iGetCentroidPos:
; End legacy function names
   RAM_PROLOGUE RAM_USE_CLASS_3
   RAM_SETPAGE_CUR 0 ; direct access ram will always be in interrupt ram area
   RAM_X_POINTS_TO_INDEXPAGE

; First get the starting location and size of this sensor group
   asl A ; multiply by two to get diplex
   mov X, A
   index CSD_Diplex_Table ; get the diplex table MSB and LSB
   mov [CSD_bDiplexMSB], A
   mov A, X
   inc A
   index CSD_Diplex_Table
   mov [CSD_bDiplexLSB], A
   mov A, X
   mov [CSD_bStartIndex], A ; Use bStartIndex as a temp storage byte
   asl A				  
   add [CSD_bStartIndex], A
   mov A, [CSD_bStartIndex] ; A now has slider offset multiplied by six
   mov X, A
   index CSD_Group_Table ; get first sensor index
   mov [CSD_bEndOfArray], A
   mov [CSD_bCurPos], A
   mov [CSD_bStartIndex], A
   mov A, X
   inc A
   index CSD_Group_Table ; get size of group
   add [CSD_bEndOfArray], A ; Store for later
   mov A, X
   add A, 2
   index CSD_Group_Table ; get if diplexed or not and max size of centroid (data = 0 if not diplexed)
   mov [CSD_bDiplexInfo], A ; Store for later
   mov A, X
   add A, 3
   mov [CSD_bSnsMaskPtr], A ; Store starting position in an unused temp CSD_bSnsMaskPtr

; First find the starting location and size of the largest centroid
   mov [CSD_bBiggestCtrdStartPos], 0
   mov [CSD_bBiggestCtrdSize], 0
   mov [CSD_bCurCtrdStartPos], 0
   mov [CSD_bCurCtrdSize], 0

   mov A, [CSD_bStartIndex] 
   asl A ; multiply by two because we are using ints
   mov X, A

.LocateCtrd:
   RAM_SETPAGE_IDX >CSD_waSnsDiff
   mov A, [X+CSD_waSnsDiff]
   jnz .DiffIsNotZero
   mov A, [X+CSD_waSnsDiff+1]
   jnz .DiffIsNotZero
; The difference is zero, we either just ended a centroid or are between centroids
; First check the Current Size, if zero, we are in the middle of zeros, else, we just ended
.DifferenceIsZero:
   mov A, [CSD_bCurCtrdSize]
   jz .LocateCtrdNextSensor
; A centroid just ended.  Is it the largest centroid?  
   cmp A, [CSD_bBiggestCtrdSize]
   jc .ClearCurCtrdSize
; It is the biggest so far, store as biggest
   mov [CSD_bBiggestCtrdSize], [CSD_bCurCtrdSize]
   mov [CSD_bBiggestCtrdStartPos], [CSD_bCurCtrdStartPos]
.ClearCurCtrdSize:
   mov [CSD_bCurCtrdSize], 0
   jmp .LocateCtrdNextSensor
   
;The difference is not zero, we either just started, or are in the middle of a centroid
.DiffIsNotZero:
   mov A, [CSD_bCurCtrdSize]
   jnz .IncCtrdSize
; Centroid just began, store the start pos   
   mov A, [CSD_bCurPos]
   mov [CSD_bCurCtrdStartPos], A
.IncCtrdSize:   
   inc [CSD_bCurCtrdSize] 
; Find out the next position
.LocateCtrdNextSensor:   
   inc X
   inc X
   inc [CSD_bCurPos] 
   mov A, [CSD_bCurPos]
   cmp A, [CSD_bEndOfArray] ; Check for the end of the array
   jc .LocateCtrd
; Either at end of array, or diplexed
   tst [CSD_bDiplexInfo], 0xff ; check if diplexed
   jz .EndOfLocateCtrd
; Diplexed, so now find out if at end
   sub A, [CSD_bEndOfArray] ; subtract the size of the array
   add A, [CSD_bStartIndex] ; for comparison
   cmp A, [CSD_bEndOfArray] ; Check for the end of the array
   jnc .EndOfLocateCtrd
; Not the end of the diplexed array, find out offset of next position
   mov A, [CSD_bCurPos]
   sub A, [CSD_bStartIndex]
   add A, [CSD_bDiplexLSB]
   mov X, A
   mov A, [CSD_bDiplexMSB]
   adc A, 0 ; check if carry
   romx ; get the offset from the start
   add A, [CSD_bStartIndex]
   asl A ; because using ints
   mov X, A
   jmp .LocateCtrd
.EndOfLocateCtrd:
; Need to check if the current centroid is biggest
   mov A, [CSD_bCurCtrdSize]
   jz .CalculateCtrd
; There was a centroid at the end, is it biggest?
   cmp A, [CSD_bBiggestCtrdSize]
   jc .CalculateCtrd ; if two are the same size, last one wins
; It is the biggest so far, store as biggest
   mov [CSD_bBiggestCtrdSize], [CSD_bCurCtrdSize]
   mov [CSD_bBiggestCtrdStartPos], [CSD_bCurCtrdStartPos]

.CalculateCtrd:
   mov A, [CSD_bBiggestCtrdSize]
   jz .COM_Error
   mov A, [CSD_bDiplexInfo] ; check if diplexed
   jz .COM_Init
   cmp [CSD_bBiggestCtrdSize], 2
   jc .COM_Error ; for diplexing, one or less is too small
   cmp A, [CSD_bBiggestCtrdSize]
   jc .COM_Error ; for diplexing, check if centroid is too large

.COM_Init:
   mov A, [CSD_bBiggestCtrdStartPos] ; Use for current position, may be diplexed
   mov [CSD_bCurPos], A
   mov [CSD_wDenom+MSB], 0 ; Clear the numerator and denominator
   mov [CSD_wDenom+LSB], 0
   mov [CSD_dNumer+MMSB], 0
   mov [CSD_dNumer+MLSB], 0
   mov [CSD_dNumer+LMSB], 0
   mov [CSD_dNumer+LLSB], 0

.COM_NextPosition:
   mov A, [CSD_bCurPos]
   cmp A, [CSD_bEndOfArray]
   jnc .COM_CheckDiplex
   asl A
   mov X, A
   jmp .COM_AddElement
; Must be diplexed, check for safe measure
.COM_CheckDiplex:
   tst [CSD_bDiplexInfo], 0xff ; check if diplexed
   jz .COM_Compute
; Find out offset of next position
   mov A, [CSD_bCurPos]
   sub A, [CSD_bStartIndex]
   add A, [CSD_bDiplexLSB]
   mov X, A
   mov A, [CSD_bDiplexMSB]
   adc A, 0 ; check if carry
   romx ; get the offset from the start
   add A, [CSD_bStartIndex]
   asl A ; because using ints
   mov X, A
.COM_AddElement:
; Subtract the noise from the difference, this will yield a more accurate result
   RAM_SETPAGE_IDX >CSD_waSnsDiff
   mov A, [CSD_bNoiseThreshold]
   sub [X+CSD_waSnsDiff+LSB], A
   sbb [X+CSD_waSnsDiff+MSB], 0
; Store a copy of the sensor difference in CSD_dMultTempX
   mov A, [X+CSD_waSnsDiff+LSB]
   mov [CSD_dMultTempX+LLSB], A
; Add LSB to denominator
   add [CSD_wDenom+LSB], A
   mov A, [X+CSD_waSnsDiff]
   mov [CSD_dMultTempX+LMSB], A
   mov [CSD_dMultTempX+MLSB], 0
   mov [CSD_dMultTempX+MMSB], 0
; Add MSB to denominator
   adc [CSD_wDenom], A
   mov A, [CSD_bCurPos]
   sub A, [CSD_bStartIndex] ; we need offset from beginning of group
   mov [CSD_wDivBtwSns+MSB], 0 
   mov [CSD_wDivBtwSns+LSB], A
   
   call .MultiplyNumeratorWhole

   inc [CSD_bCurPos]
   dec [CSD_bBiggestCtrdSize]
   jz .COM_Compute
   jmp .COM_NextPosition

.COM_Compute:
   mov A, [CSD_dNumer+LLSB] ; Move numerator to temp
   mov [CSD_dMultTempX+LLSB], A
   mov [CSD_dMultTempY+LLSB], A
   mov A, [CSD_dNumer+LMSB]
   mov [CSD_dMultTempX+LMSB], A
   mov [CSD_dMultTempY+LMSB], A
   mov A, [CSD_dNumer+MLSB]
   mov [CSD_dMultTempX+MLSB], A
   mov [CSD_dMultTempY+MLSB], A
   mov A, [CSD_dNumer+MMSB]
   mov [CSD_dMultTempX+MMSB], A
   mov [CSD_dMultTempY+MMSB], A

   mov [CSD_dNumer+MMSB], 0 ; Clear numerator
   mov [CSD_dNumer+MLSB], 0
   mov [CSD_dNumer+LMSB], 0
   mov [CSD_dNumer+LLSB], 0
   
   mov A, [CSD_bSnsMaskPtr] ; Retrieve starting position in index of the DivBtwSns
   push A
   index CSD_Group_Table ; get MSB of whole multiplier
   mov [CSD_wDivBtwSns+MSB], A
   pop A
   inc A
   push A
   index CSD_Group_Table ; get LSB of whole multiplier
   mov [CSD_wDivBtwSns+LSB], A
   call .MultiplyNumeratorWhole ; Multiplies by whole part
   pop A
   inc A
   index CSD_Group_Table ; get byte of fractional multiplier
   mov [CSD_wDivBtwSns+LSB], A
   call .MultiplyNumeratorFraction ; Multiplies by fractional part

; Now do the division of the numerator and denominator

; Round up the temp by half of the denominator (0.5 gets 1)
   mov A, [CSD_wDenom+MSB]
   mov [CSD_dMultTempX+MSB], A
   mov A, [CSD_wDenom+LSB]
   mov [CSD_dMultTempX+LSB], A
   and F, ~0x04 ; Clear carry bit if set
   rrc [CSD_dMultTempX+MSB] ; divide denominator by 2
   rrc [CSD_dMultTempX+LSB]
   mov A, [CSD_dMultTempX+LSB]
   add [CSD_dNumer+LLSB], A ; add 1/2 denominator 
   mov A, [CSD_dMultTempX+MSB]
   adc [CSD_dNumer+LMSB], A
   adc [CSD_dNumer+MLSB], 0
   adc [CSD_dNumer+MMSB], 0

; Compute the division of numerator divided by denominator
   asl [CSD_dNumer+LLSB] 
   rlc [CSD_dNumer+LMSB]	 
   rlc [CSD_dNumer+MLSB]	 
   rlc [CSD_dNumer+MMSB]
   mov X, 16	 
.DivideLoop:
; Subtract the divisor or denominator from the MMSB MLSB dividend or numerator
   mov A, [CSD_wDenom+LSB]
   sub [CSD_dNumer+MLSB], A 
   mov A, [CSD_wDenom+MSB]
   sbb [CSD_dNumer+MMSB], A
; Check if less than zero
   jnc .SetLSbToOneAndShift
.AddBackSetLSbToZeroAndShift:
; Less than zero so add back to dividend and shift a zero into the dividend
   mov A, [CSD_wDenom+LSB]
   add [CSD_dNumer+MLSB], A 
   mov A, [CSD_wDenom+MSB]
   adc [CSD_dNumer+MMSB], A
   asl [CSD_dNumer+LLSB] 
   rlc [CSD_dNumer+LMSB]	 
   rlc [CSD_dNumer+MLSB]	 
   rlc [CSD_dNumer+MMSB]
   dec X
   jnz .DivideLoop
   jmp .EndOfDivide
.SetLSbToOneAndShift:
   asl [CSD_dNumer+LLSB]
   or [CSD_dNumer+LLSB], 0x01
   rlc [CSD_dNumer+LMSB]	 
   rlc [CSD_dNumer+MLSB]	 
   rlc [CSD_dNumer+MMSB]
   dec X
   jnz .DivideLoop
.EndOfDivide:
; MMSB and MLSB have carry after shifted right once
   rrc [CSD_dNumer+MMSB]
   rrc [CSD_dNumer+MLSB]	 
; Load return value
   mov X, [CSD_wCtrdPos+MSB]
   mov A, [CSD_wCtrdPos+LSB]	 
   jmp .EndGetCtrdPos
.COM_Error:
; Load error return value
   mov A, 0xff
   mov X, 0xff
.EndGetCtrdPos:
; Algorithm is finished, position is in [CSD_wCtrdPos]
   RAM_EPILOGUE RAM_USE_CLASS_3
   ret

; Multiplication algorithm (for whole numbers)
.MultiplyNumeratorWhole:
   mov A, 0x01
.CheckToAddNext2xLSB:
   push A
   and A, [CSD_wDivBtwSns+LSB] ; See if current 2^x needs added
   jz .SkipAddAndShiftLeftLSB
   mov A, [CSD_dMultTempX+LLSB]
   add [CSD_dNumer+LLSB], A
   mov A, [CSD_dMultTempX+LMSB]
   adc [CSD_dNumer+LMSB], A
   mov A, [CSD_dMultTempX+MLSB]
   adc [CSD_dNumer+MLSB], A
   mov A, [CSD_dMultTempX+MMSB]
   adc [CSD_dNumer+MMSB], A
.SkipAddAndShiftLeftLSB:
   asl [CSD_dMultTempX+LLSB] 
   rlc [CSD_dMultTempX+LMSB] 
   rlc [CSD_dMultTempX+MLSB] 
   rlc [CSD_dMultTempX+MMSB] 
   pop A
   asl A
   jnc .CheckToAddNext2xLSB
   mov A, [CSD_wDivBtwSns+MSB]
   jz .EndMultiplyNumeratorWhole ; only multiply by MSB if needed
   mov A, 0x01
.CheckToAddNext2xMSB:
   push A
   and A, [CSD_wDivBtwSns+MSB] ; See if current 2^x needs added
   jz .SkipAddAndShiftLeftMSB
   mov A, [CSD_dMultTempX+LLSB]
   add [CSD_dNumer+LLSB], A
   mov A, [CSD_dMultTempX+LMSB]
   adc [CSD_dNumer+LMSB], A
   mov A, [CSD_dMultTempX+MLSB]
   adc [CSD_dNumer+MLSB], A
   mov A, [CSD_dMultTempX+MMSB]
   adc [CSD_dNumer+MMSB], A
.SkipAddAndShiftLeftMSB:
   asl [CSD_dMultTempX+LLSB] 
   rlc [CSD_dMultTempX+LMSB] 
   rlc [CSD_dMultTempX+MLSB] 
   rlc [CSD_dMultTempX+MMSB] 
   pop A
   asl A
   jnc .CheckToAddNext2xMSB
.EndMultiplyNumeratorWhole:
   ret

; Multiplication algorithm (for fractional numbers)
.MultiplyNumeratorFraction:
   and F, ~0x04 ; clear carry bit if set
   rrc [CSD_dMultTempY+MMSB] 
   rrc [CSD_dMultTempY+MLSB] 
   rrc [CSD_dMultTempY+LMSB] 
   rrc [CSD_dMultTempY+LLSB] 
   mov A, 0x80
.CheckToAddNextHalf:
   push A
   and A, [CSD_wDivBtwSns+LSB]
   jz .SkipAddAndShiftRight
   mov A, [CSD_dMultTempY+LLSB]
   add [CSD_dNumer+LLSB], A
   mov A, [CSD_dMultTempY+LMSB]
   adc [CSD_dNumer+LMSB], A
   mov A, [CSD_dMultTempY+MLSB]
   adc [CSD_dNumer+MLSB], A
   mov A, [CSD_dMultTempY+MMSB]
   adc [CSD_dNumer+MMSB], A
.SkipAddAndShiftRight:   
   asr [CSD_dMultTempY+MMSB] 
   rrc [CSD_dMultTempY+MLSB] 
   rrc [CSD_dMultTempY+LMSB] 
   rrc [CSD_dMultTempY+LLSB] 
   pop A
   and F, ~0x04 ; clear carry bit if set
   rrc A
   jnc .CheckToAddNextHalf
   ret

.ENDSECTION
ENDIF