Homec4science
Diffusion HHRI-software (hri-gen2) Browse

R2671/MATLAB/Functions/grabStreamPackets.m
057403177fbchri-gen2

Last Change
Raw File

grabStreamPackets.m
View Options

This document is not UTF8. It was detected as Shift JIS and converted to UTF8 for display.
function [Data,ERR] = grabStreamPackets(STREAMING_VARIABLES,COMOPT,s)
% Reads all the packets in the USART buffer of Matlab
%
% ERR = 1: There was no complete packet in the buffer
% ERR = 2: There were not only streaming packets
StreamSize = sum(dec2bin(STREAMING_VARIABLES,32)-'0'); %number of '1' in binary STREAMING_VARIABLES
ERR = 0;
BA = get(s,{'BytesAvailable'});
if(BA{1}<StreamSize*8) %There has to be at least one complete packet
ERR=1;
Data=[];
return;
end
tmp = uint8(fread(s,BA{1},'uint8')); % Grab all available bytes
ID = find(tmp >= COMOPT.StartFrame); % ID de tout les d饕uts de packet
CompletPacketNum = max(size(ID))-1;
if(CompletPacketNum<2) return; end
Vars_index = 33-find(dec2bin(STREAMING_VARIABLES,32)-'0'); %index of the streamed variables
Data = nan(CompletPacketNum,25);
% All Streamed variables are put in 'Data' as if they were uint32
for i=1:CompletPacketNum
packet = tmp( ID(i)+1 : ID(i+1)-1);
if size(packet,1) == StreamSize*8 %a packet should contain 8 half bytes per variable
if tmp(ID(i))-COMOPT.StartFrame == COMOPT.STM_MESSAGE_STREAMING_PACKET
j = StreamSize + 1;
for k = Vars_index
j = j - 1;
if sum(k-1 == [1,2,3,4,5,6,10,13:25]) %Var is a float
Data(i,k) = typecast([2^4*packet((j-1)*8 + 1) + packet((j-1)*8 + 2), ...
2^4*packet((j-1)*8 + 3) + packet((j-1)*8 + 4), ...
2^4*packet((j-1)*8 + 5) + packet((j-1)*8 + 6), ...
2^4*packet((j-1)*8 + 7) + packet((j-1)*8 + 8)],'single');
elseif sum(k-1 == [8,9]) %Var is a int32
Data(i,k) = typecast([2^4*packet((j-1)*8 + 1) + packet((j-1)*8 + 2), ...
2^4*packet((j-1)*8 + 3) + packet((j-1)*8 + 4), ...
2^4*packet((j-1)*8 + 5) + packet((j-1)*8 + 6), ...
2^4*packet((j-1)*8 + 7) + packet((j-1)*8 + 8)],'int32');
else %Var is a uint32
Data(i,k) = typecast([2^4*packet((j-1)*8 + 1) + packet((j-1)*8 + 2), ...
2^4*packet((j-1)*8 + 3) + packet((j-1)*8 + 4), ...
2^4*packet((j-1)*8 + 5) + packet((j-1)*8 + 6), ...
2^4*packet((j-1)*8 + 7) + packet((j-1)*8 + 8)],'uint32');
end
end
else
ERR = 2;
end
else
disp('some packets were not complete');
end
end
c4science · Help