readdata.m

基于串行接口的GPS驱动程序

function [ value, bytes ] = ReadData(gps, id, count, type)
% ReadData Read formatted data from the GPS, managing DLE stuffing

global pid_dle_byte pid_nak_byte

sz = sizeof(type); % Number of bytes
bytes = zeros(1, sz * count);
n = 0;
for i=1:count
    value(i) = 0;
    for j=1:sz
        % Read a byte
        byte = fread(gps, 1);
        n = n + 1;
        bytes(n) = byte;
        
        % If we've read a DLE, there should be another one immediately
        % following. If not, this data is corrupt.
        if (pid_dle_byte == byte)
            b = fread(gps, 1);
            if (pid_dle_byte ~= b)
                WritePacket(gps, pid_nak_byte, id);
                error('Missing DLE stuffing byte');
            end
        end
        
        % Convert (if necessary) a multi-byte numeric value from the stream
        % of bytes to the correct value. The GPS uses little-endian
        % storage.
        value(i) = bitor(value(i), bitshift(byte, 8*(j-1), sz*8));    
    end
    
    % Correct for 2s complement negative numbers, if necessary. Only do
    % this for integer types with the high order bit set.
    if ((type(1) == 'i') && (byte >= 128) )
        value(i) = bitset(value(i), 32, 0);
        value(i) = value(i) - (2^((sz*8)-1));
    end
    
    % Interpret the bits as an IEEE floating point number (1 sign bit, 8
    % exponent bits, 23 mantissa bits)
    %
    % If E=255 and F is nonzero, then V=NaN ("Not a number") 
    % If E=255 and F is zero and S is 1, then V=-Infinity 
    % If E=255 and F is zero and S is 0, then V=Infinity 
    % If 0<E<255 then V=(-1)**S * 2 ** (E-127) * (1.F) where "1.F" is 
    %    intended to represent the binary number created by prefixing F 
    %    with an implicit leading 1 and a binary point. 
    % If E=0 and F is nonzero, then V=(-1)**S * 2 ** (-126) * (0.F) These 
    %    are "unnormalized" values. 
    % If E=0 and F is zero and S is 1, then V=-0 
    % If E=0 and F is zero and S is 0, then V=0 
    % 
    % ANSI/IEEE Standard 754-1985
    % Standard for Binary Floating Point Arithmetic

    if (type(1) == 'f')
        s = bitget(value(i), 32);
        e = bitshift(bitand(value(i), bitshift(255, 23, 32)), -23);
        f = bitand(value(i), (2^23-1));
        if (e == 255)
            if (f ~= 0)
                value(i) = NaN;
            elseif (s == 1)
                value(i) = -Inf;
            elseif (s == 0) 
                value(i) == Inf;
            end
        elseif (e > 0)
            f = f / (2^23) + 1;
            value(i) = (2^(e-127)) * f;
            if (s), value(i) = value(i) * -1;, end
     elseif (e == 0 && f ~= 0)
            f = f / (2^23);
            value(i) = (2^-126) * f;
            if (s), value(i) = value(i) * -1;, end
        elseif (e == 0 && f == 0)
            value(i) = 0;
        end
        
        % Convert to single precision
        value(i) = single(value(i));
      
    end
end