****************************************
*	 Prozeßnummer = 1
*	 Delay = 1000
*	 Eventsource = 0
*	 Number of Loops = 0
*	 Priorität = 0
*	 Version = 1
*	 FastStop = 0
*	 AdbasicVersion = 4000001
*	 ATSRAM = 0
*	 OPT_LEVEL = 1
*	 SAVECOMPIL = 0
****************************************
#DEFINE Deg2Rad (3.141592654/180)

FUNCTION GetByteFromFOB(numPS) AS SHORT
    
		DIM TimeOutFOBInit,TimeOutFOB AS INTEGER
    DIM byte_FOB AS SHORT
    
		TimeOutFOBInit = read_timer()
    DO
    	byte_FOB = read_fifo(numPS)
    	TimeOutFOB = read_timer() - TimeOutFOBInit
    UNTIL ((byte_FOB<>-1) or (TimeOutFOB>40000))
    
		GetByteFromFOB = byte_FOB

ENDFUNCTION

SUB InitFOB(X,Y,Z,THETAX,THETAY,THETAZ,numPS)
			
			DIM SINAERCOSAER_FLOAT AS FLOAT
			DIM SINAERCOSAER_INT AS INTEGER
			DIM error AS INTEGER

			'THETAX <=> Roll
      'THETAY <=> Elevation
      'THETAZ <=> Azimuth
		
      'Position + angles
      error=write_fifo(numPS,89)
      'Demande un seul point
      error=write_fifo(numPS,66)
      'sleep(60)
      'Recup du point
      IF (ReadFOB(X,Y,Z,THETAX,THETAY,THETAZ,numPS) = 255) then
				'rien a faire... données non initialisées ms on continu qd même
      ENDIF
      'Commande ANGLE ALIGN 1
      error = write_fifo(numPS,74)
      
			SINAERCOSAER_FLOAT = sin(THETAZ*Deg2Rad) 'sin(A)
      IF (SINAERCOSAER_FLOAT >0.99996) THEN SINAERCOSAER_FLOAT =0.99998
      SINAERCOSAER_INT	=32768*SINAERCOSAER_FLOAT  'mise a l'echelle de sin(A)
      SINAERCOSAER_INT = SINAERCOSAER_INT AND 0000FFFFh
      error=write_fifo(numPS,SINAERCOSAER_INT)
      SINAERCOSAER_INT = shift_right(SINAERCOSAER_INT,8)
      error=write_fifo(numPS,SINAERCOSAER_INT)
			
			
			SINAERCOSAER_FLOAT = cos(THETAZ*Deg2Rad) 'cos(A)
      IF (SINAERCOSAER_FLOAT >0.99996) THEN SINAERCOSAER_FLOAT =0.99998
      SINAERCOSAER_INT	=32768*SINAERCOSAER_FLOAT  'mise a l'echelle de cos(A)
      SINAERCOSAER_INT = SINAERCOSAER_INT AND 0000FFFFh
      error = write_fifo(numPS,SINAERCOSAER_INT)
      SINAERCOSAER_INT = shift_right(SINAERCOSAER_INT,8)
      error=write_fifo(numPS,SINAERCOSAER_INT)


			SINAERCOSAER_FLOAT = sin(THETAY*Deg2Rad) 'sin(E)
      IF (SINAERCOSAER_FLOAT >0.99996) THEN SINAERCOSAER_FLOAT =0.99998
      SINAERCOSAER_INT	=32768*SINAERCOSAER_FLOAT  'mise a l'echelle de sin(E)
      SINAERCOSAER_INT = SINAERCOSAER_INT AND 0000FFFFh
      error = write_fifo(numPS,SINAERCOSAER_INT)
      SINAERCOSAER_INT = shift_right(SINAERCOSAER_INT,8)
      error=write_fifo(numPS,SINAERCOSAER_INT)
			
      
			SINAERCOSAER_FLOAT = cos(THETAY*Deg2Rad) 'cos(E)
      IF (SINAERCOSAER_FLOAT >0.99996) THEN SINAERCOSAER_FLOAT =0.99998
      SINAERCOSAER_INT	=32768*SINAERCOSAER_FLOAT  'mise a l'echelle de cos(E)
      SINAERCOSAER_INT = SINAERCOSAER_INT AND 0000FFFFh
      error = write_fifo(numPS,SINAERCOSAER_INT)
      SINAERCOSAER_INT = shift_right(SINAERCOSAER_INT,8)
      error=write_fifo(numPS,SINAERCOSAER_INT)
			
			
			SINAERCOSAER_FLOAT = sin(THETAX*Deg2Rad) 'sin(R)
      IF (SINAERCOSAER_FLOAT >0.99996) THEN SINAERCOSAER_FLOAT =0.99998
      SINAERCOSAER_INT	=32768*SINAERCOSAER_FLOAT  'mise a l'echelle de sin(R)
      SINAERCOSAER_INT = SINAERCOSAER_INT AND 0000FFFFh
      error = write_fifo(numPS,SINAERCOSAER_INT)
      SINAERCOSAER_INT = shift_right(SINAERCOSAER_INT,8)
      error=write_fifo(numPS,SINAERCOSAER_INT)
			
			
			SINAERCOSAER_FLOAT = cos(THETAX*Deg2Rad) 'cos(R)
      IF (SINAERCOSAER_FLOAT >0.99996) THEN SINAERCOSAER_FLOAT =0.99998
      SINAERCOSAER_INT	=32768*SINAERCOSAER_FLOAT  'mise a l'echelle de cos(R)
      SINAERCOSAER_INT = SINAERCOSAER_INT AND 0000FFFFh
      error = write_fifo(numPS,SINAERCOSAER_INT)
      SINAERCOSAER_INT = shift_right(SINAERCOSAER_INT,8)
      error=write_fifo(numPS,SINAERCOSAER_INT)
			
			'Lance le Mode Stream binaire
      error=write_fifo(numPS,64)


ENDSUB

FUNCTION ReadFOB(X,Y,Z,THETAX,THETAY,THETAZ,numPS) AS INTEGER
    
		DIM PhaseBit_FOB,byte_FOB,fin_FOB AS SHORT
    DIM Dummy_SUMMA_X,Dummy_SUMMA_Y AS INTEGER
    DIM FOB_BUFFER[12] AS SHORT
    DIM FOB_N_BYTE AS INTEGER
		DIM error AS INTEGER
    
		ReadFOB = 0
    fin_FOB = 0
    DO 
      FOB_BUFFER[1]= GetByteFromFOB(numPS)
      IF (FOB_BUFFER[1] <> - 1) THEN
        PhaseBit_FOB = FOB_BUFFER[1] and 10000000B
        IF (PhaseBit_FOB = 10000000B) THEN fin_FOB = 1
      ELSE
        ReadFOB = 1
        fin_FOB = 1
        FOB_BUFFER[1]= (FOB_BUFFER[1] AND 01111111B)
      ENDIF
    UNTIL (fin_FOB = 1)
    
		IF (ReadFOB = 0) THEN
        
			  FOR FOB_N_BYTE = 2 to 12
          
					byte_FOB = GetByteFromFOB(numPS)
          IF (byte_FOB = - 1) THEN ReadFOB = 2
          IF (ReadFOB = 0) THEN
            FOB_BUFFER[FOB_N_BYTE] = byte_FOB
            byte_FOB = byte_FOB AND 10000000B
            IF (byte_FOB = 10000000B) THEN ReadFOB = 3
            IF ((ReadFOB = 0) AND (FOB_N_BYTE=12)) THEN
              
							FOB_BUFFER[1]=shift_left((shift_left(FOB_BUFFER[1],1)+FOB_BUFFER[2]*256),1)
              IF (FOB_BUFFER[1]>7FFFH) THEN FOB_BUFFER[1]=FOB_BUFFER[1]-65536
              X = FOB_BUFFER[1]/32768*36.0*2.54
              
							FOB_BUFFER[3]=shift_left((shift_left(FOB_BUFFER[3],1)+FOB_BUFFER[4]*256),1)
              IF (FOB_BUFFER[3]>7FFFH)THEN FOB_BUFFER[3]=FOB_BUFFER[3]-65536
              Y = FOB_BUFFER[3]/32768*36.0*2.54
              
							FOB_BUFFER[5]=shift_left((shift_left(FOB_BUFFER[5],1)+FOB_BUFFER[6]*256),1)
              IF (FOB_BUFFER[5]>7FFFH)THEN FOB_BUFFER[5]=FOB_BUFFER[5]-65536
              Z = FOB_BUFFER[5]/32768*36.0*2.54
              
							FOB_BUFFER[7]=shift_left((shift_left(FOB_BUFFER[7],1)+FOB_BUFFER[8]*256),1)
              IF (FOB_BUFFER[7]>7FFFH)THEN FOB_BUFFER[7]=FOB_BUFFER[7]-65536
              THETAZ = FOB_BUFFER[7]/32768*180
              
							FOB_BUFFER[9]=shift_left((shift_left(FOB_BUFFER[9],1)+FOB_BUFFER[10]*256),1)
              IF (FOB_BUFFER[9]>7FFFH)THEN FOB_BUFFER[9]=FOB_BUFFER[9]-65536
              THETAY = FOB_BUFFER[9]/32768*180
              
							FOB_BUFFER[11]=shift_left((shift_left(FOB_BUFFER[11],1)+FOB_BUFFER[12]*256),1)
              IF (FOB_BUFFER[11]>7FFFH)THEN FOB_BUFFER[11]=FOB_BUFFER[11]-65536
              THETAX = FOB_BUFFER[11]/32768*180
              
							'Vide FIFO
              DO
                error = read_fifo(numPS)
              UNTIL (error = -1)
              ReadFOB = 255
            
						ENDIF
          ENDIF
        NEXT FOB_N_BYTE
    ENDIF
ENDFUNCTION
