blind_node.s51

zigbee location examples

        CFI CFA_XSP16 XSP16+0
        CFI Valid
??Subroutine0_0:
        POP	DPH
        CFI CFA_SP SP+-4
        CFI DPH0 SameValue
        POP	DPL
        CFI CFA_SP SP+-3
        CFI DPL0 SameValue
        LJMP	?BRET
        CFI EndBlock cfiBlock2
//  372 
//  373 
//  374 /******************************************************************************
//  375 *
//  376 * This function assum that allRefNodesInRange is approximately sorted by RSSI
//  377 * value
//  378 *
//  379 * Build a new array (refNodesUsable) with all short addresses that should be
//  380 * used and calculate offset
//  381 *
//  382 ******************************************************************************/

        RSEG BANKED_CODE:CODE:NOROOT(0)
//  383 UINT8 ChooseNodesToUse(UINT16_2 xUpperLeft, UINT16_2 yUpperLeft)
ChooseNodesToUse:
        CFI Block cfiBlock3 Using cfiCommon0
        CFI Function ChooseNodesToUse
//  384 {
        MOV	A,#-0xa
        LCALL	?BANKED_ENTER_XDATA
        CFI DPH0 load(1, XDATA, add(CFA_XSP16, literal(-1)))
        CFI DPL0 load(1, XDATA, add(CFA_XSP16, literal(-2)))
        CFI ?BRET_EXT load(1, XDATA, add(CFA_XSP16, literal(-3)))
        CFI ?RET_HIGH load(1, XDATA, add(CFA_XSP16, literal(-4)))
        CFI ?RET_LOW load(1, XDATA, add(CFA_XSP16, literal(-5)))
        CFI R7 load(1, XDATA, add(CFA_XSP16, literal(-6)))
        CFI V1 load(1, XDATA, add(CFA_XSP16, literal(-7)))
        CFI V0 load(1, XDATA, add(CFA_XSP16, literal(-8)))
        CFI VB load(1, XDATA, add(CFA_XSP16, literal(-9)))
        CFI R6 load(1, XDATA, add(CFA_XSP16, literal(-10)))
        CFI CFA_SP SP+0
        CFI CFA_XSP16 add(XSP16, 10)
        ; Saved register size: 10
        ; Auto size: 0
//  385    UINT8 i, nbrOfUsableRefNodes;
//  386    UINT16_2 deltaX, deltaY;
//  387 
//  388    nbrOfUsableRefNodes = 0;
        MOV	R6,#0x0
//  389 
//  390    for(i = 0; i < LOC_ENGINE_NODE_CAPACITY && refNodesForCalculation[i].shortAddress != 0x000; i++)
        MOV	R7,#0x0
??ChooseNodesToUse_0:
        MOV	?V0 + 0,R7
        MOV	?V0 + 1,#0x0
        MOV	A,#0x3
        MOV	R0,#?V0 + 0
        LCALL	?S_SHL
        MOV	A,?V0 + 0
        ADD	A,#(refNodesForCalculation & 0xff)
        MOV	R0,A
        MOV	A,?V0 + 1
        ADDC	A,#((refNodesForCalculation >> 8) & 0xff)
        MOV	R1,A
        MOV	DPL,R0
        MOV	DPH,R1
        MOVX	A,@DPTR
        JNZ	??ChooseNodesToUse_1
        INC	DPTR
        MOVX	A,@DPTR
??ChooseNodesToUse_1:
        JZ	??ChooseNodesToUse_2
//  391    {
//  392       if(refNodesForCalculation[i].rssiAcc <= LOC_ENGINE_RSSI_MIN) // higher value is weaker signal
        MOV	DPL,R0
        MOV	DPH,R1
        INC	DPTR
        INC	DPTR
        INC	DPTR
        INC	DPTR
        INC	DPTR
        INC	DPTR
        CLR	C
        MOVX	A,@DPTR
        SUBB	A,#0x60
        INC	DPTR
        MOVX	A,@DPTR
        SUBB	A,#0x0
        JNC	??ChooseNodesToUse_3
//  393       {
//  394          deltaX = refNodesForCalculation[i].x - xUpperLeft;
//  395          deltaY = refNodesForCalculation[i].y - yUpperLeft;
//  396 
//  397          if(deltaX < LOC_ENGINE_MAX_X && deltaY < LOC_ENGINE_MAX_Y)
        MOV	DPL,R0
        MOV	DPH,R1
        INC	DPTR
        INC	DPTR
        MOVX	A,@DPTR
        CLR	C
        SUBB	A,R2
        MOV	?V0 + 0,A
        INC	DPTR
        MOVX	A,@DPTR
        SUBB	A,R3
        MOV	?V0 + 1,A
        CLR	C
        MOV	A,?V0 + 0
        SUBB	A,#-0x1
        MOV	A,?V0 + 1
        SUBB	A,#0x0
        JNC	??ChooseNodesToUse_3
        MOV	DPL,R0
        MOV	DPH,R1
        INC	DPTR
        INC	DPTR
        INC	DPTR
        INC	DPTR
        MOVX	A,@DPTR
        CLR	C
        SUBB	A,R4
        MOV	?V0 + 0,A
        INC	DPTR
        MOVX	A,@DPTR
        SUBB	A,R5
        MOV	?V0 + 1,A
        CLR	C
        MOV	A,?V0 + 0
        SUBB	A,#-0x1
        MOV	A,?V0 + 1
        SUBB	A,#0x0
        JNC	??ChooseNodesToUse_3
//  398          {
//  399             // Make sure each node can be placed inside 64x64 a square
//  400             refNodesUsable[nbrOfUsableRefNodes] = refNodesForCalculation[i].shortAddress;
        MOV	DPL,R0
        MOV	DPH,R1
        MOVX	A,@DPTR
        MOV	?V0 + 0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	?V0 + 1,A
        MOV	A,R6
        CLR	C
        RLC	A
        MOV	R0,A
        CLR	A
        RLC	A
        MOV	R1,A
        MOV	A,R0
        ADD	A,#(refNodesUsable & 0xff)
        MOV	DPL,A
        MOV	A,R1
        ADDC	A,#((refNodesUsable >> 8) & 0xff)
        MOV	DPH,A
        MOV	A,?V0 + 0
        MOVX	@DPTR,A
        INC	DPTR
        MOV	A,?V0 + 1
        MOVX	@DPTR,A
//  401             nbrOfUsableRefNodes++;
        INC	R6
//  402          }
//  403       }
//  404    }
??ChooseNodesToUse_3:
        INC	R7
        MOV	A,R7
        CLR	C
        SUBB	A,#0x8
        JNC	$+5
        LJMP	??ChooseNodesToUse_0 & 0xFFFF
//  405    return nbrOfUsableRefNodes;
??ChooseNodesToUse_2:
        MOV	A,R6
        MOV	R1,A
        MOV	R7,#0x2
        LJMP	?BANKED_LEAVE_XDATA
        CFI EndBlock cfiBlock3
//  406 }
//  407 
//  408 /******************************************************************************
//  409 *
//  410 * This function assum that allRefNodesInRange is sorted by RSSI
//  411 * value. The node located in position 0 will therefor be the nearest node.
//  412 *
//  413 * Find upper left corner of 64x64 square
//  414 *
//  415 ******************************************************************************/

        RSEG BANKED_CODE:CODE:NOROOT(0)
//  416 void CalculateUpperLeft(UINT16_2 *xUpperLeft, UINT16_2 *yUpperLeft, UINT16_2 *xStrongesNode, UINT16_2 *yStrongesNode, UINT8 *strongestRssi){
CalculateUpperLeft:
        CFI Block cfiBlock4 Using cfiCommon0
        CFI Function CalculateUpperLeft
        MOV	A,#-0xc
        LCALL	?BANKED_ENTER_XDATA
        CFI DPH0 load(1, XDATA, add(CFA_XSP16, literal(-1)))
        CFI DPL0 load(1, XDATA, add(CFA_XSP16, literal(-2)))
        CFI ?BRET_EXT load(1, XDATA, add(CFA_XSP16, literal(-3)))
        CFI ?RET_HIGH load(1, XDATA, add(CFA_XSP16, literal(-4)))
        CFI ?RET_LOW load(1, XDATA, add(CFA_XSP16, literal(-5)))
        CFI R7 load(1, XDATA, add(CFA_XSP16, literal(-6)))
        CFI V3 load(1, XDATA, add(CFA_XSP16, literal(-7)))
        CFI V2 load(1, XDATA, add(CFA_XSP16, literal(-8)))
        CFI V1 load(1, XDATA, add(CFA_XSP16, literal(-9)))
        CFI V0 load(1, XDATA, add(CFA_XSP16, literal(-10)))
        CFI VB load(1, XDATA, add(CFA_XSP16, literal(-11)))
        CFI R6 load(1, XDATA, add(CFA_XSP16, literal(-12)))
        CFI CFA_SP SP+0
        CFI CFA_XSP16 add(XSP16, 12)
        ; Saved register size: 12
        ; Auto size: 0
        MOV	A,#0xc
        LCALL	?XSTACK_DISP0_8
        MOVX	A,@DPTR
        MOV	R0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	R1,A
        MOV	A,#0xe
        LCALL	?XSTACK_DISP0_8
        MOVX	A,@DPTR
        MOV	R6,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	R7,A
        MOV	A,#0x10
        LCALL	?XSTACK_DISP0_8
        MOVX	A,@DPTR
        MOV	?V0 + 0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	?V0 + 1,A
//  417 
//  418    // Find position for the node with highest rssi value
//  419    // allRefNodesInRange is sorted by RSSI, so the highest value will be at place 0
//  420    *strongestRssi = refNodesForCalculation[0].rssiAcc;
        MOV	DPTR,#(refNodesForCalculation + 6)
        MOVX	A,@DPTR
        MOV	DPL,?V0 + 0
        MOV	DPH,?V0 + 1
        MOVX	@DPTR,A
//  421    *xStrongesNode = refNodesForCalculation[0].x;
        MOV	DPTR,#(refNodesForCalculation + 2)
        MOVX	A,@DPTR
        MOV	?V0 + 0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	?V0 + 1,A
        MOV	DPL,R0
        MOV	DPH,R1
        MOV	A,?V0 + 0
        MOVX	@DPTR,A
        INC	DPTR
        MOV	A,?V0 + 1
        MOVX	@DPTR,A
//  422    *yStrongesNode = refNodesForCalculation[0].y;
        MOV	DPTR,#(refNodesForCalculation + 4)
        MOVX	A,@DPTR
        MOV	?V0 + 0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	?V0 + 1,A
        MOV	DPL,R6
        MOV	DPH,R7
        MOV	A,?V0 + 0
        MOVX	@DPTR,A
        INC	DPTR
        MOV	A,?V0 + 1
        MOVX	@DPTR,A
//  423 
//  424    // X direction
//  425    if (*xStrongesNode < (LOC_ENGINE_MAX_X / 2)){
        MOV	DPL,R0
        MOV	DPH,R1
        MOVX	A,@DPTR
        MOV	R0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	R1,A
        CLR	C
        MOV	A,R0
        SUBB	A,#0x7f
        MOV	A,R1
        SUBB	A,#0x0
        JNC	??CalculateUpperLeft_0
//  426       *xUpperLeft = 0;
        MOV	DPL,R2
        MOV	DPH,R3
        CLR	A
        MOVX	@DPTR,A
        INC	DPTR
        SJMP	??CalculateUpperLeft_1
//  427    }
//  428    else if (*xStrongesNode > MAX_X - (LOC_ENGINE_MAX_X / 2)){
??CalculateUpperLeft_0:
        CLR	C
        MOV	A,R0
        SUBB	A,#-0x7f
        MOV	A,R1
        SUBB	A,#-0x1
        JC	??CalculateUpperLeft_2
//  429       *xUpperLeft =  MAX_X - LOC_ENGINE_MAX_X;
        MOV	DPL,R2
        MOV	DPH,R3
        CLR	A
        MOVX	@DPTR,A
        INC	DPTR
        MOV	A,#-0x1
        SJMP	??CalculateUpperLeft_1
//  430    }
//  431    else{
//  432       *xUpperLeft = *xStrongesNode - (LOC_ENGINE_MAX_X / 2);
??CalculateUpperLeft_2:
        MOV	A,#-0x7f
        ADD	A,R0
        MOV	R0,A
        MOV	A,#-0x1
        ADDC	A,R1
        MOV	R1,A
        MOV	DPL,R2
        MOV	DPH,R3
        MOV	A,R0
        MOVX	@DPTR,A
        INC	DPTR
        MOV	A,R1
??CalculateUpperLeft_1:
        MOVX	@DPTR,A
//  433    }
//  434    *xUpperLeft &= ~0x0003; // offset in meters, not 1/4
        MOV	DPL,R2
        MOV	DPH,R3
        MOVX	A,@DPTR
        ANL	A,#0xfc
        MOVX	@DPTR,A
        INC	DPTR
        MOVX	A,@DPTR
        MOVX	@DPTR,A
//  435 
//  436    // Y direction
//  437    if (*yStrongesNode < (LOC_ENGINE_MAX_Y / 2)){
        MOV	DPL,R6
        MOV	DPH,R7
        MOVX	A,@DPTR
        MOV	R0,A
        INC	DPTR
        MOVX	A,@DPTR
        MOV	R1,A
        CLR	C
        MOV	A,R0
        SUBB	A,#0x7f
        MOV	A,R1
        SUBB	A,#0x0
        JNC	??CalculateUpperLeft_3
//  438       *yUpperLeft = 0;
        MOV	DPL,R4
        MOV	DPH,R5
        CLR	A
        MOVX	@DPTR,A
        INC	DPTR
        SJMP	??CalculateUpperLeft_4
//  439    }
//  440    else if (*yStrongesNode > MAX_X - (LOC_ENGINE_MAX_Y / 2)){
??CalculateUpperLeft_3:
        CLR	C
        MOV	A,R0
        SUBB	A,#-0x7f
        MOV	A,R1
        SUBB	A,#-0x1
        JC	??CalculateUpperLeft_5
//  441       *yUpperLeft =  MAX_Y - LOC_ENGINE_MAX_Y;
        MOV	DPL,R4
        MOV	DPH,R5
        CLR	A
        MOVX	@DPTR,A
        INC	DPTR
        MOV	A,#-0x1
        SJMP	??CalculateUpperLeft_4
//  442    }
//  443    else{
//  444       *yUpperLeft = *yStrongesNode - (LOC_ENGINE_MAX_Y / 2);
??CalculateUpperLeft_5:
        MOV	A,#-0x7f
        ADD	A,R0
        MOV	R0,A
        MOV	A,#-0x1
        ADDC	A,R1
        MOV	R1,A
        MOV	DPL,R4
        MOV	DPH,R5