
(* TODO: Add your comment here *)
FUNCTION_BLOCK PMC_ReadStreamFeedback
	IF Enable = TRUE THEN
		(* Statement section IF*)
		//calculate how many stream channels are possible		
		FeedbackPosition := 0;
		
		IF StreamChannelID > 0 AND XBotAxisID > 0 AND XBotAxisID < 7 AND PMController.ConfiguredFeedbackStreamAxis > 0 AND PMController.ConfiguredFeedbackStreamAxis < 64 THEN
			(* Statement section IF*)	
			//calculate the number of axis used
			AxisCount := 0;
			CurrentAxisOffset := 0;
			
			tempFBAxis := PMController.ConfiguredFeedbackStreamAxis;
			FOR index := 0 TO 5 DO
				IF index = XBotAxisID - 1 THEN
					CurrentAxisOffset := AxisCount;
				END_IF	
				
				IF (tempFBAxis AND 1) = 1 THEN
					AxisCount := AxisCount + 1;
				END_IF				
				tempFBAxis := SHR(tempFBAxis, 1);
			END_FOR;
			
			ByteOffset := (StreamChannelID-1) * AxisCount * 4 + CurrentAxisOffset * 4;
			tempFBAxis := PMController.ConfiguredFeedbackStreamAxis;
			tempFBAxis := SHR(tempFBAxis, XBotAxisID-1);
			tempFBAxis := tempFBAxis AND 1;
						
			IF P2H_REAL_TIME_OFFSET + ByteOffset + 4 <= P2H_DI_OFFSET AND tempFBAxis = 1 THEN
				memcpy(pDest := ADR(FeedbackPosition),pSrc := PMController.DataFromPMCAddress + P2H_START + P2H_REAL_TIME_OFFSET + ByteOffset,length := 4);
				Valid := TRUE;
			ELSE
				Valid := FALSE;				
			END_IF;										
		ELSE
			Valid := FALSE;
		END_IF;
	ELSE
		Valid := FALSE;
	END_IF;
END_FUNCTION_BLOCK
